#include "mbed.h"

#include <cmath>

#include "mbed_events.h"

#include "src/targets/revo_f4/pins.h"
#include "src/drivers/MPU6000.h"
#include "src/drivers/stepper.h"
#include "src/drivers/servo.h"
#include "src/control/lpf.h"

#ifndef PI
    #define PI 3.14159265358979f
#endif

using namespace drivers;
using namespace control;

EventQueue queue;


// 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 led1(D4); //PB_5
DigitalOut ledOrg(D5);

Stepper motor1(PC_9, PC_7, PC_8);
Stepper motor2(PB_15, PB_14, PC_6);

Servo servo(PA_0);

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

// Draft of some control function
// this runs in the context of eventThread and is triggered by the gyro ISR pin
void controlFunc()
{
    static int i = 0;
    i++;
    if (i > 100)
    {
        i = 0;
        ledOrg = !ledOrg;
    }

    static incrementalLPF filter;
    double accY = 1;
    accY = imu.read_acc(1);
    double filtered = filter.filter(accY);
    motor1.setSpeed(360.0*2*filtered);   
}

// 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
            led1.write(out);
            Thread::wait(10);
        }
    }
}

int i = 0;

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

    // Start sweeping the arm
    servo.sweep(0.0, 1, 2);

    Thread ledPulseThread;
    ledPulseThread.start(callback(&pulseLedContext));

    // 
    Thread eventThread;
    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));

    // Function to attach to gyro interrupt
    gyroINT.rise(queue.event(&controlFunc));


    // Enable motor controllers (Will power the motors with no movement)
    motor1.enable();
    motor2.enable();

    // Set motor 2 to a fixed speed 
    // (To test that timers don't interfeer with each other)
    motor2.setSpeed(90.0);


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

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

    // Read some test values
    int wmi = imu.whoami();
    int scale = imu.set_acc_scale(BITS_FS_16G);

    Thread::wait(1000);

    servo.nod();

    Thread::wait(1000);

    servo.sweep(1.0, 1.5, 3);

    servo.setPosition(-0.2);

    wait(osWaitForever);
}