oled 0.0.0

use display_interface::{DataFormat, DisplayError, WriteOnlyDataCommand};
use embedded_graphics_core::{
    prelude::{DrawTarget, GrayColor, OriginDimensions, Point, Size},
    Pixel,
};
use embedded_hal_1::{delay::DelayUs, digital::OutputPin};

use crate::pixelcolor::Gray5;

/// SSD1357 128 RGB x 128 Dot Matrix
/// OLED/PLED Segment/Common Driver with Controller
#[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct SSD1357<DI> {
    di: DI,
}

impl<DI> SSD1357<DI>
where
    DI: WriteOnlyDataCommand,
{
    pub fn new(di: DI) -> Self {
        Self { di }
    }

    pub fn hard_reset(
        &mut self,
        reset: &mut impl OutputPin,
        delay: &mut impl DelayUs,
    ) -> Result<(), ()> {
        reset.set_low();
        delay.delay_ms(10);
        reset.set_high();
        delay.delay_ms(200);
        Ok(())
    }

    pub fn reset(&mut self, delay: &mut impl DelayUs) -> Result<(), ()> {
        Ok(())
    }

    pub fn init(&mut self, delay: &mut impl DelayUs) -> Result<(), DisplayError> {
        // Unlock Basic Command
        self.di.send_commands(DataFormat::U8(&[0xFD]))?;
        self.di.send_data(DataFormat::U8(&[0x12]))?;

        self.di.send_commands(DataFormat::U8(&[0xAE]))?; // display off

        self.di.send_commands(DataFormat::U8(&[0xB1]))?; // Phase 1 and 2 period adjustment
        self.di.send_data(DataFormat::U8(&[0xff]))?;

        self.di.send_commands(DataFormat::U8(&[0xB3]))?; // Front Clock Divider (DivSet)/ Oscillator Frequency
        self.di.send_data(DataFormat::U8(&[0xF0]))?; // max freq, no div

        self.di.send_commands(DataFormat::U8(&[0xB6]))?; // CMD_SetSecondPrechargePeriod
        self.di.send_data(DataFormat::U8(&[0b1000]))?; // N DCLKS, reset default

        self.di.send_commands(DataFormat::U8(&[0xC1]))?; // must be the same for gray scale
        self.di.send_data(DataFormat::U8(&[0x7f, 0x7f, 0x7f]))?;

        self.di.send_commands(DataFormat::U8(&[0xC7]))?; // master contrast
        self.di.send_data(DataFormat::U8(&[0x0F]))?;

        // LUT
        // B9, B8, BC, BD
        self.di.send_commands(DataFormat::U8(&[0xB8]))?; // 63 bytes
        self.di.send_data(DataFormat::U8(&[
            0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E,
            0x0F, 0x10, 0x11, 0x12, 0x13, 0x15, 0x17, 0x19, 0x1B, 0x1D, 0x1F, 0x21, 0x23, 0x25,
            0x27, 0x2A, 0x2D, 0x30, 0x33, 0x36, 0x39, 0x3C, 0x3F, 0x42, 0x45, 0x48, 0x4C, 0x50,
            0x54, 0x58, 0x5C, 0x60, 0x64, 0x68, 0x6C, 0x70, 0x74, 0x78, 0x7D, 0x82, 0x87, 0x8C,
            0x91, 0x96, 0x9B, 0xA0, 0xA5, 0xAA, 0xAF,
        ]))?;

        // set_precharge_voltage
        self.di.send_commands(DataFormat::U8(&[0xBB]))?;
        self.di.send_data(DataFormat::U8(&[0x1E]))?; // reset default, 0.5xVcc

        // vcomh
        self.di.send_commands(DataFormat::U8(&[0xBE]))?;
        self.di.send_data(DataFormat::U8(&[0x07]))?; // 0.86xVcc

        // set_column_address
        self.di.send_commands(DataFormat::U8(&[0x15]))?;
        self.di.send_data(DataFormat::U8(&[0x00, 0x7F]))?;
        // set_row_address
        self.di.send_commands(DataFormat::U8(&[0x75]))?;
        self.di.send_data(DataFormat::U8(&[0x00, 0x7F]))?;

        self.di.send_commands(DataFormat::U8(&[0xCA]))?; // mux ratio
        self.di.send_data(DataFormat::U8(&[95]))?; //  4 to 127, real height -1

        self.di.send_commands(DataFormat::U8(&[0xA1]))?; // display start line
        self.di.send_data(DataFormat::U8(&[0x00]))?;
        self.di.send_commands(DataFormat::U8(&[0xA2]))?; // display offset, y
        self.di.send_data(DataFormat::U8(&[0x00]))?;

        // Set Re-map & Dual COM Line Mode (A0h)
        // 0bxx_0_xxxxx  Disable COM Split Odd Even
        // 0bxx_x_xx1xx
        // 0b01_1_10110
        self.di.send_commands(DataFormat::U8(&[0xA0]))?;
        // 0b01_1_00000
        self.di.send_data(DataFormat::U8(&[0b01_1_00000, 0x00]))?;

        // 0xA4 => Entire Display Off, All Pixels Turn Off
        // 0xA5 => Entire Display On, All Pixels Turn On at GS Level 15
        // 0xA6 => Normal Display
        // 0xA7 => Inverse Display
        self.di.send_commands(DataFormat::U8(&[0xA6]))?;

        self.di.send_commands(DataFormat::U8(&[0xAF]))?;

        Ok(())
    }

    pub fn update(&mut self, fb: &Framebuffer565) -> Result<(), DisplayError> {
        // column scan
        self.di.send_commands(DataFormat::U8(&[0x15]))?;
        self.di.send_data(DataFormat::U8(&[32, 32 + 66 - 1]))?;
        // row scan
        self.di.send_commands(DataFormat::U8(&[0x75]))?;
        self.di.send_data(DataFormat::U8(&[0, 96 - 1]))?; // same as MUX ratio

        // write_ram
        self.di.send_commands(DataFormat::U8(&[0x5C]))?;
        //di.send_data(DataFormat::U8(&[0xff; 200 * 96 - 1]));
        //di.send_data(DataFormat::U16(&fb.data[..]));
        // 1, 3 , 2
        // 0b00010_000000_00000 ??
        self.di.send_data(DataFormat::U8(&fb.data[..]))?;
        Ok(())
    }
}

const WIDTH: usize = 66 * 3; // 196 pixels = 65*3 + 1
const HEIGHT: usize = 96;
const WIDTH_UNCOMPRESSED: usize = 66;
pub struct Framebuffer565 {
    data: [u8; WIDTH_UNCOMPRESSED * HEIGHT * 2],
}

impl Framebuffer565 {
    pub fn new() -> Self {
        Self {
            data: [0; WIDTH_UNCOMPRESSED * HEIGHT * 2],
        }
    }
}

impl OriginDimensions for Framebuffer565 {
    fn size(&self) -> Size {
        Size::new(196, HEIGHT as _)
    }
}

impl DrawTarget for Framebuffer565 {
    type Color = Gray5;
    type Error = core::convert::Infallible;

    fn draw_iter<I>(&mut self, pixels: I) -> Result<(), Self::Error>
    where
        I: IntoIterator<Item = Pixel<Self::Color>>,
    {
        for Pixel(Point { x, y }, color) in pixels.into_iter() {
            let x = x as usize;
            let y = y as usize;

            if y >= HEIGHT {
                continue;
            }
            if x >= WIDTH {
                continue;
            }

            let color = color.luma() as u8;
            let index = (y * WIDTH_UNCOMPRESSED + x / 3) * 2;

            if x % 3 == 2 {
                let raw_pix = self.data[index];
                self.data[index] = (raw_pix & 0b00000_111) | (color << 3);
            } else if x % 3 == 1 {
                let raw_pix_h = self.data[index];
                self.data[index] = (raw_pix_h & 0b11111_000) | (color >> 2);
                let raw_pix_l = self.data[index + 1];
                self.data[index + 1] = (raw_pix_l & 0b000_11111) | (color << 6);
            } else {
                let raw_pix = self.data[index + 1];
                self.data[index + 1] = (raw_pix & 0b111_00000) | color;
            }
        }
        Ok(())
    }
}