#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;

    //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

#define JOG_DEBOUNCE 160

// handle state transitions
void changeState(int8_t newState, int8_t newChannel=0) {
  switch(newState) {
    case STATE_JOG:
      timer = millis() + JOG_DEBOUNCE;
      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(" ");
    Serial.println();
  }

  state = newState;
  channel = newChannel;
}


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]){
      buttonFallingEdges[i] = false;
      continue;
    }

    if (!digitalRead(53)) {
      changeState(STATE_OFF);
      setOutputs(checkSensor(i+1) ? CMD_OFF:i+1);
    } else if (!digitalRead(51)) {
      if (buttonFallingEdges[i]) {
        if (state == STATE_RUN) {
          changeState(STATE_OFF);
        } else {
          changeState(STATE_RUN, i+1);
        }
      }
    } else {
      changeState(STATE_OFF);
      setOutputs(i+1);
    }

    buttonFallingEdges[i] = false;

    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;
    }
  }
}

/*#define NUM_CMDS 8
#define DEBOUNCE_RF 500
bool lastCmdStates[NUM_CMDS]      = {LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW};
bool currentCmdStates[NUM_CMDS]   = {LOW,LOW,LOW,LOW,LOW,LOW,LOW,LOW};
bool cmdRisingEdges[NUM_CMDS]    = {0,0,0,0,0,0,0,0};
uint32_t lastCmdDebounceTimes[NUM_BUTTONS] = {0, 0, 0, 0, 0};

void checkButtons() {
  for (uint8_t i = 0; i < NUM_CMDS; i++) {
    if (reading != lastCmdStates[i]) {
      lastCmdDebounceTimes[i] = millis();
    }
    
    if ((millis() - lastCmdDebounceTimes[i]) > DEBOUNCE_DELAY) {
      cmdRisingEdges[i] = false;
      if (reading != currentCmdStates[i]) {
        currentCmdStates[i] = reading;
        
        if (currentCmdStates[i]) {
          cmdRisingEdges[i] = true;
        }
      }
    }
    
    lastCmdStates[i] = reading;

    Serial.print(currentButtonStates[i] ? '-':'T');
  }
  //Serial.println();
}*/

#define DEBOUNCE_RF 300

bool cmdFirstSeen[8]  = {0,0,0,0,0,0,0,0};
uint32_t cmdValidTil[8] = {0,0,0,0,0,0,0,0}; //{-10000,-10000,-10000,-10000,-10000,-10000,-10000,-10000};
uint32_t cmdSeenThisScan[8] = {0,0,0,0,0,0,0,0}; //{-10000,-10000,-10000,-10000,-10000,-10000,-10000,-10000};
//bool currentCmdState[8] = {0,0,0,0,0,0,0,0};
//bool currentCmdState[8]   = {0,0,0,0,0,0,0,0};

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[4] != 'J' && buf[4] != 'R') return; // valid command

    int8_t cmd_no = (buf[4] == 'J' ? 0:4) + buf[5]-49;

    if (cmdValidTil[cmd_no] < millis()) {
      cmdFirstSeen[cmd_no] = true;
    }
    cmdSeenThisScan[cmd_no] = true;
    cmdValidTil[cmd_no] = millis() + DEBOUNCE_RF;
  }

  for (uint8_t i=0;i<8;i++) {
    if (cmdSeenThisScan[i] && i<4) {
      changeState(STATE_JOG, i+1);
    } else if (cmdFirstSeen[i]) {
      if (state == STATE_RUN) {
        changeState(STATE_OFF);
      } else {
        changeState(STATE_RUN, i-4+1);
      }
    }
    cmdFirstSeen[i] = false;
    cmdSeenThisScan[i] = false;
  }

    /*if (buf[4] == 'J')
      changeState(STATE_JOG, buf[5]-48);
    if (buf[4] == 'R') {
      if (state == STATE_RUN) {
        if (channel == buf[5]-48) {
          if (lastTimeOfCmd[cmd_no] > lastTimeOfCmd+DEBOUNCE_RF) {
            changeState(STATE_OFF);
          }
        } else {
          changeState(STATE_OFF);
        }
      } else {
        if (lastTimeOfCmd[cmd_no] > lastTimeOfCmd+DEBOUNCE_RF) {
          changeState(STATE_RUN, buf[5]-48);
          lastTimeOfCmd[cmd_no] = millis();
        }
      }*/

      /*// 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);
}