use bedrock_core::*;


pub struct MathDevice {
    pub op1: u16,
    pub op2: u16,

    pub sqrt: Option<u16>,
    pub atan: Option<u16>,
    pub prod: Option<(u16, u16)>,  // (low, high)
    pub quot: Option<u16>,
    pub rem:  Option<u16>,
}

impl MathDevice {
    pub fn new() -> Self {
        Self {
            op1: 0,
            op2: 0,

            sqrt: None,
            atan: None,
            prod: None,
            quot: None,
            rem:  None,
        }
    }

    pub fn clear(&mut self) {
        self.sqrt = None;
        self.atan = None;
        self.prod = None;
        self.quot = None;
        self.rem  = None;
    }

    pub fn atan(&mut self) -> u16 {
        match self.atan {
            Some(atan) => atan,
            None => {
                let x = self.op1 as i16 as f64;
                let y = self.op2 as i16 as f64;
                const SCALE: f64 = 10430.378350470453; // PI * 32768
                self.atan = Some((f64::atan2(x, y) * SCALE) as i16 as u16);
                self.atan.unwrap()
            }
        }
    }

    pub fn sqrt(&mut self) -> u16 {
        match self.sqrt {
            Some(sqrt) => sqrt,
            None => {
                let input = ((self.op1 as u32) << 16) | (self.op2 as u32);
                self.sqrt = Some((input as f64).sqrt() as u16);
                self.sqrt.unwrap()
            }
        }
    }

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

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

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

impl Device for MathDevice {
    fn read(&mut self, port: u8) -> u8 {
        match port {
            0x0 => read_h!(self.op1),
            0x1 => read_l!(self.op1),
            0x2 => read_h!(self.op2),
            0x3 => read_l!(self.op2),
            0x4 => read_h!(self.sqrt()),
            0x5 => read_l!(self.sqrt()),
            0x6 => read_h!(self.atan()),
            0x7 => read_l!(self.atan()),
            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.op1, value); self.clear(); },
            0x1 => { write_l!(self.op1, value); self.clear(); },
            0x2 => { write_h!(self.op2, value); self.clear(); },
            0x3 => { write_l!(self.op2, value); self.clear(); },
            0x4 => (),
            0x5 => (),
            0x6 => (),
            0x7 => (),
            0x8 => (),
            0x9 => (),
            0xa => (),
            0xb => (),
            0xc => (),
            0xd => (),
            0xe => (),
            0xf => (),
            _ => unreachable!(),
        };
        return None;
    }

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