#include "motorControl.h"

MotorControl::MotorControl() {}

void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) {
    this->pwm_pin = pwm_pin;
    this->pwm_channel = pwm_channel;
    this->dir_1 = dir_1;
    this->dir_2 = dir_2;

    pinMode(this->dir_1, OUTPUT);
    pinMode(this->dir_2, OUTPUT);

    digitalWrite(this->dir_1, LOW);
    digitalWrite(this->dir_2, LOW);

    ledcSetup(this->pwm_channel, PWMFREQ, PWMRES);
    ledcAttachPin(this->pwm_pin, this->pwm_channel);
    ledcWrite(this->pwm_channel, 0);
}

void MotorControl::loop() {
    static uint32_t last_millis = 0;
    uint32_t time = millis();

    //Cancel if delay is not reached
    if (time - last_millis < delay) {
        return;
    }
    runMotorControl();
    last_millis = time;
}

void MotorControl::runMotorControl() {
    // Absolute difference between target_power and speed
    uint8_t abs_difference = abs(this->target_power - this->power);

    // Difference between target_power and speed
    int16_t difference = this->target_power - this->power;

    // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
    if (abs(this->target_power) <  powersteps && abs_difference < powersteps) {
        this->setRealPower(0);
        return;
    }

    // Correct speed
    if (abs_difference < powersteps) {
        return;
    }

    // Positive or negative tagret speed
    if (this->target_power >= 0) {
        // Positive or negative speed
        if (this->power >= 0) {
            if (difference > 0) {
                this->increasePower(powersteps);
            } else {
                this->increasePower(-powersteps);
            }
        } else {
            this->increasePower(powersteps);
        }

    } else {
        // Positive or negative speed
        if (this->power >= 0) {
            this->increasePower(-powersteps);
        } else {
            if (difference > 0) {
                this->increasePower(powersteps);
            } else {
                this->increasePower(-powersteps);
            }
        }
    }
}

void MotorControl::setTargetPower(int8_t power) {
    if (power <= 100 && power >= -100) {
        this->target_power = power;
    } else {
        char str[64];
        sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power);
    }
}

void MotorControl::stop() {
    this->target_power = 0;
}

void MotorControl::emergencyStop() {
    setRealPower(0);
    while(1)
}

void MotorControl::toString() {
    Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction);
}

int8_t MotorControl::getPower() {
    return this->power;
}

int8_t MotorControl::getTargetPower() {
    return this->target_power;
}

bool MotorControl::isTargetPowerReached() {
    if (this->target_power == this->power)
        return true;
    return false;
}

bool MotorControl::isAccelerationPositive() {
    if (power < target_power)
        return true;
    return false;
}

bool MotorControl::isAccelerationNegative() {
    if (power > target_power)
        return true;
    return false;
}

void MotorControl::setRealPower(int8_t power) {
    //TODO: Exceptionhandling
    if (power <= 100 || power <= -100) {
        this->power = power;
    } else {
        return;
    }

    if (power == 0) {
        this->direction = 0;
        digitalWrite(this->dir_1, LOW);
        digitalWrite(this->dir_2, LOW);
        ledcWrite(this->pwm_channel, 0);
        return;
    }

    uint8_t pwm_val = map(abs(power), 0, 100, this->pwm_min, this->pwm_max);

    if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
        this->direction = 2;
        digitalWrite(this->dir_1, LOW);
        digitalWrite(this->dir_2, HIGH);
    } else if ((this->direction == 2 || this->direction == 0) && power > 0){  // new direction forward
        this->direction = 1;
        digitalWrite(this->dir_1, HIGH);
        digitalWrite(this->dir_2, LOW);
    }

    ledcWrite(this->pwm_channel, pwm_val);
}

void MotorControl::increasePower(int8_t power) {
    //TODO: Exceptionhandling
    //TODO: make a stop befor a direction change
    if (abs(power) > 2 * powersteps) {
        Serial.println("Invalid Argument in MotorControl::increasePower");
        return;
    }

    this->setRealPower(this->power + power);
}