#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"

// Pin definitions
#define BUTTON_PIN 6  
#define BUZZER_PIN 10  
#define LED_CORRECT_PIN 11  // Green LED pin (correct stroke)
#define LED_WRONG_PIN 13    // Red LED pin (wrong stroke)
#define I2C_SDA_PIN 4  
#define I2C_SCL_PIN 5  
#define MPU6050_ADDR 0x68 // I2C address of the MPU6050
#define ACCEL_XOUT_H 0x3B // Address of the first acceleration data byte 

// Global variables
bool is_writing = false;  // Indicates if the pen is writing
char selected_character = '\0';  // Selected ideogram
bool button_pressed = false;  // Flag to indicate the button was pressed

// Function to read the available acceleration axes
void mpu6050_read(int16_t *accel_x, int16_t *accel_y, int16_t *accel_z) {
  uint8_t buffer[6]; // Array to store acceleration data from the MPU6050
  uint8_t reg = ACCEL_XOUT_H; // Address of the first acceleration data byte

  // Send the MPU6050 register address
  i2c_write_blocking(i2c0, MPU6050_ADDR, &reg, 1, true);

  // Read the acceleration data (x, y, z)
  i2c_read_blocking(i2c0, MPU6050_ADDR, buffer, 6, false);

  // Process the acceleration data
  *accel_x = (buffer[0] << 8) | buffer[1];
  *accel_y = (buffer[2] << 8) | buffer[3];
  *accel_z = (buffer[4] << 8) | buffer[5];
}

// Function to initialize the MPU6050
void init_i2c() {
  // Configure I2C for the MPU6050
  i2c_init(i2c_default, 400 * 1000);  // I2C at 400 kHz
  gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
  gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);

  gpio_pull_up(I2C_SDA_PIN);
  gpio_pull_up(I2C_SCL_PIN);
}

// Interrupt function
static void gpio_callback(uint gpio, uint32_t events) {
  sleep_ms(50);
  if (gpio == BUTTON_PIN && (events & GPIO_IRQ_EDGE_FALL)) {  
    button_pressed = true; 
  }
}

void start_ideogram() {
  printf("\nVocê já pode começar a escrever.\n");
  // Read data from the MPU6050
  int16_t accel_x, accel_y, accel_z;

  // Detect the start of the stroke (pen touching the surface)
  bool write_begin = true;

  while (write_begin) {
    mpu6050_read(&accel_x, &accel_y, &accel_z);

    if (accel_z < -3000 && !is_writing) {  // Example of downward acceleration detection (pen down)
      is_writing = true;
      printf("\nInício do traço detectado.\n");

      int last_state = 0; // 0 = neutral, 1 = correct, -1 = wrong

      while (is_writing) {
        mpu6050_read(&accel_x, &accel_y, &accel_z);

        if (accel_z != 0 && accel_x != 0) {
          if (accel_x >= 1000 && last_state != 1) {
            printf("\n---------------------------------");
            printf("\nOrdem do traço CORRETA");
            printf("\n---------------------------------\n");

            gpio_put(LED_CORRECT_PIN, 1);
            gpio_put(LED_WRONG_PIN, 0);
            last_state = 1;
          } else if (accel_x < 1000 && last_state != -1) {
            printf("\n---------------------------------");
            printf("\nOrdem do traço ERRADA");
            printf("\n---------------------------------\n");

            gpio_put(LED_WRONG_PIN, 1);
            gpio_put(LED_CORRECT_PIN, 0);

            gpio_put(BUZZER_PIN, 1);
            sleep_ms(200);
            gpio_put(BUZZER_PIN, 0);
            last_state = -1;
          }
        }

        if (accel_z > 3000 && is_writing) { // Pen lifted
          is_writing = false;
          write_begin = false;
          gpio_put(BUZZER_PIN, 0);
          gpio_put(LED_CORRECT_PIN, 0);
          gpio_put(LED_WRONG_PIN, 0);

          printf("\nFim do traço detectado.\n");
          printf("\n---------------------------------\n");
          printf("Até o próximo aprendizado!\n");
        }
      }
    }
  }
}

// Main function
int main() {
  stdio_init_all();  // Initialize serial communication
  init_i2c();
  sleep_ms(500);

  printf("\n---------------------------------\n");
  printf("Bem-vindo ao treinamento de ideogramas!\n");
  printf("Pressione o botão para começar\n");

  // Configure the pins
  gpio_init(BUTTON_PIN);
  gpio_set_dir(BUTTON_PIN, GPIO_IN);
  gpio_pull_down(BUTTON_PIN);

  gpio_init(BUZZER_PIN);
  gpio_set_dir(BUZZER_PIN, GPIO_OUT);

  gpio_init(LED_CORRECT_PIN);
  gpio_set_dir(LED_CORRECT_PIN, GPIO_OUT);

  gpio_init(LED_WRONG_PIN);
  gpio_set_dir(LED_WRONG_PIN, GPIO_OUT);

  // Configure the button interrupt to detect when it is pressed
  gpio_set_irq_enabled_with_callback(BUTTON_PIN, GPIO_IRQ_EDGE_FALL, true, &gpio_callback);

  while (true) {
    if (button_pressed) {
      printf("\nSelecione o ideograma (1 para 一, 2 para 二): ");
      char input_buffer[10]; // Buffer to store user input
      scanf("%9s", input_buffer); // Reads up to 9 characters, preventing buffer overflow
      selected_character = input_buffer[0]; // Takes only the first character

      // Check if number is valid
      if (selected_character == '1' || selected_character == '2') {
        printf("\nIdeograma selecionado: %c\n", selected_character);
        button_pressed = false;
        start_ideogram();
        selected_character = '\0';
      } else {
        selected_character = '\0';
        printf("\nEntrada inválida. Tente novamente.\n");
        sleep_ms(100);
      }
    }

    sleep_ms(100);
  }
}