#include <Arduino.h>
//###: #include <RCSwitch.h>

#define RC_1_CODE 13943076UL;
#define RC_2_CODE 5921060UL;

//###: RCSwitch remoteControl = RCSwitch();

int incomingByte = 0; // for incoming serial data

unsigned int SignalStatus = 0b0000000000000000; //digital data
unsigned int prevSignalStatus = 0b0000000000000000;
unsigned int DbgControlState = 0b0000000000000000; //digital data
unsigned int prevStateMachine = 0b0000000000000000;

enum ControlState {
  CTRL_INIT,
  CTRL_IDLE,
  CTRL_STOP,
  CTRL_AWAIT,
  CTRL_OPEN,
  CTRL_CLOSE,
  CTRL_EM_IR,
  CTRL_EM_BTN,
  CTRL_EM_RELEASE
};

enum GateState {
  GATE_UNKNOWN = 0,
  GATE_OPENED = 1,
  GATE_CLOSED = 2,
  GATE_IMPLAUSIBLE = 3
};

// enum RemoteControlState {
//   RC_INACTIVE = 0,
//   RC_ACTIVE = 1
// };

enum MotorState {
  MOTOR_STOP = 0,
  MOTOR_OPEN = 1,
  MOTOR_CLOSE = 2
};

enum WarnLampState {
  WARN_OFF = 0,
  WARN_ON = 1,
  WARN_BLINK = 2
};

//digital pins
                                  //DI/DO/RX = 0, RX LED connected, Serial Com - do not connect when using Serial
                                  //DI/DO/TX = 1, TX LED connected, Serial Com - do not connect when using Serial
#define PIN_RECEIVER 0            //DI/DO/INT = 2 (2 as INTERUPT = 0)
#define PIN_MOTOR_CLOSE 3         //DI/DO/PWM/INT = 3 --- W ---  //#define PIN_INT1 //DI/DO/INT = 3 (3 as INTERUPT = 1)
#define PIN_MOTOR_OPEN 4          //DI/DO = 4 --- U ---
#define PIN_WARN_LAMP 5           //DI/DO/PWM = 5 --- D1-D2 ---
#define PIN_LEARN_BTN 6           //DI/DO/PWM = 6
                                  //DI/DO = 7
#define PIN_REMOTE_CONTROL 8      //DI/DO = 8 --- EXTERNAL MODULE ---
#define PIN_IR_SENSOR 9           //DI/DO/PWM = 9 --- I.R ---
#define PIN_GATE_CLOSED 10        //DI/DO/PWM = 10 --- CCLM ---
#define PIN_GATE_OPENED 11        //DI/DO/PWM = 11 --- OPLM ---
#define PIN_STOP_BUTTON 12        //DI/DO = 12 --- STP ---
#define PIN_STATUS_LED 13         //DI/DO, Pin 13 has an LED connected on most Arduino boards - used as lifesign indicator

//#define PIN_POTI_TODO A0  //ai/di/do = A0 /* (v2.0) int pinMotorCurrent = A0; */
//#define PIN_MOTOR_CURRENT_TODO A1  //ai/di/do = A1
//ai/di/do = A2
//ai/di/do = A3
//ai/di/do = A4
//ai/di/do = A5

void setupSerial();
void setupPins();
void readDigitalInputs();
void readRemoteControl();
void controlGate();
void controlMotor(int cmd);
void controlWarnLamp(int cmd);
void writeDigitalOutputs();
void loopLifeSignLED();
void debugSignalStatus();

//rising edge detection variables
bool prevRCActive;
bool prevIRCtAct;
bool prevEmergencyStop;

//state machines
MotorState lastMotorDirection = MOTOR_STOP;
MotorState motorState = MOTOR_STOP;
ControlState controlState = CTRL_STOP;
GateState gateState = GATE_UNKNOWN;
//RemoteControlState remoteControlState = RC_INACTIVE;

// int ledTimer = 0;
// bool ledOn = false;
// int delaySerialPrint = 0;
unsigned int openTimeout = 30000; //30s
unsigned long openTime = 0;
unsigned long startTime = 0;

//remote control
unsigned long rcIdValue[2]; //Speichern der IDs von zwei Tastern
unsigned long inputDebounce[2]; //Entprellung: Speichern der Timerwerte von zwei Tastern
bool rcPressed[2] = { false, false }; //Entprellung: Speichern der Eingangssignale von zwei Tastern
bool rcCmdActive[2] = { false, false }; //Speichern des entprellten Ausgangssignals von zwei Tastern
int countRCAct = 0;

//input values
bool isEmergencyStop = false;
bool EmergencyStopLatched = false;
bool isIRActive = false;
bool RCActive = false;
bool RCActivated = false;
bool RCDeactivated = false;
bool RCLatched = false;
bool RCLearn = false;

//output values
bool CWarnLamp = false; //TODO: State!

// SETUP ########################################

// the setup routine runs initial
void setup() {
  setupSerial();  //deactivates use of DI/DO 0 and 1 !!!
  //Serial.println();
  //Serial.println("Start setup...");
  setupPins();
  //Serial.println("Setup done!");
}

// initialize serial communication at 9600 bits per second
// Arduino UNO has a serial interface constroller for USB com.
// to be able to read console inputs and send console outputs
void setupSerial() {
  //Serial com USB/RS
  Serial.begin(9600);
  //###: remoteControl.enableReceive(0); //PIN2
}

void setupPins() {
  //ACHTUNG! Summe aller Ströme DIN/AIN <! 200mA

  //digital inputs
  pinMode(PIN_RECEIVER, INPUT);
  pinMode(PIN_IR_SENSOR, INPUT);
  pinMode(PIN_GATE_CLOSED, INPUT);
  pinMode(PIN_GATE_OPENED, INPUT);
  pinMode(PIN_STOP_BUTTON, INPUT_PULLUP); //###: INPUT
  //pinMode(PIN_REMOTE_CONTROL, INPUT_PULLUP); //external relais contactor
  pinMode(PIN_LEARN_BTN, INPUT);

  //digital outputs
  pinMode(PIN_MOTOR_CLOSE, OUTPUT);
  pinMode(PIN_MOTOR_OPEN, OUTPUT);
  pinMode(PIN_WARN_LAMP, OUTPUT);
  pinMode(PIN_STATUS_LED, OUTPUT);

  //analog
}

// LOOP ########################################

// the loop routine runs over and over again forever:
void loop() {

  //INPUT
  readDigitalInputs();
  //###: readRemoteControl();

  //CONTROL
  controlGate();

  //OUTPUT
  writeDigitalOutputs();

  //DEBUG
  debugSignalStatus();
  debugStateMachine();

  //Lifesign
  loopLifeSignLED();
}

void readDigitalInputs() {

  //read end switches
  if (!digitalRead(PIN_GATE_CLOSED)) gateState = GATE_CLOSED; //negated input PIN_GATE_CLOSED!
  if (!digitalRead(PIN_GATE_OPENED)) gateState = GATE_OPENED; //negated input PIN_GATE_OPENED!
  if (digitalRead(PIN_GATE_CLOSED) && digitalRead(PIN_GATE_OPENED)) gateState = GATE_UNKNOWN;
  if (!digitalRead(PIN_GATE_CLOSED) && !digitalRead(PIN_GATE_OPENED)) gateState = GATE_IMPLAUSIBLE;

  RCLearn = !digitalRead(PIN_LEARN_BTN); //negated input PIN_GATE_CLOSED!

  //read and latch emergency button
  isEmergencyStop = !digitalRead(PIN_STOP_BUTTON); //negated input PIN_STOP_BUTTON!
  if (isEmergencyStop) EmergencyStopLatched = true;
  //read and latch IR signals
  isIRActive = (!prevIRCtAct && !digitalRead(PIN_IR_SENSOR)); //negated input PIN_IR_SENSOR!
  //latch button signals (Selbsthaltung)
  prevIRCtAct = (digitalRead(PIN_IR_SENSOR) || prevIRCtAct);
  prevEmergencyStop = (digitalRead(PIN_STOP_BUTTON) || prevEmergencyStop);
}

//###
/*
void readRemoteControl() {
  rcIdValue[0] = RC_1_CODE;
  rcIdValue[1] = RC_2_CODE;
  
  rcPressed[0] = false;
  rcPressed[1] = false;

  rcCmdActive[0] = false;
  rcCmdActive[1] = false;

  //Read remote control wireles module
  if (remoteControl.available()) {
    unsigned long value = remoteControl.getReceivedValue();

    if (value == rcIdValue[0]) {
      rcPressed[0] = true;
      if (inputDebounce[0] == 0) inputDebounce[0] = millis(); //set only once
      if ((millis()-inputDebounce[0]) >= 100) {
        rcCmdActive[0] = true;
      }
    }

    if (value == rcIdValue[1]) {
      rcPressed[1] = true;
      if (inputDebounce[1] == 0) inputDebounce[1] = millis(); //set only once
      if ((millis()-inputDebounce[1]) >= 100) {
        rcCmdActive[1] = true;
      }
    }

    //LEARN RC
    if (RCLearn) {
      //newRCcode = value;
      //WRITE TO EEPROM
    }

    //DEBUG
    // Serial.print(" / VAL:");
    // Serial.print(remoteControl.getReceivedValue());
    // Serial.print(" / LEN:");
    // Serial.print(remoteControl.getReceivedBitlength());
    // Serial.print(" / DLY:");
    // Serial.print(remoteControl.getReceivedDelay());
    // Serial.print(" / RAW:");
    // Serial.print(*remoteControl.getReceivedRawdata());
    // Serial.print(" / PRT:");
    // Serial.println(remoteControl.getReceivedProtocol());

    remoteControl.resetAvailable();
    delay(100); //IMPORTANT! DO NOT REMOVE! reduce loop speed to be able to receive all rc values
  }
  if ((rcPressed[0] == false) && (inputDebounce[0] > 0)) inputDebounce[0] = 0;
  if ((rcPressed[1] == false) && (inputDebounce[1] > 0)) inputDebounce[1] = 0;

  //check REDUNDANCY on pin 8 by remote module AND on receiver
  //--------------------------RCActive = (!digitalRead(PIN_REMOTE_CONTROL) && (rcCmdActive[0] || rcCmdActive[1])); //negated input PIN_REMOTE_CONTROL!
  RCActive = (rcCmdActive[0] || rcCmdActive[1]);
  RCActivated = (RCActive && !prevRCActive); //catch rising edge
  RCDeactivated = (!RCActive && prevRCActive); //catch falling edge
  if (RCActive && !prevRCActive) RCLatched = RCActivated; //latch, need to be released after latch!
  prevRCActive = RCActive;

  // not needed
  // if (RCActivated) {
  //   remoteControlState = RC_ACTIVE; //latched state
  // } else if (RCDeactivated) {
  //   remoteControlState = RC_INACTIVE; //latched state
  // }

  // //DEBUG
  // if (RCActivated) {
  //   if (rcCmdActive[0]) Serial.print("REMOTE ANDREA, ");
  //   if (rcCmdActive[1]) Serial.print("REMOTE TOMMY, ");
  //   if (RCActive) { Serial.print("RC Active"); } else { Serial.print("RC NOT Active"); };
  //   if (RCActivated) Serial.print(" | Latched");
  //   Serial.println();
  // }
}
*/

void controlGate() {

  DbgControlState = 0b0000000000000000;
  
  switch (controlState) {
    case CTRL_INIT:
      controlMotor(MOTOR_STOP);
      bitWrite(DbgControlState, 0, 1);
      controlState = CTRL_STOP;
      Serial.println("INF: [CTRL_INIT] Init gate state.");
      Serial.println("INF: [CTRL_INIT] Check for failures: ");
      // ------- TODO ---------
      break;
    case CTRL_STOP:
      controlMotor(MOTOR_STOP);
      controlWarnLamp(WARN_ON);
      bitWrite(DbgControlState, 10, 1);
      delay(1000);
      controlState = CTRL_IDLE;
      break;
    case CTRL_IDLE:
      controlWarnLamp(WARN_OFF);
      bitWrite(DbgControlState, 9, 1);
      openTime = (millis() - startTime); //only subtract timer to avoid overflow issued with millis() jumps to zero again after abt. 50 days
      if (EmergencyStopLatched) {
        controlState = CTRL_EM_BTN;
      } else if (RCActivated) {
        switch (gateState)
        {
          case GATE_UNKNOWN:
            bitWrite(DbgControlState, 8, 1); //REVERT
            if (lastMotorDirection == MOTOR_CLOSE) {
              controlState = CTRL_OPEN;
            } else if (lastMotorDirection == MOTOR_OPEN) {
              controlState = CTRL_CLOSE;
            } else {
              controlState = CTRL_OPEN;
              Serial.print("ERR: [CTRL_IDLE] Unknown gate state ");
              Serial.print(gateState);
              Serial.print(" and unknown motor direction: ");
              Serial.print(lastMotorDirection);
              Serial.println(". OPEN gate!");
            }
            break;
          case GATE_CLOSED:
            controlState = CTRL_OPEN;
            break;
          case GATE_OPENED:
            controlState = CTRL_CLOSE;
            break;
          case GATE_IMPLAUSIBLE:
            controlState = CTRL_STOP;
            Serial.print("ERR: [CTRL_IDLE] Implausible gate state: ");
            Serial.println(gateState);
            break;
          default:
            controlState = CTRL_STOP;
            Serial.print("ERR: [CTRL_IDLE] Unknown gate state command: ");
            Serial.println(gateState);
            break;
        }
      } else if (openTime > openTimeout) {
        Serial.println("INF: [CTRL_IDLE] Wait for timer!");
        controlState = CTRL_CLOSE;
      }
      break;
    case CTRL_OPEN:
      controlMotor(MOTOR_OPEN);
      controlWarnLamp(WARN_BLINK);
      bitWrite(DbgControlState, 4, 1);
      //--------- TODO: STOP GATE AFTER A TIMEOUT -----------
      if (EmergencyStopLatched) {
        controlState = CTRL_EM_BTN;
      } else if (RCActivated) {
        controlState = CTRL_STOP;
        Serial.println("INF: [CTRL_OPEN] Manual STOP by Remote Control!");
      } else if (gateState == GATE_OPENED) {
        bitWrite(DbgControlState, 5, 1);
        Serial.println("INF: [CTRL_OPEN] STOP by end switch. Close timer starts!");
        startTime = millis(); //start timer by new value
        controlState = CTRL_STOP;
      }
      break;
    case CTRL_CLOSE:
      controlMotor(MOTOR_CLOSE);
      controlWarnLamp(WARN_BLINK);
      bitWrite(DbgControlState, 6, 1);
      //--------- TODO: STOP GATE AFTER A TIMEOUT -----------
      if (EmergencyStopLatched) {
        controlState = CTRL_EM_BTN;
      } else if (RCActivated) {
        controlState = CTRL_STOP;
        bitWrite(DbgControlState, 3, 1);
        Serial.println("INF: [CTRL_CLOSE] Manual STOP by Remote Control!");
      } else if (gateState == GATE_CLOSED) {
        bitWrite(DbgControlState, 7, 1);
        controlState = CTRL_STOP;
      }
      break;
    case CTRL_EM_BTN:
      controlMotor(MOTOR_STOP);
      controlWarnLamp(WARN_ON);
      bitWrite(DbgControlState, 1, 1);
      if (RCActivated) {
        countRCAct++;
        //count RC press three times
        if (countRCAct >= 3) {
          controlState = CTRL_INIT;
          EmergencyStopLatched = false;
          Serial.println("INF: [CTRL_EM_BTN] Emergency STOP released!");
          countRCAct = 0;
        }
      }
      break;
    case CTRL_EM_IR:
      controlMotor(MOTOR_STOP);
      controlWarnLamp(WARN_ON);
      bitWrite(DbgControlState, 2, 1);
      delay(2000);
      controlState = CTRL_OPEN;
      break;
    default:
      controlState = CTRL_STOP;
      Serial.println("ERR: [CTRL_EM_IR] Unknown control state.");
      break;
  }
}

bool movementAllowed() {
  return (!EmergencyStopLatched && !isIRActive);
}

void controlMotor(MotorState cmd) {
  motorState = cmd;
  if ((motorState == MOTOR_CLOSE) || (motorState == MOTOR_OPEN)) {
    lastMotorDirection = motorState;
  }
  delay(300);
}

//TODO, WarnLampState
void controlWarnLamp(WarnLampState cmd) {
  switch (cmd) {
    case WARN_ON:
        CWarnLamp = HIGH;
      break;
    case WARN_OFF:
        CWarnLamp = LOW;
      break;
    case WARN_BLINK:
        ((millis() % 2000) > 1000) ? CWarnLamp = HIGH : CWarnLamp = LOW;
        CWarnLamp = HIGH;
      break;
    default:
        CWarnLamp = LOW;
        Serial.print("ERR: [WARNLAMP] Unknown warn lamp state: ");
        Serial.println(cmd);
      break;
  }
}

void writeDigitalOutputs() {
  digitalWrite(PIN_MOTOR_CLOSE, (motorState == MOTOR_OPEN) ? HIGH : LOW);
  digitalWrite(PIN_MOTOR_OPEN, (motorState == MOTOR_CLOSE) ? HIGH : LOW);
  digitalWrite(PIN_WARN_LAMP, (CWarnLamp) ? HIGH : LOW);
}

void debugSignalStatus() {
  bitWrite(SignalStatus, 0, isIRActive);
  bitWrite(SignalStatus, 1, (motorState == MOTOR_OPEN));
  bitWrite(SignalStatus, 2, (motorState == MOTOR_CLOSE));
  bitWrite(SignalStatus, 3, (motorState == MOTOR_STOP));
  bitWrite(SignalStatus, 4, (gateState == GATE_OPENED));
  bitWrite(SignalStatus, 5, (gateState == GATE_UNKNOWN));
  bitWrite(SignalStatus, 6, (gateState == GATE_CLOSED));
  bitWrite(SignalStatus, 7, EmergencyStopLatched);
  bitWrite(SignalStatus, 8, RCActivated);
  
  if (SignalStatus != prevSignalStatus)
  {
    Serial.print("SIGNALS: ");
    Serial.print((0b10000000000000000|SignalStatus),BIN);
    if (SignalStatus & 0b0000000000000001) Serial.print(" | IR Active");
    if (SignalStatus & 0b0000000000000010) Serial.print(" | Motor Opening");
    if (SignalStatus & 0b0000000000000100) Serial.print(" | Motor Closing");
    if (SignalStatus & 0b0000000000001000) Serial.print(" | Motor Stopped");
    if (SignalStatus & 0b0000000000010000) Serial.print(" | Gate Opened");
    if (SignalStatus & 0b0000000000100000) Serial.print(" | Gate Middle");
    if (SignalStatus & 0b0000000001000000) Serial.print(" | Gate Closed");
    if (SignalStatus & 0b0000000010000000) Serial.print(" | Em. Stop Latched");
    if (SignalStatus & 0b0000000100000000) Serial.print(" | RC Activated");
    Serial.println();
  }
  prevSignalStatus = SignalStatus;
}

void debugStateMachine() {
  if ((DbgControlState != prevStateMachine) && (DbgControlState > 0))
  {
    //Serial.print((0b10000000000000000|DbgControlState),BIN);
    Serial.print("STATE:");
    if (DbgControlState & 0b0000000000000001) Serial.print(" | INIT"); //0
    if (DbgControlState & 0b0000000000000010) Serial.print(" | EM BUTTON"); //1
    if (DbgControlState & 0b0000000000000100) Serial.print(" | IR SENSOR"); //2
    if (DbgControlState & 0b0000000000001000) Serial.print(" | STOP: REMOTE"); //3
    if (DbgControlState & 0b0000000000010000) Serial.print(" | NORMAL OPEN"); //4
    if (DbgControlState & 0b0000000000100000) Serial.print(" | STOP: GATE OPENED"); //5
    if (DbgControlState & 0b0000000001000000) Serial.print(" | NORMAL CLOSE"); //6
    if (DbgControlState & 0b0000000010000000) Serial.print(" | STOP: GATE CLOSED"); //7
    if (DbgControlState & 0b0000000100000000) Serial.print(" | REVERT GATE"); //8
    if (DbgControlState & 0b0000001000000000) Serial.print(" | IDLE"); //9
    if (DbgControlState & 0b0000010000000000) Serial.print(" | STOP"); //10
    Serial.println();
  }
  prevStateMachine = DbgControlState;
}

//loop blinking of lifesign LED
void loopLifeSignLED() {
  if (millis() % 2000 > 1000) {
    digitalWrite(PIN_STATUS_LED, LOW); //OFF
  } else {
    digitalWrite(PIN_STATUS_LED, HIGH); //ON
  }
}