#![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."
)]
#![feature(future_join)]

use core::ptr::addr_of_mut;

use bleps::ad_structure::{create_advertising_data, AdStructure, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE};
use bleps::attribute_server::{AttributeServer, NotificationData};
use bleps::{gatt, Ble, HciConnector};
use embassy_executor::Spawner;
use embassy_sync::channel::DynamicReceiver;
use embassy_time::{Instant, Timer};
use esp_backtrace as _;
use esp_hal::{
    gpio::Pin,
    interrupt::software::SoftwareInterruptControl,
    clock::CpuClock,
    system::{AppCoreGuard, CpuControl, Stack},
    timer::{AnyTimer, systimer::SystemTimer, timg::TimerGroup}
};

use esp_hal_embassy::{Executor, InterruptExecutor};
use esp_wifi::ble::controller::BleConnector;
use figments::surface::BufferedSurfacePool;
use log::info;
use renderbug_embassy::events::{BusGarage, Measurement, Telemetry};
use serde_json::json;
use static_cell::StaticCell;

#[cfg(feature="simulation")]
use renderbug_embassy::tasks::simulation::{motion_simulation_task, location_simulation_task};
#[cfg(not(feature="simulation"))]
use renderbug_embassy::tasks::{gps::gps_task, mpu::mpu_task};
#[cfg(not(feature="simulation"))]
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
#[cfg(not(feature="simulation"))]
use esp_hal::{
    time::Rate,
    i2c::master::{Config, I2c},
    Async,
};
#[cfg(not(feature="simulation"))]
use embassy_sync::{
    mutex::Mutex,
    blocking_mutex::raw::CriticalSectionRawMutex
};

use renderbug_embassy::tasks::{
    render::render,
    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!();

#[cfg(not(feature="simulation"))]
static I2C_BUS: StaticCell<Mutex<CriticalSectionRawMutex, I2c<'static, Async>>> = 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) {
    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 timer0 = SystemTimer::new(peripherals.SYSTIMER);
    esp_hal_embassy::init([timer0.alarm0, timer0.alarm1, timer0.alarm2]);
    info!("Embassy initialized!");

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

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

    let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
    static STATIC_HI_EXEC: StaticCell<InterruptExecutor<0>> = StaticCell::new();
    let hi_exec = STATIC_HI_EXEC.init(InterruptExecutor::new(swi.software_interrupt0));
    let hi_spawn = hi_exec.start(esp_hal::interrupt::Priority::Priority1);
    hi_spawn.must_spawn(render(peripherals.RMT, peripherals.GPIO5.degrade(), surfaces, &garage.display));

    #[cfg(not(feature="simulation"))]
    {
        info!("Launching i2c sensor tasks");
        let i2c = I2c::new(peripherals.I2C1, Config::default().with_frequency(Rate::from_khz(400)).with_timeout(esp_hal::i2c::master::BusTimeout::Maximum)).unwrap().with_scl(peripherals.GPIO4).with_sda(peripherals.GPIO3).into_async();
        let i2c_bus = I2C_BUS.init(Mutex::new(i2c));
        spawner.must_spawn(mpu_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
        spawner.must_spawn(gps_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
    }

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

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

    info!("Starting communications and UI on 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| {
            let timer1 = TimerGroup::new(peripherals.TIMG0);
            spawner.must_spawn(wifi_task(garage.telemetry.dyn_receiver(), timer1.timer0.into(), peripherals.RNG, peripherals.WIFI, peripherals.BT));
            spawner.must_spawn(ui_main(garage.scenes.dyn_receiver(), ui));
        });
    }).unwrap());

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

// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
#[embassy_executor::task]
async fn wifi_task(telemetry: DynamicReceiver<'static, Telemetry>, timer: AnyTimer<'static>, rng: esp_hal::peripherals::RNG<'static>, _wifi_device: esp_hal::peripherals::WIFI<'static>, bluetooth_device: esp_hal::peripherals::BT<'static>) {
    let rng = esp_hal::rng::Rng::new(rng);
    let wifi_init =
        esp_wifi::init(timer, rng).expect("Failed to initialize WIFI/BLE controller");

    info!("Setting up BLE stack");
    let connector = BleConnector::new(&wifi_init, bluetooth_device);
    let get_millis = || esp_hal::time::Instant::now().duration_since_epoch().as_millis();
    let hci = HciConnector::new(connector, get_millis);
    let mut ble = Ble::new(&hci);
    ble.init().unwrap();
    ble.cmd_set_le_advertising_parameters().unwrap();
    ble.cmd_set_le_advertising_data(
        create_advertising_data(&[
            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
            AdStructure::ServiceUuids16(&[Uuid::Uuid16(0x0001)]),
            AdStructure::CompleteLocalName("Renderbug!")
        ])
        .unwrap()
    ).unwrap();
    ble.cmd_set_le_advertise_enable(true).unwrap();

    let mut wf1 = |_offset: usize, data: &[u8]| {
        info!("Read serial data! {data:?}");
    };

    let s = &b""[..];
    // Other useful characteristics:
    // 0x2A67 - Location and speed
    // 0x2A00 - Device name
    // 0x2B90 - Device time
    // Permitted characteristics:
    // Acceleration
    // Force
    // Length
    // Linear position
    // Rotational speed
    // Temperature
    // Torque
    // Useful app that logs data: https://github.com/a2ruan/ArduNetApp?tab=readme-ov-file
    // Requires service 4fafc201-1fb5-459e-8fcc-c5c9c331914b, characteristic beb5483e-36e1-4688-b7f5-ea07361b26a8
    gatt!([service {
        uuid: "6E400001-B5A3-F393-E0A9-E50E24DCCA9E", // Nordic UART
        characteristics: [
            characteristic {
                uuid: "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", // TX from device, everything is sent as notifications
                notify: true,
                name: "tx",
                value: s
            },
            characteristic {
                uuid: "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", // RX from phone
                write: wf1
            },
        ]
    }]);
    let mut rng = bleps::no_rng::NoRng;
    let mut srv = AttributeServer::new(&mut ble, &mut gatt_attributes, &mut rng);

    info!("BLE running!");
    // TODO: Somehow need to recreate the attributeserver after disconnecting?
    loop {
        let notification = match telemetry.try_receive() {
            Err(_) => None,
            //TODO: Should make Telemetry values serde-encodable
            Ok(Telemetry::Measurement(Measurement::IMU { accel, gyro })) => {
                let json_data = json!({
                    "x": accel.x,
                    "y": accel.y,
                    "z": accel.z,
                    "gx": gyro.x,
                    "gy": gyro.y,
                    "gz": gyro.z
                });
                let json_buf = serde_json::to_string(&json_data).unwrap();
                Some(json_buf)
            },
            Ok(Telemetry::Measurement(Measurement::GPS(Some(measurement)))) => {
                info!("gps telemetry");
                let json_data = json!({
                    "lat": measurement.x,
                    "lng": measurement.y
                });
                let json_buf = serde_json::to_string(&json_data).unwrap();
                Some(json_buf)
            },
            _ => None
            /*Ok(Measurement::Prediction(prediction)) => {
                let json_data = json!({
                    "predict_lat": prediction.x,
                    "predict_lng": prediction.y
                });
                let json_buf = serde_json::to_string(&json_data).unwrap();
                Some(json_buf)
            }*/
        };

        match notification {
            None => {
                srv.do_work().unwrap();
                Timer::after_millis(5).await;
            },
            Some(serial_data) => {
                for chunk in serial_data.as_bytes().chunks(20) {
                    srv.do_work_with_notification(Some(NotificationData::new(tx_handle, chunk))).unwrap();
                }
                srv.do_work_with_notification(Some(NotificationData::new(tx_handle, &b"\n"[..]))).unwrap();
            }
        }

        /*
        let (mut wifi, _interfaces) = esp_wifi::wifi::new(&wifi_init, wifi_device)
            .expect("Failed to initialize WIFI controller");    }

        loop {
            //let results = wifi.scan_n_async(16).await.unwrap();
            wifi.set_configuration(&esp_wifi::wifi::Configuration::Client(
                ClientConfiguration {
                    ssid: "The Frequency".to_string(),
                    auth_method: esp_wifi::wifi::AuthMethod::WPA2Personal,
                    password: "thepasswordkenneth".to_string(),
                    ..Default::default()
                }
            )).unwrap();

            if wifi.connect_async().await.is_ok() {
                info!("Connected to wifi!");
                while wifi.is_connected().unwrap() {
                    Timer::after_secs(60).await;
                }
                info!("Disconnected.");
            }
            Timer::after_secs(30).await;
        }
        */
    }
}