#include "motorControl.h"

#include <Arduino.h>

#include "config.h"

MotorControl::MotorControl() {

}

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

    pinMode(this->direction_pin_1, OUTPUT);
    pinMode(this->direction_pin_2, OUTPUT);

    digitalWrite(this->direction_pin_1, LOW);
    digitalWrite(this->direction_pin_2, LOW);

    ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES);
    ledcAttachPin(this->pwm_pin, this->pwm_channel);
    ledcWrite(this->pwm_channel, 0);
}

void MotorControl::runMotorControl() {
    uint64_t now_millis = millis();
    if (now_millis - this->last_millis > UPDATE_TIME) {

        // Absolute difference between target_speed and speed
        uint8_t abs_difference = abs(this->target_speed - this->speed);

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

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

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

        // Positive or negative tagret speed
        if (this->target_speed >= 0) {

            // Positive or negative speed
            if (this->speed >= 0) {
                if (difference > 0) {
                    this->accelerate(SPEED_STEPS);
                } else {
                    this->accelerate(-SPEED_STEPS);
                }
            } else {
                this->accelerate(SPEED_STEPS);
            }

        } else {

            // Positive or negative speed
            if (this->speed >= 0) {
                this->accelerate(-SPEED_STEPS);
            } else {
                if (difference > 0) {
                    this->accelerate(SPEED_STEPS);
                } else {
                    this->accelerate(-SPEED_STEPS);
                }
            }
        }
    }

    this->last_millis = millis();
}

void MotorControl::setTargetSpeed(int16_t speed) {
    //TODO: Exceptionhandling
    if (speed <= 100 || speed <= -100) {
        this->target_speed = speed;
    } else {
        Serial.println("Invalid Argument in MotorControl::setTargetSpeed");
    }
}

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

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

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

int16_t MotorControl::getSpeed() {
    return this->speed;
}

bool MotorControl::isTargetSpeedReached() {
    if (this->target_speed == this->speed) {
        return true;
    }
    return false;
}

void MotorControl::setSpeed(int16_t speed) {
    //TODO: Exceptionhandling
    if (speed <= 100 || speed <= -100) {
        this->speed = speed;
    } else {
        Serial.println("Invalid Argument in MotorControl::setSpeed");
        return;
    }

    if (speed == 0) {
        this->direction = 0;
        digitalWrite(this->direction_pin_1, LOW);
        digitalWrite(this->direction_pin_2, LOW);
        ledcWrite(this->pwm_channel, 0);
        return;
    }

    uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX);

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

    ledcWrite(this->pwm_channel, pwm_val);
    Serial.printf("pwm_val: %d, ", pwm_val);
}

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

    this->setSpeed(speed + acc);
}