#include "ssd1306.h"

#include "esp_log.h"

#include "driver/gpio.h"
#include "driver/i2c.h"
#include <driver/gpio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/task.h"

#include "config.h"

#ifdef ENABLE_SSD1306


esp_err_t oled_ssd1306_init() {

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);

    i2c_master_write_byte(cmd, (SSD1306_OLED_ADDR << 1) | WRITE_BIT, ACK_CHECK_EN);

    // Initialization (page 64)
    // The next bytes are commands
    i2c_master_write_byte(cmd, COMMAND_MODE, ACK_CHECK_EN);

    // Mux Ratio
    i2c_master_write_byte(cmd, MULTIPLEX_RATIO, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x3F, ACK_CHECK_EN);

    // Set display offset
    i2c_master_write_byte(cmd, DISPLAY_OFFSET, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x00, ACK_CHECK_EN);

    // Set display line start
    i2c_master_write_byte(cmd, DISPLAY_LINE_START, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x00, ACK_CHECK_EN);

    // Set Segment re-map
    i2c_master_write_byte(cmd, SEGMENT_REMAP, ACK_CHECK_EN);

    // Set COM output scan dir
    i2c_master_write_byte(cmd, COM_OUTPUT_SCAN_DIR, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x00, ACK_CHECK_EN);

    // Set COM pins hardware config
    i2c_master_write_byte(cmd, COM_PINS_HARDWARE_CONFIG, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x12, ACK_CHECK_EN);

    // Set contrast Control
    i2c_master_write_byte(cmd, CONTRAST_CONTROL, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x7F, ACK_CHECK_EN);

    // Disable entire display ON
    i2c_master_write_byte(cmd, DISABLE_ENTIRE_DISPLAY, ACK_CHECK_EN);

    // Set normal display
    i2c_master_write_byte(cmd, NORMAL_DISPLAY, ACK_CHECK_EN);

    // Set OSC frequency
    i2c_master_write_byte(cmd, DISPLAY_CLK_RATIO, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x80, ACK_CHECK_EN);

    // Enable charge pump regulator
    i2c_master_write_byte(cmd, CHARGE_PUMP_SET, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x14, ACK_CHECK_EN);

    // Display on
    i2c_master_write_byte(cmd, DISPLAY_ON, ACK_CHECK_EN);

    // Stop bit
    i2c_master_stop(cmd);

    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);

    return ret;
}




void i2c_master_init() {

    int i2c_master_port = I2C_MASTER_NUM;
    i2c_config_t conf;

    conf.mode = I2C_MODE_MASTER;
    conf.sda_io_num = I2C_SDA_PIN;               // Data pin
    conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
    conf.scl_io_num = I2C_CLK_PIN;               // Clock pin
    conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
    conf.master.clk_speed = 100000;
    i2c_param_config(i2c_master_port, &conf);
    i2c_driver_install(i2c_master_port,
                        conf.mode,
                        I2C_MASTER_RX_BUF_DISABLE,
                        I2C_MASTER_TX_BUF_DISABLE,
                        0);
}

#endif