open Util
open Constants

let cutoff2 = cutoff *. cutoff
let debug = false

exception NullQuartet

(** Horizontal and Vertical Recurrence Relations (HVRR) *)
let hvrr_two_e (angMom_a, angMom_b, angMom_c, angMom_d)
    (totAngMom_a, totAngMom_b, totAngMom_c, totAngMom_d)
    (maxm, zero_m_array)
    (expo_b, expo_d)
    (expo_inv_p, expo_inv_q)
    (center_ab, center_cd, center_pq)
    map_1d map_2d
  =

  let totAngMom_a = Angular_momentum.to_int totAngMom_a
  and totAngMom_b = Angular_momentum.to_int totAngMom_b
  and totAngMom_c = Angular_momentum.to_int totAngMom_c
  and totAngMom_d = Angular_momentum.to_int totAngMom_d
  in
  let maxm = totAngMom_a + totAngMom_b + totAngMom_c + totAngMom_d in
  let empty = Array.make (maxm+1) 0. 
  in
  if debug then begin
    Printf.printf "\n---- %d %d %d %d ----\n"  totAngMom_a totAngMom_b totAngMom_c totAngMom_d;
    Printf.printf "%d %d %d\n"  angMom_a.(0)  angMom_a.(1)  angMom_a.(2) ;
    Printf.printf "%d %d %d\n"  angMom_b.(0)  angMom_b.(1)  angMom_b.(2) ;
    Printf.printf "%d %d %d\n"  angMom_c.(0)  angMom_c.(1)  angMom_c.(2) ;
    Printf.printf "%d %d %d\n"  angMom_d.(0)  angMom_d.(1)  angMom_d.(2) ;
    Printf.printf "%f %f %f %f\n%f %f %f\n%f %f %f\n%f %f %f\n"  expo_b expo_d
    expo_inv_p expo_inv_q
    (Coordinate.coord center_ab 0) (Coordinate.coord center_ab 1) (Coordinate.coord center_ab 2)
    (Coordinate.coord center_cd 0) (Coordinate.coord center_cd 1) (Coordinate.coord center_cd 2)
    (Coordinate.coord center_pq 0) (Coordinate.coord center_pq 1) (Coordinate.coord center_pq 2)
  end;

  (** Vertical recurrence relations *)
  let rec vrr0 angMom_a totAngMom_a = 
    if debug then
    begin
      let (x,y,z) = angMom_a in
      Printf.printf "vrr0: %d : %d %d %d\n" totAngMom_a x y z
    end;
    match totAngMom_a with 
    | 0 -> zero_m_array
    | _ ->
      let maxsze = maxm+1 in
      let key = Zkey.of_int_tuple (Zkey.Three angMom_a) in

      try Zmap.find map_1d key with
      | Not_found ->
            let result = 
	      let am, amm, amxyz, xyz  =
		match angMom_a with
		| (x,0,0) -> (x-1,0,0),(x-2,0,0), x-1, 0
		| (x,y,0) -> (x,y-1,0),(x,y-2,0), y-1, 1
		| (x,y,z) -> (x,y,z-1),(x,y,z-2), z-1, 2
	      in
              if amxyz < 0 then empty else
                let v1 = 
                    vrr0 am (totAngMom_a-1) 
                in
                let f1 = expo_inv_p *. (Coordinate.coord center_pq xyz) 
                and f2 = expo_b *. expo_inv_p *.  (Coordinate.coord center_ab xyz)
                in
                if amxyz < 1 then
                  Array.init maxsze (fun m ->
                    if m = maxm then 0. else (f1 *. v1.(m+1) ) -. f2 *. v1.(m) )
                else
                  let f3 = (float_of_int amxyz) *. expo_inv_p *. 0.5 in
                  let v3 = 
                      vrr0 amm (totAngMom_a-2) 
                  in
                  Array.init maxsze (fun m -> 
                    (if m = maxm then 0. else
                      (f1 *. v1.(m+1) ) -. f2 *. v1.(m) )
                      +. f3 *. (v3.(m) +.  if m = maxm then 0. else
                        expo_inv_p *. v3.(m+1))
                    )
              in Zmap.add map_1d key result;
      result


  and vrr angMom_a angMom_c totAngMom_a totAngMom_c =

    if debug then
    begin
      let angMom_ax, angMom_ay, angMom_az = angMom_a in
      let angMom_cx, angMom_cy, angMom_cz = angMom_c in
      Printf.printf "vrr : %d %d : %d %d %d  %d %d %d\n" totAngMom_a totAngMom_c
        angMom_ax angMom_ay angMom_az angMom_cx angMom_cy angMom_cz
    end;

    match (totAngMom_a, totAngMom_c) with
    | (i,0) -> if (i>0) then vrr0 angMom_a totAngMom_a else zero_m_array 
    | (_,_) ->
      let maxsze = maxm+1 in
      let key =  Zkey.of_int_tuple (Zkey.Six (angMom_a, angMom_c) ) in

      try Zmap.find map_2d key with
      | Not_found -> 
            let result = 
                        let am, cm, cmm, axyz, cxyz, xyz =
                          let angMom_ax, angMom_ay, angMom_az = angMom_a 
                          and angMom_cx, angMom_cy, angMom_cz = angMom_c in
			  if (angMom_cz > 0) then
			    (angMom_ax, angMom_ay, angMom_az-1),
			    (angMom_cx, angMom_cy, angMom_cz-1),
			    (angMom_cx, angMom_cy, angMom_cz-2),
			    angMom_az,angMom_cz, 2 
			  else if (angMom_cy > 0) then
			    (angMom_ax, angMom_ay-1, angMom_az),
			    (angMom_cx, angMom_cy-1, angMom_cz),
			    (angMom_cx, angMom_cy-2, angMom_cz),
			    angMom_ay,angMom_cy, 1
			  else
			    (angMom_ax-1, angMom_ay, angMom_az),
			    (angMom_cx-1, angMom_cy, angMom_cz),
			    (angMom_cx-2, angMom_cy, angMom_cz),
			    angMom_ax,angMom_cx, 0
                        in
                        (*
                        if cxyz < 1 then empty else
                        *)
                          let f1 =
                            -. expo_d *. expo_inv_q *. (Coordinate.coord center_cd xyz) 
                          in
                          let f2 = 
                            expo_inv_q *. (Coordinate.coord center_pq xyz)
                          in
                          let result = 
                            if ( (abs_float f1 < cutoff) && (abs_float f2 < cutoff) ) then empty else
                              let v1 = 
                                vrr angMom_a cm totAngMom_a (totAngMom_c-1)
                              in
                              Array.init maxsze (fun m ->
                                f1 *. v1.(m) -. (if m = maxm then 0. else f2 *. v1.(m+1)) )
                          in
                          let result = 
                            if cxyz < 2 then result else
                              let f3 =
                                (float_of_int (cxyz-1)) *. expo_inv_q *. 0.5 
                              in
                              if (abs_float f3 < cutoff) && (abs_float (f3 *. expo_inv_q) < cutoff) then result else
                                (
                                let v3 = 
                                  vrr angMom_a cmm totAngMom_a (totAngMom_c-2)
                                in
                                Array.init maxsze (fun m -> result.(m) +.
                                  f3 *. (v3.(m) +. (if m=maxm then 0. else expo_inv_q *. v3.(m+1)) ))
                                )
                          in
                          let result = 
                            if axyz < 1 || cxyz < 1 then result else
                              let f5 = 
                                (float_of_int axyz) *. expo_inv_p *. expo_inv_q *. 0.5 
                              in
                              if (abs_float f5 < cutoff) then result else
                                let v5 =
                                  vrr am cm (totAngMom_a-1) (totAngMom_c-1)
                                in
                                Array.init (maxsze) (fun m -> 
                                  result.(m) -. (if m = maxm then 0. else f5 *. v5.(m+1)))
                          in
                          result
              in Zmap.add map_2d key result;
      result





  (** Horizontal recurrence relations *)
  and hrr0 angMom_a angMom_b angMom_c 
      totAngMom_a totAngMom_b totAngMom_c =

    if debug then
    begin
      let angMom_ax, angMom_ay, angMom_az = angMom_a 
      and angMom_bx, angMom_by, angMom_bz = angMom_b 
      and angMom_cx, angMom_cy, angMom_cz = angMom_c in
      Printf.printf "hrr0: %d %d %d : %d %d %d  %d %d %d  %d %d %d\n" 
        totAngMom_a totAngMom_b totAngMom_c 
        angMom_ax angMom_ay angMom_az
        angMom_bx angMom_by angMom_bz
        angMom_cx angMom_cy angMom_cz
    end;

    match totAngMom_b with
    | 0 -> (vrr angMom_a angMom_c totAngMom_a totAngMom_c).(0)
    | 1 ->
            let angMom_ax, angMom_ay, angMom_az = angMom_a in
            let ap, xyz =
              match angMom_b with
              | (_,_,1) -> (angMom_ax,angMom_ay,angMom_az+1), 2
              | (_,1,_) -> (angMom_ax,angMom_ay+1,angMom_az), 1 
              | _       -> (angMom_ax+1,angMom_ay,angMom_az), 0
            in
            let v1 = 
              vrr ap angMom_c (totAngMom_a+1) totAngMom_c
            in
            let f2 =
              (Coordinate.coord center_ab xyz)
            in
            if (abs_float f2 < cutoff) then v1.(0) else
              let v2 = 
                vrr angMom_a angMom_c totAngMom_a totAngMom_c
              in
              v1.(0) +. f2 *. v2.(0)
    | _ ->
        let angMom_ax, angMom_ay, angMom_az = angMom_a 
        and angMom_bx, angMom_by, angMom_bz = angMom_b in
        let bxyz, xyz =
              match angMom_b with
              | (_,_,1) -> angMom_bz, 2
              | (_,1,_) -> angMom_by, 1
              | _       -> angMom_bx, 0
        in
        if (bxyz < 1) then 0. else
          let ap, bm =
                match xyz with
                | 0 -> (angMom_ax+1,angMom_ay,angMom_az),(angMom_bx-1,angMom_by,angMom_bz)
                | 1 -> (angMom_ax,angMom_ay+1,angMom_az),(angMom_bx,angMom_by-1,angMom_bz)
                | _ -> (angMom_ax,angMom_ay,angMom_az+1),(angMom_bx,angMom_by,angMom_bz-1)
          in
          let h1 = 
            hrr0 ap bm angMom_c (totAngMom_a+1) (totAngMom_b-1) totAngMom_c 
          in
          let f2 =
            (Coordinate.coord center_ab xyz)
          in
          if (abs_float f2 < cutoff) then h1 else
            let h2 = 
              hrr0 angMom_a bm angMom_c totAngMom_a (totAngMom_b-1) totAngMom_c 
            in
            h1 +. f2 *. h2

  and hrr angMom_a angMom_b angMom_c angMom_d
      totAngMom_a totAngMom_b totAngMom_c totAngMom_d =

    if debug then
    begin
      let angMom_ax, angMom_ay, angMom_az = angMom_a in
      let angMom_bx, angMom_by, angMom_bz = angMom_b in
      let angMom_cx, angMom_cy, angMom_cz = angMom_c in
      let angMom_dx, angMom_dy, angMom_dz = angMom_d in
      Printf.printf "hrr : %d %d %d %d : %d %d %d  %d %d %d  %d %d %d  %d %d %d\n"
        totAngMom_a totAngMom_b totAngMom_c totAngMom_d
        angMom_ax angMom_ay angMom_az
        angMom_bx angMom_by angMom_bz
        angMom_cx angMom_cy angMom_cz
        angMom_dx angMom_dy angMom_dz
    end;

    match (totAngMom_b, totAngMom_d) with
    | (_,0) -> if (totAngMom_b = 0) then 
                (vrr angMom_a angMom_c totAngMom_a totAngMom_c).(0)
               else
                hrr0 angMom_a angMom_b angMom_c totAngMom_a totAngMom_b totAngMom_c
    | (_,_) ->
      let (angMom_cx, angMom_cy, angMom_cz) = angMom_c 
      and (angMom_dx, angMom_dy, angMom_dz) = angMom_d in
      let cp, dm, xyz =
        match angMom_d with
        | (_,0,0) -> (angMom_cx+1, angMom_cy, angMom_cz), (angMom_dx-1, angMom_dy, angMom_dz), 0
        | (_,_,0) -> (angMom_cx, angMom_cy+1, angMom_cz), (angMom_dx, angMom_dy-1, angMom_dz), 1
        | _       -> (angMom_cx, angMom_cy, angMom_cz+1), (angMom_dx, angMom_dy, angMom_dz-1), 2
      in
      let h1 = 
        hrr angMom_a angMom_b cp dm totAngMom_a totAngMom_b (totAngMom_c+1) (totAngMom_d-1) 
      in
      let f2 = Coordinate.coord center_cd xyz in
      if (abs_float f2 < cutoff) then h1 else
        let h2 = 
          hrr angMom_a angMom_b angMom_c dm totAngMom_a totAngMom_b totAngMom_c (totAngMom_d-1) 
        in
        h1 +. f2 *. h2

  in
  hrr
    (angMom_a.(0),angMom_a.(1),angMom_a.(2))
    (angMom_b.(0),angMom_b.(1),angMom_b.(2))
    (angMom_c.(0),angMom_c.(1),angMom_c.(2))
    (angMom_d.(0),angMom_d.(1),angMom_d.(2))
    totAngMom_a totAngMom_b totAngMom_c totAngMom_d




let contracted_class_shell_pairs ~zero_m ?schwartz_p ?schwartz_q shell_p shell_q : float Zmap.t =

  let shell_a = shell_p.(0).Shell_pair.shell_a
  and shell_b = shell_p.(0).Shell_pair.shell_b
  and shell_c = shell_q.(0).Shell_pair.shell_a
  and shell_d = shell_q.(0).Shell_pair.shell_b
  in
  let maxm = 
    let open Angular_momentum in
    (to_int @@ Contracted_shell.totAngMom shell_a) + (to_int @@ Contracted_shell.totAngMom shell_b) 
    + (to_int @@ Contracted_shell.totAngMom shell_c) + (to_int @@ Contracted_shell.totAngMom shell_d)
  in

  (* Pre-computation of integral class indices *)
  let class_indices = 
    Angular_momentum.zkey_array
      (Angular_momentum.Quartet
         Contracted_shell.(totAngMom shell_a, totAngMom shell_b,
                           totAngMom shell_c, totAngMom shell_d))
  in

  let contracted_class = 
    Array.make (Array.length class_indices) 0.;
  in

  (* Compute all integrals in the shell for each pair of significant shell pairs *)

  for ab=0 to (Array.length shell_p - 1) do
    let cab = shell_p.(ab).Shell_pair.coef  in
    let b = shell_p.(ab).Shell_pair.j in
    let norm_coef_scale_p = shell_p.(ab).Shell_pair.norm_coef_scale in
    for cd=0 to (Array.length shell_q - 1) do
      let coef_prod =
        cab *. shell_q.(cd).Shell_pair.coef
      in
      (** Screening on the product of coefficients *)
      try
        if (abs_float coef_prod) < 1.e-4*.cutoff then
          raise NullQuartet;

        let expo_pq_inv =
          shell_p.(ab).Shell_pair.expo_inv +. shell_q.(cd).Shell_pair.expo_inv
        in
        let center_pq = 
          Coordinate.(shell_p.(ab).Shell_pair.center |- shell_q.(cd).Shell_pair.center)
        in
        let norm_pq_sq =
          Coordinate.dot center_pq center_pq
        in

        let zero_m_array = 
           zero_m ~maxm ~expo_pq_inv ~norm_pq_sq
        in
        begin
          match Contracted_shell.(totAngMom shell_a, totAngMom shell_b,
                                  totAngMom shell_c, totAngMom shell_d) with
          | Angular_momentum.(S,S,S,S) ->
              let integral =
                zero_m_array.(0)
              in
              contracted_class.(0) <- contracted_class.(0) +. coef_prod *. integral
          | _ -> 
              let d = shell_q.(cd).Shell_pair.j in
              let map_1d = Zmap.create (4*maxm) in
              let map_2d = Zmap.create (Array.length class_indices) in
              let norm_coef_scale_q = shell_q.(cd).Shell_pair.norm_coef_scale in
              let norm_coef_scale =
                Array.map (fun v1 ->
                  Array.map (fun v2 -> v1 *. v2) norm_coef_scale_q
                ) norm_coef_scale_p
                |> Array.to_list
                |> Array.concat
              in
              (*
              let monocentric =
                shell_p.(ab).Shell_pair.monocentric &&
                shell_q.(cd).Shell_pair.monocentric 
              in
              *)
              (* Compute the integral class from the primitive shell quartet *)
              class_indices
              |> Array.iteri (fun i key ->
                  let a = Zkey.to_int_array Zkey.Kind_12 key in
                  let (angMomA,angMomB,angMomC,angMomD) =
                    ( [| a.(0) ;  a.(1) ;  a.(2) |], 
                      [| a.(3) ;  a.(4) ;  a.(5) |], 
                      [| a.(6) ;  a.(7) ;  a.(8) |], 
                      [| a.(9) ; a.(10) ; a.(11) |] )
                  in
                  try
                    (*
                    if monocentric then
                      begin
                        if ( ((1 land (a.(0) + a.(3) + a.(6) + a.( 9)))=1) ||
                             ((1 land (a.(1) + a.(4) + a.(7) + a.(10)))=1) ||
                             ((1 land (a.(2) + a.(5) + a.(8) + a.(11)))=1) 
                           ) then
                          raise NullQuartet
                      end;
                      *)
                    (* Schwartz screening *)
                  (*
                  let schwartz_p = 
                    let key =
                    Zkey.of_int_array Zkey.Kind_12 
                        [| a.(0) ;  a.(1) ;  a.(2) ;
                          a.(3) ;  a.(4) ;  a.(5) ;
                          a.(0) ;  a.(1) ;  a.(2) ;
                          a.(3) ;  a.(4) ;  a.(5) |]
                    in
                    match schwartz_p with
                    | None -> 1.
                    | Some schwartz_p -> Zmap.find schwartz_p key
                  in
                  if schwartz_p < cutoff then raise NullQuartet;
                  let schwartz_q = 
                    let key =
                    Zkey.of_int_array Zkey.Kind_12 
                        [| a.(6) ;  a.(7) ;  a.(8) ;
                          a.(9) ; a.(10) ; a.(11) ;
                          a.(6) ;  a.(7) ;  a.(8) ;
                          a.(9) ; a.(10) ; a.(11) |]
                    in
                    match schwartz_q with
                    | None -> 1.
                    | Some schwartz_q -> Zmap.find schwartz_q key
                  in
                  if schwartz_p *. schwartz_q < cutoff2 then raise NullQuartet;
                  *)

                    let norm =  norm_coef_scale.(i) in
                    let coef_prod = coef_prod *. norm in
                    let integral = 
                        hvrr_two_e (angMomA, angMomB, angMomC, angMomD)
                          (Contracted_shell.totAngMom shell_a, Contracted_shell.totAngMom shell_b,
                           Contracted_shell.totAngMom shell_c, Contracted_shell.totAngMom shell_d)
                          (maxm, zero_m_array)
                          (Contracted_shell.expo shell_b b, Contracted_shell.expo shell_d d)
                          (shell_p.(ab).Shell_pair.expo_inv, shell_q.(cd).Shell_pair.expo_inv)
                          (shell_p.(ab).Shell_pair.center_ab, shell_q.(cd).Shell_pair.center_ab, center_pq)
                          map_1d map_2d 
                    in
                    contracted_class.(i) <- contracted_class.(i) +. coef_prod *. integral
                  with NullQuartet -> ()
                )
        end
    with NullQuartet -> ()
    done
  done;

  let result = 
    Zmap.create (Array.length contracted_class)
  in
  Array.iteri (fun i key -> Zmap.add result key contracted_class.(i)) class_indices;
  result



(** Computes all the two-electron integrals of the contracted shell quartet *)
let contracted_class ~zero_m shell_a shell_b shell_c shell_d : float Zmap.t =

  let shell_p = Shell_pair.create_array ~cutoff shell_a shell_b
  and shell_q = Shell_pair.create_array ~cutoff shell_c shell_d
  in
  contracted_class_shell_pairs ~zero_m shell_p shell_q