#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include <math.h>
#include <stdio.h>
#include "./mpu6050_reader.h"
#include "./servo_control.h"
#include "./screen_utils.h"

void warning_angle(float angle, float limit_angle, bool isAboveLimit) {

    if(isAboveLimit){
      // Limpa as linhas antes de escrever
      clear_line(100);
      clear_line(120);
      print_text_on_screen("Angulo esta acima", 100, 0, 2);
      print_text_on_screen("do limiar!", 120, 0, 2);
    } else {
      print_text_on_screen("Angulo esta na   ", 100, 0, 2);
      print_text_on_screen("margem permitida", 120, 0, 2);
    }
}

int main() {
    stdio_init_all();
    init_display();
    mpu6050_setup();
    mpu6050_init();
    servo_init();

    int16_t ax, ay, az;
    float current_angle = 0.0;
    float limit_angle = 40.0;

    // Flags de controle
    bool angleIsAbove = false;
    bool previousAngleState = false;  // Para controlar mudanças de estado

    read_mpu6050(&ax, &ay, &az);
    float inclinacao = calculate_angle(ax, ay, az);
    warning_angle(inclinacao, limit_angle, false);

    while (true) {
        read_mpu6050(&ax, &ay, &az);

        inclinacao = calculate_angle(ax, ay, az);

        if(inclinacao >= limit_angle){
          angleIsAbove = true;
        } else {
          angleIsAbove = false;
        }

        // Só atualiza o texto se o estado mudou
        if (angleIsAbove != previousAngleState) {
            warning_angle(inclinacao, limit_angle, angleIsAbove);
            previousAngleState = angleIsAbove;
        }

        servo_sweep(current_angle, fabs(inclinacao));
        current_angle = fabs(inclinacao);
        printf("Acelerômetro: X=%d, Y=%d, Z=%d\n", ax, ay, az);
        printf("Inclinação no eixo X: %.2f graus\n", inclinacao);

        sleep_ms(1000); // Atualiza a cada 1 segundo
    }
}