#include "Servo.h"
#include "./TimerOne.h"

Servo *Servo::firstInstance;

Servo::Servo(uint8_t dirpin, uint8_t steppin)
{
    if (!firstInstance)
    {
        firstInstance = this;
    }

    dirPin = Pin(dirpin);
    stepPin = Pin(steppin);
}

void Servo::init()
{
    dirPin.setOutput();
    stepPin.setOutput();
    Timer1.initialize();
    Timer1.attachInterrupt(Servo::ticking);
    Timer1.stop();
    pause();
}

void Servo::setPulse(long pulse)
{
    Timer1.setPeriod(pulse);
}

bool Servo::step(long steps, uint8_t direction)
{
    if (isStepping())
    {
        return false;
    }

    ticksRemaining = steps * 2; //converting steps to ticks

    if (direction == HIGH)
    {
        dirPin.setHigh();
    }
    else
    {
        dirPin.setLow();
    }

    return true;
}

bool Servo::step(long steps, uint8_t direction, long pulse)
{
    if (isStepping())
    {
        return false;
    }

    resume();
    setPulse(pulse);

    return step(steps, direction);
}

long Servo::getRemainingSteps()
{
    return ticksRemaining / 2;
}

//returns the remaining steps
long Servo::stop()
{
    //each step = 2 ticks
    long stepsRemaining = getRemainingSteps();

    Timer1.stop();

    if (ticksRemaining & 1)
    {
        ticksRemaining = 1;
    }
    else
    {
        ticksRemaining = 0;
    }

    Timer1.start();

    return stepsRemaining;
}

void Servo::pause()
{
    paused = true;
    Timer1.stop();
}

void Servo::resume()
{
    if (paused)
    {
        Timer1.start();
        paused = false;
    }
}

bool Servo::isStepping()
{
    return (ticksRemaining > 0);
}

bool Servo::isStopped()
{
    return (ticksRemaining <= 0);
}

bool Servo::isPaused()
{
    return paused;
}

void Servo::ticking()
{
    if (firstInstance->ticksRemaining > 0)
    {
        //generate high/low signal for the stepper driver
        firstInstance->stepPin.toggleState();
        --firstInstance->ticksRemaining;
    }
}