open Util;;

type bit = bool
type 'a vect = bit list
type nibble = [`Four] vect
type byte7 = [`Seven] vect
type byte = [`Eight] vect
type word = [`Sixteen] vect
type word11 = [`Eleven] vect

type sizes = [ `Four | `Seven | `Eight | `Eleven | `Sixteen ]

let mk_nibble b1 b2 b3 b4 = [b1; b2; b3; b4]
let from_nibble =
 function
    [b1; b2; b3; b4] -> b1,b2,b3,b4
  | _ -> assert false
let mk_byte n1 n2 = n1 @ n2
let mk_byte_from_bits ((b1,b2,b3,b4),(b5,b6,b7,b8)) = ([b1;b2;b3;b4;b5;b6;b7;b8] : [`Eight] vect)
let from_byte =
 function
    b1::b2::b3::b4::r -> [b1;b2;b3;b4],r
  | _ -> assert false
let bits_of_byte =
 function
    [b1;b2;b3;b4;b5;b6;b7;b8] -> (b1,b2,b3,b4),(b5,b6,b7,b8)
  | _ -> assert false
let mk_byte7 b1 b2 b3 n1 = b1::b2::b3::n1
let from_byte7 =
 function
    b1::b2::b3::r -> b1,b2,b3,r
  | _ -> assert false
let mk_word = mk_byte
let from_word =
 function
    b1::b2::b3::b4::b5::b6::b7::b8::r -> [b1;b2;b3;b4;b5;b6;b7;b8],r
  | _ -> assert false
let mk_word11 = mk_byte7
let from_word11 = from_byte7

let get_bit l index =
  try
    List.nth (List.rev l) index
  with (Failure _ | Invalid_argument _) -> assert false

let set_bit l index new_val =
  try
    let rec aux index l = 
      match index, l with
        _, [] -> raise (Invalid_argument "")
      | 0,_::tl -> new_val::tl
      | n,hd::tl -> hd::(aux (n-1) tl) in
          List.rev (aux index (List.rev l))
  with Invalid_argument "" -> assert false

let (-&-) l1 l2 = List.map2 (fun b1 b2 -> b1 & b2) l1 l2
let (-|-) l1 l2 = List.map2 (fun b1 b2 -> b1 || b2) l1 l2
let xor b1 b2 = b1 <> b2
let (-^-) l1 l2 = List.map2 xor l1 l2
let complement l1 = List.map (not) l1

let iter_bits f v = String.concat "" (List.map f v)
let map_bits = List.map
let map2_bits = List.map2

let string_of_bit = function false -> "0" | true -> "1"
let string_of_vect l = String.concat "" (List.map string_of_bit l)

let full_add l r c = List.fold_right2 (fun b1 b2 (c,r) -> b1 & b2 || c & (b1 || b2),xor (xor b1 b2) c::r) l r (c,[])
let half_add l r = full_add l r false

let sign_extension =
 function
    [] -> assert false
  | (he::_) as l ->
      [he;he;he;he;he;he;he;he] @ l
;;
  

let rec split_last =
  function
    [] -> assert false
  | [he] -> he,[]
  | he::tl ->
      let l,res = split_last tl in
        l,he::res

let shift_left =
  function
    [] -> assert false
  | _::tl -> tl @ [false]
let shift_right l = false :: snd (split_last l)
let rotate_left =
  function
    [] -> assert false
  | he::tl -> tl @ [he]
let rotate_right l =
  let he,tl = split_last l in
    he::tl

(* CSC: can overflow!!! *)
let int_of_vect v =
  let rec aux pow v =
    match v with
      [] -> 0
    | hd::tl ->
        if hd = true then
          pow + (aux (pow * 2) tl)
        else
          aux (pow * 2) tl
  in
    aux 1 (List.rev v)

let size_lookup =
  function
    `Four -> 4
  | `Seven -> 7
  | `Eight -> 8
  | `Eleven -> 11
  | `Sixteen -> 16

let rec pow v p =
  if p = 0 then
    1
  else
    v * (pow v (p - 1))

let divide_with_remainder x y = (x / y, x mod y)

let rec aux i =
  if i < 0 then
    raise (Invalid_argument "Negative index")
  else
    let (d, r) = divide_with_remainder i 2 in
      if (d, r) = (0, 0) then
        []
      else if r = 0 then
        false :: aux d
      else
        true :: aux d

let rec pad i l = if i = 0 then l else false :: (pad (i - 1) l)

let vect_of_int i size =
  let big_list = List.rev (aux i) in
    if List.length big_list > size_lookup size then
      raise (Invalid_argument "BitVectors.vect_of_int: size not big enough")
    else
      let diff = size_lookup size - List.length big_list in
        pad diff big_list
    
let zero size = pad (size_lookup size) []

(* CSC: can overflow!!! *)
(* CSC: only works properly with bytes!!! *)
let hex_string_of_vect v = Printf.sprintf "%0 2X" (int_of_vect v);;

module WordMap =
  Map.Make (struct type t = word let compare = compare end);;