#include <Servo.h>
#include <LiquidCrystal_I2C.h>

//Conections
//  
//Digital:
//  4 -Key Switch
//  5 -Swtich
//  6 -Button 1
//  7 -Button 2
//  9 -ServoA
//  10-ServoB
//  11-Servo360
//
//Analog:
//  A0-JoystickX
//  A1-JoystickY
//  A2-Potentiometer
//  A4-SDA (LCD I2C)
//  A5-SCL (LCD I2C)
//
//Connect Joystick and Potentiometer to 5V from arduino
//
//Connect Switch and key switch to 3.3V to power LED lights through 

//Digital pins
int KEYSWITCH = 4;
int SWITCH = 5;
int BUTTON1 = 6;
int BUTTON2 = 7;
int SERVOA = 9;
int SERVOB = 10;
int SERVO360 = 11;
//Analog pins
int JOYSTICKX = A0;
int JOYSTICKY = A1;
int POTENTIOMETER = A2;


//Create Servo and lcd objects
Servo servoA,servoB,motor360;
LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27F for a 16 chars and 2 line display

//Set angle for Servos
float angleA,angleB = 90;

//Set Motor speeed, this is controlled by the user on the box
float motorSpeed = 0.07;

bool runOneTime,runOneTime2 = true;

//Create 
byte Check[8] = {
0b00000,
0b00001,
0b00011,
0b10110,
0b11100,
0b01000,
0b00000,
0b00000
};

void servoSetup(Servo servoMotor, int pin){
  servoMotor.attach(pin);
  servoMotor.write(90);
}

// the setup routine runs once when you press reset:
void setup() {
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
  
  //Setup LCD screen
  lcd.init();
  lcd.clear();         
  lcd.backlight();
  lcd.createChar(0,Check);
  
  //Setup servos
  servoSetup(servoA, SERVOA);
  servoSetup(servoB, SERVOB);
  servoSetup(motor360, SERVO360);
  
  //Setup Buttons and switchs
  pinMode(KEYSWITCH, INPUT_PULLUP);
  pinMode(SWITCH, INPUT_PULLUP);
  pinMode(BUTTON1, INPUT_PULLUP);
  pinMode(BUTTON2, INPUT_PULLUP);

  if (digitalRead(KEYSWITCH)==false && digitalRead(SWITCH)==true){
    lcdStarting();
  }else if(digitalRead(SWITCH)==false){
    lcdSetup();
  }else {
    lcdMotorsOff();
    }
}

//Update for lowering and risign value of motor speed
void lcdUpdate() {
  int motorPercent = motorSpeed *100;
  
  if (motorPercent < 10) {
    lcd.setCursor(8,1);
    lcd.print(" ");
  } else if (motorPercent < 100) {
    lcd.setCursor(9,1);
    lcd.print(" ");
  } else if (motorPercent < 1000) {
    lcd.setCursor(10,1);
    lcd.print(" ");
  }

  lcd.setCursor(7,1);
  int test = motorPercent;
  lcd.print(test);
}

//LCD screen when only key switch is active
void lcdStarting(){
  lcd.clear();
  lcd.home();
  lcd.print("Primary ignition");
  lcd.setCursor(4,1);
  lcd.print("Engaged");
}

//LCD screen when no switch is on
void lcdMotorsOff(){
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("Motor Control");
  lcd.setCursor(6,1);
  lcd.print("Off ");
}

//LCD screen simulated setup for starting of 
void lcdSetup(){
  lcd.clear();
  lcd.setCursor(4,0);
  lcd.print("Starting");
  delay(500);
  lcd.clear();
  lcd.setCursor(3,0);
  lcd.print("Setting up");
  
  lcd.setCursor(4,1);  
  lcd.print("ServoA  ");

  delay(300);

  lcd.setCursor(11,1);
  lcd.write(0);

  delay(700);
  lcd.setCursor(0,1);
  lcd.print("             ");
  delay(300);

  lcd.setCursor(4,1);  
  lcd.print("ServoB  ");

  delay(300);

  lcd.setCursor(11,1);
  lcd.write(0);  

  delay(500);
  lcd.setCursor(4,1);
  lcd.print("             ");
  delay(500);

  lcd.setCursor(3,1);  
  lcd.print("360Servo  ");

  delay(300);

  lcd.setCursor(12,1);
  lcd.write(0);  

  delay(500);
  lcd.setCursor(3,1);
  lcd.print("             ");
  delay(500);

  lcd.setCursor(6,1);
  lcd.print("EE  ");

  delay(300);

  lcd.setCursor(9,1);
  lcd.write(0);

  delay(1000);
  lcd.clear();

  lcd.setCursor(6,1);
  lcd.print("%");
  lcdUpdate();
}

void loop() {

  if (digitalRead(SWITCH) == false) { 
    runOneTime = true;
    lcdSetup();

    while (digitalRead(SWITCH) == false){

      //add functionality to BUTTON1 AND BUTTON2 to increase and decrease motor speed
        if (digitalRead(BUTTON1) == false) {
          if (motorSpeed < 10) {
            motorSpeed += 0.01;
            lcdUpdate();
          }
          while (digitalRead(BUTTON1) == false) {
      //empty
          }
        }
      
        if (digitalRead(BUTTON2) == false) {
          if (motorSpeed > 0.02){
            motorSpeed -= 0.01;
            lcdUpdate();
          }
          while (digitalRead(BUTTON2) == false) {
            //empty
          }
        }

      
      // read the input on analog pin A0 and A1 and map the value of the joystick for servo moving:
      int mappedXValue = map(analogRead(JOYSTICKX), 0, 1024, -90, 91);
      int mappedYValue = map(analogRead(POTENTIOMETER), 0, 1024, 0, 180);
    
      //Read and Map 360 motor
      int mapped360 = map(analogRead(JOYSTICKY),0,1023,800,2000);
      //mapped360 += 109;//wwwwwweeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee
    
      //Move servo A and B
      if (mappedXValue >= 20 && angleA < 180){
        angleA += mappedXValue * motorSpeed;
        servoA.write(angleA);
      }else if (mappedXValue <= -20 && angleA > 0){
        angleA += mappedXValue * motorSpeed;
        servoA.write(angleA);
       }
      servoB.write(mappedYValue);
      //Move servo 360
      motor360.writeMicroseconds(mapped360);
    
      delay(10);        // delay in between reads for stability
    }
  }

  if (runOneTime == true && digitalRead(KEYSWITCH)==false){
    //This will run if the switch is not on
    runOneTime = false;
    runOneTime2 = true;
    lcdStarting();    
  }else if (runOneTime2==true && digitalRead(KEYSWITCH)==true){
    runOneTime = true;
    runOneTime2 = false;
    lcdMotorsOff();
  }
}