type t = 
  | Bohr of float array
  | Angstrom of float array

let a0 = Constants.a0

let zero = Bohr [| 0. ; 0. ; 0. |]

let of_float_triplet (x,y,z) = function
| `Bohr     -> Bohr     [|x;y;z|]
| `Angstrom -> Angstrom [|x;y;z|]

let of_3_floats x y z =
  of_float_triplet (x,y,z)

let to_string y =
  let result x = 
    (string_of_float x.(0))^" "^(string_of_float x.(1))^" "^(string_of_float x.(2))
  in
  match y with
  | Bohr     x -> (result x) ^ " Bohr"
  | Angstrom x -> (result x) ^ " Angstrom"


let extract_float_array = function
  | Bohr     a
  | Angstrom a -> a


(** Linear algebra *)
let (|.) s a =
  match a with
  | Bohr     [|x;y;z|] -> Bohr     [| s*.x; s*.y; s*.z |]
  | Angstrom [|x;y;z|] -> Angstrom [| s*.x; s*.y; s*.z |]
  | _ -> assert false

let to_Angstrom = function
  | Angstrom a -> Angstrom a
  | Bohr     a -> Angstrom (a0 |. Bohr a |> extract_float_array)

let to_Bohr = function
  | Angstrom a -> Bohr (1./.a0 |. Angstrom a |> extract_float_array)
  | Bohr     a -> Bohr a 

let (|-), (|+) =
  let rec op f p q =
    match (p, q) with
    | (Angstrom a, Angstrom b) -> Angstrom (f a b)
    | (Bohr     a, Bohr     b) -> Bohr     (f a b)
    | (Angstrom a, Bohr     b) -> op f (to_Bohr p) q
    | (Bohr     a, Angstrom b) -> op f p (to_Bohr q)
  in
  (op (fun a b -> 
       match a,b with
       | [|x;y;z|], [|x';y';z'|] -> [| x-.x'; y-.y'; z-.z' |]
       | _ -> assert false
     ) , 
   op (fun a b -> 
       match a,b with
       | [|x;y;z|], [|x';y';z'|] -> [| x+.x'; y+.y'; z+.z' |]
       | _ -> assert false
     )
  )


let dot p q =
  let f = function 
    | Bohr [|x;y;z|], Bohr [|x';y';z'|] ->  x*.x' +. y*.y' +. z*.z' 
    | _ -> assert false
  in
  f (to_Bohr p, to_Bohr q)


let norm u =
  sqrt @@ dot u u


let rec to_float_array a = 
  to_Bohr a |> extract_float_array

let x a = (extract_float_array @@ to_Bohr a).(0)
let y a = (extract_float_array @@ to_Bohr a).(1)
let z a = (extract_float_array @@ to_Bohr a).(2)


let coord a = function
  | 0 -> (extract_float_array @@ to_Bohr a).(0)
  | 1 -> (extract_float_array @@ to_Bohr a).(1)
  | 2 -> (extract_float_array @@ to_Bohr a).(2)
  | _ -> raise (Invalid_argument "Coordinate")