#include <Arduino_FreeRTOS.h>
#include <math.h>
#include <Wire.h>

#include "SparkFun_BNO080_Arduino_Library.h"


//----------------------------------------------------- IMU -----------------------------------------------------
BNO080 myIMU;

// Get the euler angles
struct Euler {
  float yaw;
  float pitch;
  float roll;
};
struct Quat {
  float i;
  float j;
  float k;
  float real;
};


Euler getAngles(Quat q, bool degrees);

Quat myQuat;
Euler eul;

bool gotOffset = false;

float headingOffset = 0;
float angle = 0;
float robotHeading = 0;
float softwareHeading = 90;
//----------------------------------------------------------------------------------------------------------


// ----------------------------------------------------- MOTORS -----------------------------------------------------
#define DISTANCEPERTICK (diameterOfWheel * PI) / numberOfEncoders
#define ANGLEPERTICK (DISTANCEPERTICK * 360)/ ( distanceBetweenWheel * PI)
const int diameterOfWheel = 65;
const int numberOfEncoders = 375;
const int distanceBetweenWheel = 206;
int speedOfMotors = 60;

const int IN1 = 13;
const int IN2 = 12;
const int IN3 = 11;
const int IN4 = 10;

const int M2PWM = 5;

//m1 left
//m2 right

const int m1enc1 = 2;
//const int m1enc2 = 14; // NOT USED
const int m2enc1 = 3;
//const int m2enc2 = 4; // NOT USED

volatile long leftEnc = 0, rightEnc = 0;

int turnSpeed = 120;// ----------------------------------------------------------------------------------------------------------


// define two tasks for Blink & AnalogRead
void TaskReadIMU( void *pvParameters );
void TaskMoveRobot( void *pvParameters );

long counter;

// the setup function runs once when you press reset or power the board
void setup() {

  // initialize serial communication at 9600 bits per second:
  Serial.begin(115200);

  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB, on LEONARDO, MICRO, YUN, and other 32u4 based boards.
  }
  //motor setup
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(m1enc1), updateMotorA , RISING);
  attachInterrupt(digitalPinToInterrupt(m2enc1), updateMotorB , RISING);

  // imu setup
  Wire.begin();
  Wire.setClock(400000); //Increase I2C data rate to 400kHz
  myIMU.begin();
  myIMU.enableGameRotationVector(1); //Send data update every 50ms

  // Now set up two tasks to run independently.
  xTaskCreate(
    TaskReadIMU
    ,  (const portCHAR *)"Blink"   // A name just for humans
    ,  2000  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  1  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL );

  xTaskCreate(
    TaskMoveRobot
    ,  (const portCHAR *) "AnalogRead"
    ,  2000  // Stack size
    ,  NULL
    ,  2  // Priority
    ,  NULL );

  // Now the task scheduler, which takes over control of scheduling individual tasks, is automatically started.

}

void loop()
{
  // Empty. Things are done in Tasks.
}

/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/

void TaskReadIMU(void *pvParameters)  // This is a task.
{
  (void) pvParameters;


  // initialize digital LED_BUILTIN on pin 13 as an output.

  for (;;) // A Task shall never return or exit.
  {
    if (myIMU.dataAvailable() == true)
    {
      float quatI = myIMU.getQuatI();
      float quatJ = myIMU.getQuatJ();
      float quatK = myIMU.getQuatK();
      float quatReal = myIMU.getQuatReal();
      float quatRadianAccuracy = myIMU.getQuatRadianAccuracy();
      myQuat.i = quatI;
      myQuat.j = quatJ;
      myQuat.k = quatK;
      myQuat.real = quatReal;
      eul = getAngles(myQuat);
      robotHeading = getHeading();
    }
  }
}

void TaskMoveRobot(void *pvParameters)  // This is a task.
{
  (void) pvParameters;
  pinMode(LED_BUILTIN, OUTPUT);
  bool ranOnce = false;
  for (;;)
  {
    if (ranOnce == false) {
      vTaskDelay( 1000 / portTICK_PERIOD_MS );
    }
    ranOnce = true;
    turnLeftXDegrees(90);
    //Serial.println(robotHeading);
    vTaskDelay( 2000 / portTICK_PERIOD_MS );
    //vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
  }
}


// ----------------------------------------------------- IMU -----------------------------------------------------

float getHeading() {
  angle = eul.yaw;


  if (angle >= 0 && angle < headingOffset) {

    return (360 - headingOffset + eul.yaw);

  } else if (angle >= 0) {


    return (angle - headingOffset);
  } else {
    angle = 180 + (180 - abs((eul.yaw)));


    return (angle - headingOffset);
  }


}

void getHeadingOffset() {
  if (gotOffset == false) {

    angle = eul.yaw;
    if (angle != 0) {
      angle = eul.yaw;
      if (angle >= 0) {
        headingOffset = angle;
      } else {
        headingOffset = 180 + (180 - abs((eul.yaw)));
      }
      gotOffset = true;
      delay(1000);
    }
  }
}

// Return the Euler angle structure from a Quaternion structure
Euler getAngles(Quat q) {

  Euler ret_val;
  float x; float y;

  /* YAW */
  x = 2 * ((q.i * q.j) + (q.real * q.k));
  y = square(q.real) - square(q.k) - square(q.j) + square(q.i);
  ret_val.yaw = degrees(atan2(y, x));

  /* PITCH */
  ret_val.pitch = degrees(asin(-2 * (q.i * q.k - q.j * q.real)));

  /* ROLL */
  x = 2 * ((q.j * q.k) + (q.i * q.real));
  y = square(q.real) + square(q.k) - square(q.j) - square(q.i);
  ret_val.roll = degrees(atan2(y , x));

  return ret_val;

}
//----------------------------------------------------------------------------------------------------------

// ----------------------------------------------------- MOTORS -----------------------------------------------------
void setStatesAndSpeed(int in1Speed, int in2Speed, int in3Speed, int in4Speed) {
  analogWrite(IN1, in1Speed);
  analogWrite(IN2, in2Speed);
  analogWrite(IN3, in3Speed);
  analogWrite(IN4, in4Speed);
}

void moveForward(int leftSpeed, int rightSpeed) {

  setStatesAndSpeed(0, leftSpeed, 0, rightSpeed);
}

void moveBackwards(int leftSpeed, int rightSpeed) {

  setStatesAndSpeed(leftSpeed, 0, rightSpeed, 0);
}

void turnLeftXDegrees(int angleToTurn) {
  float targetAngle = 0;
  float initialAngle = eul.yaw;
  //Serial.println(robotHeading);
  if (robotHeading - angleToTurn < 0) {

    targetAngle = (360 - angleToTurn) + robotHeading;
     Serial.print(eul.yaw);
      Serial.print("\t");
      Serial.print(robotHeading);
      Serial.print("\t");
      Serial.print(angleToTurn);
      Serial.print("\t");
      Serial.println(targetAngle);
    while ( robotHeading < targetAngle) {
      turnRight(turnSpeed, turnSpeed);
      vTaskDelay(1);
      Serial.print(eul.yaw);
      Serial.print("\t");
      Serial.print(robotHeading);
      Serial.print("\t");
      Serial.print(angleToTurn);
      Serial.print("\t");
      Serial.println(targetAngle);
    }
    while (robotHeading > targetAngle) {
      turnRight(turnSpeed, turnSpeed);
      vTaskDelay(1);
      Serial.print(eul.yaw);
      Serial.print("\t");
      Serial.print(robotHeading);
      Serial.print("\t");
      Serial.print(angleToTurn);
      Serial.print("\t");
      Serial.println(targetAngle);
    }
    Serial.println("success");
    return;

    Serial.println("3");
  } else {
    targetAngle = robotHeading - 90;

  }
}

void turnRightXDegrees(int angleToTurn) {
  float targetAngle = 0;
  Serial.println("hello");
  Serial.print(eul.yaw);
  Serial.print("\t");
  if (eul.yaw > 0) {
    if (eul.yaw + angleToTurn > 180) {

      Serial.println("1");
      targetAngle =  (-180 + ((eul.yaw + angleToTurn) - 180)) - 0.01;
      Serial.println(targetAngle);
      while (eul.yaw > targetAngle) {
        Serial.print("moving right+ \t");
        Serial.print(targetAngle);
        Serial.print("\t");
        Serial.println(eul.yaw);
        turnRight(turnSpeed, turnSpeed);
        vTaskDelay(1);
      }

      stopMoving();
      return;
    } else {
      targetAngle = eul.yaw + angleToTurn;
      Serial.println("2");
      while (eul.yaw < (targetAngle)) {
        Serial.print("moving right- \t");
        Serial.print(targetAngle);
        Serial.print("\t");
        Serial.println(eul.yaw);
        turnRight(turnSpeed, turnSpeed);
        vTaskDelay(1);
      }
      stopMoving();
      return;
    }
  } else { // eul.yaw negative
    targetAngle = eul.yaw + angleToTurn;
    Serial.println("3");
    while (eul.yaw < targetAngle) {
      Serial.print("moving right-2 \t");
      Serial.print(targetAngle);
      Serial.print("\t");
      Serial.println(eul.yaw);
      turnRight(turnSpeed, turnSpeed);
      vTaskDelay(1);
    }
    stopMoving();
    return;
  }
}

void turnLeft(int leftSpeed, int rightSpeed) {
  setStatesAndSpeed(0, leftSpeed, rightSpeed, 0);
}

void turnRight(int leftSpeed, int rightSpeed) {
  setStatesAndSpeed(leftSpeed, 0, 0, rightSpeed);
}
void stopMoving() {
  setStatesAndSpeed(255, 255, 255, 255);
}
void printEncoders() {
  Serial.print(leftEnc);
  Serial.print("\t");
  Serial.println(rightEnc);
}

void updateMotorA() {
  leftEnc++;
}

void updateMotorB() {
  rightEnc++;
}

//----------------------------------------------------------------------------------------------------------