/**
 * @file main.cpp
 * @brief Código para cálculo de odometria de um robô com ESP32-S3.
 *
 * Este código lê a velocidade das rodas de um robô e calcula a posição
 * e orientação do robô utilizando odometria diferencial.
 */

// Definição dos parâmetros fixos
#define WHEEL_RADIUS 0.0889  ///< Raio da roda em metros
#define WHEELS_TRACK 0.405   ///< Distância entre as rodas em metros

// Estrutura para armazenar a pose do robô
typedef struct {
    double x;      ///< Posição no eixo X
    double y;      ///< Posição no eixo Y
    double theta;  ///< Orientação (em radianos)
} Pose;

// Posição inicial do robô
Pose robo_pose = {0.0, 0.0, 0.0};

/**
 * @brief Configuração inicial do ESP32-S3.
 *
 * Função chamada uma vez ao iniciar o programa para configurar o 
 * serial, pinos e outros componentes do robô.
 */
void setup() {
    Serial.begin(115200);
    Serial.println("Hello, ESP32-S3!");

    pinMode(4, INPUT);
    pinMode(5, INPUT);
}

/**
 * @brief Função principal chamada repetidamente.
 *
 * Esta função lê as velocidades das rodas do robô, calcula a odometria
 * e exibe as leituras e resultados no monitor serial.
 */
void loop() {
    double leftSpeed = readWheelSpeed(5);
    double rightSpeed = readWheelSpeed(4);

    double now = micros();
    static double lastTime = 0L;
    double deltaTime = (now - lastTime) / 1000000.0;

    Serial.println("Esquerda: " + String(leftSpeed) + "\tDireita: " + String(rightSpeed));

    calcular_odometria(leftSpeed, rightSpeed, deltaTime, &robo_pose);
    lastTime = now;

    delay(1);  // Pequeno atraso para simulação
}

/**
 * @brief Lê a velocidade de uma roda a partir de um pino analógico.
 *
 * @param pin Número do pino analógico para leitura.
 * @return Velocidade medida (valor bruto do sensor).
 */
double readWheelSpeed(int pin) {
    return (double)(analogRead(pin)/4095)*10;
}

/**
 * @brief Calcula a odometria do robô com base nas velocidades das rodas.
 *
 * Esta função utiliza as velocidades das rodas para calcular a nova
 * posição (x, y) e orientação (theta) do robô.
 *
 * @param vl Velocidade da roda esquerda (em rad/s).
 * @param vr Velocidade da roda direita (em rad/s).
 * @param dt Intervalo de tempo desde a última atualização (em segundos).
 * @param pose Ponteiro para a estrutura Pose que será atualizada.
 */
void calcular_odometria(double vl, double vr, double dt, Pose *pose) {
    // Cálculo da velocidade linear (v) e angular (omega) do robô
    double v = WHEEL_RADIUS * (vl + vr) / 2.0;
    double omega = WHEEL_RADIUS * (vr - vl) / WHEELS_TRACK;

    // Atualização da posição e orientação do robô
    pose->x += v * cos(pose->theta) * dt;
    pose->y += v * sin(pose->theta) * dt;
    pose->theta += omega * dt;

    // Normalização do ângulo theta entre -pi e pi
    if (pose->theta > M_PI) {
        pose->theta -= 2 * M_PI;
    } else if (pose->theta < -M_PI) {
        pose->theta += 2 * M_PI;
    }

    // Exibe os resultados no monitor serial
    printOdometry(pose, v, omega);
}

/**
 * @brief Converte um ângulo de radianos para graus.
 *
 * @param rad Ângulo em radianos.
 * @return Ângulo em graus.
 */
double radToDeg(double rad) {
    return rad * (180.0 / M_PI);
}

/**
 * @brief Exibe os resultados da odometria no monitor serial.
 *
 * @param pose Ponteiro para a estrutura Pose atualizada.
 * @param v Velocidade linear do robô (em m/s).
 * @param omega Velocidade angular do robô (em rad/s).
 */
void printOdometry(Pose *pose, double v, double omega) {
    double theta_deg = radToDeg(pose->theta);  // Conversão de radianos para graus
    Serial.println("\nVx= " + String(v) + "\tVth = " + String(omega));
    Serial.println("X = " + String(pose->x) + "\tY = " + String(pose->y) + "\tTheta (graus) = " + String(theta_deg));
}