#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"
#include "src/drivers/M25P16.h"
// Control
#include "src/control/lpf.h"
#include "src/control/PID.h"
#include "src/control/ImuFusion.h"
// Flash Memory - TODO: This component is not correctly implemented yet
#include "src/drivers/SerialFlash/MbedSPICommand.h"
#include "src/drivers/SerialFlash/SerialFlashDeviceCreator.h"
#include "src/drivers/SpiFlash25.h"

#define WHEEL_SIZE 0.09f

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

EventQueue queue;

// Serial port (Servo Outputs)
Serial serialMotorOutputs(PA_2, PA_3, 115200);

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

// Flash setup
//SPI spiFlash(PC_12, PC_11, PC_10, PA_15);
//SPI spiFlash(PC_12, PC_11, PC_10, PB_3);
//M25P16 memory(spiFlash, PB_3);
//DigitalOut radioCS(PA_15);

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;

// Throttle PI Controller with some acceptable 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 readSerialContext()
{
    while (true)
    {
        while (serialMotorOutputs.readable()) 
        {
            serialMotorOutputs.putc(serialMotorOutputs.getc());
        }
    }
    Thread::wait(1000);
}

// 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);
        }
    }
}

void testFlash()
{
    ISPICommand* pSPICommand = new MbedSPICommand(PC_12, PC_11, PC_10, PB_3);   // MOSI, MISO, SCK, CS
    ISerialFlashDevice* pDevice = SerialFlashDeviceCreator::Create(pSPICommand);
    
    if (pDevice == NULL)
    {
        printf("Unsupported device");
        return;
    }

    // std::string deviceName = pDevice->GetDeviceName();
    // printf("%s\n", deviceName);
    int capacity = pDevice->GetCapacity() / 1024;
    printf("%dK x 8\n", capacity);

    // int *a[3];
    // *a[0] = 1;
    // *a[1] = 2;
    // *a[2] = 3;

    // int *b[3];

    int result;

    // result = pDevice->Write(100, a, sizeof(a));

    // result = pDevice->Read(100, b, sizeof(b));

    uint8_t c(-1), d(0);

    result = pDevice->Read(200, &c, sizeof(c));

    wait(1);

    c += 4;

    result = pDevice->Write(200, &c, sizeof(c));

    wait(1);

    result = pDevice->Read(200, &d, sizeof(d));

}

void testFlash2()
{
    SpiFlash25 flash(PC_12, PC_11, PC_10, PB_3);

    char* k = flash.read_id();

    char f = flash.read_status();

    char a[] = "Hej och ha";

    bool result;

    result = flash.write(0, strlen(a), a);

    char b[20];

    result = flash.read(0, strlen(a), b);

    printf("result");


}

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

    serialMotorOutputs.baud(115200);
    // Test thread for serial
    Thread serialThread;
    serialThread.start(callback(&readSerialContext));

    // Init memory
    DigitalOut radioCS(PA_15);
    radioCS = 1; // Disable radio chip. May not be necessary
    //SPI spiFlash(PC_12, PC_11, PC_10);
    
    testFlash2();

    // M25P16 memory(spiFlash, PB_3);
    // bool memoryUp = memory.init();

    //printf("Initialization of onboard memory " + memoryUp ? "succeded" : "failed");

    //memory.chipErase();
    int start = 0;

    int values[3];
    //memory.read(start, &values, sizeof(values));
    

    values[0]++;
    //values[1]++;
    values[2]++;

    //memory.write(start, values, sizeof(values));
    

    // int values2[3];
    // memory.read(start, values2, sizeof(values2));

    // memory.read(0, values2, sizeof(values2));

    // values[0]++;
    // values[1]++;
    // values[2]++;

    // start += sizeof(values);
    // memory.write(0, values, sizeof(values));

    // memory.read(0, values2, sizeof(values2));




    // 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);
}