#include <SoftwareSerial.h>
#include <PID_v1.h>
#include "DFRobot_BNO055.h"
#include "Wire.h"

// HC-05 Communication
SoftwareSerial mySerial(8, 7);  // RX, TX
String bt;

// BNO05 Gyro Sensor
typedef DFRobot_BNO055_IIC BNO;
BNO bno(&Wire, 0x28);

// PID variables
double desieredangle = 0;
double error, output;
double Kp = 0, Ki = 0, Kd = 0;

double deadzone = 5;  // Activate motors when error > ±5°

// Pin definitions [Switch Pins if Needed]
const int motorPinForward = 6;
const int motorPinBackward = 5;

// Angle Variables
int anglepercentage = 1;
int prepitch, deltapitch;
double pitch, newpitch, roll;

// PID instance
PID myPID(&pitch, &output, &desieredangle, Kp, Ki, Kd, DIRECT);

void setup() {

  // Begin Communication
  Serial.begin(115200);
  mySerial.begin(38400);

  bno.reset();
  while (bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin failed");
    delay(2000);
  }
  // Initialize LED
  pinMode(LED_BUILTIN_RX, OUTPUT);
  pinMode(LED_BUILTIN_TX, OUTPUT);
  // Initialize motors
  pinMode(6, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(4, OUTPUT);

  myPID.SetMode(AUTOMATIC);  // Start in automatic mode
  // myPID.SetOutputLimits(-70, 70);  // Limit PID output to motor range
}

void loop() {

  // Read tilt angle from gyro sensor
  BNO::sEulAnalog_t gyro;
  gyro = bno.getEul();
  pitch = gyro.roll;

  // Converting wrapped data (always stay between 0 and 360) to continuous data(any number like 400,234,197 etc)
  deltapitch = pitch - prepitch;  // Find the change in angle

  if (deltapitch != 0 && abs(deltapitch) < 100) {  // if there is change in pitch and the value is not greater than 100(to rule out the chage form 0 -> 360)
    newpitch += deltapitch;
  }
  prepitch = pitch;

  // Calculating error to display info
  error = int(desieredangle - pitch);


  // Compute PID output
  myPID.Compute();


  // End Data for Visual Representsation
  // Print is Serial Monitor
  Serial.print("new pitch:");
  Serial.print(pitch);
  Serial.print("  desieredangle:");
  Serial.print(desieredangle);
  Serial.print(" error:");
  Serial.print(error);
  Serial.print("  output:");
  Serial.print(output);

  // Print on Testing App
  mySerial.print("error:");
  mySerial.print(error);
  mySerial.print("output:");
  mySerial.print(output);
  mySerial.print("pitch:");
  mySerial.print(pitch);


  // Motor control logic based on tilt and PID output
  if (abs(error) > deadzone) {
    if (output > 0) {

      analogWrite(motorPinForward, output);
      analogWrite(motorPinBackward, 0);
    } else if (output < 0) {

      analogWrite(motorPinBackward, abs(output));
      analogWrite(motorPinForward, 0);
    }
  } else {
    // Small tilt, stop motors
    analogWrite(motorPinForward, 0);
    analogWrite(motorPinBackward, 0);
  }


  // PID Changing by app

  bt = mySerial.readString();
  bt.trim();
  Serial.print("  kp:");
  Serial.print(Kp, 3);
  Serial.print("ki:");
  Serial.print(Ki, 3);
  Serial.print("kd:");
  Serial.println(Kd, 3);


  if (bt.substring(0, 4) == "turn") {
    Serial.println("turning robot 90 degrees");
    desieredangle += 90;
    if (desieredangle > 359) {
      desieredangle = 0;
    }


  } else if (bt.substring(0, 2) == "P:") {
    bt.remove(0, 2);
    Kp = bt.toDouble();
  } else if (bt.substring(0, 2) == "I:") {
    bt.remove(0, 2);
    Ki = bt.toDouble();
  } else if (bt.substring(0, 2) == "D:") {
    bt.remove(0, 2);
    Kd = bt.toDouble();
  }

  myPID.SetTunings(Kp, Ki, Kd);
}