#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "unistd.h"

#define I2C_MASTER_SDA_IO 21
#define I2C_MASTER_SCL_IO 22
#define I2C_NUM 0
#define SLAVE_ADDRESS_LCD 0x27

static const char *TAG = "LCD EXAMPLE";

esp_err_t err;
static esp_err_t i2c_master_init(void)
{
  i2c_config_t conf = {
    .mode = I2C_MODE_MASTER,
    .sda_io_num = GPIO_NUM_21,
    .scl_io_num = GPIO_NUM_22,
    .sda_pullup_en = GPIO_PULLUP_ENABLE,
    .scl_pullup_en = GPIO_PULLUP_ENABLE,
    .master.clk_speed = 100000,
  };

  i2c_param_config(I2C_NUM, &conf);
  return i2c_driver_install(I2C_NUM, conf.mode, 0, 0, 0);
}

void lcd_send_cmd(char cmd)
{
    char data_u, data_l;
    uint8_t data_t[4];
    data_u = (cmd & 0xf0);
    data_l = ((cmd << 4) & 0xf0);
    data_t[0] = data_u | 0x0C;  // en=1, rs=0
    data_t[1] = data_u | 0x08;  // en=0, rs=0
    data_t[2] = data_l | 0x0C;  // en=1, rs=0
    data_t[3] = data_l | 0x08;  // en=0, rs=0
    err = i2c_master_write_to_device(I2C_NUM, SLAVE_ADDRESS_LCD, data_t, 4, pdMS_TO_TICKS(1000));
    if (err != ESP_OK) {
        ESP_LOGI(TAG, "Error in sending command: %s", esp_err_to_name(err));
    } else {
        ESP_LOGI(TAG, "Command 0x%02X sent successfully", cmd);
    }
}

void lcd_send_data(char data)
{
    char data_u, data_l;
    uint8_t data_t[4];
    data_u = (data & 0xf0);
    data_l = ((data << 4) & 0xf0);
    data_t[0] = data_u | 0x0D;  // en=1, rs=1
    data_t[1] = data_u | 0x09;  // en=0, rs=1
    data_t[2] = data_l | 0x0D;  // en=1, rs=1
    data_t[3] = data_l | 0x09;  // en=0, rs=1
    err = i2c_master_write_to_device(I2C_NUM, SLAVE_ADDRESS_LCD, data_t, 4, pdMS_TO_TICKS(1000));
    if (err != ESP_OK) {
        ESP_LOGI(TAG, "Error in sending data: %s", esp_err_to_name(err));
    } else {
        ESP_LOGI(TAG, "Data 0x%02X sent successfully", data);
    }
}

void lcd_clear (void)
{
	lcd_send_cmd (0x01);
	usleep(5000);
}

void lcd_put_cur(int row, int col)
{
  switch (row)
  {
    case 0:
      col |= 0x80;
      break;
    case 1:
      col |= 0xC0;
      break;
  }

  lcd_send_cmd (col);
}


void lcd_init(void)
{
  // Wait for the LCD to power up
  vTaskDelay(pdMS_TO_TICKS(40));  // wait for >40ms

  // Initialization sequence
  lcd_send_cmd(0x30);
  vTaskDelay(pdMS_TO_TICKS(5));  // wait for >4.1ms

  lcd_send_cmd(0x30);
  vTaskDelay(pdMS_TO_TICKS(1));  // wait for >100us

  lcd_send_cmd(0x30);
  vTaskDelay(pdMS_TO_TICKS(10));  // wait for >10ms

  lcd_send_cmd(0x20);  // Set to 4-bit mode
  vTaskDelay(pdMS_TO_TICKS(10));  // wait for >10ms

  // Function set: DL = 0 (4-bit mode), N = 1 (2-line display), F = 0 (5x8 characters)
  lcd_send_cmd(0x28);
  vTaskDelay(pdMS_TO_TICKS(1));  // wait for >37us

  // Display control: D = 0, C = 0, B = 0 (display off)
  lcd_send_cmd(0x08);
  vTaskDelay(pdMS_TO_TICKS(1));  // wait for >37us

  // Clear display
  lcd_send_cmd(0x01);
  vTaskDelay(pdMS_TO_TICKS(2));  // wait for >1.52ms

  // Entry mode set: I/D = 1 (increment cursor), S = 0 (no shift)
  lcd_send_cmd(0x06);
  vTaskDelay(pdMS_TO_TICKS(1));  // wait for >37us

  // Display control: D = 1 (display on), C = 0, B = 0 (cursor off, blink off)
  lcd_send_cmd(0x0C);
  vTaskDelay(pdMS_TO_TICKS(1));  // wait for >37us
}


void lcd_send_string (char *str)
{
	while (*str) lcd_send_data (*str++);
}

void i2c_scanner()
{
  printf("Scanning I2C bus...\n");
  for (int i = 1; i < 100; i++) {
    int address = i;
    esp_err_t res = i2c_master_write_to_device(I2C_NUM, address, NULL, 0, pdMS_TO_TICKS(1000));
    if (res == ESP_OK) {
      printf("Found device at address 0x%02x\n", address);
      break;
    }
  }
  printf("Scan complete.\n");
}



void app_main(void)
{
  char buffer[10];
  float num = 12.34;

  ESP_LOGI(TAG, "test");
  printf("hola");

  ESP_ERROR_CHECK(i2c_master_init());
  ESP_LOGI(TAG, "I2C initialized successfully");

  lcd_init();

  lcd_clear();

  lcd_put_cur(0, 0);
  lcd_send_string("Hello world!");

  lcd_put_cur(1, 0);
  lcd_send_string("from ESP32");

  sprintf(buffer, "val=%.2f", num);
  lcd_put_cur(0, 0);
  lcd_send_string(buffer);


}