#![no_std]
// test comment
//TODO look into using core::ffi (some types do not seem to work)
//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?)

#[macro_export]
macro_rules! sifln {
    ($(,)?) => (
        let mut stdout = Outbytes {};
        writeln!(stdout);
    );
    ($($arg:tt)*) => (
        let mut stdout = Outbytes {};
        let _alwaysok = writeln!(stdout, $($arg)*);
    );
}

#[macro_export]
macro_rules! sif {
    ($($arg:tt)*) => (
        let mut stdout = Outbytes {};
        let _alwaysok = write!(stdout, $($arg)*);
    );
}

use core::mem::size_of;
use core::panic::PanicInfo;

#[panic_handler]
fn panic(panic: &PanicInfo<'_>) -> ! {
    unsafe {
        stop_it();
    }
    // TODO: Make this unicode-safe
    sif!("In Task \"");
    unsafe {
        let task_name = get_task_name();
        let mut offset = 0;
        while *task_name.offset(offset) != 0 {
            sif!("{}", *task_name.offset(offset) as char);
            offset = offset + 1;
        }
    }
    sifln!("\":");
    sifln!("{}", panic);
    //TODO: stop RTOS, exit if hosted
    loop {}
}

type TaskFunction = unsafe extern "C" fn(*mut cty::c_void);

extern "C" {
    fn outbyte(c: cty::c_char);
    //void *create_task(TaskFunction_t taskFunction, void *parameter, size_t stack_size)
    fn create_task(
        taskFunction: TaskFunction,
        parameter: *const cty::c_void,
        stack_size: cty::size_t,
    ) -> *const cty::c_void;

    fn get_task_name() -> *const core::ffi::c_uchar;

    fn stop_it();

    fn delete_task(handle: *const cty::c_void);

    fn task_delay(milliseconds: cty::uint32_t);

    //void *create_queue(size_t length, size_t element_size)
    fn create_queue(length: cty::size_t, element_size: cty::size_t) -> *const cty::c_void;

    fn queue_receive(queue: *const cty::c_void, message: *const cty::c_void) -> cty::uint8_t;
    fn queue_send(queue: *const cty::c_void, message: *const cty::c_void) -> cty::uint8_t;
}

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

#[no_mangle]
extern "C" fn task_entry(task_object: *mut cty::c_void) {
    let task: &mut dyn TaskIF;
    unsafe {
        let pointer = task_object as *mut PeriodicTask;
        task = &mut *pointer;
    }
    task.run();
}

trait ExecutableObjectIF {
    fn perform(&mut self);
}

trait TaskIF {
    fn run(&mut self);
    fn get_stack_size(&self) -> cty::size_t;
    fn set_handle(&mut self, task_handle: *const cty::c_void);
    fn get_handle(&self) -> *const cty::c_void;
}

struct PeriodicTask<'a> {
    stack_size: cty::size_t, //TODO generic type and safety
    task_handle: *const cty::c_void,
    period: usize,
    task_objects: &'a mut [&'a mut dyn ExecutableObjectIF],
}

impl<'a> PeriodicTask<'a> {
    fn new(
        objects: &'a mut [&'a mut dyn ExecutableObjectIF],
        stack_size: usize,
        period: usize,
    ) -> PeriodicTask<'a> {
        let instance: PeriodicTask<'a> = Self {
            stack_size: stack_size,
            task_handle: 0 as *const cty::c_void,
            period: period,
            task_objects: objects,
        };
        instance
    }
}

impl<'a> TaskIF for PeriodicTask<'a> {
    fn run(&mut self) {
        loop {
            for object in self.task_objects.iter_mut() {
                object.perform();
            }
            //TODO make this exact
            unsafe {
                task_delay(self.period as cty::uint32_t); //TODO type of delay should be generic but safe (cap to max in C)
            }
        }
    }
    fn get_stack_size(&self) -> cty::size_t {
        self.stack_size
    }
    fn set_handle(&mut self, task_handle: *const cty::c_void) {
        self.task_handle = task_handle;
    }
    fn get_handle(&self) -> *const cty::c_void {
        self.task_handle
    }
}

struct TaskExecutor<'a> {
    tasks: &'a mut [&'a mut dyn TaskIF],
}

impl<'a> TaskExecutor<'a> {
    fn run_tasks(&mut self) {
        for task in self.tasks.iter_mut() {
            // we give away a raw pointer, to be called by an OS task
            // while this is generally very broken, we use a reference tied
            // to our own lifetime and destroy the task when we get dropped
            // this way, the reference is guaranteed to be valid over our
            // lifetime while the task is deleted at the end of our lifetime
            let task_pointer: *const cty::c_void = *task as *mut _ as *const cty::c_void; //TODO this does work without the "*" in front of the task -> Why??
            let handle;
            unsafe {
                handle = create_task(task_entry, task_pointer, task.get_stack_size());
            }
            if handle == 0 as *mut cty::c_void {
                panic!("could not create Task");
            } else {
                task.set_handle(handle);
            }
        }
    }
}

impl<'a> Drop for TaskExecutor<'a> {
    fn drop(&mut self) {
        for task in self.tasks.iter_mut() {
            unsafe {
                delete_task(task.get_handle());
            }
        }
    }
}

struct Handler {
    id: u32,
    command_queue: MessageQueue<Message>,
}

struct HandlerSender {
    id: u32,
    cycle: u8,
    other_handler: MessageQueueSender<Message>,
}

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

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

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

struct MessageQueue<T> {
    queue_id: *const cty::c_void,
    _unused: Option<T>, //need to constrain the queue to one message type for safety, but compiler needs that to be used
}

struct MessageQueueSender<T> {
    queue_id: Option<*const cty::c_void>,
    _unused: Option<T>, //need to constrain the sender to one message type for safety, but compiler needs that to be used
}

impl<T: Default> MessageQueue<T> {
    fn new(depth: usize) -> Self {
        let mut instance: Self;
        unsafe {
            instance = Self {
                queue_id: 0 as *const cty::c_void,
                _unused: None,
            };
            //TODO check cast of depth
            instance.queue_id = create_queue(depth, core::mem::size_of::<T>());

            if instance.queue_id == 0 as *mut cty::c_void {
                panic!("could not create Queue");
            }
            instance
        }
    }

    fn get_sender(&self) -> MessageQueueSender<T> {
        let instance: MessageQueueSender<T> = MessageQueueSender::<T> {
            queue_id: Some(self.queue_id),
            _unused: None,
        };
        instance
    }

    fn receive(&self) -> Result<T, ()> {
        let mut message: T = T::default();
        let res: cty::uint8_t;
        unsafe {
            //message = core::mem::MaybeUninit::zeroed().assume_init(); // We only return it if the queue received something
            let message_pointer: *mut cty::c_void = &mut message as *mut _ as *mut cty::c_void;
            res = queue_receive(self.queue_id, message_pointer);
        }
        if res == 1 {
            Ok(message)
        } else {
            Err(())
        }
    }
}

impl<T> MessageQueueSender<T> {
    fn new() -> Self {
        Self {
            queue_id: None,
            _unused: None,
        }
    }

    fn send(&self, message: T) -> Result<(), ()> {
        let queue_id = self.queue_id.expect("unitialized Message Queue");
        let res: cty::uint8_t;
        unsafe {
            let message_pointer: *const cty::c_void = &message as *const _ as *const cty::c_void;
            res = queue_send(queue_id, message_pointer);
        }
        if res == 1 {
            Ok(())
        } else {
            Err(())
        }
    }
}

#[derive(Clone, Copy)]
struct GenericMessageData {
    p1: u32,
    p2: u32,
}

#[derive(Copy, Clone, Default)]
enum Message {
    OK,
    #[default]
    FAILED,
    DATA(GenericMessageData),
}

struct Outbytes {}

use core::fmt::{Error, Write};
impl Write for Outbytes {
    fn write_str(&mut self, s: &str) -> Result<(), Error> {
        for c in s.as_bytes() {
            unsafe {
                outbyte(*c);
            }
        }
        Ok(())
    }
}

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

    let mut h1 = Handler {
        id: 1,
        command_queue: MessageQueue::new(5),
    };
    let mut h2 = HandlerSender {
        id: 2,
        cycle: 0,
        other_handler: MessageQueueSender::<Message>::new(),
    };

    h2.other_handler = h1.command_queue.get_sender();

    let array: &mut [&mut dyn ExecutableObjectIF] = &mut [&mut h1];

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

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

    let _i = 1;

    sifln!(
        "sizeof {}, pointer struct {:p}, pointer element {:p}, next element{:p}",
        size_of::<PeriodicTask>(),
        &t2,
        t2.task_objects,
        &_i
    );

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

    sifln!("{:p}", task_executor.tasks[0]);

    task_executor.run_tasks();

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