#![no_std]

//TODO os errors in API calls
//TODO look into a pattern for late initialized stuff, currently using Option (can we make it compile time safe?)

pub mod fsrc;

use core::fmt::Write;
use core::panic::PanicInfo;

use fsrc::objectmanager::SystemObjectIF;
use fsrc::*;

extern "C" {
    fn done();
}

#[panic_handler]
fn panic(panic: &PanicInfo<'_>) -> ! {
    unsafe {
        osal::stop_it();
    }
    // TODO: Make this unicode-safe
    sif!("In Task \"");
    unsafe {
        let task_name = core::ffi::CStr::from_ptr(osal::get_task_name());
        let task_name_utf8 = core::str::from_utf8(task_name.to_bytes());
        match task_name_utf8 {
            Ok(string) => {
                sif!("{}", string);
            }
            Err(_) => {
                sif!("Schei� Encoding");
            }
        }
    }
    sifln!("\":");
    sifln!("{}", panic);
    //TODO: stop RTOS, exit if hosted
    unsafe { done() };
    loop {}
}

#[no_mangle]
extern "C" fn rust_main() {
    sifln!("Rust startup 🚀");
    mission();
    sifln!("Mission done");
}

#[derive(Copy, Clone, Default)]
struct HandlerData {
    x: u32,
    y: f32,
}

struct Handler {
    id: objectmanager::ObjectId,
    command_queue: queues::MessageQueue<10>,
    data: datasets::OwnedDataset<HandlerData>,
}

struct HandlerSender {
    id: objectmanager::ObjectId,
    other_handler: objectmanager::ObjectId,
    cycle: u8,
    other_handler_queue: queues::MessageQueueSender,
    other_data: datasets::ReferencedDataset<HandlerData>,
}

impl Handler {
    fn handle_message(&self, message: queues::Message) {
        match message {
            queues::Message::OK => {
                sifln!("OK");
            }
            queues::Message::FAILED => {
                sifln!("FAILED");
            }
            queues::Message::DATA(data) => {
                sifln!("p1: {}, p2 {}", data.p1, data.p2);
            }
        }
    }
}

impl tasks::ExecutableObjectIF for Handler {
    fn perform(&mut self) {
        sifln!("Handler {} performs", self.id);
        let result = self.command_queue.receive();
        match result {
            Some(message) => self.handle_message(message),
            None => {
                sifln!("Handler {} got nothing", self.id);
            }
        }
    }
}

impl tasks::ExecutableObjectIF for HandlerSender {
    fn perform(&mut self) {
        sifln!("HandlerSender {} performs step {}", self.id, self.cycle);
        match self.cycle {
            0 => {
                let _ = self.other_handler_queue.send(queues::Message::OK);
            }
            1 => {
                let _ = self.other_handler_queue.send(queues::Message::FAILED);
            }
            2 => {
                let _ = self.other_handler_queue.send(queues::Message::DATA(
                    queues::GenericMessageData { p1: 13, p2: 2 },
                ));
            }
            _ => (),
        }
        self.cycle += 1;
    }
}

impl SystemObjectIF for Handler {
    fn get_command_queue(&self) -> crate::fsrc::queues::MessageQueueSender {
        self.command_queue.get_sender()
    }
    fn get_id(&self) -> objectmanager::ObjectId {
        self.id
    }
    fn initialize(&mut self, _object_manager: &dyn objectmanager::ObjectManager) -> Result<(), ()> {
        Ok(())
    }
}

impl introspection::Introspection for HandlerSender {
    fn for_each_member(&self, f: &mut dyn FnMut(&dyn core::any::Any, &str) -> ()) {}

    fn for_each_member_return(&self, f: &mut dyn FnMut(&dyn core::any::Any, &str) -> Option<()>) {}
}

impl introspection::Introspection for Handler {
    fn for_each_member(&self, f: &mut dyn FnMut(&dyn core::any::Any, &str) -> ()) {
        (*f)(&self.command_queue, "command_queue");
        (*f)(&self.data, "data");
    }

    fn for_each_member_return(&self, f: &mut dyn FnMut(&dyn core::any::Any, &str) -> Option<()>) {
        (*f)(&self.command_queue, "command_queue");
        (*f)(&self.data, "data");
    }
}

impl SystemObjectIF for HandlerSender {
    fn get_command_queue(&self) -> crate::fsrc::queues::MessageQueueSender {
        queues::MessageQueueSender::new() //TODO
    }

    fn get_id(&self) -> objectmanager::ObjectId {
        self.id
    }
    fn initialize(&mut self, object_manager: &dyn objectmanager::ObjectManager) -> Result<(), ()> {
        let other_handler_maybe = object_manager.get_object(self.other_handler);
        let other_handler = match other_handler_maybe {
            Ok(other) => other,
            Err(_) => return Err(()),
        };
        self.other_handler_queue = other_handler.get_command_queue();

        self.other_data
            .initialize(object_manager, self.other_handler)?;
        Ok(())
    }
}

fn mission() {
    sifln!("Mission enter");

    let mut h1 = Handler {
        id: 1,
        command_queue: queues::MessageQueue::new(),
        data: datasets::OwnedDataset::new(),
    };
    let mut h2 = HandlerSender {
        id: 2,
        other_handler: 3,
        cycle: 0,
        other_handler_queue: queues::MessageQueueSender::new(),
        other_data: datasets::ReferencedDataset::new(),
    };

    let array: &mut [&mut dyn objectmanager::SystemObjectIF] = &mut [&mut h1];

    let mut t1 = tasks::PeriodicTask::new(array, 512, 200);

    let mut t2: tasks::PeriodicTask = tasks::PeriodicTask {
        task_objects: &mut [&mut h2],
        stack_size: 512,
        period: 400,
        task_handle: 0 as *const core::ffi::c_void,
    };

    let mut task_executor = tasks::TaskExecutor {
        tasks: &mut [&mut t1, &mut t2],
    };

    task_executor.init_and_run(false);

    sifln!("Mission delay");
    unsafe {
        osal::task_delay(2000);
    }
    sifln!("executor dropped");
    drop(task_executor);
    unsafe {
        osal::task_delay(2000);
    }
    sifln!("Mission delay done");
}