//------------Wheel Geometry---------------
//Diameter of the wheel≈65-66 mm => Circumference=207.24mm 
//20 impulses-207.24mm => 1 impulse-10.362mm

#include <Servo.h>
Servo servo1;

int recieved=0; // Variable recieved from Rx
int pulsepin=7; // Input pin for the IR sensor
int motor=4; // Output for motor
bool pulse=false; // Boolean for reading from the IR sensor
const int n=10; //Number of impulses we have to count
int count=0; // Number of impulses from the IR sensor
unsigned long time1=0; // Time before counting N pulses
unsigned long time2=0; // Time after counting N pulses
unsigned long duration=0; // Duration of N impulses
unsigned long time3=0; // For delaying servo
unsigned long time4=0;// For delaying servo
unsigned long duration2=0; // For delaying servo for 1 second
float veliocity=0.0;
float brakingD=0.0;
bool flag=true;


void setup() {
  Serial.begin(9600);
  pinMode(pulsepin,INPUT);
  pinMode(motor,OUTPUT);
  servo1.attach(5);
}

void loop() {
//-----------------------------connection to the bluetooth module-------------------------------
 
  if(Serial.available()>0) 
  {
     recieved = Serial.read();
  }
 
//------------------------------Resets all of variables------------------------------

  if(recieved=='3'){  
    resetVariables();
  }
  resetServo();
 
  if(recieved =='1'){  //"GO" command, starts the accelerating micromotor
    accelerateMotor();
    recieved='0';
  }
  
//----------------------braking--------------------------------------

  if (recieved =='2'){     
    
  //----------------------IR sensor pulses-----------------------
  
    time1=millis();  //exact time before counting N pulses, (ms)
    countNPulses();
    time2=millis(); //exact time after counting N pulses, (ms)
   
   //---------------------- τ & V - time and speed--------------------------------
   
    calculateTimeAndSpeed();
    delay(10); // test 

   //---------------------- Breaking Distance Calculation --------------------------------
    calculateBrakingDistance();
    Serial.println(brakingD);
    recieved=0;
  
  }  
  //---------------------------ABS braking--------------------
  if (recieved =='4'){ 
    //----------------------IR sensor pulses-----------------------
    
    time1=millis();  //exact time before counting N pulses, (ms)     
    countNPulses();
    time2=millis(); //exact time after counting N pulses, (ms)
     
   //---------------------- τ & V - time and speed--------------------------------
    calculateTimeAndSpeed();
        
    //----------------------braking distance--------------------------------------
    ABSdistance();
    Serial.println(brakingD);
    recieved=0;
  }  
}
   
void resetVariables(){
  veliocity=0.0;
  brakingD=0.0;
  duration=0;
  duration2=0;
  pulse=false;
  recieved=0;
  count=0;
}

void resetServo(){  
 servo1.write(90);    // Free position of the servo
}

void brake(){
  servo1.write(80); // Servo braking position
}

void accelerateMotor(){
  digitalWrite(motor,HIGH); // Starts the accelerating micro motor
}

void stopMotor(){
  digitalWrite(motor,LOW); //Stops the motor
}

void countNPulses(){
  count=0;
  while(count!=n){// while the number of impulses isn't equal to N
    pulse=digitalRead(pulsepin);
    if((pulse==true)&&flag){
      count=count+1; 
      flag=false;
    }
    if (!pulse&&!flag){
      flag=true;
    }
  }
}

void calculateTimeAndSpeed(){
  duration=time2-time1; //duration of N impulses
  //veliocity =1.036 /(1000000*pulseIn(pulsepin, HIGH,1000000));  
   veliocity=((n*10.362)/duration)*100; // [cm/s]  
   Serial.println(veliocity);
}

void calculateBrakingDistance(){
  stopMotor();
  brake();
  time3=millis();
  duration2=0;
  count=0;  
  while(duration2<1000){
    brakingD=count*1.0362; //cm
    pulse=digitalRead(pulsepin);
    if((pulse==true)&&flag){
      count=count+1; 
      flag=false;
    }
    if (!pulse&&!flag){
      flag=true;
    }
    time4=millis(); 
    duration2=time4-time3;
  }
}

void ABSdistance(){
  stopMotor();
  brake();   
  count=0;
  time3=millis();
  while(duration2<1000){
    brake();
    brakingD=count*1.0362; //cm
    pulse=digitalRead(pulsepin);
    if((pulse==true)&&flag){
      servo1.write(86);
      delay(10);
      count=count+1; 
      flag=false;
    }
    if (!pulse&&!flag){
      flag=true;
    }
    time4=millis(); 
    duration2=time4-time3;
  }      
}

