/**
 * @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),
      leftMotor{new MotorControl()},
      rightMotor{new MotorControl()},
      leftSpeedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
      rightSpeedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
      leftPid{new PID(&this->wheelspeedLeft,
                      &this->leftPidOut,
                      &this->wheelspeedLeftTarget,
                      Settings::Pid::Left::P,
                      Settings::Pid::Left::I,
                      Settings::Pid::Left::D,
                      DIRECT)},
      rightPid{new PID(&this->wheelspeedRight,
                       &this->rightPidOut,
                       &this->wheelspeedRightTarget,
                       Settings::Pid::Right::P,
                       Settings::Pid::Right::I,
                       Settings::Pid::Right::D,
                       DIRECT)}
{
    this->leftPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
    this->leftPid->SetSampleTime(Settings::Pid::sampleTime);
    this->leftPid->SetMode(AUTOMATIC);

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

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

    this->addChildComponent(this->leftMotor);
    this->addChildComponent(this->rightMotor);
    this->addChildComponent(this->leftSpeedometer);
    this->addChildComponent(this->rightSpeedometer);
}

MoveControl::~MoveControl()
{
    this->leftMotor->emergencyStop();
    this->rightMotor->emergencyStop();

    delete this->leftMotor;
    delete this->rightMotor;
    delete this->leftSpeedometer;
    delete this->rightSpeedometer;
    delete this->leftPid;
    delete this->rightPid;
}

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

    this->rightPid->Compute();
    this->leftPid->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->leftMotor->emergencyStop();
    this->rightMotor->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->leftPid;
    }
    else if (side == 1)
    {
        selectedPID = this->rightPid;
    }

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

PID *MoveControl::getPID(uint8_t side) const
{
    if (side == 0)
    {
        return this->leftPid;
    }
    if (side == 1)
    {
        return this->rightPid;
    }
    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->wheelspeedRightTarget = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
    this->wheelspeedLeftTarget = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
}

void MoveControl::regulateMotors()
{
    switch (this->driving_status)
    {
    case Status::Stop:
        this->leftMotor->setTargetPower(0);
        this->rightMotor->setTargetPower(0);
        this->setSpeedometerDirection(this->leftSpeedometer, 0);
        this->setSpeedometerDirection(this->rightSpeedometer, 0);
        break;

    case Status::Drive:
        this->leftMotor->setTargetPower(static_cast<int8_t>(this->leftPidOut));
        this->rightMotor->setTargetPower(static_cast<int8_t>(this->rightPidOut));
        this->setSpeedometerDirection(this->leftSpeedometer, this->leftMotor->getPower());
        this->setSpeedometerDirection(this->rightSpeedometer, this->rightMotor->getPower());
        break;

    case Status::Raw:
        this->leftMotor->setTargetPower(this->rawPowerLeft);
        this->rightMotor->setTargetPower(this->rawPowerRight);
        this->setSpeedometerDirection(this->leftSpeedometer, this->rawPowerLeft);
        this->setSpeedometerDirection(this->rightSpeedometer, this->rawPowerRight);
        break;

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

void MoveControl::updateCurrentWheelSpeed()
{
    this->wheelspeedLeft = this->leftSpeedometer->getSpeedRad();
    this->wheelspeedRight = this->rightSpeedometer->getSpeedRad();
}