#include "HECB.h"

#include <RH_ASK.h> // RadioHead library
#include <SPI.h> // Required for RadioHead, though not used directly
RH_ASK rf_driver(4000, RF_PORTD, RF_PORTD); // 2000 bps, DATA pin on 17

/*** INPUTS ***/
#define SENS_F_O IN_1
#define SENS_F_C IN_2
#define SENS_R_O IN_3
#define SENS_R_C IN_4

const int8_t SENSORS[] = {SENS_F_O, SENS_F_C, SENS_R_O, SENS_R_C};
#define NUM_SENSORS 4
#define SENSOR_DEBOUNCE_DELAY 20
bool lastSensorStates[NUM_SENSORS]      = {HIGH, HIGH, HIGH, HIGH};
bool currentSensorStates[NUM_SENSORS]   = {HIGH, HIGH, HIGH, HIGH};
bool sensorFallingEdges[NUM_SENSORS]    = {false, false, false, false};
uint32_t lastSensorDebounceTimes[NUM_SENSORS] = {0, 0, 0, 0};

void checkSensors() {
  Serial.print(' ');
  for (uint8_t i = 0; i < NUM_SENSORS; i++) {
    bool reading = digitalRead(SENSORS[i]);
    
    if (reading != lastSensorStates[i]) {
      lastSensorDebounceTimes[i] = millis();
    }
    
    if ((millis() - lastSensorDebounceTimes[i]) > SENSOR_DEBOUNCE_DELAY) {
      if (reading != currentSensorStates[i]) {
        currentSensorStates[i] = reading;
      }
    }
    
    lastSensorStates[i] = reading;
    Serial.print(currentSensorStates[i] ? '-':'T');
  }
  
  Serial.print(' ');
}

bool checkSensor(int8_t ch) {
  return !currentSensorStates[ch-1];
}


//#define REPEAT_INTERVAL 100
#define NUM_BUTTONS 5
#define DEBOUNCE_DELAY 100
const uint8_t BUTTON_PINS[NUM_BUTTONS]  = {BTN1, BTN2, BTN3, BTN4, BTN5};
bool lastButtonStates[NUM_BUTTONS]      = {HIGH, HIGH, HIGH, HIGH, HIGH};
bool currentButtonStates[NUM_BUTTONS]   = {HIGH, HIGH, HIGH, HIGH, HIGH};
bool buttonFallingEdges[NUM_BUTTONS]    = {false, false, false, false, false};
uint32_t lastDebounceTimes[NUM_BUTTONS] = {0, 0, 0, 0, 0};
uint32_t lastRepeatTimes[NUM_BUTTONS]   = {0, 0, 0, 0, 0};

void checkButtons() {
  for (uint8_t i = 0; i < NUM_BUTTONS; i++) {
    bool reading = digitalRead(BUTTON_PINS[i]);
    
    if (reading != lastButtonStates[i]) {
      lastDebounceTimes[i] = millis();
    }
    
    if ((millis() - lastDebounceTimes[i]) > DEBOUNCE_DELAY) {
      if (reading != currentButtonStates[i]) {
        currentButtonStates[i] = reading;
        
        if (currentButtonStates[i] == LOW) {
          buttonFallingEdges[i] = true;
        }
      }
    }
    
    lastButtonStates[i] = reading;
    
    if (buttonFallingEdges[i]) {
      buttonFallingEdges[i] = false;
      lastRepeatTimes[i] = millis();
    }

    /*if (currentButtonStates[i] == LOW && (millis() - lastRepeatTimes[i]) >= REPEAT_INTERVAL) {
      lastRepeatTimes[i] = millis();
      onButtonPush(i);
    }*/
    Serial.print(currentButtonStates[i] ? '-':'T');
  }
  //Serial.println();
}

/*** OUTPUTS ***/
#define CMD_OFF 0
#define CMD_F_O 1
#define CMD_F_C 2
#define CMD_R_O 3
#define CMD_R_C 4

int8_t lastcmd = 0;
int8_t currentcmd = 0;
uint32_t lastt = 0;

#define SWITCH_COOLDOWN 300

void setOutputsRaw(int8_t cmd) {
  if (cmd == CMD_OFF) {
    digitalWrite(RELAY_2A, LOW);
    digitalWrite(RELAY_2B, LOW);
    digitalWrite(RELAY_3A, LOW);
    digitalWrite(RELAY_3B, LOW);
  } else {
    digitalWrite(RELAY_2A, (cmd == CMD_F_O || cmd == CMD_R_O) ? HIGH : LOW);
    digitalWrite(RELAY_2B, (cmd == CMD_F_C || cmd == CMD_R_C) ? HIGH : LOW);
    digitalWrite(RELAY_3A, (cmd == CMD_R_O || cmd == CMD_R_C) ? HIGH : LOW);
    digitalWrite(RELAY_3B, (cmd == CMD_F_O || cmd == CMD_F_C) ? HIGH : LOW);
  }
}

void setOutputs(int8_t cmd) { 

  // if requesting to shut off
  if (cmd == CMD_OFF) {
    // go ahead and turn off
    currentcmd = CMD_OFF;
  }
  
  // if we're past cooldown
  if ((millis() - lastt) > SWITCH_COOLDOWN) {
    // accept new commands
    currentcmd = cmd;
  // if not past cooldown and trying a new command
  } else if (cmd != currentcmd) {
    // we shut down
    currentcmd = CMD_OFF;
  }
   
  // if we're not off
  if (currentcmd != CMD_OFF) {
    // reset cooldown timer
    lastt = millis();
  }

  Serial.print("CMD_");
  Serial.print(currentcmd);
  Serial.print(" ");

  setOutputsRaw(currentcmd);
}

/*** FSM ***/
uint32_t timer;
int8_t channel;
int8_t state;
#define STATE_OFF 0
#define STATE_JOG 1
#define STATE_RUN 2

// handle state transitions
void changeState(int8_t newState, int8_t newChannel=0) {
  switch(newState) {
    case STATE_JOG:
      timer = millis() + 200;
      break;
    case STATE_RUN:
      if (state != newState || channel != newChannel)
        timer = millis() + ((channel%2) ? 9000 : 16000);
      break;
    case STATE_OFF:
      break;

  }


  if (state != newState || channel != newChannel) {
    Serial.print("STATE: ");
    Serial.print(newState);
    Serial.print(":");
    Serial.print(newChannel);
    Serial.print(" ");
  }

  state = newState;
  channel = newChannel;
}


uint32_t btn_runt=0;
#define DEBOUNCE_BTN_RUN 350

void runFSM() {
  // if any button is pressed, cancel any command, run the output manually.
  bool overridden = false;

  for (int8_t i=0; i<4; i++) {
    if (currentButtonStates[i]) continue;

    if (!digitalRead(53)) {
      changeState(STATE_OFF);
      setOutputs(checkSensor(i+1) ? CMD_OFF:i+1);
    } else if (!digitalRead(51)) {
      
      // TODO: iron this
      if (state == STATE_RUN) {
        // if running and a different channel, just shut off
        if (channel != i+1) {
          changeState(STATE_OFF);
        // if running and the same channel, and we are past the time of the button being pressed
        } else if (millis() > btn_runt) {
          changeState(STATE_OFF);
        } else {
          changeState(STATE_RUN, i+1);
          btn_runt = millis() + DEBOUNCE_BTN_RUN;
        }
      } else {
        changeState(STATE_RUN, i+1);
        btn_runt = millis() + DEBOUNCE_BTN_RUN;
      }

    } else {
      changeState(STATE_OFF);
      setOutputs(i+1);
    }

    overridden = true;
  }

  //if (digitalRead(53) && digitalRead(51)) overridden = true;

  if (!overridden) {
    // check for automatic state transitions
    switch(state) {
      case STATE_JOG:
        if (millis() > timer)
          changeState(STATE_OFF);

      case STATE_RUN:
        if (millis() > timer)
          changeState(STATE_OFF);
        if (checkSensor(channel))
          changeState(STATE_OFF);
        break;
    }


    // go to outputs
    switch(state) {
      case STATE_JOG:
        setOutputs(channel);
        break;

      case STATE_RUN:
        setOutputs(channel);
        break;

      case STATE_OFF:
        setOutputs(CMD_OFF);
        break;
    }
  }
}

uint32_t rf_runt = 0;
#define DEBOUNCE_RF 500

void checkRFTransmissions() {
  uint8_t buf[RH_ASK_MAX_MESSAGE_LEN]; // Buffer for incoming message
  uint8_t buflen = sizeof(buf);

  if (rf_driver.recv(buf, &buflen)) { // Check if a message is received
    buf[buflen] = '\0';
    Serial.print("COMMAND: ");
    Serial.println((char*)buf);
    // check if command is correct length
    if (buflen != 7) return;                    // length
    if (buf[0]!='h') return;
    if (buf[1]!='e') return;
    if (buf[2]!='c') return;
    if (buf[3]!='b') return;
    if ((buf[4]+buf[5]) != buf[6]) return;      // checksum
    if (buf[5] < 49 || buf[5] > 52) return;     // valid channel
    //if (buf[0] != 'J' && buf[0] != 'R') return; // valid command

    if (buf[4] == 'J')
      changeState(STATE_JOG, buf[5]-48);
    if (buf[4] == 'R') {
      // if running
      if (state == STATE_RUN) {
        // if running and a different channel, just shut off
        if (channel != buf[5]-48){
          changeState(STATE_OFF);
        // if running and the same channel, and we are past the time of the button being pressed
        } else if (millis() > rf_runt) {
          changeState(STATE_OFF);
        } else {
          changeState(STATE_RUN, buf[5]-48);
          rf_runt = millis() + DEBOUNCE_RF;
        }
      } else {
        changeState(STATE_RUN, buf[5]-48);
        rf_runt = millis() + DEBOUNCE_RF;
      }
    }
      
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Good Morning!");

  if (!rf_driver.init()) {
    Serial.println("RF Receiver initialization failed!");
  }

  setupHECB();
}

void loop() {
  checkButtons();
  checkSensors();
  checkRFTransmissions();

  runFSM();

  // check if both traps are closed
  if(!checkSensor(CMD_F_C) || !checkSensor(CMD_R_C)) {
    digitalWrite(RELAY_4A, HIGH);
  } else {
    digitalWrite(RELAY_4A, LOW);
  }

  //Serial.println("loop");

  Serial.println();

  //delay(5);
}