#include <MsTimer2.h>        // Timer2 library
#include <PinChangeInt.h>    // This library enables all pins as external interrupts
#include <MPU6050.h>         // MPU6050 library
#include <Wire.h>            // IIC communication library
#include <HCSR04.h>          // Ultrasonic sensor library

MPU6050 mpu6050;                     // Instantiate MPU6050 object
int16_t ax, ay, az, gx, gy, gz;      // Define 3-axis accelerometer and gyroscope variables

// TB6612 pin definitions
const int right_R1=12;    
const int right_R2=13;
const int PWM_R=10;

const int left_L1=6;
const int left_L2=7;
const int PWM_L=9;

const int STBY=8;


///////////////////////Angle parameters//////////////////////////////////////////////////////
float angle_X;                // X-axis tilt angle calculated from accelerometer
float angle_Y;               // Y-axis tilt angle calculated from accelerometer
float angle0 = 0;            // Mechanical balance angle (ideally 0 degrees)
float Gyro_x,Gyro_y,Gyro_z;  // Angular velocity calculated from gyroscope
///////////////////////Kalman filter//////////////////////////////////////////////
float Q_angle = 0.001;    // Gyroscope noise covariance    
float Q_gyro = 0.003;     // Gyroscope drift noise covariance
float R_angle = 0.5;      // Accelerometer covariance
char C_0 = 1;
float dt = 0.005;        // dt is the filter sampling time
float K1 = 0.05;         // Use a function containing Kalman gain to calculate optimal bias
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias;           // Gyroscope drift

float accelz = 0;
float angle;
float angleY_one;
float angle_speed;
unsigned int CLB_u8trig;

float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
///////////////////////////////////PID parameters///////////////////////////////////
double kp = 38, ki = 0.0, kd = 0.58;                                  // Angle loop parameters
double kp_speed = 3.8, ki_speed = 0.11, kd_speed = 0.0;            //  Speed loop parameters
double kp_turn = 15, ki_turn = 0, kd_turn = 0.15;                  //  Steering system parameters
double setp0 = 0;                                                  //  Angle balance point
int PD_pwm;                                                        //  Angle output
float pwm1=0,pwm2=0;

/////////////////////////////////Interrupt speed count/////////////////////////////
#define PinA_left 2    // External interrupt
#define PinA_right 4   // External interrupt
volatile long count_right = 0; // Pulse count from Hall encoder (volatile long ensures value validity)
volatile long count_left = 0;
int speedcc = 0;
//////////////////////Pulse count////////////////////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int pulseright,pulseleft;
////////////////////////////////Variable parameters//////////////////////////
float speeds_filterold=0;
float positions=0;
int flag1;
double PI_pwm;
int cc;
int speedout;
float speeds_filter;
float fultrasonic  = 0;   // Ultrasonic control variable
int Ultrasonicflag = 0;  
float Distance  = 0;

//////////////////////////////Rotation PD///////////////////////////
int turnmax,turnmin,turnout; 
float Turn_pwm = 0;
int zz=0;
int turncc=0;

//Bluetooth control//
int carfront = 0;  //  Forward variable
int carback  = 0;   //  Backward
int carleft  = 0;   //  Turn left
int carright = 0;  //  Turn right
char val;

HCSR04 hc(A2,A3);            //initialisation class HCSR04 (trig pin , echo pin)

void setup() 
{
  // Set motor control pins as OUTPUT
  pinMode(right_R1,OUTPUT);       
  pinMode(right_R2,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(PWM_L,OUTPUT);
  pinMode(STBY, OUTPUT); 

  // Assign initial state values
  digitalWrite(right_R1,1);
  digitalWrite(right_R2,0);
  digitalWrite(left_L1,0);
  digitalWrite(left_L2,1);
  digitalWrite(STBY, 1);    // Chip enable
  
  analogWrite(PWM_R,0);
  analogWrite(PWM_L,0);
  
  pinMode(PinA_left, INPUT);  // Left speed encoder input
  pinMode(PinA_right, INPUT); // Right speed encoder input
  Ultrasonicflag = 0;
  fultrasonic=0;
  CLB_u8trig = 0;
  // Join I2C bus
  Wire.begin();                                //  I2C bus start
  Serial.begin(9600);                         //  Open serial monitor, set baud rate to 9600
  delay(1500);
  mpu6050.initialize();                        // Initialize MPU6050
  delay(2);

  //5ms; Use timer2 to set timer interrupt
  MsTimer2::set(5, TimingInterrupt);    // 5ms; Execute TimingInterrupt function once
  MsTimer2::start();                // Start interrupt
}


void loop() 
{
  if(Serial.available())
  {
    val = Serial.read();          //  Assign value read from serial to val
    switch(val)                   //  switch statement
    {
      case 'A': carfront=-150; Ultrasonicflag = 1;break;       //If val = F, front=-250, car moves forward
      case 'B': carback=150;   Ultrasonicflag = 1; break;       //Backward
      case 'D': carleft=1;     Ultrasonicflag = 1;break;          //Turn right
      case 'C': carright=1;    Ultrasonicflag = 1;break;         //Turn left
      case 'F': carfront=0,carback=0,carleft=0,carright=0;   Ultrasonicflag = 0;break; //Stop
      case 'E': carfront=0,carback=0,carleft=0,carright=0;   Ultrasonicflag = 0;break; //Stop
      case 'H': carfront=0,carback=0,carleft=1,carright=1;   Ultrasonicflag = 1;break; //Special left turn
      case 'G': carfront=0,carback=0,carleft=-1,carright=-1; Ultrasonicflag = 1;break; //Special right turn
      case 'I': Serial.print(angle);break;
    }
  }
  // External interrupts; used to calculate wheel speed
  attachPinChangeInterrupt(PinA_left, Code_left, CHANGE);          // Trigger on change; execute Code_left
  attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);        // Trigger on change; execute Code_right
}

/////////////////////Hall count/////////////////////////
// Left speed encoder count
void Code_left() 
{
  count_left ++;
} 
// Right speed encoder count
void Code_right() 
{
  count_right ++;
} 
////////////////////Pulse count///////////////////////
void countpluse()
{
  lz = count_left;     // Assign encoder count value to lz
  rz = count_right;

  count_left = 0;     // Clear counter value
  count_right = 0;    // Clear counter value

  lpluse = lz;    
  rpluse = rz;

  if ((pwm1 < 0) && (pwm2 < 0))                        // Determine car direction; if backward (PWM negative), pulses are negative.
  {
    rpluse = -rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 > 0))                 // If forward (PWM positive), pulses are positive.
  {
    rpluse = rpluse;
    lpluse = lpluse;
  }
  else if ((pwm1 < 0) && (pwm2 > 0))                 // Determine car direction; if turning left, right pulse positive; left pulse negative.
  {
    rpluse = rpluse;
    lpluse = -lpluse;
  }
  else if ((pwm1 > 0) && (pwm2 < 0))               // Determine car direction; if turning right, right pulse negative; left pulse positive.
  {
    rpluse = -rpluse;
    lpluse = lpluse;
  }
  // Interrupt every 5ms, add pulse count
  pulseright += rpluse;
  pulseleft += lpluse;
}

/////////////////////////////////Timer interrupt ////////////////////////////
void TimingInterrupt()
{
  sei();                                                  // Enable global interrupts 
  countpluse();                                          // Pulse accumulation function
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);      //  IIC to get MPU6050 six-axis data  ax ay az gx gy gz
  angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      // Get angle and Kalman filter
  AnglePD();                                               //Angle loop PD control
  anglePWM();
  CLB_u8trig++;
  cc++;
  if(CLB_u8trig>=6)
  {
    UltraSonicProcessing();    // Ultrasonic processing function 
    CLB_u8trig=0;
  }
  if(cc>=8)                  // 5*8=40, speed PI algorithm every 40ms
  {
    carspeedpiout();   
    cc=0;  //Clear
  }
  turncc++;         
  if(turncc>4)       //20ms;     Steering PD algorithm 
  {
    carturnspin();
    turncc=0;      //Clear
  }
}
///////////////////////////////////////////////////////////

/////////////////////////////Tilt calculation///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
  float Angle = -atan2(ay , az) * (180/ PI);            //  Radial rotation angle formula; negative sign for direction
  Gyro_x = -gx / 131;                                   //  X-axis angular velocity from gyroscope; negative sign for direction
  Kalman_Filter(Angle, Gyro_x);                         //  Kalman filter
  // Rotation angle z-axis parameter
  Gyro_z = -gz / 131;                                  //  Z-axis angular velocity

  float angleAx = -atan2(ax, az) * (180 / PI); // Calculate tilt angle with x-axis
  Gyro_y = -gy / 131.00;                       // Y-axis angular velocity
  Yiorderfilter(angleAx, Gyro_y);              // First-order filter
}
//////////////////////////////////////////////////////////////////

///////////////////////////////Kalman filter///////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
  angle += (gyro_m - q_bias) * dt;          // Prior estimate
  angle_err = angle_m - angle;
  
  Pdot[0] = Q_angle - P[0][1] - P[1][0];    // Differential of prior estimation error covariance
  Pdot[1] = - P[1][1];
  Pdot[2] = - P[1][1];
  Pdot[3] = Q_gyro;
  
  P[0][0] += Pdot[0] * dt;                 // Integration of differential of prior estimation error covariance
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  
  // Intermediate variables in matrix multiplication
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  // denominator
  E = R_angle + C_0 * PCt_0;
  //gain value
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  
  t_0 = PCt_0;                // Intermediate variables in matrix multiplication
  t_1 = C_0 * P[0][1];
  
  P[0][0] -= K_0 * t_0;     // Posterior estimation error covariance
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  
  q_bias += K_1 * angle_err;        //  Posterior estimate
  angle_speed = gyro_m - q_bias;   //  Differential of output gives optimal angular speed
  angle += K_0 * angle_err;       //   Posterior estimation; obtain optimal angle
}

/////////////////////First-order filter/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
  angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}

//////////////////Angle PD/////////////////////
void AnglePD()
{
  PD_pwm = kp * (angle + angle0) + kd * angle_speed; // PD angle loop control
}

//////////////////Speed PI////////////////////
void carspeedpiout()
{
  float speeds = (pulseleft + pulseright) * 1.0;       // Speed pulse value
  pulseright = pulseleft = 0;                         // Clear 
  speeds_filterold *= 0.7;                            // First-order complementary filter
  speeds_filter = speeds_filterold + speeds * 0.3;
  speeds_filterold = speeds_filter;
  positions += speeds_filter;
  positions += carfront;                              // Forward control fusion
  positions += carback;                               // Backward control fusion
  positions += fultrasonic;                          // Ultrasonic fusion value
  positions = constrain(positions, -3550,3550);      // Anti-integral saturation
  PI_pwm = ki_speed * (setp0 - positions) + kp_speed * (setp0 - speeds_filter);      // Speed loop PI control
}

/////////////////////Turning///////////////////
void carturnspin()
{
  int flag = 0;      //
  float turnspeed = 0;
  float rotationratio = 0;
  
  if (carleft == 1 || carright == 1 || carleft == -1 || carright == -1)
  {
    if (flag == 0)                                                 // Check speed before turning, increase flexibility
    {
      turnspeed = ( pulseright + pulseleft);                      // Current speed; expressed in pulses
      flag=1;
    }
    if (turnspeed < 0)                                           // Absolute value of speed
    {
      turnspeed = -turnspeed;
    }
    if(carleft==1||carright==1)                                      // If left or right button pressed
    {
     turnmax=2;                                              //Maximum turn value
     turnmin=-2;                                            //Minimum turn value
    }
    rotationratio = 3 / turnspeed;                        //Speed set value
    if (rotationratio < 0.5)
    {
      rotationratio = 0.5;
    }
    if (rotationratio > 5)
    {
      rotationratio = 5;
    }
  }
  else
  {
    rotationratio = 0.5;
    flag = 0;
    turnspeed = 0;
  }
  if (carleft ==1)                     // Plus according to direction parameter
  {
    turnout += rotationratio;
  }
  else if (carright == 1 )            // Plus according to direction parameter
  {
    turnout -= rotationratio;
  }
  else if (carleft == -1)
  {
    turnout -= rotationratio;
  }
  else turnout = 0;
  if (turnout > turnmax)   turnout = turnmax;  //Maximum amplitude
  if (turnout < turnmin)   turnout = turnmin;  //Minimum amplitude 

  Turn_pwm = -turnout * kp_turn - Gyro_z * kd_turn;//Rotation PD algorithm control
}
///////////////////////////Turning/////////////////////////////////

////////////////////////////PWM final value/////////////////////////////
void anglePWM()
{
  pwm2=-PD_pwm - PI_pwm + Turn_pwm;           //Assign final PWM value to motors
  pwm1=-PD_pwm - PI_pwm - Turn_pwm;
  
  if(pwm1>255)             // Limit PWM value not exceeding 255
  {
    pwm1=255;
  }
  if(pwm1<-255) 
  {
    pwm1=-255;
  }
  if(pwm2>255)
  {
    pwm2=255;
  }
  if(pwm2<-255)
  {
    pwm2=-255;
  }
  if(angle>45 || angle<-45)     // If tilt angle > 45°, stop motors
  {
    pwm1=pwm2=0;
  }
   if(pwm2>=0)                 //  Determine motor direction and speed via PWM polarity
  {
    digitalWrite(left_L1,LOW);
    digitalWrite(left_L2,HIGH);
    analogWrite(PWM_L,pwm2);
  }
  else
  {
    digitalWrite(left_L1,HIGH);
    digitalWrite(left_L2,LOW);
    analogWrite(PWM_L,-pwm2);
  }

  if(pwm1>=0)
  {
    digitalWrite(right_R1,LOW);
    digitalWrite(right_R2,HIGH);
    analogWrite(PWM_R,pwm1);
  }
  else
  {
    digitalWrite(right_R1,HIGH);
    digitalWrite(right_R2,LOW);
    analogWrite(PWM_R,-pwm1);
  }
}
// Ultrasonic reads distance and adds value to speed PI calculation
void UltraSonicProcessing(void)
{  
  if(Ultrasonicflag==0)
  {
    Distance =  hc.dist();  // Get distance value
    if(Distance > 2.0 && Distance <  6.00)              
    {
      fultrasonic= (250);
     }
    else 
    {
      fultrasonic=0;  
     }
  }
 }