#define IMU_sensor 0x68
#define PWR_MGMT_1 0x6B
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#include "I2Cdev.h"  
#include "MPU6050.h" // mpu6050 lib
#include <NewPing.h> // ultrasonic lib
#define OUTPUT_READABLE_ACCELGYRO

//motor driver L298N
#define int1 2
#define int2 4
#define int3 7
#define int4 8
#define ENA1 3
#define ENA2 5

//UltraSoninc sensor
#define TriggerPin 12
#define EchoPin 13
NewPing sonar(TriggerPin,EchoPin,50);


// MPU6050 code
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t gyro_offset_x = 0,gyro_offset_y = 0,gyro_offset_z = 0,accel_offset_x = 0,accel_offset_y = 0,accel_offset_z = 0;
int No_of_tests =1000;

void calibrate_IMU(void)
 {
  for(int i=0;i<No_of_tests;i++)
  { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    gyro_offset_x+=gx;
    gyro_offset_y+=gy;
    gyro_offset_z+=gz;
    accel_offset_x+=ax;
    accel_offset_y+=ay;
    accel_offset_z+=az;
  }  
  gyro_offset_x= (1.0 *gyro_offset_x) /No_of_tests;
  gyro_offset_y=(1.0 * gyro_offset_y) /No_of_tests;
  gyro_offset_z=(1.0 * gyro_offset_z) /No_of_tests;
  accel_offset_x=(1.0 * accel_offset_x)/ No_of_tests;
  accel_offset_y=(1.0 * accel_offset_y)/ No_of_tests;
  accel_offset_z=(1.0 * accel_offset_z)/ No_of_tests;
 }


void MPU_init(void)
{
  Wire.beginTransmission(IMU_sensor);
  Wire.write(PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission();
}
void AccelConfiguration(void)
{
  Wire.beginTransmission(IMU_sensor);
  Wire.write(ACCEL_CONFIG);
  Wire.write(0x08);
  Wire.endTransmission();
}

void GyroConfig(void)
{

Wire.beginTransmission(IMU_sensor);
  Wire.write(GYRO_CONFIG);
  Wire.write(0x18);
  Wire.endTransmission();
 
}

////////////////////////////////////////////////////////////////



// motor driver L298N code
int motorSpeed_L = 0;
int motorSpeed_R = 0;
int Speed;
double speed_filt = 0 ;
double alpha = 0.9;
 
void stopMoving()
{
  
Speed = 0;
speed_filt =  (alpha * Speed) + ((1-alpha) *speed_filt);
   motorSpeed_L = motorSpeed_R = speed_filt;
  
  /*while(speed!=0)
  speed-=50;
 { motorSpeed_L = motorSpeed_R = Speed;
   delay(10);
  if(motorSpeed_L<0)
    motorSpeed_L = 0;
  else if(motorSpeed_R<0)
    motorSpeed_R = 0;*/

  analogWrite(ENA1,motorSpeed_L);
  analogWrite(ENA2,motorSpeed_R);
//}
 //motor L
  digitalWrite(int1,LOW);
  digitalWrite(int2,LOW);
  //motor R
  digitalWrite(int3,LOW);
  digitalWrite(int4,LOW);
}

void goForward(int S)
{  
  Speed =S;
  speed_filt =  (alpha * Speed) + ((1-alpha) *speed_filt);

  

  motorSpeed_L = motorSpeed_R = speed_filt ;
  //motor L
  digitalWrite(int1,LOW);
  digitalWrite(int2,HIGH);
  //motor R
  digitalWrite(int3,LOW);
  digitalWrite(int4,HIGH);
  analogWrite(ENA1,motorSpeed_L);
  analogWrite(ENA2,motorSpeed_R);

}

void goBackwards(int S )
{ 
   Speed = S;
   speed_filt =  (alpha * Speed) + ((1-alpha) *speed_filt);

  motorSpeed_L = motorSpeed_R = speed_filt ;
  analogWrite(ENA1,motorSpeed_L);
  analogWrite(ENA2,motorSpeed_R); 
  //motor L
  digitalWrite(int1,HIGH);
  digitalWrite(int2,LOW);
  //motor R
  digitalWrite(int3,HIGH);
  digitalWrite(int4,LOW);

   
}

void turnLeft(int S , int angle )
{
  Speed = S;
 speed_filt =  (alpha * Speed) + ((1-alpha) *speed_filt);

  motorSpeed_L = motorSpeed_R = speed_filt ;
 if(angle<0)
  {  delay(10);
  analogWrite(ENA1,motorSpeed_L);
  analogWrite(ENA2,motorSpeed_R);
   //motor L
  digitalWrite(int1,HIGH);
  digitalWrite(int2,LOW);
  //motor R
  digitalWrite(int3,LOW);
  digitalWrite(int4,HIGH);
    while(gz>angle)
      if(gz<=angle)
        stopMoving();
      
  }

  

}

void turnRight(int S , int angle)
{

   
  Speed = S;
  speed_filt =  (alpha * Speed) + ((1-alpha) *speed_filt);

  motorSpeed_L = motorSpeed_R = speed_filt ;
  if(angle>0)
  {  delay(10);
  analogWrite(ENA1,motorSpeed_L);
  analogWrite(ENA2,motorSpeed_R);
   //motor L
  digitalWrite(int1,LOW);
  digitalWrite(int2,HIGH);
  //motor R
  digitalWrite(int3,HIGH);
  digitalWrite(int4,LOW);
    while(gz<angle)
      if(gz>=angle)
        stopMoving();
      
  }

}





//////////////////////////////////////////////////////////
 int setup_time = 0;
 int timer = 0;
 int flag =0 ;
void setup() {
  
// join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
  //MPU6050
   Serial.begin(9600);
delay(1000);
    // initialize device
   // accelgyro.initialize();
     Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
   MPU_init();
   AccelConfiguration();
   GyroConfig();
   calibrate_IMU();
   setup_time = millis() * 0.001 ;

///////////////////////////////////////////////////


  //initialize pins to output data from the Arduino Uno to L298N
  pinMode(int1,OUTPUT);
  pinMode(int2,OUTPUT);
  pinMode(int3,OUTPUT);
  pinMode(int4,OUTPUT);
  pinMode(ENA1,OUTPUT);
  pinMode(ENA2,OUTPUT);

  //Decide the polarity of each motor
  //motor L
  digitalWrite(int1,LOW);
  digitalWrite(int2,LOW);
  //motor R
  digitalWrite(int3,LOW);
  digitalWrite(int4,LOW);


/////////////////////////////////////////////




}

void loop() {
//UltraSonic sensor
Serial.print("Dist:");
Serial.println(sonar.ping_cm());





 /////////////////////////////////////////////////////
   // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ax -= accel_offset_x;
    ay -= accel_offset_y;
    az -= accel_offset_z;
    gx -= gyro_offset_x;
    gy -= gyro_offset_y;
    gz -= gyro_offset_z;


    // these methods (and a few others) are also available
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);

    #ifdef OUTPUT_READABLE_ACCELGYRO
        // display tab-separated accel/gyro x/y/z values
        //Serial.print("a/g:\t");
        //Serial.print(ax); Serial.print("\t");
        //Serial.print(ay); Serial.print("\t");
        //Serial.print(az); Serial.print("\t");
        //Serial.print(gx); Serial.print("\t");
        //Serial.print(gy); Serial.print("\t");
        gz = gz*(2000.0/32768);
        //Serial.println(gz);
    #endif

    #ifdef OUTPUT_BINARY_ACCELGYRO
       // Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
        //Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
        //Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
        //Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
        //Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
        //Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
    #endif


timer = millis() * 0.001 - setup_time ;
//Serial.println(timer);
if(timer== 0 && flag == 0)
 {Serial.println("Not Moving");
  stopMoving();
  flag =1;
 }
 if(timer== 1 && flag ==1)
 {Serial.println("Forward");
  goForward(220);
  flag =0;
 }
 if(timer== 10 && flag == 0)
 {Serial.println("Right");
  turnRight(220,gz);
  flag =1;
 }
 if(timer== 12 && flag ==1)
 {Serial.println("Not Moving");
  stopMoving();
  flag=0;
 }
 if(timer== 13 && flag ==0)
 {Serial.println("Left");
  turnLeft(220,gz);
  flag =1;
 }
 if(timer== 15 && flag ==1)
 {Serial.println("Not Moving");
  stopMoving();
  flag=0;
 }
 if(timer== 16 && flag ==0)
 {Serial.println("Forward");
  goForward(220);
  flag =1;
 }
 if(timer== 20 && flag ==1)
 {Serial.println("Not Moving");
  stopMoving();
  flag =0;
 }

delay(250);



}