#![allow(dead_code)]
#![allow(unused_variables)]

use decoder;
use decoder::{IncrementMode, Instruction};
use regs::{StatusFlag, GeneralPurposeRegister, GeneralPurposeRegisterPair};
use chip_definitions;

use std::fmt;

#[derive(Debug)]
pub enum CPUError {
    UnimplementedInstruction,
    OutOfBoundsException,
    UnsupportedAddress,
    DecodingError(decoder::DecodingError),
    Exit,
}

// CPU
#[derive(Debug)]
pub struct CPU {
    // General purpose registers
    pub registers: [u8; 32],

    // PC
    pub pc: u32,

    // The same is true for the status register
    pub sreg: u8,
}


impl fmt::Display for CPU {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "CPU @ 0x{:X} (file offset = 0x{:X}) ", self.pc, 2 * self.pc)?;
        write!(f, "SREG: 0x{:02X}: ", self.sreg)?;
        for i in 0..7 {
            let t = self.sreg & (1 << i);
            if t > 0 {
                write!(f, "{}", StatusFlag::try_from(t).unwrap())?;
            } else {
                write!(f, "-")?;
            }
        }
        write!(f, "\n")?;
        for i in 0..32 {
            write!(f, " R{:-2} = 0x{:02X}  ", i, self.registers[i])?;
            if (i + 1) % 10 == 0{
                write!(f, "\n")?;
            }
        }
        write!(f, "\n")
    }
}

impl CPU {
    pub fn new() -> Self {
        CPU {
            registers: [0u8; 32],
            pc: 0,          // Reset vector
            sreg: 0,        // Uninitialized as well
        }
    }

    fn get_sp(&self, mem: &[u8]) -> u16 {
        mem[chip_definitions::IOAdress::SPL as usize] as u16 | ((mem[chip_definitions::IOAdress::SPH as usize] as u16) << 8)
    }

    fn set_sp(&self, mem: &mut [u8], val: u16) {
        mem[chip_definitions::IOAdress::SPL as usize] = val as u8;
        mem[chip_definitions::IOAdress::SPH as usize] = (val >> 8) as u8;
    }


    fn test_flag(&self, flag: StatusFlag) -> bool {
        (self.sreg & (flag as u8)) > 0
    }

    fn set_flag(&mut self, flag: StatusFlag) {
        self.sreg |= flag as u8;
    }

    fn clear_flag(&mut self, flag: StatusFlag) {
        self.sreg &= !(flag as u8);
    }

    fn set_clear_flag(&mut self, flag: StatusFlag, test: bool) {
        print!("[{}->{}] ", flag, test);
        if test {
            self.set_flag(flag);
        } else {
            self.clear_flag(flag);
        }
    }

    fn get_register_pair(&self, r: &GeneralPurposeRegisterPair) -> u16 {
        ((self.registers[r.high()] as u16) << 8) | self.registers[r.low()] as u16
    }

    fn set_register_pair(&mut self, r: &GeneralPurposeRegisterPair, v: u16) {
        self.registers[r.high()] = (v >> 8) as u8;
        self.registers[r.low()] = (v & 0xFF) as u8;
    }

    fn get_register(&self, r: &GeneralPurposeRegister) -> u8 {
        self.registers[r.as_usize()]
    }

    fn set_register(&mut self, r: &GeneralPurposeRegister, v: u8) {
        self.registers[r.as_usize()] = v;
    }

    fn ram_write(&self, ram: &mut [u8], addr: u16, val: u8) -> Result<(), CPUError> {
        print!("[RAMW:{:04X}={:02X}] ", addr, val);
        if addr as usize >= ram.len() {
            Err(CPUError::OutOfBoundsException)
        } else {
            // TODO: Hooks
            if addr == chip_definitions::IOAdress::USARTC0_DATA as _ {
                print!("USART_OUT:{: <3} ({}) ", val, (val as char).escape_debug());
            }
            ram[addr as usize] = val;
            Ok(())
        }
    }

    fn ram_read(&self, ram: &[u8], addr: u16) -> Result<u8, CPUError> {
        print!("[RAMR:{:04X}] ", addr);
        if addr as usize >= ram.len() {
            Err(CPUError::OutOfBoundsException)
        } else {
            // TODO: Hooks
            if addr == chip_definitions::IOAdress::USARTC0_DATA as _ {
                return Err(CPUError::Exit);
            } else if addr == chip_definitions::IOAdress::USARTC0_STATUS as _ {
                if self.pc == 0x5AC {
                    // USART data check -> Yes, there is indeed data available!
                    return Ok(0x80);
                } else {
                    return Ok(0x20); // Usart is ready to send.
                }
            } else if addr == chip_definitions::IOAdress::OSC_STATUS as _ {
                // HACK: Osci is set right..
                return Ok(0x02);
            }
            Ok(ram[addr as usize])
        }
    }

    fn push(&mut self, ram: &mut [u8], val: u8) -> Result<(), CPUError> {
        let sp = self.get_sp(&ram);
        self.ram_write(ram, sp, val)?;
        self.set_sp(ram, sp.wrapping_sub(1));
        Ok(())
    }

    fn pop(&mut self, ram: &mut [u8]) -> Result<u8, CPUError> {
        let sp = self.get_sp(&ram);
        self.set_sp(ram, sp.wrapping_add(1));
        self.ram_read(ram, sp.wrapping_add(1))
    }

    // Flag update functions on a single value:
    fn update_flags_zns_8(&mut self, v: u8) {
        self.set_clear_flag(StatusFlag::Zero, v == 0);
        self.set_clear_flag(StatusFlag::Negative, v & 0x80 == 0x80);
        let V = self.test_flag(StatusFlag::TwosComplementOverflow);
        self.set_clear_flag(StatusFlag::SignBit, (v & 0x80 == 0x80) ^ V);
    }

    fn update_flags_zns_16(&mut self, v: u16) {
        self.set_clear_flag(StatusFlag::Zero, v == 0);
        self.set_clear_flag(StatusFlag::Negative, v & 0x8000 == 0x8000);
        let V = self.test_flag(StatusFlag::TwosComplementOverflow);
        self.set_clear_flag(StatusFlag::SignBit, (v & 0x8000 == 0x8000) ^ V);
    }

    // Copied from simavr, let's trust them for now.
    fn update_flags_add_zns(&mut self, result: u8, d: u8, r: u8) {
        let add_carry = (d & r) | (r & !result) | (!result & d);
        self.set_clear_flag(StatusFlag::HalfCarry, add_carry & 0b1000 != 0);
        self.set_clear_flag(StatusFlag::Carry, add_carry & 0b1000_0000 != 0);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((d & r & !result) | (!d & !r & result)) & 0x80 == 0x80);
        self.update_flags_zns_8(result);
    }

    fn update_flags_sub_zns(&mut self, result: u8, d: u8, r: u8) {
        let sub_carry = (!d & r) | (r & result) | (result & !d);
        self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
        self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80);
        self.update_flags_zns_8(result);
    }

    fn update_flags_Rzns(&mut self, r: u8) {
        if r != 0 {
            self.clear_flag(StatusFlag::Zero);
        }
        self.set_clear_flag(StatusFlag::Negative, r & 0x80 == 0x80);
        let b = self.test_flag(StatusFlag::TwosComplementOverflow);
        self.set_clear_flag(StatusFlag::SignBit, r & 0x80 == 0x80 && b);
    }

    fn update_flags_sub_Rzns(&mut self, result: u8, d: u8, r: u8) {
        let sub_carry = (!d & r) | (r & result) | (result & !d);
        self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
        self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80);
        self.update_flags_Rzns(result);
    }

    fn update_flags_zcvs(&mut self, result: u8, vr: u8) {
        // Workaround lexicalic lifetimes.
        let z = result == 0;
        let c = vr & 1 == 1;
        let n = self.test_flag(StatusFlag::Negative);
        self.set_clear_flag(StatusFlag::Zero, z);
        self.set_clear_flag(StatusFlag::Carry, c);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
        self.set_clear_flag(StatusFlag::SignBit, c);
    }

    fn update_flags_zcnvs(&mut self, result: u8, vr: u8) {
        let z = result == 0;
        let c = vr & 1 == 1;
        let n = result & 0x80 == 0x80;
        self.set_clear_flag(StatusFlag::Zero, z);
        self.set_clear_flag(StatusFlag::Negative, n);
        self.set_clear_flag(StatusFlag::Carry, c);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
        self.set_clear_flag(StatusFlag::SignBit, c);
    }

    fn update_flags_znv0s(&mut self, v: u8) {
        self.clear_flag(StatusFlag::TwosComplementOverflow);
        self.update_flags_zns_8(v);
    }

    // Returns # of ticks the executed instruction took
    pub fn step(&mut self, rom: &mut [u8], ram: &mut [u8]) -> Result<usize, CPUError> {
        // Instruction fetch
        if (self.pc as usize) * 2 >= rom.len() {
            return Err(CPUError::OutOfBoundsException);
        }

        let ins = match decoder::decode(&rom[(self.pc as usize) * 2..]) {
            Ok(v) => v,
            Err(e) => return Err(CPUError::DecodingError(e)),
        };
        print!("CPU: pc={:06X} sp={:04X} Fetch: {: <40} -> ", self.pc, self.get_sp(ram), format!("{}", ins));

        self.pc += (ins.size() / 2) as u32;

        // Instruction execute
        match ins {
            Instruction::JMP(v) => self.pc = v,
            Instruction::CLR(ref r) => {
                self.set_register(r, 0);
                self.update_flags_znv0s(0);
            },
            Instruction::EOR(ref d, ref r) => {
                self.registers[d.as_usize()] ^= self.registers[r.as_usize()];
                let r = self.registers[d.as_usize()];
                self.update_flags_znv0s(r);
            },
            Instruction::LDI(ref r, v) => self.set_register(r, v),
            Instruction::SER(ref r) => self.set_register(r, 0xFF),
            Instruction::RJMP(v) => self.pc = self.pc.wrapping_add(v as _),
            Instruction::CLR_FLAG(v) => self.clear_flag(v),
            Instruction::SET_FLAG(v) => self.set_flag(v),
            Instruction::CPI(ref r, v) => {
                let rv = self.get_register(r);
                self.update_flags_sub_zns(rv.wrapping_sub(v), rv, v);
            },
            Instruction::CP(ref r, ref i) => {
                let rv = self.get_register(r);
                let iv = self.get_register(i);
                self.update_flags_sub_zns(rv.wrapping_sub(iv), rv, iv);
            },
            Instruction::CPC(ref d, ref r) => {
                let rd: u8 = self.get_register(d);
                let rr: u8 = self.get_register(r);

                let s = if self.test_flag(StatusFlag::Carry) {
                    rd.wrapping_sub(rr).wrapping_sub(1)
                } else {
                    rd.wrapping_sub(rr)
                };

                self.update_flags_sub_Rzns(s, rd, rr);
            },
            Instruction::BR_IF(offset, flag, test) => {
                if self.test_flag(flag) == test {
                    self.pc = self.pc.wrapping_add(offset as _);
                }
            },
            Instruction::ST(ref ptr, ref src_reg, ref inc_mode) => {
                let base = self.get_register_pair(ptr);
                let addr = match *inc_mode {
                    IncrementMode::None => base,
                    IncrementMode::PreDecrement => {
                        self.set_register_pair(ptr, base.wrapping_sub(1));
                        base.wrapping_sub(1)
                    },
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(ptr, base.wrapping_add(1));
                        base
                    },
                    IncrementMode::ConstantOffset(o) => base.wrapping_add(o as _),
                };
                self.ram_write(ram, addr, self.get_register(src_reg))?;
            },
            Instruction::LD(ref dst_reg, ref ptr, ref inc_mode) => {
                let base = self.get_register_pair(ptr);
                let addr = match *inc_mode {
                    IncrementMode::None => base,
                    IncrementMode::PreDecrement => {
                        self.set_register_pair(ptr, base.wrapping_sub(1));
                        base.wrapping_sub(1)
                    },
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(ptr, base.wrapping_add(1));
                        base
                    },
                    IncrementMode::ConstantOffset(o) => base.wrapping_add(o as _),
                };
                let v = self.ram_read(ram, addr)?;
                self.set_register(dst_reg, v);
            },
            Instruction::ELPM(ref dst_reg, ref inc_mode) => {
                // TODO: RAMPZ
                let Z = self.get_register_pair(&30u8.into());
                let d = rom[Z as usize];
                self.set_register(dst_reg, d);
                match *inc_mode {
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(&30u8.into(), Z.wrapping_add(1));
                    },
                    _ => {
                        // This instruction does only support None + PostIncrement
                    },
                }
            },
            Instruction::LPM(ref dst_reg, ref inc_mode) => {
                let Z = self.get_register_pair(&30u8.into());
                let d = rom[Z as usize];
                self.set_register(dst_reg, d);
                match *inc_mode {
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(&30u8.into(), Z.wrapping_add(1));
                    },
                    _ => {
                        // This instruction does only support None + PostIncrement
                    },
                }
            },
            Instruction::OUT(ref addr, ref val) => {
                let val = self.get_register(val);
                self.ram_write(ram, *addr, val)?;
            },
            Instruction::IN(ref reg, ref addr) => {
                let v = self.ram_read(ram, *addr)?;
                self.set_register(reg, v);
            }
            Instruction::CALL(ref addr) => {
                let ret_to = self.pc;
                self.push(ram, ((ret_to >> 16) & 0xFF) as u8)?;
                self.push(ram, ((ret_to >> 8) & 0xFF) as u8)?;
                self.push(ram, (ret_to & 0xFF) as u8)?;
                self.pc = *addr;
            },
            Instruction::RCALL(ref addr) => {
                let ret_to = self.pc;
                self.push(ram, ((ret_to >> 16) & 0xFF) as u8)?;
                self.push(ram, ((ret_to >> 8) & 0xFF) as u8)?;
                self.push(ram, (ret_to & 0xFF) as u8)?;
                self.pc = (self.pc as i32 + *addr as i32) as u32;
            },
            Instruction::RET => {
                let mut ret_to = self.pop(ram)? as u32;
                ret_to += (self.pop(ram)? as u32) << 8;
                ret_to += (self.pop(ram)? as u32) << 16;
                self.pc = ret_to as _;
            },
            Instruction::POP(ref reg) => {
                let v = self.pop(ram)?;
                self.registers[reg.as_usize()] = v;
            },
            Instruction::PUSH(ref reg) => {
                let v = self.registers[reg.as_usize()];
                self.push(ram, v)?;
            },
            Instruction::SUBI(ref d, ref v) => {
                let dv = self.get_register(d);
                let vres = dv.wrapping_sub(*v);

                self.update_flags_sub_zns(vres, dv, *v);
                self.set_register(d, vres);
            },
            Instruction::SBC(ref d, ref r) => {
                let dv = self.get_register(d);
                let rv = self.get_register(r);
                let mut vres = dv.wrapping_sub(rv);
                if self.test_flag(StatusFlag::Carry) {
                    vres = vres.wrapping_sub(1);
                }

                self.update_flags_sub_Rzns(vres, dv, rv);
                self.set_register(d, vres);
            },
            Instruction::SBCI(ref d, ref v) => {
                let dv = self.get_register(d);
                let mut vres = dv.wrapping_sub(*v);
                if self.test_flag(StatusFlag::Carry) {
                    vres = vres.wrapping_sub(1);
                }

                self.update_flags_sub_Rzns(vres, dv, *v);
                self.set_register(d, vres);
            },
            Instruction::MOVW(ref d, ref r) => {
                let t = self.get_register_pair(r);
                self.set_register_pair(d, t);
            },
            Instruction::OR(ref d, ref r) => {
                let t = self.get_register(d) | self.get_register(r);
                self.set_register(d, t);
                self.update_flags_znv0s(t);
            }
            Instruction::ORI(ref d, ref v) => {
                let t = self.get_register(d) | *v;
                self.set_register(d, t);
                self.update_flags_znv0s(t);
            },
            Instruction::AND(ref d, ref r) => {
                let t = self.get_register(d) & self.get_register(r);
                self.set_register(d, t);
                self.update_flags_znv0s(t);
            },
            Instruction::TST(ref r) => {
                let t = self.get_register(r);
                self.update_flags_znv0s(t);
            },
            Instruction::ANDI(ref d, ref v) => {
                let t = self.get_register(d) & *v;
                self.set_clear_flag(StatusFlag::Zero, t == 0);
                self.set_register(d, t);
            },
            Instruction::ADD(ref d, ref r) => {
                let t = self.get_register(d).wrapping_add(self.get_register(r));
                let dd = self.get_register(d);
                self.set_clear_flag(StatusFlag::Carry, t < dd);
                self.set_clear_flag(StatusFlag::Zero, t == 0);
                self.set_register(d, t);
            },
            Instruction::ADC(ref d, ref r) => {
                let mut t = self.get_register(d).wrapping_add(self.get_register(r));
                if self.test_flag(StatusFlag::Carry) {
                    t = t.wrapping_add(1);
                }
                let dd = self.get_register(d);
                self.set_clear_flag(StatusFlag::Carry, t < dd);
                self.set_clear_flag(StatusFlag::Zero, t == 0);
                self.set_register(d, t);
            },
            Instruction::SUB(ref d, ref r) => {
                let dv = self.get_register(d);
                let rv = self.get_register(r);
                let res = dv.wrapping_sub(rv);
                self.update_flags_sub_zns(res, dv, rv);
                self.set_register(d, res);
            },
            Instruction::MOV(ref d, ref r) => {
                let v = self.get_register(r);
                self.set_register(d, v);
            },
            Instruction::ADIW(ref d, ref v) => {
                let dv = self.get_register_pair(d);
                let res = dv.wrapping_add(*v);
                self.set_register_pair(d, res);
                self.set_clear_flag(StatusFlag::TwosComplementOverflow, (dv & !res) & 0x8000 == 0x8000);
                self.set_clear_flag(StatusFlag::Carry, (!dv & res) & 0x8000 == 0x8000);
                self.update_flags_zns_16(res);
            },
            Instruction::SBIW(ref d, ref v) => {
                let dv = self.get_register_pair(d);
                let res = dv.wrapping_sub(*v as _);
                self.set_clear_flag(StatusFlag::TwosComplementOverflow, (!dv & res) & 0x8000 == 0x8000);
                self.set_clear_flag(StatusFlag::Carry, (dv & !res) & 0x8000 == 0x8000);
                self.update_flags_zns_16(res);
                self.set_register_pair(d, res);
            },
            Instruction::DEC(ref r) => {
                let rv = self.get_register(r);
                let res = rv.wrapping_sub(1);
                self.set_register(r, res);
                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x7F);
                self.update_flags_zns_8(res);
            },
            Instruction::INC(ref r) => {
                let rv = self.get_register(r);
                let res = rv.wrapping_add(1);
                self.set_register(r, res);

                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
                self.update_flags_zns_8(res);
            },
            Instruction::STS16(ref addr, ref r) => {
                self.ram_write(ram, *addr, self.get_register(r))?;
            },
            Instruction::STS8(ref addr, ref r) => {
                self.ram_write(ram, *addr as u16, self.get_register(r))?;
            },
            Instruction::LDS16(ref r, ref addr) => {
                let v = self.ram_read(ram, *addr)?;
                self.set_register(r, v);
            },
            Instruction::LDS8(ref r, ref addr) => {
                let v = self.ram_read(ram, *addr as u16)?;
                self.set_register(r, v);
            },
            Instruction::LSL(ref r) => {
                let v = self.get_register(r);
                self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
                self.set_register(r, v << 1);
                self.set_clear_flag(StatusFlag::Zero, (v << 1) == 0);
            },
            Instruction::ROL(ref r) => {
                let v = self.get_register(r);
                let c = if self.test_flag(StatusFlag::Carry) { 1 } else { 0 };
                self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
                self.set_register(r, v << 1 | c);
                self.set_clear_flag(StatusFlag::Zero, (v << 1) | c == 0);
            },
            Instruction::ASR(ref r) => {
                let rv = self.get_register(r);
                let res = rv >> 1 | (rv & 0x80);
                self.set_register(r, res);
                self.update_flags_zcnvs(res, rv);
            },
            Instruction::LSR(ref r) => {
                let rv = self.get_register(r);
                self.set_register(r, rv >> 1);
                self.clear_flag(StatusFlag::Negative);
                self.update_flags_zcvs(rv >> 1, rv);
            },
            Instruction::ROR(ref r) => {
                let rv = self.get_register(r);
                let c = if self.test_flag(StatusFlag::Carry) { 0x80 } else { 0 };
                let res = rv >> 1 | c;
                self.set_register(r, res);
                self.update_flags_zcnvs(res, rv);
            },
            Instruction::SBRS(ref r, ref bit) => {
                let r = self.get_register(r);
                if (r & (1 << *bit)) > 0 {
                    self.pc += 1;
                }
            },
            Instruction::SBRC(ref r, ref bit) => {
                let r = self.get_register(r);
                if (r & (1 << *bit)) == 0{
                    self.pc += 1;
                }
            },
            Instruction::CPSE(ref r, ref d) => {
                if self.get_register(r) == self.get_register(d) {
                    // TODO: assume 2b instruction after this one.
                    self.pc += 1;
                }
            },
            Instruction::COM(ref r) => {
                let rv = self.get_register(r);
                let res = 0xFFu8.wrapping_sub(rv);
                self.set_register(r, res);
                self.update_flags_znv0s(res);
                self.set_flag(StatusFlag::Carry);
            },
            Instruction::NEG(ref r) => {
                let rv = self.get_register(r);
                let res = 0u8.wrapping_sub(rv);
                self.set_register(r, res);
                self.set_clear_flag(StatusFlag::HalfCarry, (rv | res) & 0x08 == 0x08);
                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
                self.set_clear_flag(StatusFlag::Carry, res != 0);
                self.update_flags_zns_8(rv);
            },
            Instruction::MUL(ref r, ref d) => {
                //  R1:R0 ← Rd × Rr(unsigned ← unsigned × unsigned)
                let r = self.get_register(r) as u16;
                let d = self.get_register(d) as u16;
                let v = r * d;
                self.registers[0] = (v & 0xFF) as u8;
                self.registers[1] = ((v >> 8) & 0xFF) as u8;
                self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
                self.set_clear_flag(StatusFlag::Zero, v == 0);
            },
            Instruction::BST(ref r, ref v) => {
                let r = self.get_register(r);
                self.set_clear_flag(StatusFlag::BitCopyStorage, r & (1 << *v) != 0);
            },
            Instruction::SWAP(ref r) => {
                let rv = self.get_register(r);
                self.set_register(r, rv >> 4 | ((rv << 4) & 0xF0));
            },
            Instruction::NOP => {},
            _ => return Err(CPUError::UnimplementedInstruction)
        }

        Ok(ins.cycles())
    }
}