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

// Sensor
#include "hardware/i2c.h"

// LoRa
#include <string.h>
#include "hardware/spi.h"

// SD Card
#include "sd_driver.h"
#include "simplefs.h"

// Sensor
#define I2C_PORT i2c0
#define VL53L0X_ADDR 0x29
#define SDA_PIN 4
#define SCL_PIN 5

// LoRa
#define SPI_PORT spi0
#define PIN_MISO 16
#define PIN_CS   17
#define PIN_SCK  18
#define PIN_MOSI 19
int id_alerta = 0;
char conv_msg_to_send[72];

char conv_msg_to_storage[72];
char conv_is_storage[6];
int id_leitura = 0;

void update_data_on_sd_card(){
  if (simplefs_append("storage_data.txt", conv_msg_to_storage) == 0) {
      printf("Conteúdo adicionado com sucesso no arquivo storage_data.txt!\n");
  }

  char buf[4096];
  if (simplefs_read("storage_data.txt", buf, sizeof(buf)) == 0) {
    printf("---------- Conteúdo lido do arquivo storage_data.txt: ------------\n");
    printf("%s", buf);
    printf("------------------------------------------------------------------\n\n");
  }
}

void spi_write_register(uint8_t reg, const char* message) {
    size_t len = strlen(message);
    uint8_t buf[1 + len];
    buf[0] = 0x80 | reg;
    memcpy(&buf[1], message, len);

    gpio_put(PIN_CS, 0);
    spi_write_blocking(SPI_PORT, buf, len + 1);
    gpio_put(PIN_CS, 1);
}

void spi_read_fifo(char* buffer, size_t max_len) {
    uint8_t tx[max_len + 1];
    uint8_t rx[max_len + 1];
    memset(tx, 0, sizeof(tx));

    tx[0] = 0x00; // Comando de leitura do registrador FIFO (0x00)

    gpio_put(PIN_CS, 0);
    spi_write_read_blocking(SPI_PORT, tx, rx, max_len + 1);
    gpio_put(PIN_CS, 1);

    size_t i;
    for (i = 0; i < max_len; i++) {
        if (rx[i + 1] == 0x00) break; // fim da mensagem
        buffer[i] = rx[i + 1];
    }
    buffer[i] = '\0';
}

void vl53l0x_trigger_measurement() {
    uint8_t cmd = 0x00; // Qualquer comando simula trigger no chip personalizado
    i2c_write_blocking(I2C_PORT, VL53L0X_ADDR, &cmd, 1, false);
    sleep_ms(50); // tempo de medição simulado
}

bool vl53l0x_read_distance(uint16_t *distance_mm) {
    uint8_t data[2];
    int res = i2c_read_blocking(I2C_PORT, VL53L0X_ADDR, data, 2, false);
    if (res != 2) return false;

    *distance_mm = ((uint16_t)data[0] << 8) | data[1];
    return true;
}

int main() {
    stdio_init_all();

    // Inicializa a comunicação SPI com o sensor
    i2c_init(I2C_PORT, 100 * 1000); // 100kHz
    gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(SDA_PIN);
    gpio_pull_up(SCL_PIN);

    // Inicializa a comunicação SPI com o módulo LoRa
    spi_init(SPI_PORT, 500 * 1000); // 500 kHz
    gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
    gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);
    gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);

    gpio_init(PIN_CS);
    gpio_set_dir(PIN_CS, GPIO_OUT);
    gpio_put(PIN_CS, 1);

    // Inicializa o SD Card
    printf("Inicializando SD Card...\n");
    if (sd_init() != 0) {
        printf("Erro ao iniciar SD Card\n");
        return 1;
    }

    simplefs_init();

    sleep_ms(100);

    while (true) {
        vl53l0x_trigger_measurement();

        uint16_t distance;
        if (vl53l0x_read_distance(&distance)) {
            // printf("Distance: %u mm\n", distance);
            if (distance < 100) {
              // printf(">>> ALERTA: Objeto detectado a menos de 10 cm <<<\n");
              sprintf(conv_msg_to_send, "ID%i) ALERTA: Objeto detectado a menos de 10 cm. Distancia atual: %u mm\n", id_alerta, distance);
              spi_write_register(0x00, conv_msg_to_send);
              // printf("Mensagem enviada: %s\n", conv_msg_to_send);
              id_alerta += 1;
            }
        } else {
            printf("Falha na leitura do VL53L0X\n");
        }

        // Verifica se recebeu alguma mensagem
        char recv[72];
        spi_read_fifo(recv, sizeof(recv) - 1);
        sprintf(conv_is_storage, "ID%i", id_leitura);
        
        if (strstr(recv, conv_is_storage) != NULL){
          printf("Mensagem recebida: %s\n", recv);
          sprintf(conv_msg_to_storage, "%s", recv);
          id_leitura += 1;
          update_data_on_sd_card();
        }

        sleep_ms(2000);
    }
}