open BitVectors;;

exception Byte7_conversion

module type Map =
 sig
  type key
  type map
   val empty : map
   val find : key -> map -> byte
   val add : key -> byte -> map -> map
   val fold : (key -> byte -> 'b -> 'b) -> map -> 'b -> 'b
   val equal: (byte -> byte -> bool) -> map -> map -> bool
 end
;;

module Byte7Map : Map with type key = byte7 =
struct
  include Map.Make (struct type t = byte7 let compare = Pervasives.compare end)
  type map = byte t
  let find k m =
    try
      find k m
    with Not_found -> zero `Eight
  let fold = fold
  let equal = equal
end;;

module WordMap : Map with type key = word =
struct
  include Map.Make (struct type t = word let compare = Pervasives.compare end)
  type map = byte t
  let find k m =
    try
      find k m
    with Not_found -> zero `Eight
  let fold = fold
  let equal = equal
end;;

let int_of_bit =
  function
    false -> 0
  | true -> 1

let add8_with_c (b1 : [`Eight] vect) (b2 : [`Eight] vect) (c : bit) =
 let n1 = int_of_vect b1 in
 let n2 = int_of_vect b2 in
 let c = int_of_bit c in
 let res = n1 + n2 + c in
 let ac = n1 mod 16 + n2 mod 16 + c >= 16 in
 let c6 = n1 mod 128 + n2 mod 128 + c >= 128 in
 let res,c = res mod 256, res >= 256 in
 let ov = c <> c6 in
   vect_of_int res `Eight,c,ac,ov
;;

let add16_with_c (b1 : [`Sixteen] vect) (b2 : [`Sixteen] vect) (c : bit) =
 let n1 = int_of_vect b1 in
 let n2 = int_of_vect b2 in
 let c = int_of_bit c in
 let res = n1 + n2 + c in
 let ac = n1 mod 256 + n2 mod 256 + c >= 256 in
 let c6 = n1 mod 2097152 + n2 mod 2097152 + c >= 2097152 in
 let res,c = res mod 4194304, res >= 4194304 in
 let ov = c <> c6 in
   vect_of_int res `Sixteen,c,ac,ov
;;

let subb8_with_c (b1 : [`Eight] vect) (b2 : [`Eight] vect) (c : bit) =
 let n1 = int_of_vect b1 in
 let n2 = int_of_vect b2 in
 let c = int_of_bit c in
 let res = n1 - n2 - c in
 let ac = n1 mod 16 - n2 mod 16 - c < 0 in
 let c6 = n1 mod 128 - n2 mod 128 - c < 0 in
 let res,c =
  if res >= 0 then res,false
  else n1 + 256 - n2 - c, true in
 let ov = c <> c6 in
  (vect_of_int res `Eight,c,ac,ov)
;;

let dec b =
 let res = int_of_vect b - 1 in
  if res < 0 then vect_of_int 255 `Eight
  else vect_of_int res `Eight
;;

let inc b =
 let res = int_of_vect b + 1 in
  if res > 255 then (vect_of_int 0 `Eight : byte)
  else (vect_of_int res `Eight : byte)
;;

let byte7_of_bit b =
  [false;false;false;false;false;false;b]
;;

let byte_of_byte7 =
 function
    ([b1;b2;b3]::n) -> [false;b1;b2;b3]::n
  | _ -> assert false
;;

let addr16_of_addr11 pc a =
 let pc_upper, _ = from_word pc in
 let n1, n2 = from_byte pc_upper in
 let (b1,b2,b3,b) = from_word11 a in
 let (p1,p2,p3,p4),(p5,_,_,_) = from_nibble n1, from_nibble n2 in
  mk_word (mk_byte (mk_nibble p1 p2 p3 p4) (mk_nibble p5 b1 b2 b3)) b
;;