#include "motorControl.h"

MotorControl::MotorControl(ShiftRegister* shiftRegister, uint8_t pwmPin,
                           uint8_t dirPinA, uint8_t dirPinB, uint8_t endstopPinA,
                           uint8_t endstopPinB)
            : Motor(shiftRegister, pwmPin, dirPinA, dirPinB) {
    pinMode(this->endstopPinA, INPUT);
    pinMode(this->endstopPinB, INPUT);
    this->endstopPinA = endstopPinA;
    this->endstopPinB = endstopPinB;
}

void MotorControl::loop() {
    if (millis() - this->lastMillis < this->loopDelay)
        return;

    if (readPin(this->endstopPinA))
        this->endstopTouched = Direction::Left;
    else if (readPin(this->endstopPinB))
        this->endstopTouched = Direction::Right;
    else
        this->endstopTouched = Direction::None;

    if (this->direction == this->endstopTouched)
        this->stop();

    this->lastMillis = millis();
}



bool MotorControl::start() {
    if (this->direction == this->endstopTouched)
        return false;

    this->configureMotor();
    return true;
}

bool MotorControl::getEndStopLeft() {
    return readPin(this->endstopPinA);
}

bool MotorControl::getEndStopRight() {
    return readPin(this->endstopPinB);
}

bool MotorControl::readPin(uint8_t pin) {
    bool res = false;
    
    if (pin == 20 || pin == 21)
        res = (analogRead(pin) > 512);
    else
        res = digitalRead(pin);

    return res;
}