#include <stdexcept>
#include <unistd.h>
#include <chrono>
#include <spdlog/spdlog.h>

#include "include/motor.hpp"
#include "include/spi.hpp"
#include "include/robot.hpp"
#include "include/encoder.hpp"
#include "include/mathUtils.hpp"
#include "include/pid.hpp"

void check_port(uint8_t port) {
    if (port >= MOTOR_COUNT) {
        throw std::invalid_argument("Invalid motor port specified");
    }
}

Motor::Motor() {
    reset_speed();

    speed_control_thread = std::thread(&Motor::speed_control_loop, this);
    speed_control_thread.detach();
}

void Motor::set_speed(uint8_t port, double rpm) {
    check_port(port);
    
    speed_targets[port] = rpm;
    motor_control_modes[port] = Motor::Control::SPEED;
}

void Motor::set_power(uint8_t port, double percent) {
    motor_control_modes[port] = Motor::Control::POWER;
    _set_power(port, percent);
}

void Motor::_set_power(uint8_t port, double percent) {
    check_port(port);
    if (abs(percent) > 100) {
        throw std::invalid_argument("Invalid motor percent specified. Should be -100 <= percent <= 100");
    }

    Mode mode = Motor::Mode::COAST;
    if (percent < 0) {
        mode = Motor::Mode::BACKWARD;
    } else if (percent > 0) {
        mode = Motor::Mode::FORWARD;
    }

    uint16_t pwm = abs(percent) * (MAX_MOTOR_SPEED / 100.0);
    Motor::set_pwm(port, pwm, mode);
}

void Motor::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
    check_port(port);
    if (mode > Motor::Mode::BREAK) {
        throw std::invalid_argument("Invalid motor mode specified");
    }

    if (port == 0) {
        Spi::getInstance().write(Spi::Register::MOTOR_1_PWM_H, 2, pwm);
        Spi::getInstance().write(Spi::Register::PWM_1_CTRL, 1, mode);
    } else if (port == 1) {
        Spi::getInstance().write(Spi::Register::MOTOR_2_PWM_H, 2, pwm);
        Spi::getInstance().write(Spi::Register::PWM_2_CTRL, 1, mode);
    } else if (port == 2) {
        Spi::getInstance().write(Spi::Register::MOTOR_3_PWM_H, 2, pwm);
        Spi::getInstance().write(Spi::Register::PWM_3_CTRL, 1, mode);
    } else if (port == 3) {
        Spi::getInstance().write(Spi::Register::MOTOR_4_PWM_H, 2, pwm);
        Spi::getInstance().write(Spi::Register::PWM_4_CTRL, 1, mode);
    }
}

std::vector<double> Motor::get_speed() {
    // auto current_time = std::chrono::high_resolution_clock::now();
    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time_encoders_read).count() / 1000000.0;
    
    // last_time_encoders_read = current_time;

    // if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) {
    //     reset_speed();
    //     usleep(MOTOR_FILTER_RESET_DELAY_US);
    //     return Motor::get_speed();
    // }

    // std::vector<int32_t> current_ticks = Encoder::read_all();
    // std::vector<double> velocities_rpm;
    // for (int i = 0; i < MOTOR_COUNT; i++) {
    //     long double delta_ticks = current_ticks.at(i) - last_encoder_values[i];

    //     long double velocity_rpm = (delta_ticks / delta_seconds) * 60.0L / TICKS_PER_TURN;
    //     filtered_speeds[i] = (long double) ((MOTOR_FILTER_ALPHA * velocity_rpm) + (1.0 - MOTOR_FILTER_ALPHA) * filtered_speeds[i]);
    //     velocities_rpm.push_back(filtered_speeds[i]);

    //     last_encoder_values[i] = current_ticks.at(i);
    // }

    std::vector<double> velocities_rpm;
    uint8_t result_bytes[MOTOR_COUNT * 2] = {0};

    Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, MOTOR_COUNT * 2, result_bytes);
    for (int i = 0; i < MOTOR_COUNT; i++) {
        int16_t ticks_per_second = mathUtils::int_from_bytes(result_bytes + i * 2, 2);
        velocities_rpm.push_back(ticks_per_second * 60.0L / TICKS_PER_TURN);
    }

    return velocities_rpm;
}

void Motor::reset_speed() {
    last_time_encoders_read = std::chrono::high_resolution_clock::now();

    auto current_ticks = Encoder::get_instance().read_all_cached();
    for (int i = 0; i < MOTOR_COUNT; i++) {
        filtered_speeds[i] = 0;
        last_encoder_values[i] = current_ticks.at(i);
    }
}

void Motor::speed_control_loop() {
    std::vector<PID> pids;

    for (int i = 0; i < MOTOR_COUNT; i++) {
        pids.push_back(
            PID{MOTOR_SPEED_CONTROL_KP, MOTOR_SPEED_CONTROL_KI, MOTOR_SPEED_CONTROL_KD, 100.0, 100.0}
        );
    }

    while (1 == 1) {
        float sleep_time = std::round(1000.0 / MOTOR_SPEED_CONTROL_LOOP_HZ);
        std::this_thread::sleep_for(std::chrono::microseconds((int) sleep_time * 1000));

        bool should_control_speed = false;
        for (int i = 0; i < MOTOR_COUNT; i++) {
            if (motor_control_modes[i] == Motor::Control::SPEED) {
                should_control_speed = true;
            }
        }

        if (!should_control_speed) {
            // spdlog::info("continue");
            continue;
        }

        auto speeds = Motor::get_instance().get_speed();
        for (int i = 0; i < MOTOR_COUNT; i++) {
            if (motor_control_modes[i] == Motor::Control::SPEED) {
                float power = pids.at(i)(speed_targets[i], speeds.at(i));
                _set_power(i, power);
            }
        }
    }
}