/* This code is going to be used to control the line following and autobalancing portion
of our robot.*/

//needed for I2C protocol
#include "Wire.h"

//These next three libraries are all open source and specific to our IMU, motor shield, and IR
#include "I2Cdev.h"
#include "MPU6050.h"
#include "DualMC33926MotorShield.h"
#include <QTRSensors.h>

//Basic math library
#include <math.h>

/*Define all of the variables for the program*/
//Create variables of type MPU6050 (IMU) and Dual...(motor shield)
MPU6050 accelgyro;
DualMC33926MotorShield md;

//define the ultrasonic pins and variables
#define echoPin 11 // Echo Pin
#define trigPin 6// Trigger Pin
double duration;
double USerror;
double USintegralError = 0;
double USsetpoint;
double USduration;
double UScorrection;
float Kp1;
float USkp;
float USki;

//timing for the program
float dt = 0.0001;
float time = dt/1000;

//establish the variables that the IMU will be assigning
int16_t ax, ay, az;
int16_t gx, gy, gz;

//Accelerometer variables
float AccY_Offset = -270;
double AccY = 0;//Acceleration after offset is taken into account
double gravity;//Acceleration due to gravity
double tiltAcc;//Tilt angle found by accelerometer

//Gyroscope Variables
float GyroX_Offset = -140;
double tiltRate;//Gyro reading after taking into account the offset
double previoustiltRate = 0.0;//Used for the numeric integration to find the angle

//filter
float alpha = 0.99;//Constant used for the high and low(1-alpha) pass filters
double angle = 0;//Need an initial angle to start the program

//The gain values for the program
float kp = 0;
float ki = 0;
float kd = 0;
float mid;//Desired setpoint
float setpoint;//Setpoint that can change dynamically throughout the program

//range of acceptable degrees that robot can span without touching the ground
int range = 80;

//define all of the error variables
double error;
double previousError = 0;
double pastError = 0.0;
double derivError = 0;

//line following sensor declaration
#define NUM_SENSORS   3  // number of sensors used
#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2

// sensors 0 through 2 are connected to digital pins 2,3 and 5
QTRSensorsRC qtrrc((unsigned char[]) {2,3,5},
  NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
float midIR;

//define motor variables
int correction;
int power = 400;

//define on the fly PID tuning variables
int a;
int array[] = {0,0};
int counter;


//StopIfFault() halts the motors if an error occurs, such as too much or too little current draw.
void stopIfFault()
{
  //md.getFault() is a built in function of the motor shield
  if (md.getFault())
  {
    Serial.println("fault");//display the fault message to the Serial monitor
    md.setM2Speed(0);//set the speed of motor 2 to zero
    md.setM1Speed(0);//set the speed of motor 1 to zero
    while(1);//stop the program indefinitely to prevent any damage to electronics
  }
}

/*This getAngle() function takes in the IMU data and performs the calculations necessary to obtain
reliable, accurate tilt angle measurements. The angle is calculated using both the accelerometer and
the gyroscope separately and then a high pass filter is applied to the gyro, a low pass to the
accelerometer, and the results are added. This is a simple complimentary filter that was inspired
by a presentation by MIT entitled "The Balance Filter"*/
double getAngle(){
    // read raw accel/gyro measurements from device and assign to respective variables
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // calculate tilt angle from accelerometer
    AccY = (ay - AccY_Offset);//Take into account the accelerometer's offset
    gravity = sqrt((square(az) + square(AccY)));//
    tiltAcc = asin(AccY/gravity)*180/PI;//use the acceleration components to find tilt and convert to degrees

    // calculate tilt angle from gyroscope
    tiltRate = (gx - GyroX_Offset)*90/(131*250);//account for offset and span
    //previoustiltRate = tiltRate;
    angle = alpha*(angle + dt*(previoustiltRate + tiltRate)) + (1-alpha)*(tiltAcc);//complimentary filter 0.005 instead of dt
    previoustiltRate = tiltRate;

    return angle;
}

/*This function reads in the IMU data and finds the motor values needed to balance*/
int balance() {
    //Balance PID values used for tuning
    kp = 550;
    ki = 350;
    kd = 100;
    mid = -0.85;//Desired setpoint

  //Retrieve the angle and find the errors for the PID algorithm
  angle = getAngle();//call the function used to find the filtered IMU tilt angle
  //Serial.println(angle);//print the angle to the Serial monitor
  error = angle - setpoint;//calculate the error
  error = (error/range);//normalize the error with respect to the range
  pastError = 0.9*(pastError + error);//Integral error with a decay factor to expel accumulated errors
  derivError = (error - previousError);//Derivative error proportional to the change in error
  previousError = error;//reassign the current error as the previous error for the next loop
  correction = (power/100)*(kp*error + ki*pastError + kd*derivError);//The sum of the gain values and errors


  /*This portion of the code will adjust the setpoint depending on how far the robot is leaning. If the robot is
  leaning far forward, the setpoint is moved further back to increase the observed error and therefore the response.*/
  if (angle > mid + 0.750)//If the angle is much greater than the setpoint, move the setpoint back
  {
    setpoint = mid - 0.25;
  }
  else if (correction < mid - 0.750)//If the angle is much smaller than the setpoint, move the setpoint forward
  {
    setpoint = mid + 0.25;
  }
  else//If the angle is within an acceptable range, maintain the same setpoint
  {
    setpoint = mid;
  }

  return correction; //use the correction value in the line following portion
}

void lineFollow(double correction){
    //These should be replaced with the readings from the outer sensors and 2500 for the tape
    //midIR = (qtrrc.calibratedMinimumOn[2]+qtrrc.calibratedMaximumOn[0])/2 ;//Desired setpoint
    int alpha = 100;
    //read in the raw IR data
    qtrrc.read(sensorValues);
        correction = -correction;
    //ZigZag
    if ((sensorValues[0] <= 1250) && (sensorValues[2] <= 1250))
    {
         md.setM2Speed(correction);
         md.setM1Speed(correction);
         stopIfFault();
                 //Serial.println("forward");
    }
    else if ((sensorValues[0] > 1250)&&(sensorValues[2] < 1250))
    {
         md.setM2Speed(correction+alpha);
         md.setM1Speed(correction-alpha);
         stopIfFault();
                 //Serial.println("Right");
    }
    else if ((sensorValues[2] > 1250)&&(sensorValues[0] < 1250))
    {
         md.setM2Speed(correction-alpha);
         md.setM1Speed(correction+alpha);
         stopIfFault();
                 // Serial.println("Left");
    }
        else
        {
          md.setM2Speed(correction);
          md.setM1Speed(correction);
          stopIfFault();
         // Serial.println("Shit");
        }
}

long ultrasonicDuration(){
   //pulse the ultrasonic to detect the time it takes to reflect. Use this time as the error for balancing
  duration = 0;
  for(int i = 0; i <10; i++)
  {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);

    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);

    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH)+duration;
  }
  //if ((duration/10) > 750)
  //{
  //  return 165;//return the setpoint so that the motors don't go crazy
 // }
 // else
 // {
    return duration/10;
//  }
}

void ultrasonicBalanceZigZag(){
  USduration = ultrasonicDuration();
  USsetpoint = 150;
  USerror = USduration - USsetpoint;
  Kp1 = 230;
  if (USduration == -1)//bad reading
  {
      md.setM1Speed(0);
      md.setM2Speed(0);

  }
  else if(USduration > USsetpoint+20)
  {
    md.setM1Speed(-Kp1);
    md.setM2Speed(-Kp1);
  }
  else if(USduration < USsetpoint+20)
  {
    md.setM1Speed(Kp1);
    md.setM2Speed(Kp1);
  }

  Serial.println(USduration);

}
double ultrasonicBalancePID(){
  USduration = ultrasonicDuration();
  USsetpoint = 185;
  USerror = USduration - USsetpoint;
  USkp = 2.8;
  USki = 0.015;
  USintegralError = (USerror + USintegralError)*0.9;
  correction = USkp*USerror + USki*USintegralError;
  md.setM1Speed(-correction);
  md.setM2Speed(-correction);
  //Serial.println(USduration);
  Serial.println(USduration);
  return correction;
}

void setup() {
    // join I2C bus
    Wire.begin();

    // initialize serial communication
    Serial.begin(115200);

    // initialize device
    accelgyro.initialize();

    //initialize motor shield
    md.init();

    //initialize ultrasonic
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

}

void loop(){
    /*the logic here is to find the motor power needed to stay balanced (say 100 to each motor).
    then send this value to the lineFollow() program which will determine where on the line the robot
    is. Then the motor commands will be shifted (120 to left and 80 to right for example) so that
    the robot will zig zag on the line and have the same net balancing amount. When I run this code however,
    the robot goes crazy after balancing for a small moment.*/
    //correction = balance();
    //lineFollow(correction);
        //correction = ultrasonicBalancePID();
        //lineFollow(correction);
        ultrasonicBalanceZigZag();

}