use ssd1306::{mode::TerminalMode, prelude::*, I2CDisplayInterface, Ssd1306};

use std::fmt::Write;

use esp_idf_hal::delay::{FreeRtos, BLOCK};
use esp_idf_hal::gpio::{Gpio4, Gpio5};
use esp_idf_hal::i2c::*;
use esp_idf_hal::peripherals::Peripherals;
use esp_idf_hal::prelude::*;
use esp_idf_hal::sys::{heap_caps_get_free_size, MALLOC_CAP_8BIT};

use embedded_graphics::{
    mono_font::{ascii::FONT_6X10, MonoTextStyleBuilder},
    pixelcolor::BinaryColor,
    prelude::*,
    text::{Baseline, Text},
};

const SSD1306_ADDRESS: u8 = 0x3c;

fn main() -> anyhow::Result<()> {
    // It is necessary to call this function once. Otherwise some patches to the runtime
    // implemented by esp-idf-sys might not link properly. See https://github.com/esp-rs/esp-idf-template/issues/71
    esp_idf_svc::sys::link_patches();

    // Bind the log crate to the ESP Logging facilities
    esp_idf_svc::log::EspLogger::initialize_default();

    let free_mem = unsafe { heap_caps_get_free_size(MALLOC_CAP_8BIT) };
    println!("Free memory: {} kb", free_mem / 1024);

  let peripherals = Peripherals::take()?;
    let i2c = peripherals.i2c0;
    let sda = peripherals.pins.gpio21;
    let scl = peripherals.pins.gpio26;

    println!("Starting I2C SSD1306 test");

    let config = I2cConfig::new().baudrate(100.kHz().into());
    let mut i2c = I2cDriver::new(i2c, sda, scl, &config)?;

    // initialze the display - don't worry about the meaning of these bytes - it's specific to SSD1306
    i2c.write(SSD1306_ADDRESS, &[0, 0xae], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xd4], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x80], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xa8], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x3f], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xd3], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x00], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x40], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x8d], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x14], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xa1], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xc8], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xda], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x12], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x81], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xcf], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xf1], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xdb], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x40], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xa4], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xa6], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0xaf], BLOCK)?;
    i2c.write(SSD1306_ADDRESS, &[0, 0x20, 0x00], BLOCK)?;

    // fill the display
    for _ in 0..64 {
        let data: [u8; 17] = [
            0x40, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
            0xff, 0xff, 0xff,
        ];
        i2c.write(SSD1306_ADDRESS, &data, BLOCK)?;
    }

    loop {
        // we are sleeping here to make sure the watchdog isn't triggered
        FreeRtos::delay_ms(500);
        i2c.write(SSD1306_ADDRESS, &[0, 0xa6], BLOCK)?;
        FreeRtos::delay_ms(500);
        i2c.write(SSD1306_ADDRESS, &[0, 0xa7], BLOCK)?;
    }
}