#pragma once
#include <ircolib/mem_access.hpp>
#include <MemoryRegions.hpp>
#include <array>
#include <core/RDP.hpp>
#include <core/mmio/MI.hpp>
#include <Instruction.hpp>

#define RSP_BYTE(addr) (dmem[BYTE_ADDRESS(addr) & 0xFFF])
#define GET_RSP_HALF(addr) ((RSP_BYTE(addr) << 8) | RSP_BYTE((addr) + 1))
#define SET_RSP_HALF(addr, value)                                                                                      \
  do {                                                                                                                 \
    RSP_BYTE(addr) = ((value) >> 8) & 0xFF;                                                                            \
    RSP_BYTE((addr) + 1) = (value) & 0xFF;                                                                             \
  }                                                                                                                    \
  while (0)
#define GET_RSP_WORD(addr) ((GET_RSP_HALF(addr) << 16) | GET_RSP_HALF((addr) + 2))
#define SET_RSP_WORD(addr, value)                                                                                      \
  do {                                                                                                                 \
    SET_RSP_HALF(addr, ((value) >> 16) & 0xFFFF);                                                                      \
    SET_RSP_HALF((addr) + 2, (value) & 0xFFFF);                                                                        \
  }                                                                                                                    \
  while (0)

namespace n64 {
union SPStatus {
  u32 raw;
  struct {
    unsigned halt : 1;
    unsigned broke : 1;
    unsigned dmaBusy : 1;
    unsigned dmaFull : 1;
    unsigned ioFull : 1;
    unsigned singleStep : 1;
    unsigned interruptOnBreak : 1;
    unsigned signal0 : 1;
    unsigned signal1 : 1;
    unsigned signal2 : 1;
    unsigned signal3 : 1;
    unsigned signal4 : 1;
    unsigned signal5 : 1;
    unsigned signal6 : 1;
    unsigned signal7 : 1;
    unsigned : 17;
  };
};

union SPStatusWrite {
  u32 raw;
  struct {
    unsigned clearHalt : 1;
    unsigned setHalt : 1;
    unsigned clearBroke : 1;
    unsigned clearIntr : 1;
    unsigned setIntr : 1;
    unsigned clearSstep : 1;
    unsigned setSstep : 1;
    unsigned clearIntrOnBreak : 1;
    unsigned setIntrOnBreak : 1;
    unsigned clearSignal0 : 1;
    unsigned setSignal0 : 1;
    unsigned clearSignal1 : 1;
    unsigned setSignal1 : 1;
    unsigned clearSignal2 : 1;
    unsigned setSignal2 : 1;
    unsigned clearSignal3 : 1;
    unsigned setSignal3 : 1;
    unsigned clearSignal4 : 1;
    unsigned setSignal4 : 1;
    unsigned clearSignal5 : 1;
    unsigned setSignal5 : 1;
    unsigned clearSignal6 : 1;
    unsigned setSignal6 : 1;
    unsigned clearSignal7 : 1;
    unsigned setSignal7 : 1;
    unsigned : 7;
  };
};

union SPDMALen {
  struct {
    unsigned len : 12;
    unsigned count : 8;
    unsigned skip : 12;
  };
  u32 raw;
};

union SPDMASPAddr {
  struct {
    unsigned address : 12;
    unsigned bank : 1;
    unsigned : 19;
  };
  u32 raw;
};

union SPDMADRAMAddr {
  struct {
    unsigned address : 24;
    unsigned : 8;
  };
  u32 raw;
};

union VPR {
  s16 selement[8];
  u16 element[8];
  u8 byte[16];
  u32 word[4];
  m128i single;
} __attribute__((packed));

static_assert(sizeof(VPR) == 16);

struct Mem;
struct Registers;

#define DE(x) (((x) >> 11) & 0x1F)

struct RSP {
  bool divInLoaded = false;
  bool semaphore = false;
  std::array<u8, DMEM_SIZE> dmem{};
  std::array<u8, IMEM_SIZE> imem{};
  u16 oldPC{}, pc{}, nextPC{};
  s16 divIn{}, divOut{};
  u32 steps = 0;
  SPStatus spStatus{};
  SPDMASPAddr spDMASPAddr{};
  SPDMADRAMAddr spDMADRAMAddr{};
  SPDMASPAddr lastSuccessfulSPAddr{};
  SPDMADRAMAddr lastSuccessfulDRAMAddr{};
  SPDMALen spDMALen{};
  s32 gpr[32]{};
  VPR vpr[32]{};
  VPR vte{};
  VPR vce{};

  struct {
    VPR h{}, m{}, l{};
  } acc;

  struct {
    VPR l{}, h{};
  } vcc, vco;

  RSP();
  void Reset();

  FORCE_INLINE void Step() {
    gpr[0] = 0;
    const u32 instr = ircolib::ReadAccess<u32>(imem, pc & IMEM_DSIZE);
    oldPC = pc & 0xFFC;
    pc = nextPC & 0xFFC;
    nextPC += 4;

    Exec(instr);
  }

  void SetVTE(const VPR &vt, u8 e);
  auto Read(u32 addr) -> u32;
  void Write(u32 addr, u32 val);
  void Exec(Instruction instr);
  
  FORCE_INLINE void SetPC(const u16 val) {
    oldPC = pc & 0xFFC;
    pc = val & 0xFFC;
    nextPC = pc + 4;
  }

  [[nodiscard]] FORCE_INLINE s64 GetACC(const int e) const {
    s64 val = u64(acc.h.element[e]) << 32;
    val |= u64(acc.m.element[e]) << 16;
    val |= u64(acc.l.element[e]) << 00;
    if ((val & 0x0000800000000000) != 0) {
      val |= 0xFFFF000000000000;
    }
    return val;
  }

  FORCE_INLINE void SetACC(const int e, const s64 val) {
    acc.h.element[e] = val >> 32;
    acc.m.element[e] = val >> 16;
    acc.l.element[e] = val;
  }

  [[nodiscard]] FORCE_INLINE u16 GetVCO() const {
    u16 value = 0;
    for (int i = 0; i < 8; i++) {
      const bool h = vco.h.element[7 - i] != 0;
      const bool l = vco.l.element[7 - i] != 0;
      const u32 mask = (l << i) | (h << (i + 8));
      value |= mask;
    }
    return value;
  }

  [[nodiscard]] FORCE_INLINE u16 GetVCC() const {
    u16 value = 0;
    for (int i = 0; i < 8; i++) {
      const bool h = vcc.h.element[7 - i] != 0;
      const bool l = vcc.l.element[7 - i] != 0;
      const u32 mask = (l << i) | (h << (i + 8));
      value |= mask;
    }
    return value;
  }

  [[nodiscard]] FORCE_INLINE u8 GetVCE() const {
    u8 value = 0;
    for (int i = 0; i < 8; i++) {
      const bool l = vce.element[ELEMENT_INDEX(i)] != 0;
      value |= (l << i);
    }
    return value;
  }

  [[nodiscard]] FORCE_INLINE u32 ReadWord(u32 addr) const {
    addr &= 0xfff;
    return GET_RSP_WORD(addr);
  }

  FORCE_INLINE void WriteWord(u32 addr, const u32 val) {
    addr &= 0xfff;
    SET_RSP_WORD(addr, val);
  }

  [[nodiscard]] FORCE_INLINE u16 ReadHalf(u32 addr) const {
    addr &= 0xfff;
    return GET_RSP_HALF(addr);
  }

  FORCE_INLINE void WriteHalf(u32 addr, const u16 val) {
    addr &= 0xfff;
    SET_RSP_HALF(addr, val);
  }

  [[nodiscard]] FORCE_INLINE u8 ReadByte(u32 addr) const {
    addr &= 0xfff;
    return RSP_BYTE(addr);
  }

  FORCE_INLINE void WriteByte(u32 addr, const u8 val) {
    addr &= 0xfff;
    RSP_BYTE(addr) = val;
  }

  FORCE_INLINE bool AcquireSemaphore() {
    if (semaphore) {
      return true;
    } else {
      semaphore = true;
      return false;
    }
  }

  FORCE_INLINE void ReleaseSemaphore() { semaphore = false; }

  void special(Instruction instr);
  void regimm(Instruction instr);
  void lwc2(Instruction instr);
  void swc2(Instruction instr);
  void cop2(Instruction instr);
  void cop0(Instruction instr);
  
  void add(Instruction instr);
  void addi(Instruction instr);
  void and_(Instruction instr);
  void andi(Instruction instr);
  void b(Instruction instr, bool cond);
  void blink(Instruction instr, bool cond);
  void cfc2(Instruction instr);
  void ctc2(Instruction instr);
  void lb(Instruction instr);
  void lh(Instruction instr);
  void lw(Instruction instr);
  void lbu(Instruction instr);
  void lhu(Instruction instr);
  void lui(Instruction instr);
  void luv(Instruction instr);
  void lbv(Instruction instr);
  void ldv(Instruction instr);
  void lsv(Instruction instr);
  void llv(Instruction instr);
  void lrv(Instruction instr);
  void lqv(Instruction instr);
  void lfv(Instruction instr);
  void lhv(Instruction instr);
  void ltv(Instruction instr);
  void lpv(Instruction instr);
  void j(Instruction instr);
  void jal(Instruction instr);
  void jr(Instruction instr);
  void jalr(Instruction instr);
  void nor(Instruction instr);
  void or_(Instruction instr);
  void ori(Instruction instr);
  void xor_(Instruction instr);
  void xori(Instruction instr);
  void sb(Instruction instr);
  void sh(Instruction instr);
  void sw(Instruction instr);
  void swv(Instruction instr);
  void sub(Instruction instr);
  void sbv(Instruction instr);
  void sdv(Instruction instr);
  void stv(Instruction instr);
  void sqv(Instruction instr);
  void ssv(Instruction instr);
  void suv(Instruction instr);
  void slv(Instruction instr);
  void shv(Instruction instr);
  void sfv(Instruction instr);
  void srv(Instruction instr);
  void spv(Instruction instr);
  void sllv(Instruction instr);
  void srlv(Instruction instr);
  void srav(Instruction instr);
  void sll(Instruction instr);
  void srl(Instruction instr);
  void sra(Instruction instr);
  void slt(Instruction instr);
  void sltu(Instruction instr);
  void slti(Instruction instr);
  void sltiu(Instruction instr);
  void vabs(Instruction instr);
  void vadd(Instruction instr);
  void vaddc(Instruction instr);
  void vand(Instruction instr);
  void vnand(Instruction instr);
  void vch(Instruction instr);
  void vcr(Instruction instr);
  void vcl(Instruction instr);
  void vmacf(Instruction instr);
  void vmacu(Instruction instr);
  void vmacq(Instruction instr);
  void vmadh(Instruction instr);
  void vmadl(Instruction instr);
  void vmadm(Instruction instr);
  void vmadn(Instruction instr);
  void vmov(Instruction instr);
  void vmulf(Instruction instr);
  void vmulu(Instruction instr);
  void vmulq(Instruction instr);
  void vmudl(Instruction instr);
  void vmudh(Instruction instr);
  void vmudm(Instruction instr);
  void vmudn(Instruction instr);
  void vmrg(Instruction instr);
  void vlt(Instruction instr);
  void veq(Instruction instr);
  void vne(Instruction instr);
  void vge(Instruction instr);
  void vrcp(Instruction instr);
  void vrsq(Instruction instr);
  void vrcpl(Instruction instr);
  void vrsql(Instruction instr);
  void vrndp(Instruction instr);
  void vrndn(Instruction instr);
  void vrcph(Instruction instr);
  void vsar(Instruction instr);
  void vsub(Instruction instr);
  void vsubc(Instruction instr);
  void vxor(Instruction instr);
  void vnxor(Instruction instr);
  void vor(Instruction instr);
  void vnor(Instruction instr);
  void vzero(Instruction instr);
  void mfc0(const RDP &rdp, Instruction instr);
  void mtc0(Instruction instr) const;
  void mfc2(Instruction instr);
  void mtc2(Instruction instr);

  template <bool toRdram>
  void DMA();
  void WriteStatus(u32 value);

private:
  FORCE_INLINE void branch(const u16 address, const bool cond) {
    if (cond) {
      nextPC = address & 0xFFC;
    }
  }

  FORCE_INLINE void branch_likely(const u16 address, const bool cond) {
    if (cond) {
      nextPC = address & 0xFFC;
    } else {
      pc = nextPC & 0xFFC;
      nextPC = pc + 4;
    }
  }
};
} // namespace n64