#![no_std]

//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?)

mod fsrc;

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

use fsrc::*;

#[panic_handler]
fn panic(panic: &PanicInfo<'_>) -> ! {
    unsafe {
        osal::stop_it();
    }
    // TODO: Make this unicode-safe
    sif!("In Task \"");
    unsafe {
        let task_name = osal::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 {}
}



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


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

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

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 {
            Ok(message) => self.handle_message(message),
            Err(_) => {
                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.send(queues::Message::OK);
            }
            1 => {
                let _ = self.other_handler.send(queues::Message::FAILED);
            }
            2 => {
                let _ = self
                    .other_handler
                    .send(queues::Message::DATA(queues::GenericMessageData { p1: 13, p2: 2 }));
            }
            _ => (),
        }
        self.cycle += 1;
    }
}


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

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

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

    let array: &mut [&mut dyn tasks::ExecutableObjectIF] = &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 cty::c_void,
    };

    let _i = 1;

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

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

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

    task_executor.run_tasks();

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