//-----------------------------------------------------------------------//
// Freaky Witty
//
// Serial Commands:
//
// a) Change Parameter: 
//
// kp
// ki
// kd
//
// molb Motor Offset Left Backwards
// molf Motor Offset Left Forward
// morb Motor Offset Right Forward
// morf Motor Offset Right Backwards
//
// Example: kp=4.280&ki=0&kd=0.00&molb=58&morb=58
//
// b) Print Parameters:
// 
// Example: param
// 
//
// Tuning:
// 1. Set Ki and Kd to zero and gradually increase Kp so that the robot starts to oscillate about the zero position.
// 
// 2. Increase Ki so that the response of the robot is faster when it is out of balance. Ki should be large enough so that the angle of inclination does not increase. The robot should come back to zero position if it is inclined.
//
// 3. Increase Kd so as to reduce the oscillations. The overshoots should also be reduced by now.
//
//-----------------------------------------------------------------------//

//#define DEBUG 1

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
#define mRd 7
#define mRa 6
#define mLd 4
#define mLa 5
#define Gry_offset 0  //The offset of the gyro
#define Gyr_Gain 8.000 // 16.348

#define Angle_offset 0  // The offset of the accelerator
#define Angle_to_balance 80 // The offset to the balance angle
float Angle_Delta, Angle_Recursive, Angle_Confidence;

int RMotor_offset_fwd = 0;  // The offset of the Motor
int LMotor_offset_fwd = 0;  // The offset of the Motor
int RMotor_offset_bck = 40; //56; // The offset of the Motor
int LMotor_offset_bck = 40; //56; // The offset of the Motor

#define Mramp 3
#define pi 3.14159

float kp, ki, kd;
float Angle_Raw, Angle_Filtered, omega;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0;
float LOutput, ROutput;
float LlastA, RlastA;
int LlastD, RlastD;
unsigned long preTime = 0;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;

// 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 initialize;

int16_t ax, ay, az;
int16_t gx, gy, gz;

// Serial Receive Parameters
const byte numChars = 32;
char receivedChars[numChars]; // an array to store the received data
boolean newData = false;

void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.begin(115200);
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);;
 
// Set Default Balancing Parameters
  kp = 8.380; // 22.000
  ki = 0.5;
  kd = 0.00; // 1.60
  
  for (int i = 0; i < 100; i++) // Looping 100 times to get the real gesture when starting
  {
    Filter();
  }

  pinMode(mRd, OUTPUT);
  pinMode(mRa, OUTPUT);
  pinMode(mLd, OUTPUT);
  pinMode(mLa, OUTPUT);
  LlastA = 0.0;
  RlastA = 0.0;
  LlastD = LOW;
  RlastD = LOW;
}

void loop() {
  
// check for serial value changes
recvWithEndMarker();
if (newData == true) {
processNewParameters();
}
 
  // put your main code here, to run repeatedly:
  Filter();
#ifdef DEBUG
//  Serial.print(" Raw =\t");
//  Serial.print(Angle_Raw);
  Serial.print(" Angle =\t");
  Serial.print(Angle_Filtered);
#endif
  // If angle > 60 or < -60 then stop the robot
  if (abs(Angle_Filtered - Angle_to_balance ) < 60)
  {
    // optional position to check external input
    myPID();
    PWMControl();
  }
  else
  {
    // stop wheels
    digitalWrite(mRd, LOW);
    analogWrite(mRa, 0);
    digitalWrite(mLd, LOW);
    analogWrite(mLa, 0);
    for (int i = 0; i < 100; i++) // Keep reading the gesture after falling down
    {
      Filter();
    }
    if (abs(Angle_Filtered) < 60) // Empty datas and restart the robot automaticly
    {
      for (int i = 0; i <= 100; i++) // Reset the robot and delay 2 seconds
      {
        omega = Angle_Raw = Angle_Filtered = 0;
        Filter();
        Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
        myPID();
      }
    }
  }
#ifdef DEBUG
  Serial.println();
#endif
  delay(2);
}

void Filter()
{
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset);
  omega =  -gx / Gyr_Gain + Gry_offset;
  // Filters datas to get the real gesture
  unsigned long now = micros();
  float dt = (now - preTime) * 0.000001;
  preTime = now;
  float K = 0.8;
  float A = K / (K + dt);
  Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64;
  Angle_Recursive = Angle_Delta * dt + Angle_Recursive;
  Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega;
  Angle_Filtered = Angle_Confidence * dt + Angle_Filtered;
}

void myPID()
{
  // Calculating the output values using the gesture values and the PID values.
  error = Angle_Filtered - Angle_to_balance; // move 25°
  errSum += error;
  dErr = error - lastErr;
  Output = kp * error + ki * errSum + kd * omega;
  lastErr = error;
#ifdef DEBUG
//  Serial.print("\t error:\t");
 // Serial.print(error);
//  Serial.print("\t errSum:\t");
//  Serial.print(errSum);
//  Serial.print("\t dErr:\t");
//  Serial.print(dErr);
//  Serial.print("\t omega:\t");
//  Serial.print(omega);
  Serial.print("\t output:\t");
  Serial.print(Output);
//  Output = 0;
#endif
  LOutput = Output - Turn_Speed + Run_Speed;
  ROutput = Output + Turn_Speed + Run_Speed;
}

void PWMControl()
{
  // forward min 15 both
  // reverse min 35 both
  int sL, sR;

  if (LOutput >= 0.0) {
    // move forward
    sL = min(255, LMotor_offset_fwd + LOutput);
    if (LlastD == HIGH){
      fade( mLa, LlastD, LOW, LlastA, 127);
      digitalWrite(mLd, LOW);
    }
    fade( mLa, LlastD, LOW, LlastA, sL);
    LlastD = LOW;
    LlastA = sL;
  }
  if (LOutput < 0.0) {
    sL = max(0, 255 - LMotor_offset_bck + LOutput);
    if (LlastD == LOW){
      fade( mLa, LlastD, HIGH, LlastA, 127);
      digitalWrite(mLd, HIGH);
    }
    fade( mLa, LlastD, HIGH, LlastA, sL);
    LlastD = HIGH;
    LlastA = sL;
  }
  if (ROutput >= 0.0) {
    sR = min(255, RMotor_offset_fwd + ROutput);
    if (RlastD == HIGH){
      fade( mRa, RlastD, LOW, RlastA, 127);
      digitalWrite(mRd, LOW);
    }
    fade( mRa, RlastD, LOW, RlastA, sR);
    RlastD = LOW;
    RlastA = sR;
  }
  if (ROutput < 0.0) {
    sR = max(0, 255 - RMotor_offset_bck + ROutput);
    if (RlastD == LOW){
      fade( mRa, RlastD, HIGH, RlastA, 127);
      digitalWrite(mRd, HIGH);
    }
    fade( mRa, RlastD, HIGH, RlastA, sR);
    RlastD = HIGH;
    RlastA = sR;
  }
}

void fade(int motor, int oD, int nD, int oA, int nA) {
  analogWrite(motor, nA);
}

void fadeX(int motor, int oD, int nD, int oA, int nA) {
  int wait = 30;
  if (nA == 0) {
    // stop motor
    if (oD == HIGH) {
      // fade tp A==D==High==255
      for (int i = oA; i <= 255; i++) {
        analogWrite(motor, i);
        delay(wait);
      }
    } else {
      // fade to A==D==Low==0
      for (int i = oA; i >= 0; i--) {
        analogWrite(motor, i);
        delay(wait);
      }
    }
  }
  if (nA > 0) {
    // move forward
    if (oD == HIGH) {
      // fade to no motion as last driection was reverse
      for (int i = oA; i <= 255; i++) {
        analogWrite(motor, i);
        delay(wait);
      }
      digitalWrite(motor, LOW);
      for (int i = 0; i <= nA; i++) {
        analogWrite(motor, i);
        delay(wait);
      }
    } else {
      if (oA > nA) {
        for (int i = oA; i >= nA; i--) {
          analogWrite(motor, i);
          delay(wait);
        }
      } else {
        for (int i = oA; i <= nA; i++) {
          analogWrite(motor, i);
          delay(wait);
        }
      }
    }
  }
  if (nA < 0) {
    // move reverse
    if (oD == LOW) {
      // fade to no motion as last driection was forward
      for (int i = oA; i >= 0; i--) {
        analogWrite(motor, i);
        delay(wait);
      }
      digitalWrite(motor, HIGH);
      for (int i = 255; i >= nA; i--) {
        analogWrite(motor, i);
        delay(wait);
      }
    } else {
      if (oA > nA) {
        for (int i = oA; i >= nA; i--) {
          analogWrite(motor, i);
          delay(wait);
        }
      } else {
        for (int i = oA; i <= nA; i++) {
          analogWrite(motor, i);
          delay(wait);
        }
      }
    }
  }
}

void processNewParameters() {

 Serial.print("New Received Parameters: ");
 Serial.println(receivedChars);
 Serial.println("Process Parameters: ");
 newData = false;

char* command = strtok(receivedChars, "&");
while (command != 0)
{
    // Split the parameters in two key & values
    char* separator = strchr(command, '=');
    if (separator != 0)
    {
        // Actually split the string in 2: replace '=' with 0
        *separator = 0;
        String parameter = command;
        ++separator;
        String  separator_s =  separator;
        float par_value = separator_s.toFloat();
        Serial.print(" key: ");
        Serial.print(parameter);
        Serial.print(" Val: ");
        Serial.println(par_value);

if ( parameter == "kp" ) kp = par_value;
if ( parameter == "ki" ) ki = par_value;
if ( parameter == "kd" ) kd = par_value;    
if ( parameter == "molb" ) LMotor_offset_bck = par_value;    
if ( parameter == "molf" ) LMotor_offset_fwd = par_value;    
if ( parameter == "morb" ) RMotor_offset_bck = par_value;    
if ( parameter == "morf" ) RMotor_offset_fwd = par_value;    


    }
    // Find the next command in input string
    command = strtok(0, "&");
}

// Print all actual Parameters

Serial.println("Actual Parameters: ");
Serial.print("kp: ");
Serial.print(kp);
Serial.print(" ki: ");
Serial.print(ki);
Serial.print(" kd: ");
Serial.print(kd);

Serial.print(" molb: ");
Serial.print(LMotor_offset_bck);
Serial.print(" molf: ");
Serial.print(LMotor_offset_fwd);
Serial.print(" morb: ");
Serial.print(RMotor_offset_bck);
Serial.print(" morf: ");
Serial.print(RMotor_offset_fwd);

Serial.print(" ");

}

void recvWithEndMarker() {
 static byte ndx = 0;
 char endMarker = '\n';
 char rc;
 
 // if (Serial.available() > 0) {
           while (Serial.available() > 0 && newData == false) {
 rc = Serial.read();

 if (rc != endMarker) {
 receivedChars[ndx] = rc;
 ndx++;
 if (ndx >= numChars) {
 ndx = numChars - 1;
 }
 }
 else {
 receivedChars[ndx] = '\0'; // terminate the string
 ndx = 0;
 newData = true;
 }
 }
}