#include "src/drivers/stepper.h"
#include "src/math/Utilities.h"

using namespace math;

namespace drivers {

Stepper::Stepper(PinName stepPin, 
                 PinName dirPin, 
                 PinName enPin)
: m_step(stepPin)
, m_dir(dirPin)
, m_en(enPin)
, m_accelerationLimitOn(true)
, m_accelerationLimit(50.0f)
, m_stepsPerRevolution(200)
, m_microStepResolution(8)
, m_currentPeriod(0)
, m_configuredDirection(1)
, m_lastDirection(1)
, m_latestSpeed(0.0f)
{
    m_step.period_us(1000000);
    m_step.pulsewidth_us(5.0f);

    // Start controller deactivated
    m_en.write(1);
    m_dir.write(m_configuredDirection);
}

void Stepper::init()
{

}

void Stepper::enable()
{
    // Controller is enabled when IO is low
    m_en.write(0);
}

void Stepper::disable()
{
    // Controller is disabled when IO is high
    m_en.write(1);
}

bool Stepper::isEnabled()
{
    // Controller is enabled when IO is low
    return m_en.read() == 0;
}

void Stepper::setDirection(int dir)
{
    m_lastDirection = -1; // Deem invalid
    m_configuredDirection = dir;
    m_dir.write(dir == 1 ? 1 : 0);
}

int Stepper::getDirection()
{
    return m_configuredDirection;
}

float Stepper::limitAcceleration(float DPS)
{
    float delta = DPS - m_latestSpeed;

    if (abs(delta) > m_accelerationLimit)
    {
        return (delta > 0) ? 
            m_latestSpeed + m_accelerationLimit : 
            m_latestSpeed - m_accelerationLimit;
    }
    return DPS;
}

void Stepper::setSpeed(const float& DPS)
{
    m_latestSpeed = limitAcceleration(DPS);
    
    float revPerSecond = abs(m_latestSpeed)/(float)360.0;
    revPerSecond = constrain(revPerSecond, 100.0f);

    float stepsPerSecond = 
        m_stepsPerRevolution*m_microStepResolution*revPerSecond;

    float usPerSecond = 1000000.0f;

    // Some high value to make motors "stop" | And to avoid division by zero
    float periodUs = (stepsPerSecond == 0.0) ? 100000.0 : usPerSecond/stepsPerSecond;
    
    // Precaution. Don't know how close the period can be to 
    // the pulsewidth or if it will cause any issues
    if (periodUs < (float)100.0f)
    {
        periodUs = (float)100.0f;
    }
    if (periodUs > (float)100000.0f)
    {
        periodUs = (float)100000.0f;
    }

    // No need to set anything new
    if (periodUs != m_currentPeriod)
    {
        m_step.period_us(periodUs);
        m_step.pulsewidth_us(5.0f);
    }
    m_currentPeriod = periodUs;

    int dir = (m_latestSpeed > (float)0.0f) ? 1 : -1;

    if (m_lastDirection != dir)
    {
        // Give dir to motor controller
        m_dir.write((m_configuredDirection*dir == 1) ? 1 : 0);

        m_lastDirection = dir;
    }
}

float Stepper::getSpeed()
{
    return m_latestSpeed;
}

} // namespace drivers