#include <avr/wdt.h>   // Include the AVR watchdog timer library
#include "debug.hpp"
#include <avr/io.h>
#include <avr/interrupt.h>
#include "OneButton.h"
#include "CaliperState.hpp"

// Define states
enum State {
  BOOT = 0,
  FAILED = 1,
  WATCHDOG = 2,
  BRAKING = 3,
  OPENING = 4,
  CLOSED = 5,
  OPENED = 6,
  SERVICE_OPENING = 7,
  SERVICE_OPENED = 8,
  UNDEFINED = 9
};

CaliperState leftCaliperState;
CaliperState rightCaliperState;

volatile bool watchdogFired = false;


#define STOP_ON_PIN 2 //STOP Light
#define LED_ON_PIN 3 //PC3

// #define IGNITION_ON_PIN PIN_PC1 //PC1

#define OVERCURRENT_PIN1 4 //PD3
#define OVERCURRENT_PIN2 5 //PD2

#define INH1_PIN 6 // PB1
#define IN1_PIN  7 // PE3 („1” zamykanie, „0”- otwieranie: ustawienie kierunku, a włączenie sygnałem INH)
#define INH2_PIN 8 // PD1
#define IN2_PIN 9 // PD0

// pins "moved" from ESP
#define HOPER_PIN 10
#define PARKING_GEAR_PIN 11
#define PARKING_BUTTON_PIN 12

OneButton parkingButton(PARKING_BUTTON_PIN, false, false);
OneButton parkingGearButton(PARKING_GEAR_PIN, false, false);
OneButton hopperButton(HOPER_PIN, false, false);


// Initial state
State currentState = BOOT;

volatile unsigned long defaultCalipersOpeningTime = 2500; 
volatile unsigned long defaultCalipersClosingTime = 2500;
volatile unsigned long minimumCaliperOpeningTime = 100;
volatile unsigned long maxOvercurrentPeriodLength = 70;
volatile unsigned long initialDelayTime = 2000;
volatile unsigned long serviceModeOpeningTime = 5000;
volatile unsigned long caliperClosingCurrent = 5;
volatile unsigned long serviceModePressSeconds = 5000;
volatile unsigned long towingModePressSeconds = 30000;
volatile unsigned long paringGearDebounceTime = 100;
volatile unsigned long parkingButtonDebounceTime = 100;
volatile unsigned long hoperDebounceTime = 100;
volatile unsigned long brakeStartingTime = 0;
volatile unsigned long openingStartingTime = 0;
volatile unsigned long calipersOpeningTime = defaultCalipersClosingTime;
volatile unsigned long calipersClosingTime = defaultCalipersOpeningTime;

volatile int currentButtonState = LOW;

void disableBTN() {
  digitalWrite(IN1_PIN, LOW);
  digitalWrite(IN2_PIN, LOW);
  digitalWrite(INH1_PIN, LOW);
  digitalWrite(INH2_PIN, LOW);
}

void disableLeftBTN() { // disable BTN pins
  PRINTS("\ndisableLeftBTN");
  digitalWrite(INH1_PIN, LOW);
  digitalWrite(IN1_PIN, LOW);
}

void disableRightBTN() { // disable BTN pins
  PRINTS("\ndisableRightBTN");
  digitalWrite(INH2_PIN, LOW);
  digitalWrite(IN2_PIN, LOW);
}

void enableRightBTNopen()
{
  PRINTS("\nenableRightBTNopen");
  digitalWrite(IN2_PIN, LOW);    // activate OPEN
  digitalWrite(INH2_PIN, HIGH);  // activate RIGHT BTN
}

void enableLeftBTNopen()
{
  PRINTS("\nenableLeftBTNopen");
  digitalWrite(IN1_PIN, LOW);    // activate OPEN
  digitalWrite(INH1_PIN, HIGH);  // activate LEFT BTN
}

void enableRightBTNclose()
{
  PRINTS("\nenableRightBTNclose");
  digitalWrite(IN2_PIN, HIGH);   // activate CLOSE
  digitalWrite(INH2_PIN, HIGH);  // activate RIGHT BTN
}

void enableLeftBTNclose()
{
  PRINTS("\nenableLeftBTNclose");
  digitalWrite(IN1_PIN, HIGH);   // activate CLOSE
  digitalWrite(INH1_PIN, HIGH);  // activate LEFT BTN
}

void startBraking() {

  brakeStartingTime = millis();

  if (leftCaliperState.getState() == CaliperState::IDLE 
      || leftCaliperState.getState() == CaliperState::OPENED) {

    PRINTS("\n   - closing left caliper");
    leftCaliperState.setState(CaliperState::BRAKING_ACTIVATED);
    enableLeftBTNclose();
  }
  

}

void startOpening() {

  openingStartingTime = millis();

  if (leftCaliperState.getState() == CaliperState::IDLE 
      || leftCaliperState.getState() == CaliperState::CLOSED) {

    PRINTS("\n   - opening left caliper");
    leftCaliperState.setState(CaliperState::OPENING_ACTIVATED);
    enableLeftBTNopen();
  }
  

}

// this function will be called when the button started long pressed.
void ParkingShortPressStart(void *oneButton)
{
  PRINT("\nserviceMode: ", currentState);
  PRINT("\nParkingShortPressStart: ", ((OneButton *)oneButton)->getPressedMs());
  // log_i("brakesClosed: %i, brakeSignalSent: %i, serviceMode: %i, hoperActive: %i, brakingInProgress: %i, openingInProgress: %i", 
  // //        brakesClosed, brakeSignalSent, serviceMode, hoperActive, brakingInProgress, openingInProgress);
  // if (!brakesClosed && !brakeSignalSent
  //     && !serviceMode && !hoperActive && !brakingInProgress 
  //     && !openingInProgress && !parkingGearPressed) {
  //   brakeSignal = true;
  //   openSignal = false;
  //   parkingButtonPressed = true;
  //   if (!serviceMode) {
  //     calipersClosingTime = defaultCalipersClosingTime;
  //   }
  //   PRINTS("\nClosing brakes");
  // } else if (!serviceMode && !hoperActive && !openingInProgress 
  // && !brakingInProgress && !parkingGearPressed){
  //   calipersOpeningTime = defaultCalipersOpeningTime;
  //   brakeSignal = false;
  //   openSignal = true;
  //   parkingButtonPressed = true;
  //   PRINTS("\nOpening brakes");
  // }
}


// this function will be called when the button started long pressed.
void ParkingLongPressStart(void *oneButton)
{
  PRINT("\nParkingLongPressStart: ",((OneButton *)oneButton)->getPressedMs());
}

// this function will be called when the button is released.
void ParkingLongPressStop(void *oneButton)
{
  PRINT("\nserviceMode:", currentState);
  PRINT("\nParkingLongPressStop: ", ((OneButton *)oneButton)->getPressedMs());
  // if (((OneButton *)oneButton)->getPressedMs() >= serviceModePressSeconds) {
  //   PRINTS("\nButton pressed for longer than 5 seconds");
  //   // longPressDetected = true; // Set the flag to prevent repeated action
  //   buttonPressStartTime = 0;
  //   buttonPressTime = 0;
  //   if (serviceMode && !hoperActive) {
  //     PRINTS("\nExiting service mode after long press");
  //     // brakeCommandResult = sendBoardCommand(SLAVE_ADDRESS, 10, 1, 0);
  //     serviceMode = false;
  //     serviceModeStartTime = millis();
  //     brakeSignal = true;
  //     openSignal = false;
  //     brakesClosed = false;
  //     leftCaliperClosed = false;
  //     rightCaliperClosed = false;
  //   } else if (!hoperActive) {
  //     PRINTS("\nEntering service mode after long press");
  //     // brakeCommandResult = sendBoardCommand(SLAVE_ADDRESS, 11, 1, 0);
  //     calipersOpeningTime = serviceModeOpeningTime;
  //     calipersClosingTime = serviceModeOpeningTime;
  //     serviceMode = true;
  //     brakeSignal = false;
  //     openSignal = true;
  //     brakesOpened = false;
  //     leftCaliperOpened = false;
  //     rightCaliperOpened = false;
  //   }
  // }
}

// this function will be called when the button is held down.
void DuringLongPress(void *oneButton)
{
  PRINT("\nDuringLongPress: ", ((OneButton *)oneButton)->getPressedMs());
  
}

bool isWatchdogFired() {
  // Check if watchdog timer fired
  return false;
}

bool isOvercurrent() {
  // Check for overcurrent condition
  return false;
}

bool isTimeout() {
  // Check for timeout condition
  return false;
}

bool hasParkingGearChanged() {

  bool hasParkingGearChanged = false;
  // Variables to store the current and last button state
  static int lastButtonState = LOW;
  

  // Variables for debouncing
  unsigned long lastDebounceTime = 0;

  int reading = digitalRead(PARKING_GEAR_PIN);

  // Check if the button state has changed (i.e. due to noise)
  if (reading != lastButtonState) {
    // Reset the debouncing timer
    lastDebounceTime = millis();
  }

  // Check if the time since the last change is greater than the debounce delay
  if ((millis() - lastDebounceTime) > parkingButtonDebounceTime) {
    // If the button state has changed
    if (reading != currentButtonState) {
      currentButtonState = reading;
      hasParkingGearChanged = true;
      
          // if (currentButtonState == HIGH) {
          //   PRINTS("\n*Parking gear is pressed, braking");
          //   hasParkingGearChanged = true;
          // } else {
          //   PRINTS("\n*Parking gear is released, opening");
          //   hasParkingGearChanged = false;
          // }        
    } else {
      hasParkingGearChanged = false;
    }
  }
  // Save the reading. Next time through the loop, it'll be the lastButtonState
  lastButtonState = reading;

  return hasParkingGearChanged;
}

bool isLongPress() {
  // Check if button is long pressed
  return false;
}

// Function to handle state transitions
void handleState() {
  switch (currentState) {
    case BOOT:
      disableBTN();
      // Assuming a way to determine if the state should go to Failed or Undefined
      currentState = UNDEFINED; // or FAILED if some condition is met
      break;

    case WATCHDOG:
      currentState = BOOT;
      break;

    case BRAKING:
      if (isOvercurrent()) {
        currentState = CLOSED;
      } else if (isTimeout()) {
        currentState = UNDEFINED;
      }
      break;

    case OPENING:
      if (isOvercurrent()) {
        currentState = OPENED;
      } else if (isTimeout()) {
        currentState = UNDEFINED;
      }
      break;

    case CLOSED:
      if (hasParkingGearChanged()) {
          if (currentButtonState == HIGH) {
          currentState = OPENING;
        } else if (isLongPress()) {
          currentState = SERVICE_OPENING;
        }
      }
      break;

    case OPENED:
      if (hasParkingGearChanged()) {
        if (currentButtonState == HIGH) {
          currentState = BRAKING;
        } else if (isLongPress()) {
          currentState = SERVICE_OPENING;
        }
      }
      break;

    case SERVICE_OPENING:
      if (isOvercurrent()) {
        currentState = SERVICE_OPENED;
      }
      break;

    case SERVICE_OPENED:
      if (isLongPress()) {
        // Assuming a way to determine if the state should be Closed or Undefined on long press
        currentState = CLOSED; // or UNDEFINED
      }
      break;

    case UNDEFINED:
      if (hasParkingGearChanged()) {
        if (currentButtonState == HIGH) {
          currentState = BRAKING;
          startBraking();
        } else {
          currentState = OPENING;
          startOpening();
        }
      }
      break;

    case FAILED:
      // Handle failed state
      break;
  }
}

void setup() {
  pinMode(STOP_ON_PIN, OUTPUT);
  pinMode(LED_ON_PIN, OUTPUT);
  pinMode(INH1_PIN, OUTPUT);
  pinMode(IN1_PIN, OUTPUT);
  pinMode(INH2_PIN, OUTPUT);
  pinMode(IN2_PIN, OUTPUT);
  pinMode(OVERCURRENT_PIN1, INPUT);
  pinMode(OVERCURRENT_PIN2, INPUT);
  
  #if (DEBUG)
    Serial.begin(115200);
    delay(10);
    Serial.println ();
    Serial.println("\n\n*** Boot after reset ***\n");
  #endif

  delay(initialDelayTime); //wait for board to wakeup

  PRINTS("\nInit BTN");
  digitalWrite(IN1_PIN, HIGH);
  digitalWrite(IN2_PIN, HIGH);
  delay(5);
  digitalWrite(IN1_PIN, LOW);
  digitalWrite(IN2_PIN, LOW);

  // link functions to be called on events.
  parkingButton.attachLongPressStart(ParkingLongPressStart, &parkingButton);
  parkingButton.attachDuringLongPress(DuringLongPress, &parkingButton);
  parkingButton.attachLongPressStop(ParkingLongPressStop, &parkingButton);
  // parkingButton.attachClick(ParkingShortPressStart, &parkingButton);

  parkingButton.setDebounceMs(int(parkingButtonDebounceTime));
  parkingButton.setPressMs(2000);
  parkingButton.setIdleMs(1000);
  parkingButton.setLongPressIntervalMs(500);

  parkingGearButton.setDebounceMs(int(paringGearDebounceTime));

  hopperButton.setDebounceMs(hoperDebounceTime);
  parkingButton.setPressMs(500);

  currentButtonState = digitalRead(PARKING_GEAR_PIN);
}

void loop() {
  handleState();
  // Add a small delay to avoid busy-waiting
  PRINT("\nState: ", currentState);
  PRINT(" leftCaliper: ", leftCaliperState.getState());
  delay(500);
}