#include "MPU6050.h"

//Setting PID parameters

double kp_balance = 55, kd_balance = 0.75;
double kp_speed = 10, ki_speed = 0.26;
double kp_turn = 2.5, kd_turn = 0.5;

//Setting MPU6050 calibration parameters
double angle_zero = 0;            //x axle angle calibration
double angular_velocity_zero = 0; //x axle angular velocity calibration

volatile unsigned long encoder_count_right_a = 0;
volatile unsigned long encoder_count_left_a = 0;
float dt = 0.005, Q_angle = 0.001, Q_gyro = 0.005, R_angle = 0.5, C_0 = 1, K1 = 0.05;

int encoder_left_pulse_num_speed = 0;
int encoder_right_pulse_num_speed = 0;
double speed_control_output = 0;
double rotation_control_output = 0;
double speed_filter = 0;
int speed_control_period_count = 0;
double car_speed_integeral = 0;
double speed_filter_old = 0;
int setting_car_speed = 0;
int setting_turn_speed = 0;
double pwm_left = 0;
double pwm_right = 0;
//float kalmanfilter_angle;
double outputControlOutput = 0;
char balance_angle_min = -22;
char balance_angle_max = 22;



void carStop()
{
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(STBY_PIN, HIGH);
  analogWrite(PWMA_LEFT, 0);
  analogWrite(PWMB_RIGHT, 0);
}

void balanceCar()
{
  sei(); // Use this to allow interrupts
  mpuData(); // Calling the data recieved from the MPU6050
  cli(); // Also needed for the interrupt
  
  double accelYAngle = atan2(AccY, AccZ);
  double degAccelAngle = accelYAngle*57.29578;

  double scaledZGyro = ((gx*0.00000075)*57.29578);
  double scaledYGyro = ((gy*0.00000075)*57.29578);

  static double angle = degAccelAngle; 
  angle = (0.9934 * (angle + scaledZGyro)) + (0.0066 * degAccelAngle); // complimentary filter

 // This if statement gives a maximum or minimum angle for the car so it will stop so it doesn't go crazy
  if (angle < balance_angle_min or angle > balance_angle_max and angle != 0){
    carStop();
    return;
  }

  double pTerm, iTerm, dTerm, output, angleError;
  float targetAngle = .5; 
  /*
   This is the term you need to adjust inorder to get the robot to stay in a single place
   If it is always moving forward make it a smaller positive or negative and vise versa
   */
  float Kp = 30, kpOutput = .07;
  float Ki = .0, kiOutput = .14;
  float Kd = 4;
  angleError = (angle - targetAngle); // subtract angle by offset    
  // calculate the proportional component
  pTerm = angleError*Kp;
  // calculate the integral component (summation of past errors * i scalar)
  static double integral = 0;
  integral += angleError;
  integral =  constrain(integral, -100,100);
  iTerm = Ki * integral;
  // calculate the derivative component
  static double lastAngle = 0;
  dTerm = Kd * (angle-lastAngle);
  lastAngle = angle;
  output = (pTerm + iTerm - dTerm);
  static int count =0;
  count ++;
  if(count >= 2){
    count = 0;
    // angle adjusting
    static double outputFilter = output;
    static double outputFilterOld, outputIntegral = 0;
    outputFilter = outputFilterOld * 0.7 + output * 0.3;
    outputFilterOld = outputFilter;
    outputIntegral += outputFilter;
    outputIntegral += -setting_car_speed;
    outputIntegral = constrain(outputIntegral, -3000, 3000);
    outputControlOutput = -kpOutput * outputFilter - kiOutput * outputIntegral;
    rotation_control_output = setting_turn_speed + kd_turn * scaledYGyro; 
  }
  pwm_left  = output - outputControlOutput - rotation_control_output;
  pwm_right = output - outputControlOutput + rotation_control_output;

  static int count2 =0;
  count2 ++;
  if(count2 % 10 == 0){
    static double oldtime = 0;
    double deltatime = millis() - oldtime;
    oldtime =  millis();
  }
  
  pwm_left = constrain(pwm_left, -255, 255);
  pwm_right = constrain(pwm_right, -255, 255);
    if (pwm_left < 0)
    {
      digitalWrite(AIN1, 1);
      analogWrite(PWMA_LEFT, -pwm_left);
    }
    else
    {
      digitalWrite(AIN1, 0);
      analogWrite(PWMA_LEFT, pwm_left);
    }
    if (pwm_right < 0)
    {
      digitalWrite(BIN1, 1);
      analogWrite(PWMB_RIGHT, -pwm_right);
    }
    else
    {
      digitalWrite(BIN1, 0);
      analogWrite(PWMB_RIGHT, pwm_right);
    }
}

void carInitialize()
{
  pinMode(AIN1, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(PWMA_LEFT, OUTPUT);
  pinMode(PWMB_RIGHT, OUTPUT);
  pinMode(STBY_PIN, OUTPUT);
  carStop();
  Wire.begin();
  mpuSetUp(); // Calling the car setup will also setup the mpu6050
}