#include "src/drivers/servo.h"

#include "mbed-os/rtos/Thread.h"

namespace drivers {

namespace {

    Thread* pServoThread;

    double posToPulseWidthInUs(double position)
    {
        return (position/2+1.5)*1000;
    }

    typedef struct {
        double from;
        double to;
        double intervalInSec;
        PwmOut* pServo;
    } SweepData;

    typedef struct {
        double latestPosition;
        PwmOut* pServo;
    } NodData;
    
    void sweepWorker(SweepData* pSweepData)
    {
        SweepData sweepData = *pSweepData;

        double resolution(24);
        double range(sweepData.to - sweepData.from);
        double value(sweepData.to);
        double increment(range/resolution);

        while (true)
        {
            value += increment;
            
            if (value >= sweepData.to)
            {
                increment = -abs(increment);
            }
            else if (value <= sweepData.from)
            {
                increment = abs(increment);
            }
            sweepData.pServo->pulsewidth_us(posToPulseWidthInUs(value));

            // E.g resolution 10 will give a 100ms delay between steps
            Thread::wait(1000/resolution);
        }
    }

    void nodWorker(NodData* pNodData)
    {
        pNodData->pServo->pulsewidth_us(posToPulseWidthInUs(pNodData->latestPosition-0.4));
        Thread::wait(500);
        pNodData->pServo->pulsewidth_us(posToPulseWidthInUs(pNodData->latestPosition));
    }
}

Servo::Servo(PinName servoArm)
: m_servo(servoArm)
, m_dir(1)
, m_inMovement(false)
, m_latestPosition(0.0)
{
    center();
}

void Servo::center()
{
    setPosition(0.0);
}

void Servo::resetTimers()
{
     m_servo.period_ms(20);
}

void Servo::setPosition(double position)
{
    stop(); 

    double increment = position*500.0;
    
    if (increment < -500)
    {
        increment = -500;
    }
    else if (increment > 500)
    {
        increment = 500;
    }

    m_servo.pulsewidth_us(1500 + increment*(double)m_dir);

    m_latestPosition = position;
}

void Servo::invert()
{
    m_dir = -1;
}

void Servo::sweep(double from, double to, double intervalInSec)
{
    stop();

    pServoThread = new Thread;

    SweepData sweepData;
    sweepData.from = from;
    sweepData.to = to;
    sweepData.intervalInSec = intervalInSec;
    sweepData.pServo = &m_servo;

    pServoThread->start(callback(sweepWorker, &sweepData));
    wait(5); // Task will run for at least this long (can get destroyed after)
} 

void Servo::nod()
{   
    stop();

    NodData nodData;
    nodData.pServo = &m_servo;
    nodData.latestPosition = m_latestPosition;

    pServoThread = new Thread;

    pServoThread->start(callback(nodWorker, &nodData));
    wait(1);
    pServoThread->join(); // Wait for the nod to end
}

void Servo::stop()
{
    if (pServoThread != NULL && pServoThread->get_state() != Thread::Deleted)
    {
        pServoThread->terminate();
        delete pServoThread;
        pServoThread = NULL;
    }
}

} // namespace drivers