// Code using timers
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_rom_sys.h"   // esp_rom_delay_us()
#include "driver/i2c.h"

#define CLOCK_PIN 8
#define DATA_LINE_PIN 9
#define I2C_MASTER_NUM I2C_NUM_0
#define OLED_ADDR 0x3C
#define transmissionSpeed 100000 // 100 kHz

const int oledWidth = 128; // Pixels
const int oledHeight = 64;
uint8_t oled_buffer[128 * 8]; // 1024 bytes


void i2c_command_ping(void) {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();

  i2c_master_start(cmd);
  i2c_master_write_byte(
    cmd,
    (OLED_ADDR << 1) | I2C_MASTER_WRITE,
    true
  );
  i2c_master_stop(cmd);

  esp_err_t ret = i2c_master_cmd_begin(
    I2C_MASTER_NUM,
    cmd, 
    pdMS_TO_TICKS(10)
  );

  i2c_cmd_link_delete(cmd);

  if(ret == ESP_OK) {
    printf("OLED detected\n");
  } else {
    printf("OLED not detected\n");
  }

}

void oled_display_on(void) {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (OLED_ADDR << 1) | I2C_MASTER_WRITE, true);
  i2c_master_write_byte(cmd, 0x00, true);
  i2c_master_write_byte(cmd, 0xAF, true);
  i2c_master_stop(cmd);

  i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(10));
  i2c_cmd_link_delete(cmd);
}

void oled_init(void) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();

    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (OLED_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, 0x00, true); // Command mode

    i2c_master_write_byte(cmd, 0xAE, true); // Display OFF
    i2c_master_write_byte(cmd, 0x8D, true); // Charge pump
    i2c_master_write_byte(cmd, 0x14, true); // Enable charge pump
    i2c_master_write_byte(cmd, 0xA1, true); // Segment remap
    i2c_master_write_byte(cmd, 0xC8, true); // COM scan direction
    i2c_master_write_byte(cmd, 0xAF, true); // Display ON

    i2c_master_stop(cmd);
    i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(10));
    i2c_cmd_link_delete(cmd);
}

void oled_set_pixel(uint8_t x, uint8_t y, bool on) {
    if (x >= 128 || y >= 64) return;

    uint8_t page = y / 8;
    uint8_t bit  = y % 8;
    uint16_t index = page * 128 + x;

    if (on)
        oled_buffer[index] |= (1 << bit);
    else
        oled_buffer[index] &= ~(1 << bit);
}

void oled_set_cursor(uint8_t page, uint8_t column) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();

    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (OLED_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, 0x00, true);   // Command mode
    i2c_master_write_byte(cmd, 0xB0 | page, true); // Page
    i2c_master_write_byte(cmd, 0x00 | (column & 0x0F), true);
    i2c_master_write_byte(cmd, 0x10 | (column >> 4), true);
    i2c_master_stop(cmd);

    i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(10));
    i2c_cmd_link_delete(cmd);
}

void oled_update(void) {
    for (uint8_t page = 0; page < 8; page++) {
        oled_set_cursor(page, 0);

        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (OLED_ADDR << 1) | I2C_MASTER_WRITE, true);
        i2c_master_write_byte(cmd, 0x40, true); // Data mode

        for (int col = 0; col < 128; col++) {
            i2c_master_write_byte(
                cmd,
                oled_buffer[page * 128 + col],
                true
            );
        }

        i2c_master_stop(cmd);
        i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(10));
        i2c_cmd_link_delete(cmd);
    }
}

void oled_all_on(void) {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();

  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, (OLED_ADDR << 1) | I2C_MASTER_WRITE, true);
  i2c_master_write_byte(cmd, 0x00, true);
  i2c_master_write_byte(cmd, 0xA5, true);
  i2c_master_stop(cmd);

  i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(10));
  i2c_cmd_link_delete(cmd);
}

void clear_all(void) {
}

void app_main(void) {
    i2c_config_t i2c_conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = DATA_LINE_PIN,
        .scl_io_num = CLOCK_PIN,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = transmissionSpeed,
    };

    ESP_ERROR_CHECK(i2c_param_config(I2C_MASTER_NUM, &i2c_conf));
    ESP_ERROR_CHECK(i2c_driver_install(
        I2C_MASTER_NUM,
        I2C_MODE_MASTER,
        0,
        0,
        0
    ));

    i2c_command_ping();
    oled_init();  // Power + config
    int on = 1;

    memset(oled_buffer, 0x00, sizeof(oled_buffer)); // Clear RAM

    while (1) {
        for(int i = 0; i < 128; i+=2) {
          for(int j = 0; j < 64; j++) {
            oled_set_pixel(i, j+1, true);
            oled_set_pixel(i, j+2, true);
            oled_set_pixel(i, j+3, true);
            oled_set_pixel(i+1, j+1, true);
            oled_set_pixel(i+1, j+2, true);
            oled_set_pixel(i+1, j+3, true);
            oled_set_pixel(i+2, j+1, true);
            oled_set_pixel(i+2, j+2, true);
            oled_set_pixel(i+2, j+3, true);
            oled_update();
            oled_set_pixel(i, j+1, false);
            oled_set_pixel(i, j+2, false);
            oled_set_pixel(i, j+3, false);
            oled_set_pixel(i+1, j+1, false);
            oled_set_pixel(i+1, j+2, false);
            oled_set_pixel(i+1, j+3, false);
            oled_set_pixel(i+2, j+1, false);
            oled_set_pixel(i+2, j+2, false);
            oled_set_pixel(i+2, j+3, false);
          }
        }
    }
}