#include "mbed.h"
#include "motordriver.h"
#include "LSM9DS0.h"

/*/////// PID control gains
 / These values need to be adjusted in accordance with the individual
 / actuators (motors) either by trial and error or computation. The information
 / here should help: http://developer.mbed.org/cookbook/PID
*////////
#define RATE  0.1
#define Kp    0.0
#define Ki    0.0
#define Kd    0.0

// SDO_XM and SDO_G are pulled up, so our addresses are:
#define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW

// peripheral objects
DigitalOut f(LED1); // forward
DigitalOut b(LED2); // backward
DigitalOut l(LED3); // left
DigitalOut r(LED4); // right
//See http://mbed.org/cookbook/Motor
//Connections to dual H-brdige driver for the two drive motors
Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor right(p26, p25, p24, 1);
AnalogIn ir_c(p20); // center IR sensor
AnalogIn ir_l(p19); // left IR sensor
AnalogIn ir_r(p18); // right IR sensor
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // imu sensor
Serial pc(USBTX, USBRX); // usb serial for debugging

// function prototypes
void setup();
void initIMU();
float getHeading();
float updatePID();

// global variables
int count; // counter for IR
float initHeading; // initial heading
float calcHeading; // calculated heading
float heading[1]; // index 0 = goal, index 1 = current
float headingAvg[100];
float sum;
float previousError; // previous error
float pidError; // error
float steadyError; // steady-state error
float P; // proportional error
float I; // integral error
float D; // derivative error
float dt; // update frequency
float kp; // proportional constant
float ki; // integral constant
float kd; // derivative constant
float output; // PID controller output
float motorSpeed1; // motor speed for left motor
float motorSpeed2; // motor speed for right motor

// function implementations
void setup()
{
    // Use the begin() function to initialize the LSM9DS0 library.
    // You can either call it with no parameters (the easy way):
    uint16_t status = imu.begin();

    //Make sure communication is working
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
    pc.printf("Should be 0x49D4\n\n");
}

// obtains the initial heading upon bootup
void initIMU()
{
    imu.readMag();
    initHeading = imu.calcHeading();
    heading[0] = initHeading;
    heading[1] = initHeading;
}

// obtains an updated heading
float getHeading()
{
    imu.readMag(); // reads current value
    calcHeading = imu.calcHeading(); // sets current value

    //pc.printf("Compass Heading (in degrees): ");
    //pc.printf("%2f\r\n",calcHeading);
    //pc.printf("%2f\r\n",heading[0]);

    return calcHeading;
}

// pid controller
float updatePID(float current, float target, float dt)
{
    // calculate difference between actual and goal values
    pidError = target - current;
    if (pidError < -270) pidError += 360;
    if (pidError >  270) pidError -= 360;

    // accumulates error over time
    steadyError += pidError*dt;

    // integrator windup compensation (saturates to actuator's output)
    if (steadyError < -10.0) steadyError = -10.0;
    if (steadyError > 10.0) steadyError = 10.0;

    P = kp*pidError; // proportional error
    I = ki*steadyError; // integral error
    D = kd*(pidError - previousError)/dt;  // derivative error

    // save current error
    previousError = pidError;

    float pidOutput = P + I + D;  // calculate the PID output
    pidOutput /= 100.0;  // scale it down to get it within the range of the actuator - probably needs tuning
    if(pidOutput < -0.1) pidOutput = -0.1;
    if(pidOutput > 0.1) pidOutput = 0.1;

    wait(dt);

    //pc.printf("P = %f | I = %f | D = %f | Output = %f\n",P,I,D,pidOutput);
    return pidOutput;
}

int main()
{
    // initialization functions
    setup();
    initIMU();

    // variables
    dt = RATE;
    kp = Kp;
    ki = Ki;
    kd = Kd;
    motorSpeed1 = 0.4;
    motorSpeed2 = 0.4;

    while(1) {
        if(ir_c > 0.25f) { // Turn around if about to hit an obstacle
            f = 0;
            b = 1;
            l = 1;
            r = 0;
            while(count < 50) {
                left.speed(0.5);
                right.speed(-0.5);
                wait(0.02);
                count++;
            }
            count = 0;
            heading[0] = getHeading(); // updates new goal heading
        } else if(ir_l > 0.25f) { // Turn right if about to hit an obstacle
            f = 0;
            b = 0;
            l = 1;
            r = 0;
            while(count < 20) {
                left.speed(0.5);
                right.speed(-0.5);
                wait(0.02);
                count++;
            }
            count = 0;
            heading[0] = getHeading(); // updates new goal heading
        } else if(ir_r > 0.25f) { // Turn left if about to hit an obstacle
            f = 0;
            b = 0;
            l = 0;
            r = 1;
            while(count < 20) {
                left.speed(-0.5);
                right.speed(0.5);
                wait(0.02);
                count++;
            }
            count = 0;
            heading[0] = getHeading(); // updates new goal heading
        } else { // Moves forward indefinitely
            f = 1;
            b = 0;
            l = 0;
            r = 0;
            
            // average the headings to elliminate noise (LPF)
            // possibly needed to help smooth out sensor noise
            /*
            for(int x = 0; x < 100; x++) {
                headingAvg[x] = getHeading();
                sum += headingAvg[x];
                }
            heading[1] = sum/100;
            */
            
            // updates new pid values
            heading[1] = getHeading();
            output = updatePID(heading[1],heading[0],dt);
            
            // updates new motor speeds
            motorSpeed1 += output;
            motorSpeed2 -= output;

            // set motor speeds
            left.speed(motorSpeed1);
            right.speed(motorSpeed2);
        }
    }
}