open OpcodeTable
open Processor

type nixbpe = {
  n : int;
  i : int;
  x : int;
  b : int;
  p : int;
  e : int
}

let string_of_nixbpe nixbpe =
  Printf.sprintf "n=%d,i=%d,x=%d,b=%d,p=%d,e=%d"
    nixbpe.n nixbpe.i nixbpe.x nixbpe.b nixbpe.p nixbpe.e
(*----Funkcije izvajalnika----*)
(*TODO - brali bomo vedno relativno od začetka instrukcije, PC bomo na koncu ukaza povečali za pravo velikost!*)

(*funkcije za javljanje napak*)
let notImplemented (mnemonic : string) = Printf.printf "mnemonic %s is not implemented!\n" mnemonic
let invalidOpcode (opcode : int) = Printf.printf "opcode %d is invalid!\n" opcode
let invalidAdressing () = Printf.printf "invalid adressing!\n"

(*spremeni disp v signed če se uporablja PC-relativno*)
let to_signed (x : int) : int =
  let mask12 = 0xFFF in      (* keep only 12 bits *)
  let x = x land mask12 in       (* mask input to 12 bits *)
  if x land 0x800 <> 0 then   (* if the 24th bit (sign bit) is set *)
    x - (1 lsl 12)               (* subtract 2^24 to get negative value *)
  else
    x 

(*beri drugi byte ukaza formata 2 in vrni r1 in r2, kot int njunih vrednosti (reg a -> 1, reg x -> 2 ...)*)
let readR1R2 (state : Processor.state) : (int * int) =
  let byte2 = Char.code (Processor.readMem state 1) in 
  let highNibble = (byte2 land 0xF0) lsr 4 in (*pridobi prvi nibble*)
  let lowNibble = byte2 land 0x0F in (*pridobi drugi nibble*)
  (highNibble, lowNibble)

(*preberi byta 1 in 2 in dobi nixbpe bite*)
let readNIXBPE (state : Processor.state) : nixbpe =
  let byte1 = Char.code (Processor.readMem state 0) in 
  let byte2 = Char.code (Processor.readMem state 1) in 
  {n = (byte1 lsr 1) land 1;
   i = byte1 land 1;
   x = (byte2 lsr 7) land 1;
   b = (byte2 lsr 6) land 1;
   p = (byte2 lsr 5) land 1;
   e = (byte2 lsr 4) land 1;}


(*dobi disp iz tipa ukaza 3*)
(*TODO pretvori v negativno število glede na nxibpe*)
let readDisp (state : Processor.state) : int = 
  let byte2 = Char.code (Processor.readMem state 1) in 
  let byte3 = Char.code (Processor.readMem state 2) in
  let disp_high = byte2 land 0x0F in
  let disp_low = byte3 in
  let disp = (disp_high lsl 8) lor disp_low in
  let hex_string = Printf.sprintf "%02X%02X" disp_high disp_low in
  Printf.printf "[Izvajalnik/readDisp]prebral disp:  0x%s = 0x%04X = %d\n" hex_string disp disp;
  disp

(*dobi address iz tip ukaza 4*)
(*TODO preveri ali mores paziti negativnost*)
let readAddress (state : Processor.state) : int =
  let byte2 = Char.code (Processor.readMem state 1) in 
  let byte3 = Char.code (Processor.readMem state 2) in
  let byte4 = Char.code (Processor.readMem state 3) in

  let addr_highest = byte2 land 0x0F in
  let addr_high = (addr_highest lsl 8) lor byte3 in
  let address = (addr_high lsl 8) lor byte4 in 
  address

(*pridobi effective address in operand*)
let getOperandF3 (state : Processor.state) (nixbpe : nixbpe) (disp : int) : int * int= 
    let ea =
      if nixbpe.b = 1 && nixbpe.p = 0 then state.regs.b + disp (*B relativno*)
      else if nixbpe.p = 1 && nixbpe.b = 0 then state.regs.pc + to_signed disp + 3(*PC relativno, ker PC povečamo šele po klicu te funkcije moramo tu +3*)
      else disp (*direktno*)
    in
    let ea = if nixbpe.x = 1 then ea + state.regs.x else ea in (*+ (X) po potrebi*)
      Printf.printf "[Izvajalnik/getOperand]effective address: 0x%06X = %d\n" ea ea;
    (*pridobi operand*)
    let value =
      if nixbpe.n = 1 && nixbpe.i = 1 then Processor.readMemAddr state ea (*direktno*)
      else if nixbpe.n = 0 && nixbpe.i = 1 then disp  (* immediate value *)
      else if nixbpe.n = 1 && nixbpe.i = 0 then Processor.readMemAddr state (Processor.readMemAddr state ea)  (* indirect *)
      else failwith "Invalid addressing mode"
    in
    ea ,value

let getOperandF4 (state : Processor.state) (nixbpe : nixbpe) (disp : int) : int * int = 
    let ea =
      if nixbpe.b = 1 && nixbpe.p = 0 then state.regs.b + disp (*B relativno*)
      else if nixbpe.p = 1 && nixbpe.b = 0 then state.regs.pc + disp + 4(*PC relativno, ker PC povečamo šele po klicu te funkcije moramo tu +4*)
      else disp (*direktno*)
    in
    let ea = if nixbpe.x = 1 then ea + state.regs.x else ea in (*+ (X) po potrebi*)
      Printf.printf "[Izvajalnik/getOperand]effective address: 0x%06X = %d\n" ea ea;
    (*pridobi operand*)
    let value =
      if nixbpe.n = 1 && nixbpe.i = 1 then Processor.readMemAddr state ea (*direktno*)
      else if nixbpe.n = 0 && nixbpe.i = 1 then disp  (* immediate value *)
      else if nixbpe.n = 1 && nixbpe.i = 0 then Processor.readMemAddr state (Processor.readMemAddr state ea)  (* indirect *)
      else failwith "Invalid addressing mode"
    in
    ea, value

(*execute format 1*)
let executeFormat1 (state : Processor.state) (mnemonic : OpcodeTable.mnemonic) : unit =
  Processor.pcIncrement state 1;
  (*debugging*)
  Printf.printf "[Izvajalnik/executeFormat1] Mnemonic: %s\n" 
    (string_of_mnemonic mnemonic);

  match mnemonic with
  | FIX -> IzvajalnikF1.fix state
  | FLOAT -> IzvajalnikF1.floatF state
  | HIO -> notImplemented "HIO"
  | NORM -> notImplemented "NORM"
  | SIO -> notImplemented "SIO"
  | TIO -> notImplemented "TIO"
  |_ -> failwith ("Mnemonic" ^  (OpcodeTable.string_of_mnemonic mnemonic) ^ "falsely flaged as format 1")



(*execute format 2*)
let executeFormat2 (state: Processor.state) (mnemonic : OpcodeTable.mnemonic) : unit =
  let (r1, r2) = readR1R2 state in 
  (*debugging*)
  Printf.printf "[Izvajalnik/executeFormat2] Mnemonic: %s, r1: %d, r2: %d\n" 
    (string_of_mnemonic mnemonic) r1 r2;

  Processor.pcIncrement state 2;
  match mnemonic with 
  | ADDR -> IzvajalnikF2.addr state r1 r2
  | CLEAR -> IzvajalnikF2.clear state r1
  | COMPR -> IzvajalnikF2.compr state r1 r2
  | DIVR -> IzvajalnikF2.divr state r1 r2
  | MULR -> IzvajalnikF2.mulr state r1 r2
  | RMO -> IzvajalnikF2.rmo state r1 r2
  | SHIFTL -> IzvajalnikF2.shiftl state r1 r2
  | SHIFTR -> IzvajalnikF2.shiftr state r1 r2
  | SUBR -> IzvajalnikF2.subr state r1 r2
  | SVC -> notImplemented "F2"
  | TIXR -> IzvajalnikF2.tixr state r1
  |_ -> failwith ("Mnemonic" ^  (OpcodeTable.string_of_mnemonic mnemonic) ^ "falsely flaged as format 2")



(*execute Format 3*)
let executeFormat3 (state : Processor.state) (nixbpe : nixbpe) (mnemonic: OpcodeTable.mnemonic): unit = 
  let disp = readDisp state in
  let ea ,operand = getOperandF3 state nixbpe disp in
  (*debugging*)
  Printf.printf "[Izvajalnik/executeFormat3] Mnemonic: %s, nixbpe: %s, operand: %d = 0x%02X\n" 
    (string_of_mnemonic mnemonic) (string_of_nixbpe nixbpe) operand operand;

  Processor.pcIncrement state 3; (*povecamo pc pred izvedbo ukaza*)
  match mnemonic with
  | ADD -> IzvajalnikF3.add state operand
  | ADDF -> notImplemented "ADDF3"
  | AND -> IzvajalnikF3.andF state operand
  | COMP -> IzvajalnikF3.comp state operand
  | COMPF -> notImplemented "COMPF3"
  | DIV -> IzvajalnikF3.div state operand
  | MUL -> IzvajalnikF3.mul state operand
  | OR -> IzvajalnikF3.orF state operand
  | SUB -> IzvajalnikF3.sub state operand
  | SUBF -> notImplemented "SUBF3"
  | TD -> notImplemented "TD3"
  | WD -> notImplemented "WD3"

  (* Jump / subroutine *)
  | J -> IzvajalnikF3.j state ea
  | JEQ -> IzvajalnikF3.jeq state ea
  | JGT -> IzvajalnikF3.jgt state ea
  | JLT -> IzvajalnikF3.jlt state ea
  | JSUB -> IzvajalnikF3.jsub state ea
  | RSUB -> IzvajalnikF3.rsub state

  (* Load/store *)
  | LDA -> IzvajalnikF3.lda state operand
  | LDB -> IzvajalnikF3.ldb state operand
  | LDCH -> notImplemented "LDCH3"
  | LDF -> notImplemented "LDF3"
  | LDL -> IzvajalnikF3.ldl state operand
  | LDS -> IzvajalnikF3.lds state operand
  | LDT -> IzvajalnikF3.ldt state operand
  | LDX -> IzvajalnikF3.ldx state operand
  | LPS -> notImplemented "LPS4"
  | STA -> IzvajalnikF3.sta state operand
  | STB -> IzvajalnikF3.stb state operand
  | STCH -> notImplemented "STCH4"
  | STF -> notImplemented "STF4"
  | STL -> IzvajalnikF3.stl state operand
  | STS -> IzvajalnikF3.sts state operand
  | STSW -> IzvajalnikF3.stsw state operand
  | STT -> IzvajalnikF3.stt state operand
  | STX -> IzvajalnikF3.stx state operand

  (* Control / IO *)
  | TIX -> IzvajalnikF3.tix state operand
  |_ -> failwith ("Mnemonic" ^  (OpcodeTable.string_of_mnemonic mnemonic) ^ "falsely flaged as format 3")


let executeFormat4 (state : Processor.state) (nixbpe : nixbpe) (mnemonic: OpcodeTable.mnemonic): unit = 
  let address = readAddress state in
  let _, operand = getOperandF4 state nixbpe address in
  (*debugging*)
  Printf.printf "[Izvajalnik/executeFormat4] Mnemonic: %s, nixbpe: %s, operand: %d = 0x%02X\n" 
    (string_of_mnemonic mnemonic) (string_of_nixbpe nixbpe) operand operand;

  Processor.pcIncrement state 4;
  match mnemonic with
  | ADD -> IzvajalnikF4.add state operand
  | ADDF -> notImplemented "ADDF3"
  | AND -> IzvajalnikF4.andF state operand
  | COMP -> IzvajalnikF4.comp state operand
  | COMPF -> notImplemented "COMPF3"
  | DIV -> IzvajalnikF4.div state operand
  | MUL -> IzvajalnikF4.mul state operand
  | OR -> IzvajalnikF4.orF state operand
  | SUB -> IzvajalnikF4.sub state operand
  | SUBF -> notImplemented "SUBF3"
  | TD -> notImplemented "TD3"
  | WD -> notImplemented "WD3"

  (* Jump / subroutine *)
  | J -> IzvajalnikF4.j state operand
  | JEQ -> IzvajalnikF4.jeq state operand
  | JGT -> IzvajalnikF4.jgt state operand
  | JLT -> IzvajalnikF4.jlt state operand
  | JSUB -> IzvajalnikF4.jsub state operand
  | RSUB -> IzvajalnikF4.rsub state

  (* Load/store *)
  | LDA -> IzvajalnikF4.lda state operand
  | LDB -> IzvajalnikF4.ldb state operand
  | LDCH -> notImplemented "LDCH3"
  | LDF -> notImplemented "LDF3"
  | LDL -> IzvajalnikF4.ldl state operand
  | LDS -> IzvajalnikF4.lds state operand
  | LDT -> IzvajalnikF4.ldt state operand
  | LDX -> IzvajalnikF4.ldx state operand
  | LPS -> notImplemented "LPS4"
  | STA -> IzvajalnikF4.sta state operand
  | STB -> IzvajalnikF4.stb state operand
  | STCH -> notImplemented "STCH4"
  | STF -> notImplemented "STF4"
  | STL -> IzvajalnikF4.stl state operand
  | STS -> IzvajalnikF4.sts state operand
  | STSW -> IzvajalnikF4.stsw state operand
  | STT -> IzvajalnikF4.stt state operand
  | STX -> IzvajalnikF4.stx state operand

  (* Control / IO *)
  | TIX -> IzvajalnikF3.tix state operand
  |_ -> failwith ("Mnemonic" ^  (OpcodeTable.string_of_mnemonic mnemonic) ^ "falsely flaged as format 3")



(*execute format 3_4*)
let executeFormat3_4 (state : Processor.state) (mnemonic : OpcodeTable.mnemonic) : unit = 
  let nixbpe = readNIXBPE state in 
  (*debugging*)
  Printf.printf "[Izvajalnik/executeFormat3_4]n=%d,i=%d,x=%d,b=%d,p=%d,e=%d\n" nixbpe.n nixbpe.i nixbpe.x nixbpe.b nixbpe.p nixbpe.e;
  
  match nixbpe.e with
  | 0 -> executeFormat3 state nixbpe mnemonic
  | 1 -> executeFormat4 state nixbpe mnemonic
  | _ -> failwith "invalid computation of nxbpe"

(*execute ukaza*)
let execute (state : Processor.state) : unit =
  let byte1 = Processor.readMem state 0 in (*read the 1st byte of the instruction*)
 (*debugging*)
  Printf.printf "[Izvajalnik/execute]Prebral byte1 %c = 0x%02X\n" byte1 (Char.code byte1);
 
  try
    let info  = Decoder.decoder byte1 in (*determen the format of the instruction from the 1st byte*)
    (*debugging*)
    Printf.printf "[Izvajalnik/execute]Opcode %s, format %s\n" (string_of_mnemonic info.mnemonic) (format_str info.format);
    
    match info.format with
    | F1 -> executeFormat1 state info.mnemonic
    | F2 -> executeFormat2 state info.mnemonic
    | F3_4 -> executeFormat3_4 state info.mnemonic
  with
    | Failure msg -> Printf.printf "Cought faliure %s \n" msg;