#include "motor.h"

Motor::Motor(double max, double min, int down_pin, int up_pin, Brake* brake, int sensor_pin, double degree, double resolution, double span)
{
    max_ = max;
    min_ = min;
    span_ = span;
    brake_ = brake;
    degree_ = degree;
    up_pin_ = up_pin;
    down_pin_ = down_pin;
    sensor_pin_ = sensor_pin;
    resolution_ = resolution;

    //Pin Settings
    pinMode(up_pin_, OUTPUT);
    pinMode(down_pin_, OUTPUT);

    //All Relais Off
    digitalWrite(up_pin_, HIGH);
    digitalWrite(down_pin_, HIGH);

    should_rotate_ = false;
    brake_tightened_ = true;
}

void Motor::doMotor()
{
    double diff;
    readMotSensor();
    if(should_rotate_)
    {
        diff = position_ - destination_;
        if(abs(diff) < resolution_)
            stop();
        else {
            if(diff > 0)
                rotateDown();
            else
                rotateUp();
        }
    }
}

Motor::motorError Motor::rotateUp()
{
    releaseBrake();
    digitalWrite(down_pin_, HIGH);
    digitalWrite(up_pin_, LOW);
    return Motor::ROT_OK;
}

Motor::motorError Motor::rotateDown()
{
    releaseBrake();
    digitalWrite(up_pin_, HIGH);
    digitalWrite(down_pin_, LOW);
    return Motor::ROT_OK;
}

void Motor::releaseBrake()
{
    if(brake_tightened_) {
        brake_->release();
        brake_tightened_ = false;
    }
}

void Motor::tightenBrake()
{
    if(brake_tightened_ == false) {
        brake_->tighten();
        brake_tightened_ = true;
    }
}

void Motor::stop()
{
    should_rotate_ = false;
    tightenBrake();
    digitalWrite(down_pin_, HIGH);
    digitalWrite(up_pin_, HIGH);
}

double Motor::getPosition() const
{
    return position_;
}

Motor::motorError Motor::setPosition(double destination)
{
    if(destination >= min_ && destination <= max_) {
        destination_ = destination;
        if(abs(destination_ - position_) > span_) {
            should_rotate_ = true;
            return Motor::ROT_OK;
        }
        return Motor::ROT_ALREADY_IN_SPAN;
    }
    return Motor::ROT_VAL_OUT_OF_RANGE;
}

Motor::motorError Motor::readMotSensor()
{
    position_ = readADC(sensor_pin_)/degree_;
    return Motor::ROT_OK;
}

double Motor::readADC(int AdcPin)
{
    //Read ADC 10 times and calculate average
    uint32_t adcValue = 0;
    for(int i = 0; i < 10; i++)
        adcValue += analogRead(AdcPin);
        delay(1);
    return (((double)adcValue)/10);
}