//! Project: CMtec CMDR joystick 24
//! Date:    2023-08-01
//! Author:  Christoffer Martinsson
//! Email:   cm@cmtec.se
//! License: Please refer to LICENSE in root directory

use core::default::Default;
use fugit::ExtU32;
use usb_device::bus::UsbBus;
use usb_device::class_prelude::UsbBusAllocator;
use usbd_human_interface_device::usb_class::prelude::*;

// Fetched from https://github.com/embassy-rs/embassy/blob/e3efda2249640e0b4881289aa609c96a26a7479a/embassy-hal-common/src/fmt.rs
#[cfg(feature = "defmt")]
macro_rules! unwrap {
    ($($x:tt)*) => {
        ::defmt::unwrap!($($x)*)
    };
}

#[cfg(not(feature = "defmt"))]
macro_rules! unwrap {
    ($arg:expr) => {
        match $crate::usb_joystick_device::Try::into_result($arg) {
            ::core::result::Result::Ok(t) => t,
            ::core::result::Result::Err(e) => {
                ::core::panic!("unwrap of `{}` failed: {:?}", ::core::stringify!($arg), e);
            }
        }
    };
    ($arg:expr, $($msg:expr),+ $(,)? ) => {
        match $crate::usb_joystick_device::Try::into_result($arg) {
            ::core::result::Result::Ok(t) => t,
            ::core::result::Result::Err(e) => {
                ::core::panic!("unwrap of `{}` failed: {}: {:?}", ::core::stringify!($arg), ::core::format_args!($($msg,)*), e);
            }
        }
    }
}

#[derive(Debug, Copy, Clone, Eq, PartialEq)]
pub struct NoneError;

pub trait Try {
    type Ok;
    type Error;
    fn into_result(self) -> Result<Self::Ok, Self::Error>;
}

impl<T> Try for Option<T> {
    type Ok = T;
    type Error = NoneError;

    #[inline]
    fn into_result(self) -> Result<T, NoneError> {
        self.ok_or(NoneError)
    }
}

impl<T, E> Try for Result<T, E> {
    type Ok = T;
    type Error = E;

    #[inline]
    fn into_result(self) -> Self {
        self
    }
}

// Based on example device from https://github.com/dlkj/usbd-human-interface-device/blob/main/src/device/joystick.rs
// Updated to 6x 12bit axis, 25x buttons and 4x hat switches
#[rustfmt::skip]
pub const JOYSTICK_DESCRIPTOR: &[u8] = &[
    0x05, 0x01,         // Usage Page (Generic Desktop) 
    0x09, 0x04,         // Usage (Joystick)            
    0xa1, 0x01,         // Collection (Application)   
    0x09, 0x01,         //   Usage Page (Pointer)    
    0xa1, 0x00,         //   Collection (Physical)  
    0x09, 0x30,         //     Usage (X)           
    0x09, 0x31,         //     Usage (Y)           
    0x09, 0x32,         //     Usage (Z)          
    0x09, 0x33,         //     Usage (RX)       
    0x09, 0x34,         //     Usage (RY)        
    0x09, 0x35,         //     Usage (RZ)          
    0x15, 0x00,         //     Logical Minimum (0) 
    0x26, 0xFF, 0x0F,   //     Logical Maximum (4095)
    0x75, 0x0C,         //     Report Size (12)           
    0x95, 0x06,         //     Report count (6)         
    0x81, 0x02,         //     Input (Data, Variable, Absolute) 
    0xc0,               //   End Collection                  
    0x05, 0x09,         //   Usage Page (Button)            
    0x19, 0x01,         //   Usage Minimum (1)             
    0x29, 0x20,         //   Usage Maximum (32)           
    0x15, 0x00,         //   Logical Minimum (0)         
    0x25, 0x01,         //   Logical Maximum (1)        
    0x75, 0x01,         //   Report Size (1)           
    0x95, 0x20,         //   Report Count (32)        
    0x81, 0x02,         //   Input (Data, Variable, Absolute)
    0x15, 0x00,			//   Logical Minimum (0)
	0x25, 0x07,			//   Logical Maximum (7)
	0x35, 0x00,			//   Physical Minimum (0)
	0x46, 0x3B, 0x01,	//   Physical Maximum (315)
	0x75, 0x04,			//   Report Size (4)
	0x95, 0x04,			//   Report Count (4)
	0x65, 0x14,			//   Unit (20)
    0x05, 0x01,         //   Usage Page (Generic Desktop)
	0x09, 0x39,			//   Usage (Hat switch)
	0x09, 0x39,			//   Usage (Hat switch)
	0x09, 0x39,			//   Usage (Hat switch)
	0x09, 0x39,			//   Usage (Hat switch)
	0x81, 0x42,			//   Input (variable,absolute,null_state)
    0xc0,               // End Collection                     
];

#[derive(Clone, Copy, Debug, Eq, PartialEq, Default)]
pub struct JoystickReport {
    pub x: u16,       // 12bit
    pub y: u16,       // 12bit
    pub z: u16,       // 12bit
    pub rx: u16,      // 12bit
    pub ry: u16,      // 12bit
    pub rz: u16,      // 12bit
    pub buttons: u32, // 32bit
    pub hat1: u8,     // 4bit
    pub hat2: u8,     // 4bit
    pub hat3: u8,     // 4bit
    pub hat4: u8,     // 4bit
}

pub struct Joystick<'a, B: UsbBus> {
    interface: Interface<'a, B, InBytes16, OutNone, ReportSingle>,
}

impl<'a, B: UsbBus> Joystick<'a, B> {
    pub fn write_report(&mut self, report: &JoystickReport) -> Result<(), UsbHidError> {
        let mut data: [u8; 15] = [0; 15];

        // Did not make the packed struct work, so doing it manually
        // TODO: make this work with packed struct
        data[0] = report.x as u8;
        data[1] = ((report.x >> 8) as u8) | ((report.y << 4) as u8);
        data[2] = (report.y >> 4) as u8;
        data[3] = report.z as u8;
        data[4] = ((report.z >> 8) as u8) | ((report.rx << 4) as u8);
        data[5] = (report.rx >> 4) as u8;
        data[6] = report.ry as u8;
        data[7] = ((report.ry >> 8) as u8) | ((report.rz << 4) as u8);
        data[8] = (report.rz >> 4) as u8;
        data[9] = report.buttons as u8;
        data[10] = (report.buttons >> 8) as u8;
        data[11] = (report.buttons >> 16) as u8;
        data[12] = (report.buttons >> 24) as u8;
        data[13] = (report.hat1) | (report.hat2 << 4);
        data[14] = (report.hat3) | (report.hat4 << 4);

        self.interface
            .write_report(&data)
            .map(|_| ())
            .map_err(UsbHidError::from)
    }
}

impl<'a, B: UsbBus> DeviceClass<'a> for Joystick<'a, B> {
    type I = Interface<'a, B, InBytes16, OutNone, ReportSingle>;

    fn interface(&mut self) -> &mut Self::I {
        &mut self.interface
    }

    fn reset(&mut self) {}

    fn tick(&mut self) -> Result<(), UsbHidError> {
        Ok(())
    }
}

pub struct JoystickConfig<'a> {
    interface: InterfaceConfig<'a, InBytes16, OutNone, ReportSingle>,
}

impl<'a> Default for JoystickConfig<'a> {
    #[must_use]
    fn default() -> Self {
        Self::new(
            unwrap!(unwrap!(InterfaceBuilder::new(JOYSTICK_DESCRIPTOR))
                .boot_device(InterfaceProtocol::None)
                .description("Joystick")
                .in_endpoint(10.millis()))
            .without_out_endpoint()
            .build(),
        )
    }
}

impl<'a> JoystickConfig<'a> {
    #[must_use]
    pub fn new(interface: InterfaceConfig<'a, InBytes16, OutNone, ReportSingle>) -> Self {
        Self { interface }
    }
}

impl<'a, B: UsbBus + 'a> UsbAllocatable<'a, B> for JoystickConfig<'a> {
    type Allocated = Joystick<'a, B>;

    fn allocate(self, usb_alloc: &'a UsbBusAllocator<B>) -> Self::Allocated {
        Self::Allocated {
            interface: Interface::new(usb_alloc, self.interface),
        }
    }
}
