#include <LiquidCrystal_I2C.h>
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>

///DECLARATION SERVOS FOR SERVO FUNCTION
Servo servo_5;
Servo servo_6;

////LCD INITIALIZATION (I2C ADDRESS, AMOUNT OF COLUMNS, AMOUNT OF ROWS)
LiquidCrystal_I2C lcd(0x27,20,4);

////BLUETOOTH PIN DECLARATION  MODULE HC05 (RX, TX)
SoftwareSerial Bluetooth(10,11);

///BLUETOOTH VARIABLES
boolean RECEIVING_DATA = false;
///VARIBALE TO STORE WHAT IS SENT TO THE HC06
char Data;
////
String MessageBuffer = "";


///LOCAL OR GLOBAL?////GENERIC SELECTOR
//int NEXT_NEW;
//int PREV_NEW;
boolean NEXT_OLD;
boolean PREV_OLD;
//int VARIABLE_TO_CHANGE = 0;

/////////////////////////////////////
int PROGRAM_X = 0;

/////////////////////////////////
int BUTTON_NEW = 0;
int BUTTON_OLD = 0;
int SWITCH_STATE = 0;
int LCD_REFRESH = 0;

//////////////////////////////
int NEXT_PROGRAM_NEW = 0;
int NEXT_PROGRAM_OLD = 0;
int PREV_PROGRAM_NEW = 0;
int PREV_PROGRAM_OLD = 0;

////////////////////////////////
int PWM_CALIB_1 = 0;
int PWM_CALIB_2 = 0;
int SPEED_1 = 0;
int SPEED_2 = 0;
int ANGLE_1 = 0;
int ANGLE_2 = 0;
int DELAY_SWEEP;
///////////////////////////////





void ON_OFF (){

  BUTTON_NEW = digitalRead(2);

  ///structure to read the button press or data received from BT in paralel

    boolean NEXT_NEW = digitalRead(2) || Data==(byte ('5'));
    
  ///BLUETOOTH READ
    
    if(Data == (byte ('5'))) {
    delay(50);
    
    if(SWITCH_STATE == 0){
          SWITCH_STATE = 1;

      }else{
          SWITCH_STATE = 0;

      }
      LCD_REFRESH = 0;
    }
 
 
 
  ///BUTTON READ
  
    if(!((BUTTON_NEW == BUTTON_OLD))){
    
    delay(50);
    BUTTON_NEW = digitalRead(2);
 
    if((!((BUTTON_NEW == BUTTON_OLD)))  &&  (BUTTON_NEW == 1)){
        if(SWITCH_STATE == 0){
          SWITCH_STATE = 1;

          }else{
          SWITCH_STATE = 0;

        }
        LCD_REFRESH = 0;
      }

    }
  
    BUTTON_OLD = BUTTON_NEW;

}

void PROGRAM_SELECTOR (){

  //PREV_PROGRAM_NEW = digitalRead(7);
  //NEXT_PROGRAM_NEW = digitalRead(8);

  ///BUTTON AND BLUETOOTH READ IN PARALLEL

    boolean PREV_PROGRAM_NEW = digitalRead(3) || Data==(byte ('3'));
    
  
    boolean NEXT_PROGRAM_NEW = digitalRead(4) || Data==(byte ('4'));

  ///BLUETOOTH READ
    if(Data == (byte ('3'))) {
    delay(50);
    PROGRAM_X = (PROGRAM_X - 1);
          LCD_REFRESH = 0;
          SWITCH_STATE = 0;
    }
  
    if(Data == (byte ('4'))) {
    delay(50);
    PROGRAM_X = (PROGRAM_X + 1);
          LCD_REFRESH = 0;
          SWITCH_STATE = 0;
    }
  
  
  ///DOUBLE BUTTON READ 

    if(PREV_PROGRAM_NEW == PREV_PROGRAM_OLD){
      delay(50);
      PREV_PROGRAM_NEW = digitalRead(3);
      if((PREV_PROGRAM_NEW == PREV_PROGRAM_OLD)  &&  (PREV_PROGRAM_NEW == 1)){
          PROGRAM_X = (PROGRAM_X - 1);
          LCD_REFRESH = 0;
          SWITCH_STATE = 0;

      }

    }
    if(NEXT_PROGRAM_NEW == NEXT_PROGRAM_OLD){
      delay(50);
      NEXT_PROGRAM_NEW = digitalRead(4);
      if((NEXT_PROGRAM_NEW == NEXT_PROGRAM_OLD)  &&  (NEXT_PROGRAM_NEW == 1)){
          PROGRAM_X = (PROGRAM_X + 1);
          LCD_REFRESH = 0;
          SWITCH_STATE = 0;

      }

    }
  
    NEXT_PROGRAM_OLD = NEXT_PROGRAM_NEW;
    PREV_PROGRAM_OLD = PREV_PROGRAM_NEW;

}

int GENERIC_SELECTOR (int VARIABLE_TO_CHANGE){


    //int VARIABLE_TO_CHANGE;
    /*
    int NEXT_NEW ;
    int PREV_NEW ;
    int NEXT_OLD = 0;
    int PREV_OLD = 0;
    */

    ///structure to read the button press or data received from BT in paralel

    boolean NEXT_NEW = digitalRead(7) || Data==(byte ('1'));
    
  
    boolean PREV_NEW = digitalRead(8) || Data==(byte ('2'));

    
    ///BLUETOOTH READ
    if(Data == (byte ('1'))) {
    delay(50);
    VARIABLE_TO_CHANGE = (VARIABLE_TO_CHANGE + 10);
            LCD_REFRESH = 0;
    }
  
    if(Data == (byte ('2'))) {
    delay(50);
    VARIABLE_TO_CHANGE = (VARIABLE_TO_CHANGE - 10);
            LCD_REFRESH = 0;
    }

    
    ///BUTTON READ

    if(NEXT_NEW == NEXT_OLD){
        delay(25);
        NEXT_NEW = digitalRead(7);
        if((NEXT_NEW == NEXT_OLD)  &&  (NEXT_NEW == 1)){
            VARIABLE_TO_CHANGE = (VARIABLE_TO_CHANGE + 10);
            LCD_REFRESH = 0;
        }
         
    }

    if(PREV_NEW == PREV_OLD){
        delay(25);
        PREV_NEW = digitalRead(8);
        if((PREV_NEW == PREV_OLD)  &&  (PREV_NEW == 1)){
            VARIABLE_TO_CHANGE = (VARIABLE_TO_CHANGE - 10);
            LCD_REFRESH = 0;
        }

    }
    
    ///FUNCTION TO CREATE RANGE 0-180
    
    if(VARIABLE_TO_CHANGE > 180){
         VARIABLE_TO_CHANGE = (0);
    }

    if(VARIABLE_TO_CHANGE < 0){
         VARIABLE_TO_CHANGE = (180);
    }


    NEXT_OLD = NEXT_NEW;
    PREV_OLD = PREV_NEW;
     
    return VARIABLE_TO_CHANGE;

}

void MULTI_LEVEL_N_N (int PWM1, int PWM2){


  if((SWITCH_STATE == 1)  &&  (LCD_REFRESH == 0)){

      //analogWrite(5,PWM1);
      //analogWrite(6,PWM2);
      servo_5.write(PWM1);
      servo_6.write(PWM2);

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("PROGRAM: ") + String(PROGRAM_X));
      lcd.setCursor(17,0);
      lcd.print(String("ON"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM1: ") + String(PWM1));
      lcd.setCursor(0,2);
      lcd.print(String("PWM2: ") + String(PWM2));
      LCD_REFRESH = 1;

  }

  if((SWITCH_STATE == 0)  &&  (LCD_REFRESH == 0)){

      //analogWrite(5,0);
      //analogWrite(6,0);
      servo_5.write(0);
      servo_6.write(0);
      
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("PROGRAM: ") + String(PROGRAM_X));
      lcd.setCursor(17,0);
      lcd.print(String("OFF"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM1: ") + String(PWM1));
      lcd.setCursor(0,2);
      lcd.print(String("PWM2: ") + String(PWM2));
      LCD_REFRESH = 1;

    }

}

void SWEEP (int SPEED_1, int SPEED_2, int DELAY_SWEEP){


  if((SWITCH_STATE == 1)  &&  (LCD_REFRESH == 0)){

      //analogWrite(5,PWM1);
      //analogWrite(6,PWM2);
      servo_5.write(ANGLE_1);
      servo_6.write(ANGLE_2);

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("SWEEP MODE"));
      lcd.setCursor(17,0);
      lcd.print(String("ON"));
      lcd.setCursor(0,1);
      lcd.print(String("ANGLE 1: ") + String(ANGLE_1));
      lcd.setCursor(0,2);
      lcd.print(String("ANGLE 2: ") + String(ANGLE_2));
      ANGLE_1 = (ANGLE_1 + SPEED_1);
      ANGLE_2 = (ANGLE_2 + SPEED_2);
      
      if(ANGLE_1 > 180){
         ANGLE_1 = (0);
      }

      if(ANGLE_2 > 180){
         ANGLE_2 = (0);
      }
      //LCD_REFRESH = 1;
    delay(DELAY_SWEEP);
  }

  if((SWITCH_STATE == 0)  &&  (LCD_REFRESH == 0)){

      //analogWrite(5,0);
      //analogWrite(6,0);
      servo_5.write(0);
      servo_6.write(0);

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("SWEEP MODE"));
      lcd.setCursor(17,0);
      lcd.print(String("OFF"));
      lcd.setCursor(0,1);
      lcd.print(String("SP1: ") + String(SPEED_1));
      lcd.setCursor(0,2);
      lcd.print(String("SP2: ") + String(SPEED_2));
      LCD_REFRESH = 1;

    }


}

void CALIBRATION_M1 (){

  if((SWITCH_STATE == 1)  &&  (LCD_REFRESH == 0)){
      servo_5.write(PWM_CALIB_1);
      //analogWrite(5,PWM_CALIB_1);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("CALIBRATION M1: ") + String(" ON"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM1: ") + String(PWM_CALIB_1));
      LCD_REFRESH = 1;

    }
  
  if((SWITCH_STATE == 0)  &&  (LCD_REFRESH == 0)){
      servo_5.write(0);
      //analogWrite(5,0);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("CALIBRATION M1: ") + String("OFF"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM1: ") + String(PWM_CALIB_1));
      LCD_REFRESH = 1;

    }

}

void CALIBRATION_M2 (){

  if((SWITCH_STATE == 1)  &&  (LCD_REFRESH == 0)){
      servo_6.write(PWM_CALIB_2);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("CALIBRATION M2: ") + String(" ON"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM2: ") + String(PWM_CALIB_2));
      LCD_REFRESH = 1;

    }
  
  if((SWITCH_STATE == 0)  &&  (LCD_REFRESH == 0)){
      servo_6.write(0);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("CALIBRATION M2: ") + String("OFF"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM2: ") + String(PWM_CALIB_2));
      LCD_REFRESH = 1;

    }

}

void SERVO_CONTROL (int PWM1){
  servo_5.write(map(PWM1, 1, 255, 1, 180));
  delay(20);

}

void BLUETOOTH_CONTROL(){
 
  ///FLAGGING STRUCTURE - USEFUL ONLY FOR BOOLEAN STATEMENTS THAT NEED TO BE EXECUTED ONCE ONLY WHEN THE STATEMENT IS TRUE/FALSE
    if(Bluetooth.available()) {

       Data=Bluetooth.read();
       Serial.println(char (Data));
       RECEIVING_DATA = true;
    }

    else if(RECEIVING_DATA){
      Data=0;
      Serial.println(Data);
      RECEIVING_DATA = false;
    }
  
}

void setup() {

  Bluetooth.begin(115200);
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  
  ///BUTTONS PIN DECLARATION
  pinMode(2,INPUT);     //ON OFF
  pinMode(3,INPUT);     //PREVIOUS PROGRAM
  pinMode(4,INPUT);     //NEXT PROGRAM
  pinMode(7,INPUT);     //UP
  pinMode(8,INPUT);     //DOWN
  
  //INTERNAL LED
  pinMode(13,OUTPUT);

  //FREE PINS
  pinMode(9,OUTPUT);


  ///MOTOR, SERVO OR LED PINS
  //pinMode(5,OUTPUT);    //M1
  servo_5.attach(5);
  //pinMode(6,OUTPUT);    //M2
  servo_6.attach(6);

}

void loop() {
          
      BLUETOOTH_CONTROL();
      
      ON_OFF();
      PROGRAM_SELECTOR();
      
      if(PROGRAM_X == -1){
          PROGRAM_X = 4;
      }

      if(PROGRAM_X == 0){
         SWEEP (3, 9, 50); 
      }

      if(PROGRAM_X == 1){
          MULTI_LEVEL_N_N(150, 150);
      }

      if(PROGRAM_X == 2){
          MULTI_LEVEL_N_N(250, 250);
      }

      if(PROGRAM_X == 3){
         PWM_CALIB_1 = GENERIC_SELECTOR(PWM_CALIB_1);
         CALIBRATION_M1 ();   
      }

      if(PROGRAM_X == 4){
          PWM_CALIB_2 = GENERIC_SELECTOR(PWM_CALIB_2);
          CALIBRATION_M2 ();   
      }
    
      if(PROGRAM_X == 5){
          PROGRAM_X = 0;
      }
      
}