#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"
#include "src/control/MadgwickAHRS.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;

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

// MemoryPool<RCProtocol::Packet, 16> mpool;
// Queue<RCProtocol::Packet, 16> RCQueue;

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

Servo servo(PA_0);

DigitalOut motorL(PC_8);
DigitalOut motorR(PC_6);

//Timer timer;
// 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 serialContext(void)
{
    //ledOrg = 0;
    static int i = 0;
    static RCProtocol::Packet pkt;
    // while (true)
    // {
        // Receive
        while (serial.readable())
        {
            bool newPackage = RC.appendByte(serial.getc());

            if (newPackage)
            {
                //RCProtocol::Packet* pPacket = mpool.alloc();
                pkt = RC.read();
                //RCQueue.put(pPacket);
                servo.setPosition((float)pkt.Throttle/1000.0f);
                //ledOrg.write((int)pkt.Enabled);
                
                i++;
                i = i % 100;
                if (i == 0)
                {
                    ledOrg = !ledOrg;
                }
            }
        // }
        // Sleep some?
        //Thread::wait(1);
        
        // Transmit 
        // serial.putc()...
    }
}


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

    motorR.write(1);
    motorL.write(1);
    ledOrg.write(1);

    serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
    // (USART2)->CR1 &=  ~USART_CR1_UE; 
    // wait(1);
    serial.attach(&serialContext, Serial::RxIrq);
    // wait(1);
    // (USART2)->CR1 |=  USART_CR1_UE;

    // ISR won't be called if the serial is not emptied first.
    serialContext();
    // Serial in / out
    // Thread readUartThread;
    // readUartThread.start(callback(&serialContext));
    //serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
    //serial.attach(&serialContext);

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

    servo.setPosition(-0.2);

    wait(osWaitForever);
}