#include <driver/i2c.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <stdio.h>
#include "sdkconfig.h"
#include "rom/ets_sys.h"
#include <esp_log.h>

#define LCD_LINEONE             0x00
#define LCD_LINETWO             0x40
#define LCD_LINETHREE           0x14
#define LCD_LINEFOUR            0x54

#define LCD_BACKLIGHT           0x08
#define LCD_ENABLE              0x04
#define LCD_COMMAND             0x00
#define LCD_WRITE               0x01

#define LCD_SET_DDRAM_ADDR      0x80
#define LCD_READ_BF             0x40

#define LCD_CLEAR               0x01
#define LCD_HOME                0x02
#define LCD_ENTRY_MODE          0x06
#define LCD_DISPLAY_OFF         0x08
#define LCD_DISPLAY_ON          0x0C
#define LCD_FUNCTION_RESET      0x30
#define LCD_FUNCTION_SET_4BIT   0x28
#define LCD_SET_CURSOR          0x80

static uint8_t LCD_addr;
static uint8_t SDA_pin;
static uint8_t SCL_pin;
static uint8_t LCD_cols;
static uint8_t LCD_rows;

static void LCD_writeNibble(uint8_t nibble, uint8_t mode);
static void LCD_writeByte(uint8_t data, uint8_t mode);
static void LCD_pulseEnable(uint8_t nibble);

#define LCD_ADDR 0x27
#define SDA_PIN  21
#define SCL_PIN  22
#define LCD_COLS 16
#define LCD_ROWS 2

void LCD_Task(void* param)
{
  char num[20];
  while (true) {
    LCD_home();
    LCD_clearScreen();
    LCD_writeStr("BCSPN");
    vTaskDelay(pdMS_TO_TICKS(3000));
    LCD_clearScreen();
    LCD_writeStr("Lets Count 0-10!");
    vTaskDelay(pdMS_TO_TICKS(3000));
    LCD_clearScreen();
    for (int i = 0; i <= 10; i++) {
      LCD_setCursor(8, 1);
      sprintf(num, "%d", i);
      LCD_writeStr(num);
      vTaskDelay(pdMS_TO_TICKS(1000));
    }

  }
}

void app_main(void)
{
  LCD_init(LCD_ADDR, SDA_PIN, SCL_PIN, LCD_COLS, LCD_ROWS);
  xTaskCreate(&LCD_Task, "LCD Task", 2048, NULL, 5, NULL);

}


static esp_err_t I2C_init(void)
{
  i2c_config_t conf = {
    .mode = I2C_MODE_MASTER,
    .sda_io_num = SDA_pin,
    .scl_io_num = SCL_pin,
    .sda_pullup_en = GPIO_PULLUP_ENABLE,
    .scl_pullup_en = GPIO_PULLUP_ENABLE,
    .master.clk_speed = 100000
  };
  i2c_param_config(I2C_NUM_0, &conf);
  i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0);
  return ESP_OK;
}

void LCD_init(uint8_t addr, uint8_t dataPin, uint8_t clockPin, uint8_t cols, uint8_t rows)
{
  LCD_addr = addr;
  SDA_pin = dataPin;
  SCL_pin = clockPin;
  LCD_cols = cols;
  LCD_rows = rows;
  I2C_init();
  vTaskDelay(pdMS_TO_TICKS(100));                                 // Initial 40 mSec delay
  // Reset the LCD controller
  LCD_writeNibble(LCD_FUNCTION_RESET, LCD_COMMAND);                   // First part of reset sequence
  vTaskDelay(pdMS_TO_TICKS(10));                                  // 4.1 mS delay (min)
  LCD_writeNibble(LCD_FUNCTION_RESET, LCD_COMMAND);                   // second part of reset sequence
  ets_delay_us(200);                                                  // 100 uS delay (min)
  LCD_writeNibble(LCD_FUNCTION_RESET, LCD_COMMAND);                   // Third time's a charm
  LCD_writeNibble(LCD_FUNCTION_SET_4BIT, LCD_COMMAND);                // Activate 4-bit mode
  ets_delay_us(80);                                                   // 40 uS delay (min)

  // --- Busy flag now available ---
  // Function Set instruction
  LCD_writeByte(LCD_FUNCTION_SET_4BIT, LCD_COMMAND);                  // Set mode, lines, and font
  ets_delay_us(80);

  // Clear Display instruction
  LCD_writeByte(LCD_CLEAR, LCD_COMMAND);                              // clear display RAM
  vTaskDelay(pdMS_TO_TICKS(2));                                   // Clearing memory takes a bit longer

  // Entry Mode Set instruction
  LCD_writeByte(LCD_ENTRY_MODE, LCD_COMMAND);                         // Set desired shift characteristics
  ets_delay_us(pdMS_TO_TICKS(80));

  LCD_writeByte(LCD_DISPLAY_ON, LCD_COMMAND);                         // Ensure LCD is set to on
}

void LCD_setCursor(uint8_t col, uint8_t row)
{
  if (row > LCD_rows - 1) {
    printf("Cannot write to row %d. Please select a row in the range (0, %d)", row, LCD_rows - 1);
    row = LCD_rows - 1;
  }
  uint8_t row_offsets[] = {LCD_LINEONE, LCD_LINETWO, LCD_LINETHREE, LCD_LINEFOUR};
  LCD_writeByte(LCD_SET_DDRAM_ADDR | (col + row_offsets[row]), LCD_COMMAND);
}

void LCD_writeChar(char c)
{
  LCD_writeByte(c, LCD_WRITE);                                        // Write data to DDRAM
}

void LCD_writeStr(char* str)
{
  while (*str) {
    LCD_writeChar(*str++);
  }
}

void LCD_home(void)
{
  LCD_writeByte(LCD_HOME, LCD_COMMAND);
  vTaskDelay(pdMS_TO_TICKS(2));
}

void LCD_clearScreen(void)
{
  LCD_writeByte(LCD_CLEAR, LCD_COMMAND);
  vTaskDelay(pdMS_TO_TICKS(2));
}

static void LCD_writeNibble(uint8_t nibble, uint8_t mode)
{
  uint8_t data = (nibble & 0xF0) | mode | LCD_BACKLIGHT;
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (LCD_addr << 1) | I2C_MASTER_WRITE, 1);
  i2c_master_write_byte(cmd, data, 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(1000));
  i2c_cmd_link_delete(cmd);
  LCD_pulseEnable(data);
}

static void LCD_writeByte(uint8_t data, uint8_t mode)
{
  LCD_writeNibble(data & 0xF0, mode);
  LCD_writeNibble((data << 4) & 0xF0, mode);
}

static void LCD_pulseEnable(uint8_t data)
{
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (LCD_addr << 1) | I2C_MASTER_WRITE, 1);
  i2c_master_write_byte(cmd, data | LCD_ENABLE, 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(1000));
  i2c_cmd_link_delete(cmd);
  ets_delay_us(1);
  cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (LCD_addr << 1) | I2C_MASTER_WRITE, 1);
  i2c_master_write_byte(cmd, (data & ~LCD_ENABLE), 1);
  i2c_master_stop(cmd);
  i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(1000));
  i2c_cmd_link_delete(cmd);
  ets_delay_us(500);
}