/**
 * @file moveControl.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains an implementation of the class MoveControl
 * @version 0.1
 * @date 2022-02-15
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "moveControl.h"

MoveControl::MoveControl() {
    this->left_motor = new MotorControl();
    this->right_motor = new MotorControl();
    this->left_speedometer = new Speedometer(M_ENCODE_LEFT, WHEEL_DIAMETER, ENC_STEPS);
    this->right_speedometer = new Speedometer(M_ENCODE_RIGHT, WHEEL_DIAMETER, ENC_STEPS);

    this->left_pid = new PID(   &this->wheelspeed_left,
                                &this->left_pid_out,
                                &this->wheelspeed_left_target,
                                PID_LEFT_P, PID_LEFT_I, PID_LEFT_D,
                                DIRECT);
    this->right_pid = new PID(  &this->wheelspeed_right,
                                &this->right_pid_out,
                                &this->wheelspeed_right_target,
                                PID_RIGHT_P, PID_RIGHT_I, PID_RIGHT_D,
                                DIRECT);

    this->left_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
    this->left_pid->SetSampleTime(PID_SAMPLETIME);
    this->left_pid->SetMode(AUTOMATIC);

    this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
    this->right_pid->SetSampleTime(PID_SAMPLETIME);
    this->right_pid->SetMode(AUTOMATIC); 

    this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
    this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);                              
}

MoveControl::~MoveControl() {
    this->left_motor->emergencyStop();
    this->right_motor->emergencyStop();

    delete this->left_motor;
    delete this->right_motor;
    delete this->left_speedometer;
    delete this->right_speedometer;
}

void MoveControl::loop() {
    uint16_t left_motor_time = this->left_motor->loop();
    uint16_t right_motor_time = this->right_motor->loop();
    uint16_t left_speed_time = this->left_speedometer->loop();
    uint16_t right_speed_time = this->right_speedometer->loop();

    if (left_motor_time > 50
        ||  right_motor_time > 50
        ||  left_speed_time > 50
        ||  right_speed_time > 50) {

        // Dont count overTimeCounter if one speedometer is in TestMode.
        if (left_speed_time == UINT16_MAX || right_speed_time == UINT16_MAX)
            this->overTimeCounter--;

        this->overTimeCounter++;
        if (this->overTimeCounter >= this->overTimeMax) {
            char buf[128];
            sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n",
                    left_motor_time, right_motor_time, left_speed_time, right_speed_time);
            std::cout << buf;
            this->overTimeCounter = 0;
        }
    }

    if (millis() - this->lastMillis < this->delay) 
        return;

    this->runMoveControl();
    this->lastMillis = millis();
}

void MoveControl::runMoveControl() {
    this->updateCurrentWheelSpeed();
    this->calcTargetWheelSpeed();

    this->right_pid->Compute();
    this->left_pid->Compute();

    this->regulateMotors();
}

void MoveControl::setDrivingStatus(DrivingStatus status) {
    this->setSpeed(0);
    this->setRotationSpeed(0);
    this->setRawPowerLeft(0);
    this->setRawPowerRight(0);
    this->driving_status = status;
    // switch (this->driving_status) {
    //     case DrivingStatus::stop :
    //         Serial.println("New drivingState = stop in MoveControl::setDrivingStatus");
    //         break;

    //     case DrivingStatus::drive :
    //         Serial.println("New drivingState = drive in MoveControl::setDrivingStatus");
    //         break;

    //     default:
    //         Serial.println("Wrong drivingState in MoveControl::regulateMotors");
    //         break;
    // }
}

void MoveControl::emergencyStop() {
    this->left_motor->emergencyStop();
    this->right_motor->emergencyStop();
    this->setSpeed(0);
    this->setRotationSpeed(0);
    this->setRawPowerLeft(0);
    this->setRawPowerRight(0);
    this->driving_status = DrivingStatus::stop;
}

void MoveControl::setSpeed(double speed) {
    if (speed < 0.1 && speed > -0.1) {
        this->x_speed = 0;
        return;
    }

    this->x_speed = speed;
}

void MoveControl::setRotationSpeed(double speed) {
    if (speed < 0.1 && speed > -0.1){
        this->rotation_speed = 0;
        return;
    }

    this->rotation_speed = speed;
}

void MoveControl::setRawPowerLeft(int16_t power) {
    if (power <= 100 && power >= -100)
        this->rawPowerLeft = power;
}

void MoveControl::setRawPowerRight(int16_t power) {
    if (power <= 100 && power >= -100)
        this->rawPowerRight = power;
}

void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
    PID* selectedPID = nullptr;
    if (side == 0)
        selectedPID = this->left_pid;
    else if (side == 1)
        selectedPID = this->right_pid;

    selectedPID->SetTunings(p, i, d);
}

PID* MoveControl::getPID(uint8_t side) {
    if (side == 0)
        return this->left_pid;
    else if (side == 1)
        return this->right_pid;
    else
        return nullptr;
}

void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
    if (value > 0)
        speedometer->setDirection(Speedometer::Direction::Forward);
    else if (value < 0)
        speedometer->setDirection(Speedometer::Direction::Backward);
    else
        speedometer->setDirection(Speedometer::Direction::None);
}

void MoveControl::calcTargetWheelSpeed() {
    /* original formula:
       (1 / r) / 1  b \ / x \ = / Xl \
               \ 1 -b / \ T /   \ Xr /  */

    // (1 / r) * 1
    const static double A = 15.82278481;
    // (1 / r) * b
    const static double B = 2.096518987;

    this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
    this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);
}

void MoveControl::regulateMotors() {
    switch (this->driving_status) {
        case DrivingStatus::stop :
            this->left_motor->setTargetPower(0);
            this->right_motor->setTargetPower(0);
            this->setSpeedometerDirection(this->left_speedometer, 0);
            this->setSpeedometerDirection(this->right_speedometer, 0);
            break;

        case DrivingStatus::drive :
            this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
            this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
            this->setSpeedometerDirection(this->left_speedometer, this->left_pid_out);
            this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out);
            break;

        case DrivingStatus::raw :
            this->left_motor->setTargetPower(this->rawPowerLeft);
            this->right_motor->setTargetPower(this->rawPowerRight);
            this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
            this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
            break;

        default:
            Serial.println("Wrong drivingState in MoveControl::regulateMotors");
            break;
    }
}

void MoveControl::updateCurrentWheelSpeed() {
    this->wheelspeed_left = this->left_speedometer->getSpeed();
    this->wheelspeed_right = this->right_speedometer->getSpeed();
}