//! Blinks an LED
//!
//! This assumes that a LED is connected to the pin assigned to `led`. (GPIO4)

#![no_std]
#![no_main]
#![feature(type_alias_impl_trait)]

extern crate embassy_executor;

use embassy_executor::Spawner;
use embassy_sync::{blocking_mutex::raw::NoopRawMutex};
use embassy_sync::pubsub::{PubSubChannel, Publisher, Subscriber, WaitResult};
use embassy_time::{Duration, Timer};
use esp_backtrace as _;
use esp_println::println;
use hal::gpio::{Gpio4, Gpio12, Gpio2, Input, Io, Level, Output, Pull};
use hal::{
    clock::ClockControl,
    delay::Delay,
    peripherals::Peripherals,
    prelude::*,
    system::SystemControl,
};
use static_cell::make_static;

extern crate alloc;
use core::mem::MaybeUninit;

#[global_allocator]
static ALLOCATOR: esp_alloc::EspHeap = esp_alloc::EspHeap::empty();

fn init_heap() {
    const HEAP_SIZE: usize = 32 * 1024;
    static mut HEAP: MaybeUninit<[u8; HEAP_SIZE]> = MaybeUninit::uninit();

    unsafe {
        ALLOCATOR.init(HEAP.as_mut_ptr() as *mut u8, HEAP_SIZE);
    }
}
#[main]
async fn main(spawner: Spawner) {
    let peripherals = Peripherals::take();
    let system = SystemControl::new(peripherals.SYSTEM);

    let clocks = ClockControl::max(system.clock_control).freeze();
    let _delay = Delay::new(&clocks);
    init_heap();

    // Set GPIO4 as an output, and set its state high initially.
    let io = Io::new(peripherals.GPIO, peripherals.IO_MUX);
    let button_pin = Input::new(io.pins.gpio12, Pull::Down);
    let led_pin = Output::new(io.pins.gpio4, Level::Low);
    let relay_pin = Output::new(io.pins.gpio2, Level::Low);


    // led_pin.set_high();

    // Initialize the Delay peripheral, and use it to toggle the LED state in a
    // loop.
    println!("Hello world!");
    let channel: &'static mut PubSubChannel<
        NoopRawMutex,
        Command,
        QUEUE_SIZE,
        QUEUE_SIZE,
        QUEUE_SIZE,
    > = make_static!(PubSubChannel::new());

    let publisher = channel.publisher().unwrap();
    let led_subscriber = channel.subscriber().unwrap();
    let relay_subscriber = channel.subscriber().unwrap();

    hal::interrupt::enable(
        hal::peripherals::Interrupt::GPIO,
        hal::interrupt::Priority::Priority1,
    )
    .unwrap();

    println!("We are connected!");

    let timer_group0 = hal::timer::timg::TimerGroup::new_async(peripherals.TIMG0, &clocks);
    esp_hal_embassy::init(&clocks, timer_group0);

    println!("Starting embassy executor ...");

    spawner.spawn(button_task(button_pin, publisher)).unwrap();
    spawner.spawn(blink_green(led_pin, led_subscriber)).unwrap();
    spawner
        .spawn(relay_task(relay_pin, relay_subscriber))
        .unwrap();

    println!("Start busy loop on main");
    loop {
        esp_println::println!("Bing!");
        embassy_time::Timer::after(embassy_time::Duration::from_millis(5_000)).await;
    }
}

#[embassy_executor::task]
pub async fn blink_green(
    mut pin: Output<'static, Gpio4>,
    mut subscriber: Subscriber<'static, NoopRawMutex, Command, QUEUE_SIZE, QUEUE_SIZE, QUEUE_SIZE>,
) {
    let mut led_state = false;

    loop {
        match subscriber.try_next_message() {
            Some(WaitResult::Message(command)) => {
                led_state = command.led_state;
            }
            Some(WaitResult::Lagged(_)) | None => {
                // No new message, continue with the current state
            }
        }
        if led_state {
            pin.toggle();
            Timer::after(Duration::from_millis(200)).await;
        } else {
            pin.set_low();
            Timer::after(Duration::from_millis(50)).await;
        }
    }
}
#[embassy_executor::task]
pub async fn button_task(
    button_pin: Input<'static, Gpio12>,
    publisher: Publisher<'static, NoopRawMutex, Command, QUEUE_SIZE, QUEUE_SIZE, QUEUE_SIZE>,
) {
    let _command_current_state = Command {
        is_on: false,
        led_state: false,
        relay_state: false,
    };

    let mut previous_button_state = false;
    let mut led_state = false;
    let mut relay_state = false;
    loop {
        // println!("Button State: {}", command_current_state.is_on);
        let current_button_state = button_pin.is_high();

        if current_button_state && !previous_button_state {
            // Button was pressed
            led_state = !led_state;
            relay_state = !relay_state;
            let payload = Command {
                is_on: true,
                led_state,
                relay_state,
            };

            // Send the payload through the channel
            publisher.publish(payload).await;
            esp_println::println!(
                "Button pressed, LED State: {}, Relay State: {}",
                led_state,
                relay_state
            );
        }
        previous_button_state = current_button_state;

        // Debouncing delay
        embassy_time::Timer::after(embassy_time::Duration::from_millis(50)).await;
    }
}

pub const QUEUE_SIZE: usize = 10;

#[derive(Clone, Debug)]
pub struct Command {
    pub is_on: bool,
    pub led_state: bool,
    pub relay_state: bool,
}

const SAFE_MODE: bool = false;

#[derive(Debug)]
pub struct Sens<T>(pub T);

impl<T: core::fmt::Display> core::fmt::Display for Sens<T> {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        let Self(inner) = self;
        if SAFE_MODE {
            "[REDACTED]".fmt(f)
        } else {
            inner.fmt(f)
        }
    }
}

#[embassy_executor::task]
pub async fn relay_task(
    mut relay: Output<'static, Gpio2>,
    mut subscriber: Subscriber<'static, NoopRawMutex, Command, QUEUE_SIZE, QUEUE_SIZE, QUEUE_SIZE>,
) {
    loop {
        log::info!("{}", Sens("Loop..."));
        let command = subscriber.next_message_pure().await;
        // if command.is_on {
        if command.relay_state {
            relay.set_high();
            println!("Set Relay High: {}", command.is_on);
        } else {
            relay.set_low();
            println!("Set Relay Low: {}", command.is_on);
        }
        // }
        embassy_time::Timer::after(embassy_time::Duration::from_secs(1)).await;
    }
}