///// libraries
  #include <Wire.h>
  #include <Adafruit_SSD1306.h>
  #include <Keypad.h>
  #include <Servo.h>
  #include <Ultrasonic.h>


///// hardware pins
  
  // right motor
  const int pwm_right_motor = 2; // right motor pins
  const int inA_right_motor = 3;  
  const int inB_right_motor = 4;
  #define PinA_right_encoder 18   // right encoder chanels
  #define PinB_right_encoder 14

  // left motor
  const int pwm_left_motor = 5; // left motor pins
  const int inA_left_motor = 6;  
  const int inB_left_motor = 7;
  #define PinA_left_encoder 19  // left encoder chanels
  #define PinB_left_encoder 15

  // vertical motor
  const int pwm_vertical_motor = 8; // motor pins
  const int inA_vertical_motor = 9;  
  const int inB_vertical_motor = 10;
  #define PinA_vertical_encoder 20   //encoder chanels
  #define PinB_vertical_encoder 16

  // horizontal motor
  const int pwm_horizontal_motor = 11; // motor pins
  const int inA_horizontal_motor = 13;  
  const int inB_horizontal_motor = 12;
  #define PinA_horizontal_encoder 21   //encoder chanels
  #define PinB_horizontal_encoder 17

  // servo
  const int servo_pin = 22;

  // line sensor
  const int right_lineSensor = A0;
  const int middle_lineSensor = A1;
  const int left_lineSensor = A2;

  // ultrasonic (HC-SR04)
  const int front_ultra_trig = 34; // front ultrasonic
  const int front_ultra_echo = 35;
  const int back_ultra_trig = 36; // back ultrasonic
  const int back_ultra_echo = 37;
  const int left_ultra_trig = 38; // left ultrasonic
  const int left_ultra_echo = 39;

  // limit switch
  const int gripper_limit_SW = 23;
  const int vertical_limit_SW = 24;
  const int horizontal_limit_SW = 25;

  // keypad
  byte rowPins[4] = {26, 27, 28, 29}; //connect to the row pinOuts of the keypad
  byte colPins[4] = {30, 31, 32, 33}; //connect to the column pinOuts of the keypad

  // OLED (I2C)
  // connected with SDA, SCL

//// variables

  // hardware
  const double wheel_distance = 50 ;    // Measure the diameter of the two wheels in cm
  int rows_pos[3]  = {17700,10700,3300};// shelves position with encoder in vertical robot axis in order A,B,C
  int servo_home_pos = 100;             // servo home position suitable for user to put or get book
  double vertical_home_pos = 17000;     // vertical home position suitable for user to put or get book
  double horizontal_home_pos = 150;     // horizontal home position suitable for user to put or get book

  //motor
  int max_pwm = 255;           // maximum pwm command for motors
  const double max_linear = 3; // Max linear speed in cm/s
  const double max_angular = max_linear/(wheel_distance/2); // Max angular speed in cm/s

  // time variables
  const int loop_time = 300 ;       // Loop time in millisecond (important for PID sample time)
  unsigned long lastMilli = 0;      // time history

  // encoder
  const double right_encoder_Puls_per_cm = 42; // encoder puls per cm
  const double left_encoder_Puls_per_cm = 42; 
  const double vertical_encoder_Puls_per_cm = 242;
  const double horizontal_encoder_Puls_per_cm = 124; 
  double right_encoder_pulses_1cmps = (right_encoder_Puls_per_cm/1000) * loop_time; //pulses that gives speed 1cm/s within loop time(ms)
  double left_encoder_pulses_1cmps = (left_encoder_Puls_per_cm/1000) * loop_time; 
  double vertical_encoder_pulses_1cmps = (vertical_encoder_Puls_per_cm/1000) * loop_time; 
  double horizontal_encoder_pulses_1cmps = (horizontal_encoder_Puls_per_cm/1000) * loop_time; 
  double right_encoder_pulses=0, left_encoder_pulses=0, vertical_encoder_pulses=0, horizontal_encoder_pulses=0;

  // PID
  double smaKp = 0.5, smaKi = 0.5, smaKd = 0.03; // used with small error gape
  double aggKp = 0.4, aggKi = 0.02, aggKd = 0.001; // used with aggressive error gape
  unsigned long previousTime = 0;
  double lastError = 0, cumError = 0;
  
  // servo
  Servo gripper_servo;  // create servo object to control a servo
  
  // ultrasonic
  Ultrasonic front_ultrasonic(front_ultra_trig, front_ultra_echo); // trig and echo pins for ultrasonic
  Ultrasonic back_ultrasonic(back_ultra_trig, back_ultra_echo);
  Ultrasonic left_ultrasonic(left_ultra_trig, left_ultra_echo);
  double min_ultra_distance = 20; // minimum distance from ultrasonic sensor in cm (after this robot will stop)

  // keypad
  const byte ROWS = 4; //four rows
  const byte COLS = 4; //four columns
  char keys[ROWS][COLS] = {
    {'1','2','3','A'},
    {'4','5','6','B'},
    {'7','8','9','C'},
    {'*','0','#','D'}
  };
  Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );//Create an object of keypad

  // OLED display
  #define SCREEN_WIDTH 128 // OLED display width, in pixels
  #define SCREEN_HEIGHT 64 // OLED display height, in pixels
  #define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
  #define SCREEN_ADDRESS 0x3C
  Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); // Declaration for SSD1306 display connected using I2C

  
//// methods

  // right encoders interrupts
  void read_right_encoder () {
    if (digitalRead(PinB_right_encoder) > 0){
      right_encoder_pulses++;
    }
    else {
      right_encoder_pulses--;
    }
  }

  // left encoders interrupts
  void read_left_encoder () {
    if (digitalRead(PinB_left_encoder) > 0){
      left_encoder_pulses++;
    }
    else {
      left_encoder_pulses--;
    }
  }

  // vertical encoders interrupts
  void read_vertical_encoder () {
    if (digitalRead(PinB_vertical_encoder) > 0){
      vertical_encoder_pulses++;
    }
    else {
      vertical_encoder_pulses--;
    }
  }

  // horizontal encoders interrupts
  void read_horizontal_encoder () {
    if (digitalRead(PinB_horizontal_encoder) > 0){
      horizontal_encoder_pulses++;
    }
    else {
      horizontal_encoder_pulses--;
    }
  }

  // initialization for library robot hardware
  void init_HW(){

    // initialization serial for communication
    Serial.begin(9600);

    // right motor pins mode
    pinMode (pwm_right_motor, OUTPUT); // motor
    pinMode (inA_right_motor, OUTPUT); 
    pinMode (inB_right_motor, OUTPUT);
    // right encoder pins

    pinMode (PinA_right_encoder, INPUT_PULLUP);
    pinMode (PinB_right_encoder, INPUT_PULLUP); 
    attachInterrupt (digitalPinToInterrupt(PinA_right_encoder), read_right_encoder, RISING);
    
    // left motor pins mode
    pinMode (pwm_left_motor, OUTPUT);
    pinMode (inA_left_motor, OUTPUT); 
    pinMode (inB_left_motor, OUTPUT);
    
    // right encoder pins
    pinMode (PinA_left_encoder, INPUT_PULLUP);
    pinMode (PinB_left_encoder, INPUT_PULLUP); 
    attachInterrupt (digitalPinToInterrupt(PinA_left_encoder), read_left_encoder, RISING);
    
    // vertical motor pins mode
    pinMode (pwm_vertical_motor, OUTPUT);
    pinMode (inA_vertical_motor, OUTPUT); 
    pinMode (inB_vertical_motor, OUTPUT);
    attachInterrupt (digitalPinToInterrupt(PinA_vertical_encoder), read_vertical_encoder, RISING);  
    
    // horizontal encoder pins 
    pinMode (PinA_horizontal_encoder, INPUT_PULLUP);
    pinMode (PinB_horizontal_encoder, INPUT_PULLUP); 
    attachInterrupt (digitalPinToInterrupt(PinA_horizontal_encoder), read_horizontal_encoder, RISING);

    // servo
    gripper_servo.attach(servo_pin);  // attaches the servo on pin to the servo object

    // line sensor
    pinMode(right_lineSensor,INPUT);
    pinMode(middle_lineSensor,INPUT);
    pinMode(left_lineSensor,INPUT);

    // ultrasonic
    // initialized in ultrasonic library

    // limit switch
    pinMode(vertical_limit_SW, INPUT);
    pinMode(horizontal_limit_SW, INPUT);
    pinMode(gripper_limit_SW, INPUT);

    // initialize the OLED object
    if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
      Serial.println(("SSD1306 allocation failed"));
      for(;;); // Don't proceed, loop forever
    }
    display.clearDisplay(); // Clear the buffer.
    display.setTextSize(1);
    display.setTextColor(WHITE);

  }

  // setting motor pwm
  void set_motor( int PWM_val ,int pwm_pin ,int inpA ,int inpB ){

    PWM_val = constrain(PWM_val,-max_pwm,max_pwm);
    
    if(PWM_val > 0 ){
      analogWrite(pwm_pin , abs(PWM_val) );
      digitalWrite(inpA , HIGH);
      digitalWrite(inpB , LOW);
    }
    else{
      analogWrite(pwm_pin , abs(PWM_val) );
      digitalWrite(inpA , LOW);
      digitalWrite(inpB , HIGH);
    }
  }

  // PID control function(proportional, integral, derivative)
  double computePID(double gaolPoint, double currentPoint, double minOut, double maxOut, double Kp, double Ki, double Kd){

    unsigned long currentTime = millis();                      // get current time
    double elapsedTime = (double)(currentTime - previousTime); // compute time elapsed from previous computation in (ms)
    elapsedTime = elapsedTime/1000;                            // convert elapsed to (s)
    double error = gaolPoint - currentPoint;                   // determine error
    cumError += error * elapsedTime;                           // compute integral
    double rateError = (error - lastError)/elapsedTime;        // compute derivative
    double output = Kp*error + Ki*cumError + Kd*rateError;     // compute PID output
    output = constrain(output, minOut, maxOut);                // constrain output
    previousTime = currentTime;                                // record current time
    lastError = error;                                         // record current error
    return output;
    
  }

  // read line sensors
  bool* read_line(){

    int color_threshold = 512;

    static bool output[3]; // right, middle and left line sensors

    if(analogRead(A0)>color_threshold) output[0] = true;
    else output[0] = false;

    if(analogRead(A1)>color_threshold) output[1] = true;
    else output[1] = false;

    if(analogRead(A2)>color_threshold) output[2] = true;
    else output[2] = false;

    return output;
  }

  // follow line until reach the end state
  void follow_line(int end_state = -1){ // -1 means 'hom state'

    Serial.println("following line to " + String(end_state) + " state");

    int state_count = -1; // start with home state
    bool* line_sens;      // line sensor readings

    while(true){
      
      // read line
      line_sens = read_line();

      // follow line
      if (line_sens[0] && !line_sens[1] && line_sens[2]){ // forward
        // check ultrasonic distance
        if (front_ultrasonic.read() < min_ultra_distance) {
          // stop motors
          set_motor(0,pwm_right_motor,inA_right_motor,inB_right_motor);
          set_motor(0,pwm_left_motor,inA_left_motor,inB_left_motor);
        }

        else{
          set_motor(255,pwm_right_motor,inA_right_motor,inB_right_motor);
          set_motor(255,pwm_left_motor,inA_left_motor,inB_left_motor);
          Serial.println("forward...");
        }
      }

      if (line_sens[0] && line_sens[1] && line_sens[2]){ // backward (all sense white)
        // check ultrasonic distance
        if (back_ultrasonic.read() < min_ultra_distance) {
          // stop motors
          set_motor(0,pwm_right_motor,inA_right_motor,inB_right_motor);
          set_motor(0,pwm_left_motor,inA_left_motor,inB_left_motor);
        }

        else{
          set_motor(-255,pwm_right_motor,inA_right_motor,inB_right_motor);
          set_motor(-255,pwm_left_motor,inA_left_motor,inB_left_motor);
          Serial.println("backward...");
        }
      }

      else if (!line_sens[0] && line_sens[2]){ // turn right
        set_motor(-255,pwm_right_motor,inA_right_motor,inB_right_motor);
        set_motor(255,pwm_left_motor,inA_left_motor,inB_left_motor);
        Serial.println("turn right...");
      }

      else if (line_sens[0] && !line_sens[2]){ // turn left
        set_motor(255,pwm_right_motor,inA_right_motor,inB_right_motor);
        set_motor(-255,pwm_left_motor,inA_left_motor,inB_left_motor);
        Serial.println("turn left...");
      }

      // check state
      if (!line_sens[0] && !line_sens[2]){ // end

        // update state count
        if (line_sens[1]) state_count = -1;
        else state_count++;

        // check end state
        if (end_state == state_count) {
          set_motor(0,pwm_right_motor,inA_right_motor,inB_right_motor);
          set_motor(0,pwm_left_motor,inA_left_motor,inB_left_motor);
          Serial.println("state " + String(end_state) + " reached");
          break;
        }

        // pass current state
        while(!line_sens[0] && !line_sens[2]){
          line_sens = read_line();
          set_motor(255,pwm_right_motor,inA_right_motor,inB_right_motor);
          set_motor(255,pwm_left_motor,inA_left_motor,inB_left_motor);
          Serial.println("passing state " + String(state_count) + "...");
          delay(100);
        }
      }

      // delay
      delay(100);
      
    }
  }

  // move vertical motor to reach target postion
  void move_vertical ( double target_pos ) {

    // init pwm variable
    double pwm_req;
    double error_gape = 20000;
    // reset PID parameters
    previousTime = millis();
    lastError = 0;
    cumError = 0;

    while (error_gape >= 50 && vertical_encoder_pulses >= 0 && vertical_encoder_pulses <= 19000){

      // check limit switch
      if (digitalRead(vertical_limit_SW) == HIGH) vertical_encoder_pulses = 0;
      
      // compute error gape
      error_gape = abs(target_pos - vertical_encoder_pulses);

      // compute PID
      if ( error_gape > 500){ // aggressive error gape
        pwm_req = computePID(target_pos, vertical_encoder_pulses, -max_pwm, max_pwm, aggKp, aggKi, aggKd);
      }
      else{
        pwm_req = computePID(target_pos, vertical_encoder_pulses, -max_pwm, max_pwm, smaKp, smaKi, smaKd);
      }

      // motor action
      set_motor(round(pwm_req),pwm_vertical_motor,inA_vertical_motor,inB_vertical_motor);      

      // delay
      delay(100);

      Serial.println("V_pos: " + String(vertical_encoder_pulses));

    }
  
    // stop motor
    set_motor(0,pwm_vertical_motor,inA_vertical_motor,inB_vertical_motor);

  }

  // move horizontal motor to reach target postion
  void move_horizontal ( double target_pos ) {

    // init pwm variable
    double pwm_req;
    double error_gape = 20000;
    // reset PID parameters
    previousTime = millis();
    lastError = 0;
    cumError = 0;

    while (error_gape >= 50 ){
      
      // check limit switch
      if (digitalRead(horizontal_limit_SW) == HIGH) horizontal_encoder_pulses = 0;

      // compute error gape
      error_gape = abs(target_pos - horizontal_encoder_pulses);

      // compute PID
      if ( error_gape > 500){ // aggressive error gape
        pwm_req = computePID(target_pos, horizontal_encoder_pulses, -max_pwm, max_pwm, aggKp, aggKi, aggKd);
      }
      else{
        pwm_req = computePID(target_pos, horizontal_encoder_pulses, -max_pwm, max_pwm, smaKp, smaKi, smaKi);
      }

      // motor action
      set_motor(round(pwm_req),pwm_horizontal_motor,inA_horizontal_motor,inB_horizontal_motor);      

      // delay
      delay(10);

      Serial.println("H_pos: " + String(horizontal_encoder_pulses));

    }
  
    // stop motor
    set_motor(0,pwm_horizontal_motor,inA_horizontal_motor,inB_horizontal_motor);

  }

  // grape book from row position
  void get_book(int target_row_pos){

    Serial.println("getting book from " + String(target_row_pos) + " row");

    // init position variables
    double vertical_d = 1000;
    double horizontal_pos1 = 1500;
    double horizontal_pos2 = 2700;

    // action steps to get book
    gripper_servo.write(servo_home_pos);        // step 1
    move_vertical(target_row_pos - vertical_d); // step 2
    move_horizontal(horizontal_pos1);           // step 3
    move_vertical(target_row_pos + vertical_d); // step 4
    delay(2000);
    move_horizontal(horizontal_pos2);           // step 5
    delay(2000);
    move_horizontal(horizontal_home_pos);       // step 6
    move_vertical(vertical_home_pos);           // step 7

  }

  // grape book from row position
  void put_book(int target_row_pos){

    Serial.println("putting book from " + String(target_row_pos) + " row");
    
    // init position variables
    int servo_put_pos = 20;
    double vertical_d = 1000;
    double horizontal_pos1 = 1800;

    // action steps to get book
    gripper_servo.write(servo_home_pos);        // step 1
    move_vertical(target_row_pos + vertical_d); // step 2
    move_horizontal(horizontal_pos1);           // step 3
    delay(1000);
    move_vertical(target_row_pos - vertical_d); // step 4
    delay(1000);
    gripper_servo.write(servo_put_pos);         // step 5
    delay(2000);
    move_horizontal(horizontal_home_pos);       // step 6
    gripper_servo.write(servo_home_pos);        // step 7
    move_vertical(vertical_home_pos);           // step 8

  }

  // reset home position for gripper
  void gripper_reset_home(){

    Serial.println("reseting gripper home position");

    // reset horizontal
    while(digitalRead(horizontal_limit_SW) == LOW){
      set_motor(-150,pwm_horizontal_motor,inA_horizontal_motor,inB_horizontal_motor);
      delay(10);
    }
    set_motor(0,pwm_horizontal_motor,inA_horizontal_motor,inB_horizontal_motor);
    Serial.println("There was an horizontal error: " + String(horizontal_encoder_pulses - 0));
    horizontal_encoder_pulses = 0; // reset zero point
    move_horizontal(horizontal_home_pos);

    // reset servo
    gripper_servo.write(servo_home_pos);

    // reset vertical
    while(digitalRead(vertical_limit_SW) == LOW){
      set_motor(-200,pwm_vertical_motor,inA_vertical_motor,inB_vertical_motor);
      delay(10);
    }
    set_motor(0,pwm_vertical_motor,inA_vertical_motor,inB_vertical_motor);
    Serial.println("There was an vertical error: " + String(vertical_encoder_pulses - 0));
    vertical_encoder_pulses = 0; // reset zero point
    move_vertical(vertical_home_pos);
    
    delay(100);
    
  }

  // get job from user
  char* get_job(){
    
    Serial.println("getting jop from user...");

    static char job[3]; // job array in order of: row, column, operation

    // showing massage on display
    display.clearDisplay();
    display.setCursor(5,5);
    display.print("Enter book position.");
    display.display();

    // get Row position
    while(true){
      display.setCursor(5,20);
      display.print("Row: ");
      display.display();
      char key = keypad.getKey();// Read the key
      if(isAlpha(key) && key != 'D'){
        job[0] = key;
        display.print(key);
        display.display();
        break;
      }
    }

    // get column position
    while(true){
      display.setCursor(5,35);
      display.print("Col: ");
      display.display();
      char key = keypad.getKey();// Read the key
      if(isDigit(key)){
        job[1] = key;
        display.print(key);
        display.display();
        break;
      }
    }

    // get operation (get/put)
    while(true){
      display.setCursor(5,50);
      display.print("Get(*)");
      display.setCursor(90,50);
      display.print("Put(#)");
      display.display();
      char key = keypad.getKey();// Read the key
      if(key == '*' || key == '#'){ 
        job[2] = key;
        break;
      }
    }

    return job;
  }

  // doing a job
  void do_job(char* job){

    // sowing massage on screen
    display.clearDisplay();
    display.setCursor(2,30);
    display.print("On way to "+ String(job[0])+String(job[1])+" ...");
    display.display();
    display.startscrollleft(0x00, 0x07);

    // go to column position
    follow_line( String(job[1]).toInt());

    // get Row position
    int row_pos=0;
    switch(job[0]){
      case 'A':
        row_pos = rows_pos[0];
        break;
      case 'B':
        row_pos = rows_pos[1];
        break;
      case 'C':
        row_pos = rows_pos[2];
        break;
    }

    // get or put book
    if (job[2] == '*'){ // getting book from row position
      display.clearDisplay();
      display.setCursor(2,30);
      display.print("getting from "+ String(job[0])+ "...");
      display.display();
      get_book(row_pos); 
    }
    else if(job[2] == '#'){ // putting book from row position
      display.clearDisplay();
      display.setCursor(2,30);
      display.print("putting on "+ String(job[0])+ "...");
      display.display();
      put_book(row_pos); 
    }
    
    // go back to home state
    display.clearDisplay();
    display.setCursor(2,30);
    display.print("going home...");
    display.display(); 
    follow_line(-1);
    
    // sowing completion massage on screen
    display.stopscroll();
    display.clearDisplay();
    display.setCursor(5,10);
    display.print("task completed.");
    display.display();
    delay(3000);
  }


  
  
//// main


  int job_count = 0;  // job counter for library robot

  // the setup routine runs once when you press reset
  void setup() {

    init_HW(); // hardware initialization for library robot

    // gripper_reset_home(); // reset gripper home point
    
  }

  // the loop routine runs over and over again forever:
  void loop() {

    // init target job
    char* job_arr; // job array in order of: row(A,B,C), column(1,2,3..), operation(get(*) or put(#))

    // get job from user
    job_arr = get_job();

    // doing job
    // do_job(job_arr);

    // increment job counter
    job_count++;

    // reset gripper hom point after each 5 jobs
    // if (job_count == 5){
    //   gripper_reset_home();
    //   job_count = 0;
    // }

  }