#include "ServoMotorComponent.h"


/////////////////////////////////////////////////////////////////////////////////////
// Functions for setting up the ServoMotor
/////////////////////////////////////////////////////////////////////////////////////

/**
 * Constructor.
 * Prepares the ServoMotor.
 */
ServoMotorComponent::ServoMotorComponent(int PIN,  unsigned long updatePeriod, float step) {
    this->PIN = PIN;
    this->myservo.attach(PIN);
    this->desiredposition = Position::MIDDLE;
    this->currentPosition = MIDDLE_POS;
    this->lastUpTime = millis();
    this->updatePeriod = updatePeriod;
    this->step = step;
    this->myservo.write(this->PIN, this->currentPosition);
}

/**
 * Set the desired position
 * @desiredPosition: Give desired position
 */
void ServoMotorComponent::setDesiredPosition(Position desiredPosition) {
    switch (desiredPosition) {
        case Position::LEFT:
            this->desiredposition = LEFT_POS;
            break;
        case Position::MIDDLE:
            this->desiredposition = MIDDLE_POS;
            break;
        case Position::RIGHT:
            this->desiredposition = RIGHT_POS;
            break;
        default:
            break;
    }
}


/**
 * Write a new servoMotor position when it's necessary
 */
void ServoMotorComponent::refresh() {
    if (this->desiredposition == this->currentPosition 
    ||  millis() - this->lastUpTime <= this->updatePeriod) return;

    if (this->currentPosition > this->desiredposition) {
        this->currentPosition -= this->step;
    }
    if (this->currentPosition < this->desiredposition) {
        this->currentPosition += this->step;
    }
    this->lastUpTime = millis();
    this->myservo.write(this->PIN, this->currentPosition);
}

bool ServoMotorComponent::isConnected() {
    return true;
}