#![no_std]

//TODO look into using core::ffi (some types do not seem to work)
//TODO os errors in API calls

#[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::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) {
    sifln!("Task Entry");
    sifln!("running pointer {:p}", task_object);
    let task: &mut dyn TaskIF;
    unsafe {
        let pointer = task_object as *mut PeriodicTask;
        task = &mut *pointer;
    }
    sifln!("running cast {:p}", task);
    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_object: &'a mut dyn ExecutableObjectIF,
}

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

impl<'a> TaskIF for PeriodicTask<'a> {
    fn run(&mut self) {
        sifln!("Task running {}", self.period);
        loop {
            self.task_object.perform();
            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??
                sifln!("create task {:p}", task_pointer);
            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, 10>,
}

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: 1, p2: 2 }));},
        //     _ => (),
        // }
        // self.cycle += 1;
    }
}

/*struct MessageQueue<T, const LENGTH: usize> {
    queue_id: *const cty::c_void,
    buffer: [T; LENGTH],
}

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: Copy, const LENGTH: usize> MessageQueue<T, LENGTH> {
    fn new() -> Self {
        let mut instance: Self;
        unsafe {
            instance = Self {
                queue_id: 0 as *const cty::c_void,
                buffer: [core::mem::MaybeUninit::zeroed().assume_init(); LENGTH], //Gets passed to C/FreeRTOS, so we never ever touch it ever again
            };
            let buffer_pointer: *mut cty::c_void =
                &mut instance.buffer as *mut _ as *mut cty::c_void;
            instance.queue_id = create_queue(LENGTH, core::mem::size_of::<T>(), buffer_pointer);

            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;
        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)]
enum Message {
    OK,
    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(),
    };
    let mut h2 = Handler {
        id: 2,
        //     cycle: 0,
        //     other_handler: MessageQueueSender::<Message>::new(),
    };

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

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

    let mut t2: PeriodicTask = PeriodicTask::new(&mut h2, 512, 300);

    sifln!("t1 {:p}", &t1);

    let r1: &mut dyn TaskIF = &mut t1;

    sifln!("as dyn {:p}", r1);

    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);
    //t2.period = 100; //Invalid
    h1.id = 2;
    unsafe {
        task_delay(2000);
    }
    sifln!("Mission delay done");
}