#![no_std]
#![no_main]
#![deny(
    clippy::mem_forget,
    reason = "mem::forget is generally not safe to do with esp_hal types, especially those \
    holding buffers for the duration of a data transfer."
)]

use core::ptr::addr_of_mut;

use embassy_executor::Spawner;
use embassy_time::{Instant, Timer};

#[allow(unused_imports)]
use esp_hal::{
    clock::CpuClock, interrupt::software::SoftwareInterruptControl, system::{AppCoreGuard, CpuControl, Stack}, timer::{systimer::SystemTimer, timg::{TimerGroup, Wdt}},
    gpio::Pin
};

use esp_hal_embassy::{Executor, InterruptExecutor};
use log::*;
use renderbug_embassy::{logging::RenderbugLogger, tasks::ui::UiSurfacePool};
use renderbug_embassy::events::BusGarage;
use static_cell::StaticCell;
use esp_backtrace as _;

use renderbug_embassy::tasks::{
    motion::motion_task,
    ui::{Ui, ui_main}
};

extern crate alloc;

// This creates a default app-descriptor required by the esp-idf bootloader.
// For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
esp_bootloader_esp_idf::esp_app_desc!();

static STATIC_HI_EXEC: StaticCell<InterruptExecutor<0>> = StaticCell::new();

static BUS_GARAGE: StaticCell<BusGarage> = StaticCell::new();
static mut CORE2_STACK: Stack<8192> = Stack::new();
static CORE_HANDLE: StaticCell<AppCoreGuard> = StaticCell::new();

#[esp_hal_embassy::main]
async fn main(spawner: Spawner) {
    critical_section::with(|_| {
        RenderbugLogger::init_logger();
        //esp_println::logger::init_logger_from_env();
    });

    let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
    let peripherals = esp_hal::init(config);

    esp_alloc::heap_allocator!(size: 128 * 1024);

    let sys_timer = SystemTimer::new(peripherals.SYSTIMER);
    esp_hal_embassy::init([sys_timer.alarm0, sys_timer.alarm1, sys_timer.alarm2]);
    info!("Embassy initialized!");

    let timer0 = TimerGroup::new(peripherals.TIMG0);

    let timer1 = TimerGroup::new(peripherals.TIMG1);
    let mut ui_wdt = timer1.wdt;
    ui_wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(10));
    //ui_wdt.enable(); //FIXME: Re-enable UI watchdog once we have a brain task running

    let garage = BUS_GARAGE.init(Default::default());

    info!("Setting up rendering pipeline");
    let mut surfaces = UiSurfacePool::default();
    let ui = Ui::new(&mut surfaces, garage.display.clone());

    let mut wdt = timer0.wdt;
    wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(3));
    let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
    let hi_exec = STATIC_HI_EXEC.init(InterruptExecutor::new(swi.software_interrupt0));
    #[allow(unused_variables)]
    let hi_spawn = hi_exec.start(esp_hal::interrupt::Priority::max());

    #[cfg(not(feature="headless"))]
    {
        wdt.enable();
        hi_spawn.must_spawn(renderbug_embassy::tasks::render::render(peripherals.RMT, peripherals.GPIO5.degrade(), surfaces, garage.display.clone(), wdt));
    }
    #[cfg(feature="headless")]
    garage.display.render_is_running.signal(true);

    #[cfg(feature="motion")]
    {
        use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
        use esp_hal::{i2c::master::{Config, I2c}, Async};
        use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex};

        static I2C_BUS: StaticCell<Mutex<CriticalSectionRawMutex, I2c<'static, Async>>> = StaticCell::new();

        info!("Launching i2c sensor tasks");
        let sda = peripherals.GPIO3;
        let scl = peripherals.GPIO4;
        let i2c = I2c::new(peripherals.I2C1, Config::default()).unwrap().with_scl(scl).with_sda(sda).into_async();
        let i2c_bus = I2C_BUS.init(Mutex::new(i2c));
        #[cfg(feature="mpu")]
        spawner.must_spawn(renderbug_embassy::tasks::mpu::mpu_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
        #[cfg(feature="gps")]
        spawner.must_spawn(renderbug_embassy::tasks::gps::gps_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
    }

    #[cfg(feature="simulation")]
    {
        spawner.must_spawn(renderbug_embassy::tasks::simulation::motion_simulation_task(garage.motion.dyn_sender()));
        spawner.must_spawn(renderbug_embassy::tasks::simulation::location_simulation_task(garage.motion.dyn_sender()));
    }

    info!("Launching motion engine");
    spawner.must_spawn(motion_task(garage.motion.dyn_receiver(), garage.notify.dyn_sender(), garage.predict.dyn_sender()));

    info!("Starting core 2");
    let mut cpu_control = CpuControl::new(peripherals.CPU_CTRL);
    CORE_HANDLE.init(cpu_control.start_app_core(unsafe { &mut *addr_of_mut!(CORE2_STACK) }, || {
        static STATIC_EXEC: StaticCell<Executor> = StaticCell::new();
        let exec = STATIC_EXEC.init(Executor::new());
        exec.run(|spawner| {
            #[cfg(feature="radio")]
            {
                info!("Launching wifi");
                spawner.must_spawn(renderbug_embassy::tasks::wifi::wireless_task(garage.notify.dyn_receiver(), timer0.timer0.into(), peripherals.RNG, peripherals.WIFI, peripherals.BT));
            }
            info!("Launching UI");
            spawner.must_spawn(ui_main(garage.notify.dyn_receiver(), ui));
            #[cfg(feature="demo")]
            {
                warn!("Launching with demo sequencer");
                spawner.must_spawn(renderbug_embassy::tasks::demo::demo_task(garage.notify.dyn_sender()));
            }
            #[cfg(not(feature="demo"))]
            {
                info!("Launching prediction engine");
                spawner.must_spawn(renderbug_embassy::tasks::predict::prediction_task(garage.predict.dyn_receiver(), garage.notify.dyn_sender()));
            }
            info!("Launching core 2 watchdog");
            spawner.must_spawn(wdt_task(ui_wdt));

            info!("System is ready in {}ms", Instant::now().as_millis());
        });
    }).unwrap());
}

#[embassy_executor::task]
async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
    loop {
        // Watchdog is set to trip after 5 seconds, so we wait just long enough before it trips
        // TODO: We should maybe extend this timeout to 30s or 1m when the system is sleeping
        wdt.feed();
        Timer::after_secs(5).await;
    }
}