#include "mbed.h"

// STD
#include <cmath>

// Mbed
#include "mbed_events.h"

// target definitions
#include "src/targets/revo_f4/pins.h"
// Mmath
#include "src/math/Utilities.h"
// Drivers
#include "src/drivers/MPU6000.h"
#include "src/drivers/stepper.h"
#include "src/drivers/servo.h"
// Control
#include "src/control/lpf.h"
#include "src/control/PID.h"
#include "src/control/ImuFusion.h"
// Serialization
#include "src/serialization/RCProtocol.h"

#define WHEEL_SIZE 0.09f

using namespace targets::revo_f4;
using namespace drivers;
using namespace control;
using namespace math;
using namespace serialization;

EventQueue queue;

// Serial port (Servo Outputs)
Serial serial(PA_2, PA_3, 57600);

// MPU setup
SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
mpu6000_spi imu(spi,PA_4);   //define the mpu6000 object

PwmOut ledBlue(D4);
DigitalOut ledOrg(D5);

Stepper motorL(PC_9, PC_7, PC_8);
Stepper motorR(PB_15, PB_14, PC_6);

Servo servo(PA_0);

// Interrupt pin from Gyro to MCU
InterruptIn gyroINT(PC_4);

Timer timer;

// TODO: Figure out some good values
controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40

// TODO: Figure out some good values
controllerPD angleControl(10.0, 146.0, 400);
// Alternatively try controllerPD2 which has another Dterm implementation

// Draft of some control function
// this runs in the context of eventThread and is triggered by the gyro ISR pin
void controlFunc()
{
    static ImuFusion imuFusion(&imu);
    static float controlOutput(0.0f);

    // Calculate dT in sec
    float dT = timer.read_us()/(float)1000000.0;
    timer.reset();

    // Retrieve IMU angle (Only x-axis implemented for now)
    float angleX = imuFusion.getAngle(dT);
    float forceX = sin(angleX/180*PI);
    // Reset anything left in the IMU FIFO queue
    imu.fifo_reset();

    // If the robot is above this angle we turn off motors
    static bool disabled = false;
    if (abs(angleX) > (float)50.0f && !disabled)
    {
        controlOutput = 0.0f; // rinse integral
        disabled = true;
        motorL.disable();
        motorR.disable();
    }
    else if (abs(angleX) < (float)50.0f && disabled)
    {
        disabled = false;
        motorL.enable();
        motorR.enable();
    }

    /* --------------------------
    Calculate estimated groundspeed 
    of the robot. 
    TODO: Inject correct input (compensate for robot rotation)
    -------------------------- */
    // Calculate filtered groundspeed of the robot (From deg/s to m/s)
    float estimatedSpeed((motorL.getSpeed() + motorR.getSpeed())/2);
    static incrementalLPF speedFilter;
    float filteredEstSpeed(speedFilter.filter(estimatedSpeed));
    float speedScale((1.0f/360.0f)*WHEEL_SIZE*PI);
    float groundSpeed(filteredEstSpeed*speedScale);

    /* --------------------------
    Calculate the setpoint for 
    the main control (control the angle)
    through this throttle control loop (PI)
    TODO: TEMP turned off the throttle 
    control to tune the PD angle control
    -------------------------- */
    float throttle(0.0f); // TODO: This will be the input from remote
    float angleSP = 0.0f; // Temporarily emulate an output
    //float angleSP = throttleControl.run(dT, groundSpeed, throttle);

    
    /* --------------------------
    The last control loop. Angle (PD)
    TODO:
    -------------------------- */
    // Integrating the output to obtain acceleration
    if (!disabled)
    {
        controlOutput += angleControl.run(dT, forceX, angleSP);

        controlOutput = constrain(controlOutput, 3000.0f);
    }
    
    /* --------------------------
     Lastly the steering is added
     straight on the output
     TODO: Activate when implemented
    -------------------------- */
    float steering(0.0f); // This will come from remote
    motorL.setSpeed(controlOutput - steering);
    motorR.setSpeed(controlOutput + steering);

    // Blink LED at 1hz
    static int i = 0;
    i++;
    if (i > 100)
    {
        i = 0;
        ledOrg = !ledOrg;
    }
}

void serialContext()
{
    static RCProtocol RC;

    while (true)
    {
        // Receive
        while (serial.readable())
        {
            bool newPackage = RC.appendByte(serial.getc());

            if (newPackage)
            {
                RCProtocol::Packet packet = RC.read();
            }
        }
        // Sleep some?

        // Transmit 
        // serial.putc()...
    }
}


// This context just pulses the blue LED
void pulseLedContext()
{
    while (true)
    {
        float in, out;
  
        for (in = 0; in < 2*PI; in = in + PI/50)
        {
            out = sin(in) + 0.5;
            // We cannot set pulsewidth or period due to
            // sharing timer with motor outputs. "write"
            // instead sets the dutycycle in range 0-1
            ledBlue.write(out);
            Thread::wait(10);
        }
    }
}

// main() runs in its own thread
int main() {

    // Serial in / out
    Thread readUartThread;
    readUartThread.start(callback(&serialContext));

    // MPU startup at 100hz
    if(imu.init(10,BITS_DLPF_CFG_188HZ)){
        printf("\nCouldn't initialize MPU6000 via SPI!");
    }  

    wait(0.1);
    // Set IMU scale
    int accScale = imu.set_acc_scale(BITS_FS_16G);
    wait(0.1);
    int gyroScale = imu.set_gyro_scale(BITS_FS_2000DPS);
    wait(0.1);

    // Calibrate and Trim acc & gyro
    bool calibrationResult = imu.resetOffset_gyro();
    imu.calib_acc(0);
    calibrationResult = imu.resetOffset_acc();

    // Enable/Activate the Gyro interrupt
    imu.enableInterrupt();

    // Start the timer used by the control loop
    timer.start(); // Used to calc dT

    /*-------------- Visible start sequence ------------*/
    // Start sweeping the arm
    servo.sweep(0.0, 1, 2);

    // Start the pulsing blue led
    Thread ledPulseThread;
    ledPulseThread.start(callback(&pulseLedContext));

    // Create realtime eventhandler for control loop
    Thread eventThread(osPriorityRealtime);
    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));

    // Attach gyro interrupt to add a control event
    gyroINT.rise(queue.event(&controlFunc));

    // Enable motor controllers (Will power the motors with no movement)
    motorL.enable();
    motorR.enable();
    motorL.setDirection(1);
    motorR.setDirection(-1);

    Thread::wait(1000);

    // Servo Nod to tell us that we are done
    servo.nod();

    servo.setPosition(-0.2);

    wait(osWaitForever);
}