//PINS
#define M1_START_RELAY 18
#define M1_STOP_RELAY 34

#define M2_START_RELAY 35
#define M2_STOP_RELAY 21

#define M3_START_RELAY 36
#define M3_STOP_RELAY 33

#define M1_MOTOR_INPUT 3
#define M2_MOTOR_INPUT 4
#define M3_MOTOR_INPUT 5

#define M1_SWITCH 6
#define M2_SWITCH 7
#define M3_SWITCH 8

#define FLOAT_SWITCH_EMPTY 9
#define FLOAT_SWITCH_FULL 10

#define POWER_SENSE 11

#define MODE_SELECT_SWITCH 12

//Main parameter
//Rekay logic
#define RELAY_ON HIGH
#define RELAY_OFF LOW
#define RELAY_DELAY 1000

//Float logic
#define FLOAT_ON true
#define FLOAT_OFF false

//MOTOR logic
#define MOTOR_ON HIGH
#define MOTOR_OFF LOW
#define TANK_MOTOR 1

// SOFTWARE logic
#define FROM_FLOAT_LOGIC 1
#define FROM_NORMAL_LOGIC 0

//Important variables
bool M1_STATE;
bool M2_STATE;
bool M3_STATE;

bool M1_SWITCH_STATE;
bool M2_SWITCH_STATE;
bool M3_SWITCH_STATE;

bool FLOAT_MODE;
bool FLOAT_EMPTY_STATE;
bool FLOAT_FULL_STATE;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S2!");
  pinMode(M1_MOTOR_INPUT, INPUT);
  pinMode(M2_MOTOR_INPUT, INPUT);
  pinMode(M3_MOTOR_INPUT, INPUT);

  pinMode(M1_SWITCH, INPUT);
  pinMode(M2_SWITCH, INPUT);
  pinMode(M3_SWITCH, INPUT);

  pinMode(M1_START_RELAY,OUTPUT);
  pinMode(M1_STOP_RELAY, OUTPUT);
  pinMode(M2_START_RELAY,OUTPUT);
  pinMode(M2_STOP_RELAY, OUTPUT);
  pinMode(M3_START_RELAY,OUTPUT);
  pinMode(M3_STOP_RELAY, OUTPUT);

  digitalWrite(M1_START_RELAY,RELAY_OFF);
  digitalWrite(M1_STOP_RELAY,RELAY_OFF);
  digitalWrite(M2_START_RELAY,RELAY_OFF);
  digitalWrite(M2_STOP_RELAY,RELAY_OFF);
  digitalWrite(M3_START_RELAY,RELAY_OFF);
  digitalWrite(M3_STOP_RELAY,RELAY_OFF);

  M1_STATE = false;
  M2_STATE = false;
  M3_STATE = false;

  M1_SWITCH_STATE = false;
  M2_SWITCH_STATE = false;
  M3_SWITCH_STATE = false;

  FLOAT_MODE = false;
  FLOAT_EMPTY_STATE = false;
  FLOAT_FULL_STATE = false;

  //For simulation only need to remove on production : From here
  digitalWrite(M1_STOP_RELAY,RELAY_ON);
  digitalWrite(M2_STOP_RELAY,RELAY_ON);
  digitalWrite(M3_STOP_RELAY,RELAY_ON);
  delay(10);
  digitalWrite(M1_STOP_RELAY,RELAY_OFF);
  digitalWrite(M2_STOP_RELAY,RELAY_OFF);
  digitalWrite(M3_STOP_RELAY,RELAY_OFF);
  //to here

  Serial.println("INIT Done!");
  delay(2000);
  
}

void read_motor_status(){
  Serial.println("Read motor status called");
  if(digitalRead(M1_MOTOR_INPUT) == MOTOR_ON){
    M1_STATE = true;
  }
  else{
    M1_STATE = false;
  } 
  if(digitalRead(M2_MOTOR_INPUT) == MOTOR_ON){
    M2_STATE = true;
  }
  else{
    M2_STATE = false;
  } 
  if(digitalRead(M3_MOTOR_INPUT) == MOTOR_ON){
    M3_STATE = true;
  }
  else{
    M3_STATE = false;
  }
  Serial.print("M1 Status: ");
  Serial.println(M1_STATE);
  Serial.print("M2 Status: ");
  Serial.println(M2_STATE);
  Serial.print("M3 Status: ");
  Serial.println(M3_STATE);
}

void motor_turn_on(int motor_num, int from){
  if(FLOAT_MODE == true) if(from != FROM_FLOAT_LOGIC) if(motor_num == TANK_MOTOR){
    Serial.println("Error: Trying to turn on TANK MOTOR with FLOAT MODE ON");
    Serial.println("Trun off float mode before doing this operation");
    return;
  }
  if(motor_num == 1){
    digitalWrite(M1_START_RELAY, RELAY_ON);
    delay(RELAY_DELAY);
    digitalWrite(M1_START_RELAY, RELAY_OFF);
    Serial.println("MOTOR 1 Started");
    read_motor_status();
    if(M1_STATE != true){
      Serial.println("Error: Motor 1 not turned on");
    }
  }
  else if(motor_num == 2){
    digitalWrite(M2_START_RELAY, RELAY_ON);
    delay(RELAY_DELAY);
    digitalWrite(M2_START_RELAY, RELAY_OFF);
    Serial.println("MOTOR 2 Started");
    read_motor_status();
    if(M2_STATE != true){
      Serial.println("Error: Motor 2 not turned on");
    }
  }
  else if(motor_num == 3){
    digitalWrite(M3_START_RELAY, RELAY_ON);
    delay(RELAY_DELAY);
    digitalWrite(M3_START_RELAY, RELAY_OFF);
    Serial.println("MOTOR 3 Started");
    read_motor_status();
    if(M3_STATE != true){
      Serial.println("Error: Motor 3 not turned on");
    }
  }
  else{
    Serial.print("Error: unknown motor number to turn on - ");
    Serial.println(motor_num);
  }
}

void motor_turn_off(int motor_num, int from){
  if(FLOAT_MODE == true) if(from != FROM_FLOAT_LOGIC) if(motor_num == TANK_MOTOR){
    Serial.println("Error: Trying to turn off TANK MOTOR with FLOAT MODE ON");
    Serial.println("Trun off float mode before doing this operation");
    return;
  }
  if(motor_num == 1){
    digitalWrite(M1_STOP_RELAY, RELAY_ON);
    delay(RELAY_DELAY);
    digitalWrite(M1_STOP_RELAY, RELAY_OFF);
    Serial.println("MOTOR 1 Stoped");
    read_motor_status();
    if(M1_STATE != false){
      Serial.println("Error: Motor 1 not turned on");
    }
  }
  else if (motor_num == 2){
    digitalWrite(M2_STOP_RELAY, RELAY_ON);
    delay(RELAY_DELAY);
    digitalWrite(M2_STOP_RELAY, RELAY_OFF);
    Serial.println("MOTOR 2 Stoped");
    read_motor_status();
    if(M2_STATE != false){
      Serial.println("Error: Motor 2 not turned on");
    }
  }
  else if(motor_num == 3){
    digitalWrite(M3_STOP_RELAY, RELAY_ON);
    delay(RELAY_DELAY);
    digitalWrite(M3_STOP_RELAY, RELAY_OFF);
    Serial.println("MOTOR 3 Stoped");
    read_motor_status();
    if(M3_STATE != false){
      Serial.println("Error: Motor 3 not turned on");
    }
  }
  else{
    Serial.print("Error: unknown motor number to turn off - ");
    Serial.println(motor_num);
  }
}

void read_switch_status(){
  #ifdef DEBUG_ON
  Serial.println("Read switch status called");
  #endif
  M1_SWITCH_STATE = digitalRead(M1_SWITCH);
  M2_SWITCH_STATE = digitalRead(M2_SWITCH);
  M3_SWITCH_STATE = digitalRead(M3_SWITCH);
  
  Serial.print("M1_SWITCH_SATE: ");
  Serial.print(M1_SWITCH_STATE);
  Serial.print(" M2_SWITCH_SATE: ");
  Serial.print(M2_SWITCH_STATE);
  Serial.print(" M3_SWITCH_SATE: ");
  Serial.println(M3_SWITCH_STATE);
}

void update_motor_with_switch_state(){
  #ifdef DEBUG
  Serial.println("Update Motor called");
  #endif
  bool m1_switch_state;
  bool m2_switch_state;
  bool m3_switch_state;

  m1_switch_state = M1_SWITCH_STATE;
  m2_switch_state = M2_SWITCH_STATE;
  m3_switch_state = M3_SWITCH_STATE;

  #ifdef DEBUG_ON
  Serial.print("m1 switch state: ");
  Serial.print(m1_switch_state);
  Serial.print(" m2_switch_state:");
  Serial.print(m2_switch_state);
  Serial.print(" m3_switch_state:");
  Serial.println(m3_switch_state);
 #endif

  read_switch_status();


  if(m1_switch_state != M1_SWITCH_STATE){
    if((m1_switch_state == true) & (M1_SWITCH_STATE == false)) motor_turn_off(1,FROM_NORMAL_LOGIC);
    if((m1_switch_state == false) & (M1_SWITCH_STATE == true)) motor_turn_on(1,FROM_NORMAL_LOGIC);
  }

  if(m2_switch_state != M2_SWITCH_STATE){
    if((m2_switch_state == true) & (M2_SWITCH_STATE == false)) motor_turn_off(2,FROM_NORMAL_LOGIC);
    if((m2_switch_state == false) & (M2_SWITCH_STATE == true)) motor_turn_on(2,FROM_NORMAL_LOGIC);
  }

  if(m3_switch_state != M3_SWITCH_STATE){
    if((m3_switch_state == true) & (M3_SWITCH_STATE == false)) motor_turn_off(3,FROM_NORMAL_LOGIC);
    if((m3_switch_state == false) & (M3_SWITCH_STATE == true)) motor_turn_on(3,FROM_NORMAL_LOGIC);
  }
}

void check_mode_select(){
  if(digitalRead(MODE_SELECT_SWITCH) == HIGH){
    FLOAT_MODE = true;
  }
  else{
    FLOAT_MODE = false;
  }
  Serial.print("FLOAT MODE: ");
  Serial.println(FLOAT_MODE);
}

void actuate_tank_motor(bool state){
  if(TANK_MOTOR == 1){
    if(state == true) motor_turn_on(1,FROM_FLOAT_LOGIC);
    if(state == false) motor_turn_off(1,FROM_FLOAT_LOGIC);
  }
  if(TANK_MOTOR == 2){
    if(state == true) motor_turn_on(2,FROM_FLOAT_LOGIC);
    if(state == false) motor_turn_off(2,FROM_FLOAT_LOGIC);
  }
  if(TANK_MOTOR == 3){
    if(state == true) motor_turn_on(3,FROM_FLOAT_LOGIC);
    if(state == false) motor_turn_off(3,FROM_FLOAT_LOGIC);
  }
  if(state == true){
    Serial.println("TANK MOTOR turned on");
  }
  else{
    Serial.println("TANK MOTOR turned off");
  }
}

void float_operation(){
  bool float_empty_state = FLOAT_EMPTY_STATE;
  bool float_full_state = FLOAT_FULL_STATE;

  FLOAT_EMPTY_STATE = digitalRead(FLOAT_SWITCH_EMPTY);
  FLOAT_FULL_STATE = digitalRead(FLOAT_SWITCH_FULL);

  if((float_empty_state != FLOAT_EMPTY_STATE) | (float_full_state != FLOAT_FULL_STATE)){
    if((FLOAT_EMPTY_STATE == FLOAT_ON) | (FLOAT_FULL_STATE == FLOAT_OFF)){
      actuate_tank_motor(true);
    }

    if(FLOAT_FULL_STATE == FLOAT_ON){
      actuate_tank_motor(false);
    }
  }
}

void loop() {
  check_mode_select();
  if(FLOAT_MODE == true) float_operation();
  update_motor_with_switch_state();
  delay(2000);
}