#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "soc/gpio_reg.h"
#include "driver/i2c.h"

#define I2C_DATA_PIN       4
#define I2C_CLOCK_PIN      0
#define I2C_PORT           I2C_NUM_0
#define SCREEN_ADDR        0x3C

i2c_cmd_handle_t start_link() {
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();

  i2c_master_start(cmd);

  i2c_master_write_byte(cmd, (SCREEN_ADDR << 1), I2C_MASTER_ACK);

  return cmd;
}

void execute_link(i2c_cmd_handle_t cmd) {
  i2c_master_stop(cmd);

  i2c_master_cmd_begin(I2C_PORT, cmd, pdMS_TO_TICKS(50));

  i2c_cmd_link_delete(cmd);

}

void add_command_group(i2c_cmd_handle_t i2c_cmd, uint8_t cmd) {
  // Control byte has Co bit = 1 and the rest of the bits 0
  uint8_t control_byte = 1 << 7;
  
  i2c_master_write_byte(i2c_cmd, control_byte, I2C_MASTER_ACK);
  i2c_master_write_byte(i2c_cmd, cmd, I2C_MASTER_ACK);
}

void add_data_group(i2c_cmd_handle_t i2c_cmd, uint8_t *data, int data_length) {
  // Control byte has Co bit = 0, D/C# bit = 1, and the rest of the bits 0
  uint8_t control_byte = 1 << 6;
  
  i2c_master_write_byte(i2c_cmd, control_byte, I2C_MASTER_ACK);

  i2c_master_write(i2c_cmd, data, data_length, I2C_MASTER_ACK);

}

void send_commands(uint8_t *screen_cmds, int len) {

  i2c_cmd_handle_t cmd = start_link();

  for(int i = 0; i<len; i++) {
    add_command_group(cmd, screen_cmds[i]);
  }

  execute_link(cmd);

}

void clear_screen() {
  i2c_cmd_handle_t cmd = start_link();

  // Data Control byte has Co bit = 0, D/C# bit = 1, and the rest of the bits 0
  uint8_t control_byte = 1 << 6;
  i2c_master_write_byte(cmd, control_byte, I2C_MASTER_ACK);

  for(int i = 0; i<1024; i++) {
    i2c_master_write_byte(cmd, 0, I2C_MASTER_ACK);
  }

  execute_link(cmd);

}

void fill_page(int page_nr) {
  i2c_cmd_handle_t cmd = start_link();

  // Data Control byte has Co bit = 0, D/C# bit = 1, and the rest of the bits 0
  uint8_t control_byte = 1 << 6;
  i2c_master_write_byte(cmd, control_byte, I2C_MASTER_ACK);

  for(int p=0; p<8; p++) {
    for(int i = 0; i<128; i++) {
      i2c_master_write_byte(cmd, (p==page_nr? 255: 0), I2C_MASTER_ACK);
    }
  }
  execute_link(cmd);

}

void app_main(void)
{
  i2c_config_t i2c_conf = {
    .mode = I2C_MODE_MASTER,
    .sda_io_num = I2C_DATA_PIN,         // select SDA GPIO specific to your project
    .scl_io_num = I2C_CLOCK_PIN,         // select SCL GPIO specific to your project
    .sda_pullup_en = GPIO_PULLUP_ENABLE,
    .scl_pullup_en = GPIO_PULLUP_ENABLE,
    .master.clk_speed = 400*1000,         // 400kHz, supported by screen
  };
  i2c_param_config(I2C_PORT, &i2c_conf);
  i2c_driver_install(I2C_PORT, i2c_conf.mode, 0, 0, 0);

  uint8_t init_commands[] = {
    0xAE, 
    0xD5, 
    0x80, 
    0xA8, 
    0x3F, 
    0xD3, 
    0x00, 
    0x40, 
    0x8D, 
    0x14, 
    0xAD, 
    0x30, 
    0xA1, 
    0xC8, 
    0xDA, 
    0x12, 
    0x81, 
    0xFF, 
    0xD9, 
    0x22, 
    0xDB, 
    0x30, 
    0xA4, 
    0xA6,
    0xAF,
    0x20, // Set horizontal addressing mode
    0x00,
  };
  send_commands(init_commands, 27);

  clear_screen();

  int page = 0;

  while (true) {
    fill_page(page);
    page = (page + 1)%8;
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}