#include <LiquidCrystal_I2C.h>
#include <Servo.h>
Servo myservo;
LiquidCrystal_I2C lcd(0x27, 16, 02);
int Pin1 = 8;        //IN1 is connected to 10
int Pin2 = 9;        //IN2 is connected to 11
int Pin3 = 10;       //IN3 is connected to 12
int Pin4 = 11;       //IN4 is connected to 13
int switchSTOP = 2;  //define input pin for STOP push button
int switchCW = 3;    //define input pin for CW push button
int switchCCW = 4;   //define input pin for CCW push button
int speedFactor = 1;  //1=fastest, 2=slower or 3 more slower
long goToAngle = 2000;
int correction_CW = 150;   //watch video for details
int correction_CCW = 150;  //watch video for details
const int CW = 1;
const int CCW = 2;
const int STOP = 3;
int poleStep = 0;
long stepVale = 0;
const int SPR = 64 * 64;
/*
  int pole1[] = { 0, 0, 0, 0, 0, 1, 1, 1, 0 };  //pole1, 8 step values
  int pole2[] = { 0, 0, 0, 1, 1, 1, 0, 0, 0 };  //pole2, 8 step values
  int pole3[] = { 0, 1, 1, 1, 0, 0, 0, 0, 0 };  //pole3, 8 step values
  int pole4[] = { 1, 1, 0, 0, 0, 0, 0, 1, 0 };  //pole4, 8 step values
*/
int pole1[] = { 0, 0, 0, 0, 0, 1, 1, 1, 0 };  //pole1, 8 step values
int pole2[] = { 0, 0, 0, 1, 1, 1, 0, 0, 0 };  //pole2, 8 step values
int pole3[] = { 0, 1, 1, 1, 0, 0, 0, 0, 0 };  //pole3, 8 step values
int pole4[] = { 1, 1, 0, 0, 0, 0, 0, 1, 0 };  //pole4, 8 step values

int count = 0;
int dirStatus = STOP;
int counter = 0;
int distance = 0;
int pos = 0;
int cur_floor = 1;
int prev_floor;
int to_floor;
int cur_to_floor;

int sense1Pressed;
int sense2Pressed;
int sense3Pressed;
int sense4Pressed;

String receivedData = " ";
const int BUTTON1_PIN = 16;
const int BUTTON2_PIN = 17;
const int BUTTON3_PIN = 18;
const int BUTTON4_PIN = 19;
const int detect_1st = 22;
const int detect_2nd = 23;
const int detect_3rd = 24;
const int detect_4th = 25;
const int call_1st = 26;
const int call_2nd = 27;
const int call_3rd = 28;
const int call_4th = 29;
const int trigPin = 12;
const int echoPin = 13;
const int buzzer = 7;
bool button1Pressed = false;
bool button2Pressed = false;
bool button3Pressed = false;
bool button4Pressed = false;
bool open_1st_floor = false;
bool open_2nd_floor = false;
bool open_3rd_floor = false;
bool open_4th_floor = false;
bool going_up = true;
bool selected = false;
bool disable = false;
bool in_progess = false;
unsigned long previousMillis = 0;
const long interval = 1000;


void setup() {
  Serial.begin(9600);
  Serial3.begin(9600);    //mega pin 14 and 15 RX_TX - TX_RX
  lcd.init();
  lcd.backlight();
  pinMode(Pin1, OUTPUT);  //define pin for ULN2003 in1
  pinMode(Pin2, OUTPUT);  //define pin for ULN2003 in2
  pinMode(Pin3, OUTPUT);  //define pin for ULN2003 in3
  pinMode(Pin4, OUTPUT);  //define pin for ULN2003 in4
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  pinMode(BUTTON3_PIN, INPUT_PULLUP);
  pinMode(BUTTON4_PIN, INPUT_PULLUP);
  pinMode(switchSTOP, INPUT_PULLUP);
  pinMode(switchCW, INPUT_PULLUP);
  pinMode(switchCCW, INPUT_PULLUP);
  pinMode(detect_1st, INPUT_PULLUP);
  pinMode(detect_2nd, INPUT_PULLUP);
  pinMode(detect_3rd, INPUT_PULLUP);
  pinMode(detect_4th, INPUT_PULLUP);
  pinMode(call_1st, INPUT_PULLUP);
  pinMode(call_2nd, INPUT_PULLUP);
  pinMode(call_3rd, INPUT_PULLUP);
  pinMode(call_4th, INPUT_PULLUP);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.println("SYSTEM POWER UP");
  myservo.attach(6);
  myservo.write(0);

}
void loop() {
  //ultrasonics();

  recieve_serial_bt();
  controls();
  process_movement();
  on_stop();
  // counts(); //this is test only must be remove here
  stepper_drive();

}

void LCD() {
  lcd.setCursor(0, 0);
  lcd.print("floor: ");
  lcd.print(cur_floor);

  /*
    if (cur_floor > prev_floor) {
      lcd.setCursor(0, 1);
      lcd.print("going UP  ");
      Serial.println("going UP  ");
      going_up = true;
      prev_floor = cur_floor;
    } else if (cur_floor < prev_floor) {
      lcd.setCursor(0, 1);
      lcd.print("going DOWN");
      going_up = false;
      prev_floor = cur_floor;
    }
  */
}

void controls() {
  //hardware buttons/
  int button1State = digitalRead(BUTTON1_PIN);
  int button2State = digitalRead(BUTTON2_PIN);
  int button3State = digitalRead(BUTTON3_PIN);
  int button4State = digitalRead(BUTTON4_PIN);
  int call1Pressed = digitalRead(call_1st);
  int call2Pressed = digitalRead(call_2nd);
  int call3Pressed = digitalRead(call_3rd);
  int call4Pressed = digitalRead(call_4th);

  //if disable is true no hardware buttons can be use. it was trigger into app
  if (!disable) {
    open_1st_floor = !button1State && cur_floor != 1  ?
                     true : open_1st_floor = open_1st_floor;
    open_2nd_floor = !button2State && cur_floor != 2  ?
                     true : open_2nd_floor = open_2nd_floor;
    open_3rd_floor = !button3State && cur_floor != 3  ?
                     true : open_3rd_floor = open_3rd_floor;
    open_4th_floor = !button4State && cur_floor != 4  ?
                     true : open_4th_floor = open_4th_floor;

    open_1st_floor = !call1Pressed && cur_floor != 1 ?
                     true : open_1st_floor = open_1st_floor;
    open_2nd_floor = !call2Pressed && cur_floor != 2 ?
                     true : open_2nd_floor = open_2nd_floor;
    open_3rd_floor = !call3Pressed && cur_floor != 3 ?
                     true : open_3rd_floor = open_3rd_floor;
    open_4th_floor = !call4Pressed && cur_floor != 4 ?
                     true : open_4th_floor = open_4th_floor;
  } else {
    if (!button1State || !button2State || !button3State || !button4State
        || !call1Pressed || !call2Pressed || !call3Pressed || !call4Pressed) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Hardware buttons");
      lcd.setCursor(0, 1);
      lcd.print("disabled");
      delay(2000);
      lcd.clear();
    }
  }

  //just get what floor in sense
  sense1Pressed = digitalRead(detect_1st);
  sense2Pressed = digitalRead(detect_2nd);
  sense3Pressed = digitalRead(detect_3rd);
  sense4Pressed = digitalRead(detect_4th);
  !sense1Pressed && open_1st_floor ? cur_floor = 1 : cur_floor = cur_floor;
  !sense2Pressed && open_2nd_floor ? cur_floor = 2 : cur_floor = cur_floor;
  !sense3Pressed && open_3rd_floor ? cur_floor = 3 : cur_floor = cur_floor;
  !sense4Pressed && open_4th_floor ? cur_floor = 4 : cur_floor = cur_floor;
  LCD();
  /*
    Serial.print("call");
    Serial.print("  ");
    Serial.print(open_1st_floor);
    Serial.print("  ");
    Serial.print(open_2nd_floor);
    Serial.print("  ");
    Serial.print(open_3rd_floor);
    Serial.print("  ");
    Serial.print(open_4th_floor);

    Serial.print("  cur_floor ");
    Serial.print("  ");
    Serial.println(cur_floor);
  */
}


void stepper_drive() {
  stepVale = (SPR * goToAngle) / 360 ;
  //Robojax.com Stepper Push button Any Angle STPB-4
  if (digitalRead(switchCCW) == LOW)
  {
    dirStatus = CCW;
    count = 0;

  } else if (digitalRead(switchCW) == LOW)
  {
    dirStatus  = CW;
    count = 0;
  }
  if (digitalRead(switchSTOP) == LOW)
  {
    dirStatus  = STOP;
    delay(200);
  }

  if (dirStatus == CCW) {
    poleStep++;
    count++;
    if (poleStep > 7) {
      poleStep = 0;
    }
    if (count + correction_CCW <= stepVale)
    {
      //Serial.print("CCW ");
      //Serial.println(poleStep);
      driveStepper(poleStep);
    } else {
      //Serial.println("default CCW");
      driveStepper(8);
    }

  } else if (dirStatus == CW) {
    poleStep--;
    count++;
    if (poleStep < 0) {
      poleStep = 7;
    }

    if (count + correction_CW <= stepVale)
    {
      //Serial.print("CW ");
      //Serial.println(poleStep);
      driveStepper(poleStep);
    } else {
      //Serial.println("default CW");
      driveStepper(8);
    }
  } else {
    //Serial.println("default all");
    driveStepper(8);
  }
  /*
    if (poleStep > 7) {
      poleStep = 0;
    }
    if (poleStep < 0) {
      poleStep = 7;
    }
  */
  // delay(speedFactor);
}

void driveStepper(int c) {
  digitalWrite(Pin1, pole1[c]);
  digitalWrite(Pin2, pole2[c]);
  digitalWrite(Pin3, pole3[c]);
  digitalWrite(Pin4, pole4[c]);
  if (c == 8) {
    digitalWrite(switchCW, HIGH);
    digitalWrite(switchCCW, HIGH);
  }
}

void recieve_serial_bt() {
  if (Serial3.available()) {
    receivedData = Serial3.readString();
    receivedData.trim();
    Serial.print("Received data: ");
    Serial.println(receivedData);
    disable = receivedData == "disable" ? true : disable = disable;
    disable = receivedData == "enable" ? false : disable = disable;
    open_1st_floor = receivedData == "1st" && cur_floor != 1  ? true : open_1st_floor = open_1st_floor;
    open_2nd_floor = receivedData == "2nd" && cur_floor != 2  ? true : open_2nd_floor = open_2nd_floor;
    open_3rd_floor = receivedData == "3rd" && cur_floor != 3  ? true : open_3rd_floor = open_3rd_floor;
    open_4th_floor = receivedData == "4th" && cur_floor != 4  ? true : open_4th_floor = open_4th_floor;
    Serial.println(open_1st_floor);
  }
}

void ultrasonics() {
  // Clear the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  distance = (duration * 0.0343) / 2;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print(" cm  ");

  Serial.print(" counter  ");
  Serial.println(counter);

}

bool opendoor(bool status) {
  counter = 0;
  Serial.println("Open Door");
  for (pos = 0; pos <= 180; pos += 1) {
    // in steps of 1 degree
    myservo.write(pos);
    recieve_serial_bt();
    controls();
    delay(1);
  }
  while (1) {
    ultrasonics();
    recieve_serial_bt();
    controls();
    if (distance < 100) {
      counts();
      recieve_serial_bt();
      controls();
      if (counter == 5) {
        Serial.println("Close Door");
        for (pos = 180; pos >= 0; pos -= 1) {
          myservo.write(pos);
          delay(1);
          recieve_serial_bt();
          controls();
        }
        counter = 0;
        break;
      }
    } else {
      counter = 0;
    }
  }
  status =  false;
  return status;
}

void counts() {
  unsigned long currentMillis = millis(); // Get the current time in milliseconds
  // Check if it's time to change the tone state
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    counter ++;
    /*
      Serial.print("call");
      Serial.print(" ");
      Serial.print(open_1st_floor);
      Serial.print(" ");
      Serial.print(open_2nd_floor);
      Serial.print(" ");
      Serial.print(open_3rd_floor);
      Serial.print(" ");
      Serial.print(open_4th_floor);

      Serial.print(" cur_floor ");
      Serial.print(" ");
      Serial.println(cur_floor);
    */
  }
}

void on_stop() {
  if (open_1st_floor && !sense1Pressed) {
    Serial.println("openning floor no.1");
    dirStatus = STOP;
    tone(buzzer, 1000);
    delay(500);
    noTone(buzzer);
    open_1st_floor = opendoor(open_1st_floor);
  } else if (open_2nd_floor && !sense2Pressed) {
    Serial.println("openning floor no.2");
    dirStatus = STOP;
    tone(buzzer, 1000);
    delay(500);
    noTone(buzzer);
    open_2nd_floor = opendoor(open_2nd_floor);
  } else if (open_3rd_floor && !sense3Pressed) {
    Serial.println("openning floor no.3");
    dirStatus = STOP;
    tone(buzzer, 1000);
    delay(500);
    noTone(buzzer);
    open_3rd_floor = opendoor(open_3rd_floor);
  } else if (open_4th_floor && !sense4Pressed) {
    Serial.println("openning floor no.4");
    dirStatus = STOP;
    tone(buzzer, 1000);
    delay(500);
    noTone(buzzer);
    open_4th_floor = opendoor(open_3rd_floor);
  }
}

void process_movement() {

  if (open_1st_floor || open_2nd_floor || open_3rd_floor || open_4th_floor) {
    selected = true;
  } else {
    selected = false;
  }

  //this part determine if we must go up or down now "tricky not sure about this"
  if (going_up && cur_floor >= 2 && open_1st_floor && !open_2nd_floor && !open_3rd_floor) {
    going_up = false;
    lcd.setCursor(0, 1);
    lcd.print("going DOWN");
  } else if (going_up && cur_floor >= 3 && open_1st_floor && open_2nd_floor && !open_3rd_floor) {
    going_up = false;
    lcd.setCursor(0, 1);
    lcd.print("going DOWN");
  } else if (cur_floor == 3) {
    going_up = false;
    lcd.setCursor(0, 1);
    lcd.print("going DOWN");
  } else if (!going_up && cur_floor <= 2 && !open_1st_floor && !open_2nd_floor && open_3rd_floor) {
    going_up = true;
    lcd.setCursor(0, 1);
    lcd.print("going UP  ");
  } else if (!going_up && cur_floor <= 1 && !open_1st_floor && !open_2nd_floor && open_3rd_floor) {
    going_up = true;
    lcd.setCursor(0, 1);
    lcd.print("going UP  ");
  } else if (cur_floor == 1) {
    going_up = true;
    lcd.setCursor(0, 1);
    lcd.print("going UP  ");
    //Serial.println("truing 3");
  }


  if (going_up && selected) {
    //going up movement
    if (dirStatus == STOP) {
      dirStatus = CW;
      lcd.setCursor(0, 1);
      Serial.println("UP");
      delay(1000);
    } else {
      going_up = false;
    }
  }
  //Serial.println(going_up);


  if (!going_up && selected) {
    //going  down movement
    if (dirStatus == STOP) {
      dirStatus = CCW;
      Serial.println("DOWN");
      delay(1000);
    } else {
      going_up = true;
    }
  }



  /*
    //Serial.println(going_up);
    if (cur_floor == 1) {
      going_up = true;
    } else if (cur_floor == 4) {
      going_up = false;
    }
  */
  counter ++;
  if (counter >= 30) {
    Serial.print("call");
    Serial.print(" ");
    Serial.print(open_1st_floor);
    Serial.print(" ");
    Serial.print(open_2nd_floor);
    Serial.print(" ");
    Serial.print(open_3rd_floor);
    Serial.print(" ");
    Serial.print(open_4th_floor);

    Serial.print(" cur_floor ");
    Serial.print(" ");
    Serial.println(cur_floor);
    counter = 0;
  }
}