/**
 * @file motorControl.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Implemention of the class motorControl.h.
 * @see motorControl.h
 * @version 0.1
 * @date 2021-12-13
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#include "motorControl.h"

MotorControl::MotorControl() {
    this->setMinPwm(PWMMIN);
    this->setMaxPwm(PWMMAX);
}

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, this->pwm_res);
    ledcAttachPin(this->pwm_pin, this->pwm_channel);
    ledcWrite(this->pwm_channel, 0);
}

uint16_t MotorControl::loop() {
    uint32_t time = millis();
    uint16_t elapsed_time = time - this->lastMillis;

    //Cancel if delay is not reached
    if (elapsed_time < delay)
        return elapsed_time;

    runMotorControl();
    this->lastMillis = time;
    return elapsed_time;
}

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

    // Difference between target_power and power
    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::setMinPwm(uint8_t min) {
    if (min > 80) min = 80;
    //transform percentage to real pwm value
    min = (uint8_t) (((1 << pwm_res) - 1) * (min / 100.0));
    this->dutycycle_min = min;
}

void MotorControl::setMaxPwm(uint8_t max) {
    if (max > 100) max = 100;
    //transform percentage to real pwm value
    max = (uint8_t) (((1 << pwm_res) - 1) * (max / 100.0));
    this->dutycycle_max = max;
}

uint16_t MotorControl::setPowerSteps(uint8_t increment) {
    this->powersteps = increment;
    return (uint16_t) (delay * ( 100 / powersteps ));
}

void MotorControl::setTargetPower(int8_t power) {
    if (power <= 100 && power >= -100)
        this->target_power = power;
    else
        std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
}

uint16_t MotorControl::setDelay(uint8_t delay) {
    this->delay = delay;
    return (uint16_t) (delay * ( 100 / powersteps ));
}

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

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

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 (this->power == 0) {
        this->direction = 0;
        digitalWrite(this->dir_1, LOW);
        digitalWrite(this->dir_2, LOW);
        ledcWrite(this->pwm_channel, 0);
        this->dutycycle = 0;
        return;
    }

    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycle_min, this->dutycycle_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);
    this->dutycycle = 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);
}