#include "moveControl.h"

#include <Arduino.h>

MoveControl::MoveControl() {
    this->debug = new DebugMqtt("MoveControl");
}

void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
                        Speedometer *left_speedometer, Speedometer *right_speedometer) {
    debug->sendMsg(Loglevel::info, "Init...");
    this->left_motor = left_motor;
    this->right_motor = right_motor;
    this->left_speedometer = left_speedometer;
    this->right_speedometer = right_speedometer;
    debug->sendMsg(Loglevel::info, "Init finished!");
}

void MoveControl::runMoveControl() {
    static uint64_t last_millis = 0;
    if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
        return;
    }
    last_millis = millis();

    this->calcWheelSpeed();
    this->regulateMotors();
}

void MoveControl::setDrivingStatus(DrivingStatus status) {
    this->driving_status = status;
}

void MoveControl::setSpeed(double speed) {
    this->x_speed = speed;
    Serial.printf("New speed: %f in moveControll.cpp \n", speed);
}

void MoveControl::setRotationspeed(double speed) {
    this->rotation_speed = speed;
}

void MoveControl::calcWheelSpeed() {
    // 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);
    debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target);
    debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target);
}

void MoveControl::regulateMotors() {
    double ratio = 0;
    switch (this->driving_status) {
    case DrivingStatus::stop :
        this->left_motor->setTargetPower(0);
        this->right_motor->setTargetPower(0);
        break;

    case DrivingStatus::straightForward :
        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
            && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
                    && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::straightBackward :
        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
            && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
                    && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight
        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;

        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed()) * ratio) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed()) * ratio) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
            && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
                    && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft
        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;

        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
            && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
                    && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;

        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
            && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
                    && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;

        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
            && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
                    && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::rotateLeft :
        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
            && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
                    && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
        }
        break;

    case DrivingStatus::rotateRight :
        // Left motor
        // Too slow
        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
            && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);

        // Too fast
        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
                    && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {

            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
        }

        // Right motor
        // Too slow
        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
            && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);

        // Too fast
        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
                    && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {

            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
        }
        break;

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


}