/**
 * @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()
{
    Component::loopDelay = MotorControl::loopDelay;
}

void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2)
{
    this->pwmPin = pwmPin;
    this->pwmChannel = pwmChannel;
    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->pwmChannel, MotorControl::pwmFreq, MotorControl::pwmRes);
    ledcAttachPin(this->pwmPin, this->pwmChannel);
    ledcWrite(this->pwmChannel, 0);
    std::cout << "Init pwm" << std::endl;
}

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

    // Difference between targetPower and power
    const int16_t difference = this->targetPower - this->power;

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

    // Correct speed
    if (abs_difference < MotorControl::powerSteps)
    {
        return;
    }

    // Positive or negative target speed
    if (this->targetPower >= 0)
    {
        // Positive or negative speed
        if (this->power >= 0)
        {
            if (difference > 0)
            {
                this->increasePower(MotorControl::powerSteps);
            }
            else
            {
                this->increasePower(-MotorControl::powerSteps);
            }
        }
        else
        {
            this->increasePower(MotorControl::powerSteps);
        }
    }
    else
    {
        // Positive or negative speed
        if (this->power >= 0)
        {
            this->increasePower(-MotorControl::powerSteps);
        }
        else
        {
            if (difference > 0)
            {
                this->increasePower(MotorControl::powerSteps);
            }
            else
            {
                this->increasePower(-MotorControl::powerSteps);
            }
        }
    }
}

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

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

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

bool MotorControl::isTargetPowerReached() const
{
    return this->targetPower == this->power;
}

bool MotorControl::isAccelerationPositive() const
{
    return power < targetPower;
}

bool MotorControl::isAccelerationNegative() const
{
    return power > targetPower;
}

void MotorControl::setRealPower(int8_t power)
{
    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->pwmChannel, 0);
        this->dutycycle = 0;
        std::cout << "abort" << std::endl;
        return;
    }

    const uint8_t pwm_val = map(abs(power), 0, 100, MotorControl::minPwmVal, MotorControl::maxPwmVal);

    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);
    }

    std::cout << "MotorControl::setRealPower pwm_val: " << (int) pwm_val << " channel:" << (int) this->pwmChannel << std::endl;
    ledcWrite(this->pwmChannel, pwm_val);
    this->dutycycle = pwm_val;
}

void MotorControl::increasePower(int8_t power)
{
    if (abs(power) > 2 * MotorControl::powerSteps)
    {
        Serial.println("Invalid Argument in MotorControl::increasePower");
        return;
    }

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