/**
 * @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;
    this->right_speedometer = new Speedometer;

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

    this->left_speedometer->init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
    this->right_speedometer->init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);                               
}

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

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

    static uint64_t last_millis = 0;
    if (millis() - last_millis < delay) 
        return;

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

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

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

    this->regulateMotors();
}

void MoveControl::setDrivingStatus(DrivingStatus status) {
    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::setSpeed(double speed) {
    if (speed < 0.2 && speed > -0.2)
        this->x_speed = 0;
    else
        this->x_speed = speed;
}

void MoveControl::setRotationspeed(double speed) {
    if (speed < 0.7 && speed > -0.7)
        this->rotation_speed = 0;
    else
        this->rotation_speed = speed;
}

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::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);
            // Serial.println("in stop MoveControl::regulateMotors");
            // if (abs(left_speedometer->getSpeed()) > 0.2) {
            //     Serial.printf("Left Motor Target: %d, Ist: %d\n", this->left_motor->getTargetPower(), this->left_motor->getPower());
            //     Serial.printf("Left Speedometer speed: %f\n", this->left_speedometer->getSpeed());
            // }
            // if (abs(right_speedometer->getSpeed()) > 0.2) {
            //     Serial.printf("Right Motor Target: %d, Ist: %d\n", this->right_motor->getTargetPower(), this->right_motor->getPower());
            //     Serial.printf("Right Speedometer speed: %f\n", this->right_speedometer->getSpeed());
            // }
            break;

        case DrivingStatus::drive :
            this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
            this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
            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();
}