#include <ModbusMaster.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,  16, 2);

#define RS485_RE_DE 2  // Control pin for RS485 mode
#define VFD_SLAVE_ID 1  // VFD Modbus address

// Modbus Registers for VFD control
#define VFD_CONTROL_REGISTER 0x2000 // Placeholder - Adjust as per VFD manual
#define VFD_STOP_REGISTER 0x2000 // Adjust as per VFD manual for stop functionality
#define VFD_SPEED_REGISTER 0x2001   // Placeholder - Adjust as per VFD manual

#define FORWARD 1  // Command value for forward (example)
#define REVERSE 2  // Command value for reverse (example)
#define STOP 0     // Command value for stop (example)

#define BRAKE_PIN 7 // Relay control pin for the brake system
#define MOTOR_RUN_PIN 4 // Relay control pin for the MOTOR system
#define MOTOR_DIRECTION_PIN 11 // High = Drive Foward -  Low = Reverse


// Pins for door and position sensors
#define CABIN_DOOR_PIN 2
#define FLOOR1_DOOR_PIN 3

#define LOWER_LEVEL__PIN 6
#define HIGHER_LEVEL_PIN 5

#define HIGHEST_FLOOR 4
#define LOWEST_FLOOR -1

#define APPROACHING_SIGNAL_PIN 8
#define SOFT_STOP_SIGNAL_PIN 9
#define FULL_STOP_SIGNAL_PIN 10


#define DOOR_OPEN_PIN 12    // MOTOR Pin to control the door opening mechanism
#define DOOR_CLOSE_PIN 13   // MOTOR Pin to control the door closing mechanism

bool isMotorMoving = false;
bool motorDirectionForward = FORWARD; // true for forward, false for reverse
bool isDoorsClosed = false;
int currentSpeed = 1000; // Example default speed

#define BUTTON_UP_PIN A0  // Define your pin for the "up" button
#define BUTTON_DOWN_PIN A1 // Define your pin for the "down" button

int floorStatus = LOWEST_FLOOR - 1;
int floorRequest = LOWEST_FLOOR;
int btnUp, btnDown;

ModbusMaster modbus;

// Placeholder register addresses - replace with actual addresses from the VFD manual
#define MOTOR_STATUS_REGISTER 0x2000 // Register holding motor running status
#define MOTOR_DIRECTION_REGISTER 0x2000 // Register holding motor direction status

// Function to initialize Modbus communication
void setupModbus() {
  // Assuming Serial2 is connected to RS485 module
  Serial.begin(115200); // Qualquer taxa de transmissão deve funcionar
  Serial.println("setupModbus(): Inicio Setup\n");

  modbus.begin(VFD_SLAVE_ID, Serial);
  pinMode(RS485_RE_DE, OUTPUT);
  digitalWrite(RS485_RE_DE, LOW); // Enable RE/DE for transmission
  Serial.println("setupModbus(): Modbus Configurado\n");

}

void setup() {

  //initialize lcd screen
  lcd.init();
  // turn on the backlight
  lcd.backlight();
  setupModbus();
  pinMode(BRAKE_PIN, OUTPUT);
  pinMode(CABIN_DOOR_PIN, INPUT_PULLUP);
  pinMode(FLOOR1_DOOR_PIN, INPUT_PULLUP);
  pinMode(APPROACHING_SIGNAL_PIN, INPUT_PULLUP);
  pinMode(SOFT_STOP_SIGNAL_PIN, INPUT_PULLUP);
  pinMode(FULL_STOP_SIGNAL_PIN, INPUT_PULLUP);

  pinMode(DOOR_OPEN_PIN, OUTPUT);
  pinMode(DOOR_CLOSE_PIN, OUTPUT);
  pinMode(MOTOR_RUN_PIN, OUTPUT);
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  pinMode(BUTTON_UP_PIN, INPUT);
  pinMode(BUTTON_DOWN_PIN, INPUT);



  digitalWrite(BRAKE_PIN, LOW); // Assuming HIGH disengages the brake
  digitalWrite(MOTOR_RUN_PIN, LOW); // Assuming HIGH disengages the motor
  digitalWrite(MOTOR_DIRECTION_PIN, LOW); // Assuming Reverse direction (going down!!)

  lcd.setBacklight(HIGH);

  begining(); 

}

void begining(){
   lcd.setCursor(0, 0);
  lcd.print("Iniciando... The Ramp Church");
  delay(500);

  lcd.setCursor(0, 1);
  lcd.print("Atualizando Status Motor\n");
  Serial.println("setup(): Atualizando Status Motor\n");
  updateMotorStatus();
  delay(500);

  lcd.setCursor(0, 1);
  lcd.print("StopMotor\n");
  Serial.println("setup(): stopMotor\n");
  stopMotor();

  delay(500);

  lcd.setCursor(0, 1);
  lcd.print("Fechando Portas \n");
  Serial.println("setup(): Fechando Portas para iniciar descida\n");
  closeDoors();

  delay(500);

  lcd.setCursor(0, 1);
  lcd.print("Fechando Portas \n");

  delay(5000);

  lcd.setCursor(0, 1);
  lcd.print("Iniciando descida\n");

  Serial.println("setup(): Iniciando descida\n");

  controlMotor(false, 60.01);

  delay(500);

  while (!isOnLimit() && !isMotorMoving) {

    Serial.println("setup(): motor não esta descendo, enviando comando novamente\n");
    controlMotor(false, 60.01);
  }

  if (isOnLimit() && motorDirectionForward == REVERSE) {

    Serial.println("setup(): Chegou no subsolo\n");
    Serial.println("setup(): stopMotor\n");
    stopMotor();
  }
}

void updateLCD() {
  //lcd.clear(); // Clear the screen for fresh update
  lcd.setCursor(0, 0);
  // Display the first line with motor and brake status, and direction
  lcd.print("INV:");
  lcd.print(isMotorMoving ? "ON" : "OFF");
  lcd.print(" B:");
  lcd.print(digitalRead(BRAKE_PIN) == HIGH ? "ON" : "OFF");
  lcd.print(" D:");
  lcd.print(motorDirectionForward ? "UP" : "OFF");

  // Display the second line with door status and current floor
  lcd.setCursor(0, 1);
  lcd.print("Door:");
  lcd.print(isDoorsClosed ? "C" : "O");
  lcd.print(" Andar:");
  lcd.print(floorStatus, DEC);
}


void updateMotorStatus() {
  // Example logic, adjust based on actual VFD feedback capability
  return ;
  readMotorStatusAndDirection();
}

// Function to read motor status and direction from VFD
void readMotorStatusAndDirection() {
  uint8_t result;
  uint16_t motorStatus, motorDirection;

  // Read motor status
  result = modbus.readInputRegisters(MOTOR_STATUS_REGISTER, 1);
  if (result == modbus.ku8MBSuccess) {
    motorStatus = modbus.getResponseBuffer(0);
    isMotorMoving = motorStatus;

    // Assuming 1 indicates motor running; adjust based on your VFD's manual
    if (motorStatus == 1) {
      Serial.println("readMotorStatusAndDirection(): Motor is running.\n");
    } else {
      Serial.println("readMotorStatusAndDirection(): Motor is stopped.\n");
    }
  }
  // DESATIVADO
  return;
  // Read motor direction
  result = modbus.readInputRegisters(MOTOR_DIRECTION_REGISTER, 1);
  if (result == modbus.ku8MBSuccess) {
    motorDirection = modbus.getResponseBuffer(0);
    motorDirectionForward = FORWARD;
    // Assuming 1 indicates forward and 0 indicates reverse; adjust based on your VFD's manual
    if (motorDirection == 1) {
      Serial.println("readMotorStatusAndDirection(): Motor direction: Forward.\n");
    } else if (motorDirection == 0) {
      Serial.println("readMotorStatusAndDirection(): Motor direction: Reverse.\n");
    } else {
      Serial.println("readMotorStatusAndDirection(): Motor direction: Unknown.\n");
    }
  }
}

void updateMotorStatusForced(bool isMoving, bool directionForward) {
  isMotorMoving = isMoving;
  motorDirectionForward = directionForward? FORWARD : REVERSE;
}


bool checkDoorsClosed() {
  if (digitalRead(CABIN_DOOR_PIN) == LOW && digitalRead(FLOOR1_DOOR_PIN) == LOW) {

    if (!isDoorsClosed) {
      Serial.println("checkDoorsClosed(): Change door state to Close.\n");
    }

    isDoorsClosed = true;

    return true; // All doors closed
  } else {

    if (isDoorsClosed) {
      Serial.println("checkDoorsClosed(): Change door state to Open.\n");
    }

    isDoorsClosed = false;
    return false; // At least one door is open

  }




}

void controlMotor(bool direction, uint16_t speed) {
  uint16_t controlValue;

  if (!checkDoorsClosed()) {
    updateLCD();
    return; // Safety check
  }

  if (direction) {

    if (controlValue != FORWARD) {
      // Before changing direction or speed, stop the motor
      digitalWrite(MOTOR_RUN_PIN, LOW);
      Serial.println("controlMotor(): change direction FORWARD" );

    }

    controlValue = FORWARD;
    
  } else {

    if (controlValue != REVERSE) {
      // Before changing direction or speed, stop the motor
      digitalWrite(MOTOR_RUN_PIN, LOW);
      Serial.println("controlMotor(): change direction REVERSE" );

    }

    controlValue = REVERSE;
  }

  // Before changing direction or speed, stop the motor
  //modbus.writeSingleRegister(VFD_STOP_REGISTER, STOP);



  delay(1000); // Delay for VFD to process

  // Update motor status: moving with specified direction
  updateMotorStatusForced(true, direction);

  //modbus.writeSingleRegister(VFD_CONTROL_REGISTER, controlValue);
  //modbus.writeSingleRegister(VFD_SPEED_REGISTER, speed);

  digitalWrite(MOTOR_RUN_PIN, HIGH);
  delay(300);
  digitalWrite(BRAKE_PIN, HIGH); // Engage brake
  isMotorMoving = true;
  // Additional logic...
  updateLCD();
}

// Example of stopping the motor
void stopMotor() {

  if (isMotorMoving) {
    Serial.println("stopMotor(): ---- STOP-------");
    //modbus.writeSingleRegister(VFD_STOP_REGISTER, STOP);

    digitalWrite(BRAKE_PIN, LOW); // Engage brake
    delay(300);
    digitalWrite(MOTOR_RUN_PIN, LOW);
    // Update motor status: not moving
    updateMotorStatusForced(false, motorDirectionForward);
    isMotorMoving = false;
  }

  updateLCD();

}

void openDoors() {
  if (isMotorMoving) {
    Serial.println("openDoors(): ERRO GRAVE DE SEGURANÇA, Abrir porta com elevador movendo.\n");
    return;
  }
  Serial.println("openDoors(): Acionando motor PORTA para ABRIR");
  digitalWrite(DOOR_OPEN_PIN, HIGH);
  digitalWrite(DOOR_CLOSE_PIN, LOW);
  
      for (int i = 0; i < 50 && checkDoorsClosed(); i++) {


    digitalWrite(DOOR_OPEN_PIN, HIGH);
    digitalWrite(DOOR_CLOSE_PIN, LOW);
    delay(300);
    
  }

  digitalWrite(DOOR_OPEN_PIN, LOW);
  Serial.println("openDoors(): Parando motor PORTA para ABRIR");
  updateLCD();

}

void closeDoors() {

  if (checkDoorsClosed()) {
    Serial.println("closeDoors(): Porta esta fechada, nao precisa acionar motor.\n");
    return;
  }

  Serial.println("closeDoors(): Acionando motor PORTA para FECHAR.\n");
  digitalWrite(DOOR_CLOSE_PIN, HIGH);
  digitalWrite(DOOR_OPEN_PIN, LOW);
  delay(3000);
  digitalWrite(DOOR_CLOSE_PIN, LOW);
  Serial.println("closeDoors(): Parando motor PORTA, FECHADO.\n");
  updateLCD();

}

void doorMonitor() {
  //Verifica se a porta esta aberta, se sim, para tudo!
  if (!checkDoorsClosed()) {
    if (isMotorMoving || digitalRead(MOTOR_RUN_PIN) == HIGH) {
      Serial.print("doorMonitor(): checkDoorsClosed() as to stopMotor STOP!\n");
      stopMotor();
    }

    return;
  }
}



void upOrDown() {
  // Check for button presses to move the elevator up or down
  btnUp = analogRead(BUTTON_UP_PIN);
  btnDown = analogRead(BUTTON_DOWN_PIN);

  if (btnUp > 500) {
    // Make sure doors are closed and we're not at the highest floor
    if (isDoorsClosed && floorStatus < HIGHEST_FLOOR && motorDirectionForward != FORWARD ) {
      floorRequest = floorStatus + 1;
      Serial.print("upOrDown(): SOBE!\n");
      Serial.println(btnUp);

      updateLCD(); // Update LCD with new status
      return;
    }
  }

  if (btnDown > 500) {
    // Make sure doors are closed and we're not at the lowest floor
    if (isDoorsClosed && floorStatus > LOWEST_FLOOR && motorDirectionForward != REVERSE) {
      floorRequest = floorStatus - 1;
      Serial.print("upOrDown(): DESCE!\n");
      Serial.println(btnDown);
      updateLCD(); // Update LCD with new status
      return;
    }
  }


}

bool isOnLimit() {
  //Stop if get on limits
  if (digitalRead(HIGHER_LEVEL_PIN) == LOW) {
    if (isMotorMoving && motorDirectionForward != FORWARD) {
      Serial.print("loop(): Chegou no ultimo andar chamando o stopMotor() STOP!\n");
    }
    floorStatus = HIGHEST_FLOOR;
    stopMotor();
    return true;
  }

  if (digitalRead(LOWER_LEVEL__PIN) == LOW) {
    if (isMotorMoving && motorDirectionForward != REVERSE) {
      Serial.print("loop(): Chegou no Subsolo chamando o stopMotor() STOP!\n");
    }
    floorStatus = LOWEST_FLOOR;
    stopMotor();
    return true;
  }
  return false;
}

void loop() {



  //if(floorStatus > 4) floorStatus = 4;
  //if(floorStatus < -1 && floorStatus != -9) floorStatus = -1;

  //updateMotorStatus(); Precisa implementar leitura via ModBus

  // Ensure safety first
  isOnLimit();
  doorMonitor();

  if (!isMotorMoving) {
    upOrDown();
  }




  // User input handling for floor selection within loop

  if (floorRequest >= LOWEST_FLOOR && floorRequest <= HIGHEST_FLOOR
      && floorRequest != floorStatus) {
    Serial.print("loop(): Need to move to floor\n");

    Serial.print(" Andar destno:\n");
    Serial.print(floorRequest);
    Serial.print(" Andar atual:\n");
    Serial.print(floorStatus);
    if (!checkDoorsClosed()) {
      closeDoors();
    }

    Serial.print(" Aguarda 1 Segundo... \n");
    delay(1000);
    // Check for button presses to move the elevator up or down
    if (isDoorsClosed && floorStatus < HIGHEST_FLOOR) {
      controlMotor(true, currentSpeed); // Move up
      updateLCD(); // Update LCD with new status
    }

    // Make sure doors are closed and we're not at the lowest floor
    if (isDoorsClosed && floorStatus > LOWEST_FLOOR) {
      controlMotor(false, currentSpeed); // Move down
      updateLCD(); // Update LCD with new status
    }

    // Adjust motor control based on the approach and stop signals dynamically
  }


  // Dynamic handling of elevator movement based on sensor inputs
  if (digitalRead(APPROACHING_SIGNAL_PIN) == LOW) {
    Serial.print(" APPROACHING_SIGNAL_PIN is LOW reduce speed for soft approach\n");
    currentSpeed = currentSpeed / 2; // Reduce speed for soft approach
  } else if (digitalRead(SOFT_STOP_SIGNAL_PIN) == LOW) {
    Serial.print(" SOFT_STOP_SIGNAL_PIN is LOW  Wait for soft stop 2Seconds \n");
    delay(2000); // Wait for soft stop
    currentSpeed = 0; // Stop the elevator
  } else if (digitalRead(FULL_STOP_SIGNAL_PIN) == LOW) {
     Serial.print(" FULL_STOP_SIGNAL_PIN is LOW, FULL STOP NOW! \n");
    if (motorDirectionForward) {
      floorStatus = floorStatus + 1;
      updateLCD();
    } else {
      floorStatus = floorStatus - 1;
      updateLCD();
    }
    stopMotor();
    Serial.print(" FULL_STOP_SIGNAL_PIN is LOW, openDoors! \n");
    //Verificar marcadores de nivel
    openDoors(); // Open doors when the full stop is achieve
  } else {
    // Resume or start moving at original speed
    Serial.print(" RUN MOTOR AND RELEASE BREAK \n");
    digitalWrite(MOTOR_RUN_PIN, HIGH);
    delay(300);
    digitalWrite(BRAKE_PIN, HIGH);

    // Disengage brake for movement
    //modbus.writeSingleRegister(VFD_CONTROL_REGISTER, FORWARD);
    //modbus.writeSingleRegister(VFD_SPEED_REGISTER, currentSpeed);
  }

}