////////////////////////////////////////////////////////
//Define libraries
////////////////////////////////////////////////////////
//The libary section of the code does not need to be altered

// The Intel Curie libraries are required for reading sensor values and
// communicating with Bluetooth
#include <CurieBLE.h>
#include <CurieIMU.h>

// Servo library is required for sending  frequency PWM
// signals to the ESCs.
#include <Servo.h>
#include <math.h>
// BLE library is required to send data to the USB BLE dongle
#include <BLESerial.h>

////////////////////////////////////////////////////////
//Define variables
////////////////////////////////////////////////////////


//Variables for interrupt functions
long timerStartThrottle;
long timerStartPitch;
long timerStartRoll;
long timerStartYaw;
long timerStartMotorsOff;

volatile long timeThrottle;
volatile long timePitch;
volatile long timeRoll;
volatile long timeYaw;
volatile long timeMotorsOff;

//System variables
int serialPrintCounter = 0;
int bluetoothPrintCounter = 0;
long sampleTime;
long sampleTimeOld;
float sampleTimeMillis;

//IMU sensor variables
float sensorPitch;
float sensorRoll;
float sensorYaw;
float sensorPitchAngle;
float sensorRollAnge;
float sensorAZ;

float gx;
float gy;
float gz;
float ax;
float ay;
float az;

int aix, aiy, aiz;
int gix, giy, giz;

//Transmitter variables
int addressThrottle =  2;
long transmitterThrottleRaw = 0;
long transmitterThrottleMapped = 0;

int addressMotorsOff = 4;
long transmitterMotorsOffRaw = 0;
long transmitterMotorsOffMapped = 0;

int addressPitch = 10;
long transmitterPitchRaw = 0;
long transmitterPitchMapped = 0;

int addressRoll = 7;
long transmitterRollRaw = 0;
long transmitterRollMapped = 0;

int addressYaw = 8;
long transmitterYawRaw = 0;
long transmitterYawMapped = 0;

int trigPin = 11;
long UltraTrigger = 0;
int echoPin = 12;
long UltraEcho = 0;

//PID variables
float errorPitch;
float errorRoll;
float errorYaw;
float errorAPitch;
float errorARoll;
float errorAYaw;

float errorPitchOld = 0;
float errorRollOld = 0;
float errorYawOld = 0;
float errorAPitchOld = 0;
float errorARollOld = 0;
float errorAYawOld = 0;

float KpPitch = 0.6;
float KiPitch = 0.000000045;
float KdPitch = 15;

float KpRoll = 0.6;
float KiRoll = 0.000000045;
float KdRoll = 15;

float KpYaw = 0.3;
float KiYaw = 0;
float KdYaw = 15;

float KpPitchAngle = 0;
float KiPitchAngle = 0;
float KdPitchAngle = 0;

float KpRollAngle = 0;
float KiRollAngle = 0;
float KdRollAngle = 0;

float KpYawAngle = 0;
float KiYawAngle = 0;
float KdYawAngle = 0;

float integralPitch = 0;
float integralRoll = 0;
float integralYaw = 0;

float derivativePitch;
float derivativeRoll;
float derivativeYaw;

float derivativePitchMean = 0;
float derivativeRollMean = 0;
float derivativeYawMean = 0;

float filterFactorDPart = 50;

float PpartPitch;
float PpartRoll;
float PpartYaw;
float IpartPitch;
float IpartRoll;
float IpartYaw;
float DpartPitch;
float DpartRoll;
float DpartYaw;
float PpartPitchAngle;
float PpartRollAngle;
float PpartYawAngle;
float IpartPitchAngle;
float IpartRollAngle;
float IpartYawAngle;
float DpartPitchAngle;
float DpartRollAngle;
float DpartYawAngle;

float AccelerationF;
float AccelerationN;
float AccelerationS;

float RootofAS_AN;
float AS_AN;


float PIDPitch;
float PIDRoll;
float PIDYaw;
float PIDPitchAngle;
float PIDRollAngle;
float PIDYawAngle;

float duration;
float distance;

//Motor control variables
int controlSignalRightFront;
int controlSignalLeftFront;
int controlSignalRightBack;
int controlSignalLeftBack;

int motorsoff = 1000;

Servo ESCRightFront;
Servo ESCLeftFront;
Servo ESCRightBack;
Servo ESCLeftBack;

////////////////////////////////////////////////////////
//Functions for handling interrupts
////////////////////////////////////////////////////////

//Interrupts for the receiver channels are triggered by change of
//input value. On change these functions are executed:

void interruptThrottle() {
  //Scan input to start or stop timer
  if (digitalRead(addressThrottle) == HIGH) {
    //Start timer (save current time)
      timerStartThrottle = micros();
  }
  else {
    //Stop timer (compare current time to saved time)
      timeThrottle = micros() - timerStartThrottle;


  }
}

void interruptPitch() {
  if (digitalRead(addressPitch) == HIGH) {
      timerStartPitch = micros();
  }
  else {
     timePitch = micros() - timerStartPitch;
  }
}

void interruptRoll() {
  if (digitalRead(addressRoll) == HIGH) {
      timerStartRoll = micros();
  }
  else {
      timeRoll = micros() - timerStartRoll;
  }
}

void interruptYaw() {
  if (digitalRead(addressYaw) == HIGH) {
      timerStartYaw = micros();
  }
  else {
      timeYaw = micros() - timerStartYaw;
  }
}
void interruptMotorsOff() {
  if (digitalRead(addressMotorsOff) == HIGH) {
     timerStartMotorsOff = micros();
  }
  else {
      timeMotorsOff = micros() - timerStartMotorsOff;
  }
}

void setup()
{

  //Code in setup() function will only execute once, when power is supplied to
  //the CPU, when restarted or when new firmware is loaded

  ////////////////////////////////////////////////////////
  //System setup
  ////////////////////////////////////////////////////////

  Serial.begin(57600); // Start serial communication (with computer) at 57600 bps

  sampleTimeOld = micros();

/*  BLESerial.setName("Bluno101");
  BLESerial.begin();

  while(!BLESerial); */

  ////////////////////////////////////////////////////////
  //Initiate interrupts for receiver
  ////////////////////////////////////////////////////////
  attachInterrupt(addressThrottle, interruptThrottle, CHANGE);
  attachInterrupt(addressPitch, interruptPitch, CHANGE);
  attachInterrupt(addressRoll, interruptRoll, CHANGE);
  attachInterrupt(addressYaw, interruptYaw, CHANGE);
  attachInterrupt(addressMotorsOff, interruptMotorsOff, CHANGE);

  ////////////////////////////////////////////////////////
  //Initiate the IMU sensor
  ////////////////////////////////////////////////////////

  // start the IMU and filter
    CurieIMU.begin();
  // Set the gyro range in degrees/second
    CurieIMU.setGyroRange(250);
  // Set the accelerometer range in G
    CurieIMU.setAccelerometerRange(2);

  ////////////////////////////////////////////////////////
  //Initiate the ESCs
  ////////////////////////////////////////////////////////

  //Declare the address (pin) of the ESCs
    ESCRightFront.attach(3);
    ESCRightBack.attach(5);
    ESCLeftFront.attach(9);
    ESCLeftBack.attach(6);

  //Start with zero speed command to the ESCs

    ESCRightFront.writeMicroseconds(1000);
    ESCRightBack.writeMicroseconds(1000);
    ESCLeftFront.writeMicroseconds(1000);
    ESCLeftBack.writeMicroseconds(1000);

  //setup pins for ultrasonic sensor
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);


}

void loop()
{
  //Code in loop() function will execute repeatedly for as long as the CPU has power

  ////////////////////////////////////////////////////////
  //System update
  ////////////////////////////////////////////////////////

  serialPrintCounter++;
  //bluetoothPrintCounter++;

  sampleTime = micros() - sampleTimeOld;
  sampleTimeMillis = (float) sampleTime / 1000;
  sampleTimeOld = micros();

/*  if (BLESerial.operator bool() == true) {
    digitalWrite(13, HIGH);
  } */

  ////////////////////////////////////////////////////////
  //Read the sensor values
  ////////////////////////////////////////////////////////
    CurieIMU.readGyroScaled(gx, gy, gz);
    CurieIMU.readAccelerometerScaled(ax, ay, az);
  ////////////////////////////////////////////////////////
  //Interpret and calibrate sensor values
  ////////////////////////////////////////////////////////

    AccelerationF = pow(ax, 2);
    AccelerationS = pow(ay, 2);
    AccelerationN = pow(az, 2);
    AS_AN = (AccelerationS - AccelerationN);


    RootofAS_AN = sqrt(AS_AN);

    sensorPitch = 0.24 + gy;
    sensorRoll = 0.52 + gx;
    sensorYaw = 0.36 - gz;
    sensorPitchAngle = atan2 (AccelerationF, RootofAS_AN);
//    sensorRollAngle = ;
    sensorAZ = az;

  ////////////////////////////////////////////////////////
  //Read transmitter values
  ////////////////////////////////////////////////////////

  noInterrupts();
    transmitterThrottleRaw = timeThrottle;
    transmitterPitchRaw = timePitch;
    transmitterRollRaw = timeRoll;
    transmitterYawRaw = timeYaw;
    transmitterMotorsOffRaw = timeMotorsOff;
  interrupts();


 //Ultrasonic sensor range
 /*
  digitalWrite(trigPin, LOW);

  digitalWrite(trigPin, HIGH);

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) * 0.0344 - 1;
*/
  ////////////////////////////////////////////////////////
  //Transmitter value mapping
  ////////////////////////////////////////////////////////

  transmitterThrottleMapped = map(transmitterThrottleRaw, 980, 1640, 1000, 1600);
  transmitterPitchMapped = map(transmitterPitchRaw, 1230, 1670, -30 , 30);
  transmitterRollMapped = map(transmitterRollRaw, 1310, 1733 ,-30 , 30);
  transmitterYawMapped = map(transmitterYawRaw, 1330, 1736, -90 , 90);
  transmitterMotorsOffMapped = map(transmitterMotorsOffRaw, 981, 1960, 0 , 1);
  ////////////////////////////////////////////////////////
  //PID regulators
  ////////////////////////////////////////////////////////

  //Regulator errors
  errorPitch = transmitterPitchMapped - sensorPitch;  // r(Referenssignal - y(Mätsignal)= e(Reglerfel)
  errorYaw = transmitterYawMapped - sensorYaw;        // r(Referenssignal - y(Mätsignal)= e(Reglerfel)
  errorRoll = transmitterRollMapped - sensorRoll;     // r(Referenssignal - y(Mätsignal)= e(Reglerfel)
/*
 * errorPitchAngle = transmitterPitchMapped - sensorPitchAngle;
 * errorRollAngle = transmitterRollMapped - sensorRollAngle;
  errorPitch = sensorPitch - transmitterPitchMapped;  // r(Referenssignal - y(Mätsignal)= e(Reglerfel)
  errorYaw = sensorYaw - transmitterYawMapped;        // r(Referenssignal - y(Mätsignal)= e(Reglerfel)
  errorRoll = sensorRoll - transmitterRollMapped;     // r(Referenssignal - y(Mätsignal)= e(Reglerfel)
 */
  /////////////////////////////
  //P part
  /////////////////////////////

  PpartPitch = KpPitch * errorPitch;
  PpartYaw = KpYaw * errorYaw;
  PpartRoll = KpRoll * errorRoll;
  /////////////////////////////
  //I part
  /////////////////////////////

  //Update integrals
  integralPitch = integralPitch + (errorPitch * sampleTimeMillis);
  integralRoll = integralRoll + (errorRoll * sampleTimeMillis);
  integralYaw = integralYaw + (errorYaw * sampleTimeMillis);

  IpartPitch = KiPitch * integralPitch;
  IpartRoll = KiRoll * integralRoll;
  IpartYaw = KiYaw * integralYaw;
  //Reset integrals

  if (transmitterThrottleMapped < 1100) {
    integralPitch = 0;
    integralRoll = 0;
    integralYaw = 0;
  }

  /////////////////////////////
  //D part
  /////////////////////////////

      derivativePitch = (errorPitch - errorPitchOld) / sampleTimeMillis;
      derivativeRoll =  (errorRoll - errorRollOld) / sampleTimeMillis;
      derivativeYaw = (errorYaw - errorYawOld) / sampleTimeMillis;

  //Filter D-part

      derivativePitchMean = ((derivativePitchMean * (filterFactorDPart - 1) + derivativePitch) / filterFactorDPart);
      derivativeRollMean = ((derivativeRollMean * (filterFactorDPart - 1) + derivativeRoll) / filterFactorDPart);
      derivativeYawMean = ((derivativeYawMean * (filterFactorDPart - 1) + derivativeYaw) / filterFactorDPart);

      DpartPitch = KdPitch * derivativePitchMean;
      DpartRoll = KdRoll * derivativeRollMean;
      DpartYaw = KdPitch * derivativeYawMean;


  //Save error for next iteration

    errorPitchOld = errorPitch;
    errorRollOld = errorRoll;
    errorYawOld = errorYaw;
  /////////////////////////////
  //Update regulators
  /////////////////////////////

    PIDPitch = PpartPitch + IpartPitch + DpartPitch;
    PIDRoll = PpartRoll + IpartRoll + DpartRoll;
    PIDYaw = PpartYaw + IpartYaw + DpartYaw;

  ////////////////////////////////////////////////////////
  //Control signals
  ////////////////////////////////////////////////////////
 /*
  controlSignalRightFront = transmitterThrottleMapped + PIDPitch + PIDRoll - PIDYaw; // front right
  controlSignalLeftFront = transmitterThrottleMapped + PIDPitch - PIDRoll + PIDYaw; // front left
  controlSignalLeftBack = transmitterThrottleMapped - PIDPitch - PIDRoll - PIDYaw; // back left
  controlSignalRightBack = transmitterThrottleMapped - PIDPitch + PIDRoll + PIDYaw; // back right
*/

  controlSignalLeftBack = transmitterThrottleMapped + PIDPitch + PIDRoll + PIDYaw; // Left back
  controlSignalRightBack = transmitterThrottleMapped + PIDPitch - PIDRoll - PIDYaw; // Right back
  controlSignalRightFront = transmitterThrottleMapped - PIDPitch - PIDRoll + PIDYaw; // Right front
  controlSignalLeftFront = transmitterThrottleMapped - PIDPitch + PIDRoll - PIDYaw; // Left front

/*
  controlSignalLeftFront = transmitterThrottleMapped + PIDPitch + PIDRoll - PIDYaw; // Left back
  controlSignalRightFront = transmitterThrottleMapped + PIDPitch - PIDRoll + PIDYaw; // Right back
  controlSignalRightBack = transmitterThrottleMapped - PIDPitch - PIDRoll - PIDYaw; // Right front
  controlSignalLeftBack = transmitterThrottleMapped - PIDPitch + PIDRoll + PIDYaw; // Left front
*/

  ////////////////////////////////////////////////////////
  //Control signal saturation check
  ////////////////////////////////////////////////////////

 // if (controlSignalRightFront > 1000) {

  //  }
  //else if

  ////////////////////////////////////////////////////////
  //Throttle safety function
  ////////////////////////////////////////////////////////

  //Specify a safety limit where all motors are stopped
  if (transmitterMotorsOffMapped == 1) {

  ////////////////////////////////////////////////////////
  //ESC control signal write
  ////////////////////////////////////////////////////////

    ESCRightFront.writeMicroseconds(controlSignalRightFront);
    ESCLeftFront.writeMicroseconds(controlSignalLeftFront);
    ESCLeftBack.writeMicroseconds(controlSignalLeftBack);
    ESCRightBack.writeMicroseconds(controlSignalRightBack);
  }
  else {
    ESCRightFront.writeMicroseconds(1000);
    ESCLeftFront.writeMicroseconds(1000);
    ESCLeftBack.writeMicroseconds(1000);
    ESCRightBack.writeMicroseconds(1000);

  }
  ////////////////////////////////////////////////////////
  //Print signal values
  ////////////////////////////////////////////////////////
/*  if (distance >= 400 || distance <= 0){
    Serial.print("Distance = ");
    Serial.println("Out of range");
  }
  else {
    Serial.print("Distance = ");
    Serial.print(distance);
    Serial.println(" cm");
   }

*/

  if (serialPrintCounter == 999) {
    /*
    Serial.print("G: ");
    Serial.print(gx);
    Serial.print(", ");
    Serial.print(gy);
    Serial.print(", ");
    Serial.println(gz);
    Serial.print("A: ");
    Serial.print(ax);
    Serial.print(", ");
    Serial.print(ay);
    Serial.print(", ");
    Serial.println(az);
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);
    Serial.print("IMU Pitch: ");
    Serial.println(sensorPitch);
    Serial.print("IMU Roll: ");
    Serial.println(sensorRoll);
    Serial.print("IMU Yaw: ");
    Serial.println(sensorYaw);
    Serial.print("Transmitter Throttle: ");
    Serial.print(transmitterThrottleRaw);
    Serial.println(", ");
    Serial.println(transmitterThrottleMapped);
    Serial.print("Transmitter Pitch: ");
    Serial.print(transmitterPitchRaw);
    Serial.print(", ");
    Serial.println(transmitterPitchMapped);
    Serial.print("Transmitter Roll: ");
    Serial.print(transmitterRollRaw);
    Serial.print(", ");
    Serial.println(transmitterRollMapped);
    Serial.print("Transmitter Yaw: ");
    Serial.print(transmitterYawRaw);
    Serial.print(", ");
    Serial.println(transmitterYawMapped);
    Serial.print("PID Pitch: ");
    Serial.print(errorPitch);
    Serial.print(", ");
    Serial.print(PpartPitch);
    Serial.print(", ");
    Serial.print(IpartPitch);
    Serial.print(", ");
    Serial.print(DpartPitch);
    Serial.print(", ");
    Serial.println(PIDPitch);
    Serial.print("PID Roll: ");
    Serial.print(errorRoll);
    Serial.print(", ");
    Serial.print(PpartRoll);
    Serial.print(", ");
    Serial.print(IpartRoll);
    Serial.print(", ");
    Serial.print(DpartRoll);
    Serial.print(", ");
    Serial.println(PIDRoll);
    Serial.print("PID Yaw: ");
    Serial.print(errorYaw);
    Serial.print(", ");
    Serial.print(PpartYaw);
    Serial.print(", ");
    Serial.print(IpartYaw);
    Serial.print(", ");
    Serial.print(DpartYaw);
    Serial.print(", ");
    Serial.println(PIDYaw);
    Serial.print("Control Signals: ");
    Serial.print(controlSignalRightFront);
    Serial.print(", ");
    Serial.print(controlSignalLeftFront);
    Serial.print(", ");
    Serial.print(controlSignalRightBack);
    Serial.print(", ");
    Serial.println(controlSignalLeftBack);
    Serial.print("Sample Time: ");
    Serial.println(sampleTimeMillis);
    Serial.println(transmitterMotorsOffMapped);
    Serial.print("");
    Serial.print("a:\t");
    Serial.print(ax);
    Serial.print("\t");
    Serial.print(ay);
    Serial.print("\t");
    Serial.print(az);
    Serial.println();
  */  Serial.print(sensorPitchAngle);
    Serial.println(""); /*
    */
    serialPrintCounter = 0;
  }

  ////////////////////////////////////////////////////////
  //Collect bluetooth data for PID parameter analyzing
  ////////////////////////////////////////////////////////

/*  if (bluetoothPrintCounter == 200 && BLESerial.operator bool() == true) {

  BLESerial.print(transmitterPitchMapped);
  BLESerial.print(" ");
  BLESerial.print(sensorPitch);
  BLESerial.print(" ");
  BLESerial.print(errorPitch);
  BLESerial.print(" ");
  BLESerial.print(PpartPitch);
  BLESerial.print(" ");
  BLESerial.print(IpartPitch);
  BLESerial.print(" ");
  BLESerial.print(DpartPitch);
  BLESerial.print(" ");
  BLESerial.print(PIDPitch);
  BLESerial.print(" ");
  BLESerial.print(transmitterRollMapped);
  BLESerial.print(" ");
  BLESerial.print(sensorRoll);
  BLESerial.print(" ");
  BLESerial.print(errorRoll);
  BLESerial.print(" ");
  BLESerial.print(PpartRoll);
  BLESerial.print(" ");
  BLESerial.print(IpartRoll);
  BLESerial.print(" ");
  BLESerial.print(DpartRoll);
  BLESerial.print(" ");
  BLESerial.print(PIDRoll);
  BLESerial.print(" ");
  BLESerial.print(transmitterYawMapped);
  BLESerial.print(" ");
  BLESerial.print(sensorYaw);
  BLESerial.print(" ");
  BLESerial.print(errorYaw);
  BLESerial.print(" ");
  BLESerial.print(PpartYaw);
  BLESerial.print(" ");
  BLESerial.print(IpartYaw);
  BLESerial.print(" ");
  BLESerial.print(DpartYaw);
  BLESerial.print(" ");
  BLESerial.print(PIDYaw);

  BLESerial.println();

  bluetoothPrintCounter = 0;

  }
  */

}