use crate::*;

const ANGLE_SCALE: f64 = 10430.378350470453; // 65536 / 2π


pub struct MathDevice {
    pub x: u16,
    pub y: u16,
    pub r: u16,
    pub t: u16,
    pub x_read: Option<u16>,
    pub y_read: Option<u16>,
    pub r_read: Option<u16>,
    pub t_read: Option<u16>,
    /// (low, high)
    pub prod: Option<(u16, u16)>,
    pub quot: Option<u16>,
    pub rem:  Option<u16>,
}


impl Device for MathDevice {
    fn read(&mut self, port: u8) -> u8 {
        match port {
            0x0 => read_h!(self.x()),
            0x1 => read_l!(self.x()),
            0x2 => read_h!(self.y()),
            0x3 => read_l!(self.y()),
            0x4 => read_h!(self.r()),
            0x5 => read_l!(self.r()),
            0x6 => read_h!(self.t()),
            0x7 => read_l!(self.t()),
            0x8 => read_h!(self.prod().1),
            0x9 => read_l!(self.prod().1),
            0xa => read_h!(self.prod().0),
            0xb => read_l!(self.prod().0),
            0xc => read_h!(self.quot()),
            0xd => read_l!(self.quot()),
            0xe => read_h!(self.rem()),
            0xf => read_l!(self.rem()),
            _ => unreachable!(),
        }
    }

    fn write(&mut self, port: u8, value: u8) -> Option<Signal> {
        match port {
            0x0 => { write_h!(self.x, value); self.clear_polar(); },
            0x1 => { write_l!(self.x, value); self.clear_polar(); },
            0x2 => { write_h!(self.y, value); self.clear_polar(); },
            0x3 => { write_l!(self.y, value); self.clear_polar(); },
            0x4 => { write_h!(self.r, value); self.clear_cartesian(); },
            0x5 => { write_l!(self.r, value); self.clear_cartesian(); },
            0x6 => { write_h!(self.t, value); self.clear_cartesian(); },
            0x7 => { write_l!(self.t, value); self.clear_cartesian(); },
            0x8 => (),
            0x9 => (),
            0xa => (),
            0xb => (),
            0xc => (),
            0xd => (),
            0xe => (),
            0xf => (),
            _ => unreachable!(),
        };
        return None;
    }

    fn wake(&mut self) -> bool {
        false
    }
}


impl MathDevice {
    pub fn new() -> Self {
        Self {
            x: 0,
            y: 0,
            r: 0,
            t: 0,
            x_read: None,
            y_read: None,
            r_read: None,
            t_read: None,

            prod: None,
            quot: None,
            rem:  None,
        }
    }

    pub fn clear_cartesian(&mut self) {
        self.x_read = None;
        self.y_read = None;
    }

    pub fn clear_polar(&mut self) {
        self.r_read = None;
        self.t_read = None;
        self.prod = None;
        self.quot = None;
        self.rem = None;
    }

    pub fn x(&mut self) -> u16 {
        match self.x_read {
            Some(x) => x,
            None => {
                let r = self.r as f64;
                let t = self.t as f64;
                let angle = t / ANGLE_SCALE;
                let x = angle.cos() * r;
                self.x_read = Some(x as i16 as u16);
                self.x_read.unwrap()
            }
        }
    }

    pub fn y(&mut self) -> u16 {
        match self.y_read {
            Some(y) => y,
            None => {
                let r = self.r as f64;
                let t = self.t as f64;
                let angle = t / ANGLE_SCALE;
                let y = angle.sin() * r;
                self.y_read = Some(y as i16 as u16);
                self.y_read.unwrap()
            }
        }
    }

    pub fn r(&mut self) -> u16 {
        match self.r_read {
            Some(r) => r,
            None => {
                let sum = (self.x as f64).powi(2) + (self.y as f64).powi(2);
                self.r_read = Some(sum.sqrt() as u16);
                self.r_read.unwrap()
            }
        }
    }

    pub fn t(&mut self) -> u16 {
        match self.t_read {
            Some(t) => t,
            None => {
                let x = self.x as i16 as f64;
                let y = self.x as i16 as f64;
                let angle = f64::atan2(y, x) * ANGLE_SCALE;
                self.t_read = Some(angle as i16 as u16);
                self.t_read.unwrap()
            }
        }
    }

    pub fn prod(&mut self) -> (u16, u16) {
        match self.prod {
            Some(prod) => prod,
            None => {
                self.prod = Some(self.x.widening_mul(self.y));
                self.prod.unwrap()
            }
        }
    }

    pub fn quot(&mut self) -> u16 {
        match self.quot {
            Some(quot) => quot,
            None => {
                self.quot = Some(self.x.checked_div(self.y).unwrap_or(0));
                self.quot.unwrap()
            }
        }
    }

    pub fn rem(&mut self) -> u16 {
        match self.rem {
            Some(rem) => rem,
            None => {
                self.rem = Some(self.x.checked_rem(self.y).unwrap_or(0));
                self.rem.unwrap()
            }
        }
    }
}