/**
 * @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()
    : Component(MoveControl::loopDelay),
      left_motor{new MotorControl()},
      right_motor{new MotorControl()},
      left_speedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
      right_speedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
      left_pid{new PID(&this->wheelspeed_left,
                       &this->left_pid_out,
                       &this->wheelspeed_left_target,
                       Settings::Pid::Left::P,
                       Settings::Pid::Left::I,
                       Settings::Pid::Left::D,
                       DIRECT)},
      right_pid{new PID(&this->wheelspeed_right,
                        &this->right_pid_out,
                        &this->wheelspeed_right_target,
                        Settings::Pid::Right::P,
                        Settings::Pid::Right::I,
                        Settings::Pid::Right::D,
                        DIRECT)}
{
    this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
    this->left_pid->SetSampleTime(Settings::Pid::sampleTime);
    this->left_pid->SetMode(AUTOMATIC);

    this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
    this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
    this->right_pid->SetMode(AUTOMATIC);

    this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
    this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);

    this->addChildComponent(this->left_motor);
    this->addChildComponent(this->right_motor);
    this->addChildComponent(this->left_speedometer);
    this->addChildComponent(this->right_speedometer);
}

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;
    delete this->left_pid;
    delete this->right_pid;
}

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

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

    this->regulateMotors();
}

void MoveControl::setDrivingStatus(Status status)
{
    this->setSpeed(0);
    this->setRotationSpeed(0);
    this->setRawPowerLeft(0);
    this->setRawPowerRight(0);
    this->driving_status = status;
    // switch (this->driving_status) {
    //     case Status::Stop :
    //         std::cout << "New drivingState = Stop in MoveControl::setDrivingStatus" << std::endl;
    //         break;

    //     case Status::Drive :
    //         std::cout << "New drivingState = Drive in MoveControl::setDrivingStatus" << std::endl;
    //         break;

    //     case Status::Raw :
    //         std::cout << "New drivingState = Raw in MoveControl::setDrivingStatus" << std::endl;
    //         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 = Status::Stop;
}

void MoveControl::setSpeed(double speed)
{
    if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
    {
        this->drivingSpeeds.x = 0;
        return;
    }

    this->drivingSpeeds.x = speed;
}

void MoveControl::setRotationSpeed(double speed)
{
    if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
    {
        this->drivingSpeeds.rot = 0;
        return;
    }

    this->drivingSpeeds.rot = speed;
}

void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds)
{
    this->setSpeed(drivingSpeeds.x);
    this->setRotationSpeed(drivingSpeeds.rot);
}

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

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

void MoveControl::setPidTunings(uint8_t side, double pPart, double iPart, double dPart)
{
    PID *selectedPID = nullptr;
    if (side == 0)
    {
        selectedPID = this->left_pid;
    }
    else if (side == 1)
    {
        selectedPID = this->right_pid;
    }

    selectedPID->SetTunings(pPart, iPart, dPart);
}

PID *MoveControl::getPID(uint8_t side) const
{
    if (side == 0)
    {
        return this->left_pid;
    }
    if (side == 1)
    {
        return this->right_pid;
    }
    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
    constexpr double A1r1 = 1.0 / (Settings::wheelDiameter / 2);
    // (1 / r) * b
    constexpr double B1rb = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);

    this->wheelspeed_right_target = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
    this->wheelspeed_left_target = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
}

void MoveControl::regulateMotors()
{
    switch (this->driving_status)
    {
    case Status::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 Status::Drive:
        this->left_motor->setTargetPower(static_cast<int8_t>(this->left_pid_out));
        this->right_motor->setTargetPower(static_cast<int8_t>(this->right_pid_out));
        this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
        this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
        break;

    case Status::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->getSpeedRad();
    this->wheelspeed_right = this->right_speedometer->getSpeedRad();
}