#include "mbed.h" 
#include "TextLCD.h"
#include <string.h>
#include "motordriver.h"
#include "mpr121.h"
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalIn a(p5);
DigitalIn b(p6);
DigitalIn c(p7);
DigitalIn d(p8);
 
InterruptIn interrupt(p11);
// Setup the i2c bus on pins 28 and 27
I2C i2c(p9, p10);
// Setup the Mpr121:
// constructor(i2c object, i2c address of the mpr121)
Mpr121 mpr121(&i2c, Mpr121::ADD_VSS);
 
//Motor
Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor right(p26, p25, p24, 1);
 
//TextLCD lcd(p15, p16, p17, p18, p19, p20); // rs, e, d4-d7
TextLCD lcd(p15, p16, p17, p18, p19, p20);
Serial pc(USBTX, USBRX); // tx, rx 
Serial device(p13, p14);  // tx, rx 
 
//forward or in reverse mode
int forward;
 
//stores directions
int direction[10];
 
//stores speeds
float speed[10];
 
//stores durations
int duration[10];
 
//speed of the car
float car_speed;
 
//duration of the car
int car_duration;
 
//stores the command of the touchpad
int _command;
 
//check if there has been input to the touchpad
bool no_command;
 
//count variable for card reader
int count=0; 
 
//Number of total moves 
int num_moves;
 
//stores buzzcard information
char output[50];
 
//number of card swipes on the reader
int num_swipe=0;
int o_a, o_b;
//int count,ch, ignore_count;
int command[12];
int count_1=0; 
 
 
//password variables
int p_1, p_2,p_3,p_4;
 
//interrupt routine for touchpad
void fallInterrupt() {
    no_command = true;
    int s2=0;
    int key_code=0;
    int i=0;
    int value=mpr121.read(0x00);
    value +=mpr121.read(0x01)<<8;
    // LED demo mod by J. Hamblen
    //pc.printf("MPR value: %x \r\n", value);
    i=0;
    // puts key number out to LEDs for demo
    for (i=0; i<12; i++) {
        if (((value>>i)&0x01)==1) key_code=i+1;
        }
        
    s2 = key_code - 1;
    if(s2 > 0){
     
    _command=s2;
    no_command = false;
    led4=s2 & 0x01;
    led3=(s2>>1) & 0x01;
    led2=(s2>>2) & 0x01;
    led1=(s2>>3) & 0x01;
    }
     
    count_1++;
    //pc.printf("hello %f\n",s);
 
}
 
//function to turn left
void turnLeft(){
  lcd.cls();
  lcd.printf("Turning Left");
  left.speed(0);
  right.speed(0.4);
  wait(2);
  right.speed(0);
  wait(1);
}
  
//function to turn right
void turnRight(){
  lcd.cls();
  lcd.printf("Turning Right");
  left.speed(0.4);
  right.speed(0);
  wait(2);
  left.speed(0);
  wait(1);
}
 
//function to turn around
void turnAround(){
  lcd.cls();
  lcd.printf("Turning Around");
   
  left.speed(0.4);
  right.speed(0);
  wait(4);
  left.speed(0);
  wait(1);
}
 
 
//to move with input speed and duration of time
void go(float _speed, int _time){
    lcd.cls();
    lcd.printf("Speed %f Duration %d sec", _speed, _time);
    left.speed(_speed);
    right.speed(_speed);
    wait(_time);
    left.speed(0);
    right.speed(0);
    wait(1);
}
 
//starts the card reader and verifies the user
int cardReader(){
    num_swipe++;  
    int count=0;
    int ch=0;
    int ignore_count=0;
   // char output[50];
    while(1){
        if(device.readable()){
 
            ch=device.getc();
            if(ignore_count>5){
            output[count]=ch;
            count++;
            }
            ignore_count++;
            pc.putc(ch);
            }
            if (ignore_count>14){
                lcd.cls();
                 
                 if (strcmp(output,"902512409")==0){
                   lcd.printf("Confirmed\n");
                   lcd.printf("Welcome Sehoon\n");
                   for (int i=0; i<1000; i++){
                        if(device.readable())
                        device.getc();
                        }
                   return 1;               
                  }
                  else if (strcmp(output,"902536564")==0){
                   lcd.printf("Confirmed\n");
                   lcd.printf("Welcome Tanvir\n");
                   for (int i=0; i<1000; i++){
                        if(device.readable())
                        device.getc();
                        }
                   return 1;   
                   }       
                  else {
                       for (int i=0; i<100; i++){
                             if(device.readable())
                             ch=device.getc();
                             wait(.01);
                        }
                if (num_swipe==5){
                    lcd.printf("Too many tries\nEnter password");
                    
                    _command=0;
                    no_command=true;
                    while(1){
                    while(no_command){
                        wait(0.1);
                        }
                        p_1=_command;
                        _command=0;
                        no_command=true;
                        while(no_command){
                        wait(0.1);
                        }
                        p_2=_command;
                        _command=0;
                        no_command=true;
                        while(no_command){
                        wait(0.1);
                        }
                        p_3=_command;
                        _command=0;
                        no_command=true;
                        while(no_command){
                        wait(0.1);
                        }
                        p_4=_command;
                        _command=0;
                        no_command=true;
                         
                        if (p_1==1&&p_2==3&&p_3==5&&p_4==7)
                        {
                        lcd.cls();
                        lcd.printf("Correct!!!");
                        wait(2);
                        lcd.cls();
                        lcd.printf("Try swiping AGAIN!!");
                           break;
                           }
                           else{
                           lcd.cls();
                           lcd.printf("WRONG WRONG wROGNWRWEWERWER!@#!@");
                           wait(2);
                           lcd.cls();
                           } 
                            
                        }
                         
                         
                    }
                else
                lcd.printf("Try again\n");
                return 0;
                }
        }
    }    
     
}
 
//number of moves for the car
int number_of_moves(){
      //select duration
   //Selecting number of moves
   lcd.printf("Select Number of Moves");
   num_moves=100;
   _command=100;
   no_command = true;
   while(no_command){
    wait(0.1);
 
   }
 
   num_moves = _command;
 
     
  
    lcd.cls();
    lcd.printf("Number of Moves: %d", num_moves); 
    wait(2);
    return 0;
    }
     
int main() { 
 
    interrupt.fall(&fallInterrupt);   
  interrupt.mode(PullUp); 
  //Swiping buzz card 
   lcd.printf("Please Swipe your Buzzcard");
  while(!cardReader());
   wait(2);
   lcd.cls();
 
 
 
    number_of_moves();
   if (a||b||c||d){
        lcd.cls();
        lcd.printf("please turn off all switches");
        while(1){
            if (!(a||b||c||d))
            break;
        }
    }
  
    lcd.cls();
     
    
    for (int i=0; i<num_moves; i++){
     
    //select direction
   lcd.printf("Select Direction2=L 10=R 7=F 5=B");
   no_command = true;
   _command=0;
   while(_command!=2&&_command!=10&&_command!=7&&_command!=5){
    wait(0.1);
   }
        if(_command==2){
            direction[i]=1;
            lcd.cls();
            lcd.printf("Left");
   
            }
        else if (_command==10){
            direction[i]=2;
            lcd.cls();
            lcd.printf("Right");
 
             
            }
        else if (_command==7){
            direction[i]=3;
            lcd.cls();
            lcd.printf("Forward");
 
            }
        else if (_command==5){
            direction[i]=4;
            lcd.cls();
            lcd.printf("Reverse");
   
            }
             
        
    
   wait(2);
    lcd.cls();
     
     
    //select speed
   lcd.printf("Move Forward-1 or in Reverse-2");
   car_speed=0;
   _command=0;
   forward=0;;
   no_command = true;
   while(_command!=1&&_command!=2){
    wait(0.1);
   }
   if (_command==1){
        forward=1;
        lcd.cls();
        lcd.printf("Car moves forward");
        }
        else if (_command==2){
        forward=-1;
        lcd.cls();
        lcd.printf("Car moves in reverse");
        }
        wait(2);
        lcd.cls();
        _command=0;
   lcd.printf("Select Car Speed 1-10");
   while(_command!=1&&_command!=2&&_command!=3&&_command!=4&&_command!=5&&_command!=6&&_command!=7&&_command!=8&&_command!=9&&_command!=10){
        wait(0.1);
        }
        car_speed=forward*_command*0.1;
    
   speed[i]=car_speed;
   lcd.cls();
   lcd.printf("car speed: %d\n", _command);
   wait(2);
    
    lcd.cls();
     
     
     //select duration
  
   lcd.printf("Select Car duration 1-10 seconds");
   car_duration=0;
   _command=0;
   while(_command!=1&&_command!=2&&_command!=3&&_command!=4&&_command!=5&&_command!=6&&_command!=7&&_command!=8&&_command!=9&&_command!=10){
        wait(0.1);
        }
    car_duration=_command;
    duration[i]=car_duration;
    lcd.cls();
   lcd.printf("car duration: %d seconds\n", _command);
   wait(2);
    
 
  
    lcd.cls();
     
    }
     
    for (int i=10; i>0; i--){
        lcd.printf("Car will start in %d seconds",i);
        wait(1);
        lcd.cls();
    }
    lcd.printf("GO GO GO GO GO GO GO GO GO !!!");
    wait(1);
    for (int i=0; i<num_moves; i++){
  
       switch (direction[i]){
            case 1:
                turnLeft();
                break;
            case 2:
                turnRight();
                break;
            case 3:
                break;
            case 4:
                turnAround();
                break;
                }
      go(speed[i], duration[i]);
      }
      lcd.cls();
      lcd.printf("Finished");
}