use embassy_stm32::i2c;
use embassy_stm32::peripherals;
use embassy_stm32::time::Hertz;
use embassy_time::{Duration, Timer};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::Receiver;
use embassy_util::{select, Either};

use embedded_graphics::{
    mono_font::{
        MonoTextStyle,
        {ascii::FONT_5X7, ascii::FONT_7X13},
    },
    pixelcolor::BinaryColor,
    prelude::*,
    primitives::{PrimitiveStyleBuilder, Rectangle},
    text::Text,
};

use heapless::String;
use ufmt::uwrite;

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

use crate::{AzElPair, RotorState};

#[embassy_executor::task]
pub async fn display_task(
    i2c1: peripherals::I2C1,
    sda: peripherals::PB6,
    scl: peripherals::PB7,
    cmd_receiver: Receiver<'static, ThreadModeRawMutex, RotorState, 1>,
) {
    let i2c = i2c::I2c::new(i2c1, sda, scl, Hertz::hz(100_000), i2c::Config::default());

    let interface = I2CDisplayInterface::new(i2c);

    let mut display = Ssd1306::new(interface, DisplaySize128x32, DisplayRotation::Rotate0)
        .into_buffered_graphics_mode();
    display.init().unwrap();

    let text_large = MonoTextStyle::new(&FONT_7X13, BinaryColor::On);
    let text_large_inv = MonoTextStyle::new(&FONT_7X13, BinaryColor::Off);
    let text_small = MonoTextStyle::new(&FONT_5X7, BinaryColor::On);

    let style_filled = PrimitiveStyleBuilder::new()
        .fill_color(BinaryColor::On)
        .build();

    let mut rotor_state = RotorState {
        actual_pos: AzElPair { az: 0, el: 0 },
        setpoint_pos: AzElPair { az: 0, el: 0 },
        stopped: true,
    };

    loop {
        display.clear();

        Text::new("AFG rotor ctrl v0.1.0", Point::new(0, 6), text_small)
            .draw(&mut display)
            .unwrap();

        let mut tmp: String<16> = String::new();
        uwrite!(tmp, "AZ: {}", rotor_state.actual_pos.az).unwrap();
        Text::new(&tmp, Point::new(1, 17), text_large)
            .draw(&mut display)
            .unwrap();

        tmp.clear();
        uwrite!(tmp, "EL: {}", rotor_state.actual_pos.el).unwrap();
        Text::new(&tmp, Point::new(64, 17), text_large)
            .draw(&mut display)
            .unwrap();

        if !rotor_state.stopped {
            Rectangle::new(Point::new(0, 19), Size::new(128, 23))
                .into_styled(style_filled)
                .draw(&mut display)
                .unwrap();
        };

        let setpoint_style = if !rotor_state.stopped {
            text_large_inv
        } else {
            text_large
        };

        tmp.clear();
        uwrite!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
        Text::new(&tmp, Point::new(1, 30), setpoint_style)
            .draw(&mut display)
            .unwrap();

        tmp.clear();
        uwrite!(tmp, "EL: {}", rotor_state.setpoint_pos.el).unwrap();
        Text::new(&tmp, Point::new(64, 30), setpoint_style)
            .draw(&mut display)
            .unwrap();

        display.flush().unwrap();

        let result = select(
            Timer::after(Duration::from_millis(500)),
            cmd_receiver.recv(),
        )
        .await;

        match result {
            Either::First(_) => {}
            Either::Second(new_state) => rotor_state = new_state,
        }
    }
}