open Util

let cutoff = Constants.cutoff 
let cutoff2 = cutoff *. cutoff

exception NullQuartet
exception Found

let at_least_one_valid arr =
  try
    Array.fold_left (fun _ x -> if (abs_float x > cutoff) then raise Found else false ) false arr
  with Found -> true

(** Horizontal and Vertical Recurrence Relations (HVRR) *)
let hvrr_two_e_vector (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)
    coef_prod map_1d map_2d
  =

  let ncoef = (Array.length coef_prod) in
  let empty =
    Array.make ncoef 0. 
  in

  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

  (** Vertical recurrence relations *)
  let rec vrr0_v m angMom_a = function
    | 1 ->
      let xyz =
        match angMom_a with 
        | (1,_,_) -> 0 
        | (_,1,_) -> 1 
        | _       -> 2 
      in
      let f = expo_b *. (Coordinate.coord center_ab xyz) in
      Array.init ncoef (fun k -> coef_prod.(k) *.  expo_inv_p *.
        ( (Coordinate.coord center_pq.(k) xyz) *. zero_m_array.(m+1).(k)
          -. f *. zero_m_array.(m).(k) ) ) 
    | 0 -> Array.map2 ( *. ) zero_m_array.(m) coef_prod
    | totAngMom_a ->
      let key = Zkey.of_int_tuple (Zkey.Three angMom_a)
      in

      try Zmap.find map_1d.(m) 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 = 
		  let f = 
		    -. expo_b *. expo_inv_p *. (Coordinate.coord center_ab xyz)
		  in
		  if (abs_float f < cutoff) then empty else
		    Array.map (fun v1k -> f *. v1k) (vrr0_v m am (totAngMom_a-1) )
		in
		let p1 = 
		      
		  Array.mapi (fun k v2k -> v1.(k) +. expo_inv_p *. (Coordinate.coord center_pq.(k) xyz) *. v2k) (vrr0_v (m+1) am (totAngMom_a-1))
		in
		if amxyz < 1 then p1 else
		  let f = (float_of_int amxyz) *. expo_inv_p *. 0.5
		  in
		  if (abs_float f < cutoff) then empty else
		    let v1 = vrr0_v m amm (totAngMom_a-2)  
		    in
		    let v2 = 
		      if (abs_float (f *. expo_inv_p)) < cutoff then empty else
		      vrr0_v (m+1) amm (totAngMom_a-2) 
		    in
		    Array.init ncoef (fun k -> p1.(k) +.
			f *. (v1.(k) +. v2.(k) *. expo_inv_p ) ) 
		in Zmap.add map_1d.(m) key result;
	    result

  and vrr_v m angMom_a angMom_c totAngMom_a totAngMom_c =

    match (totAngMom_a, totAngMom_c) with
    | (i,0) -> if (i>0) then
                 vrr0_v m angMom_a totAngMom_a
               else
                 Array.map2 ( *. ) zero_m_array.(m) coef_prod
    | (_,_) ->

      let key =  Zkey.of_int_tuple (Zkey.Six (angMom_a, angMom_c))
      in

      try Zmap.find map_2d.(m) key with
        | Not_found -> 
	    let result = 
              begin
		let am, cm, cmm, axyz, cxyz, xyz =
		  let (aax, aay, aaz) = angMom_a 
		  and (acx, acy, acz) = angMom_c in
                  if (acz > 0) then
		    (aax, aay, aaz-1),
		    (acx, acy, acz-1),
		    (acx, acy, acz-2),
		     aaz, acz, 2
		  else if (acy > 0) then
		    (aax, aay-1,aaz),
		    (acx, acy-1,acz),
		    (acx, acy-2,acz),
		     aay,acy, 1
		  else
		    (aax-1,aay,aaz),
		    (acx-1,acy,acz),
		    (acx-2,acy,acz),
		     aax,acx, 0
		in
                (*
		if cxyz < 1 then empty else
                *)
		  let f1 =
		    Array.init ncoef (fun k -> 
			expo_d.(k) *. expo_inv_q.(k) *.
                        (Coordinate.coord center_cd.(k) xyz) ) 
		  in
		  let f2 =
		    Array.init ncoef (fun k -> 
			expo_inv_q.(k) *. (Coordinate.coord center_pq.(k) xyz) )
		  in
		  let v1 = 
		    if (at_least_one_valid f1) then
		      vrr_v m angMom_a cm totAngMom_a (totAngMom_c-1) 
		    else empty
		  and v2 =
		    if (at_least_one_valid f2) then
		      vrr_v (m+1) angMom_a cm totAngMom_a (totAngMom_c-1)
		    else empty
		  in
		  let p1 = 
		    Array.init ncoef (fun k -> -. v1.(k) *. f1.(k) -. v2.(k) *. f2.(k)) 
		  in
		  let p2 = 
		    if cxyz < 2 then p1 else
		      let fcm = 
			(float_of_int (cxyz-1)) *. 0.5
		      in
		      let f1 =
			Array.map (fun e -> fcm *. e) expo_inv_q
		      in
		      let f2 =
			Array.map2 ( *. ) f1 expo_inv_q 
		      in
		      let v1 = 
			if (at_least_one_valid f1) then
			  vrr_v m angMom_a cmm totAngMom_a (totAngMom_c-2)
			else empty
		      in
		      let v2 = 
			if (at_least_one_valid f2) then
			  vrr_v (m+1) angMom_a cmm totAngMom_a (totAngMom_c-2) 
			else empty
		      in
		      Array.init ncoef (fun k -> p1.(k) +.  f1.(k) *. v1.(k) +. f2.(k) *. v2.(k)) 
		    in
		    if (axyz < 1) || (cxyz < 1) then p2 else
		      let fa =
			(float_of_int axyz) *. expo_inv_p *. 0.5
		      in
		      let f1 =
			Array.map (fun e -> fa *. e ) expo_inv_q
		      in
		      if (at_least_one_valid f1) then
			let v = 
			  vrr_v (m+1) am cm (totAngMom_a-1) (totAngMom_c-1) 
			in
			Array.init ncoef (fun k -> p2.(k) -. f1.(k) *. v.(k)) 
		      else p2
              end
	      in Zmap.add map_2d.(m) key result;
      result




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

    match totAngMom_b with
    | 0 ->
      begin
        match (totAngMom_a, totAngMom_c) with
        | (0,0) -> Array.map2 ( *. )  zero_m_array.(0) coef_prod
        | (_,0) -> vrr0_v 0 angMom_a totAngMom_a
        | (_,_) -> vrr_v 0 angMom_a angMom_c totAngMom_a totAngMom_c
      end
    | 1 ->
      let (aax, aay, aaz) = angMom_a in
      let ap, xyz =
	match angMom_b with
	| (_,_,1) -> (aax,aay,aaz+1), 2
	| (_,1,_) -> (aax,aay+1,aaz), 1
	| (_,_,_) -> (aax+1,aay,aaz), 0
      in
      let f = Coordinate.coord center_ab xyz in
      let v1 = 
          vrr_v 0 ap angMom_c (totAngMom_a+1) totAngMom_c
      in
      if (abs_float f < cutoff) then v1 else
        let v2 = 
          vrr_v 0 angMom_a angMom_c totAngMom_a totAngMom_c
        in
        Array.map2 (fun v1 v2 -> v1 +. v2 *. f) v1 v2
    | _ ->
      let (aax, aay, aaz) = angMom_a
      and (abx, aby, abz) = angMom_b in
      let bxyz, xyz =
	    match angMom_b with
	    | (_,_,1) -> abz, 2
	    | (_,1,_) -> aby, 1
	    | (_,_,_) -> abx, 0
      in
      if (bxyz < 1) then empty else
	let ap, bm =
	      match xyz with
	      | 0 -> (aax+1,aay,aaz),(abx-1,aby,abz)
	      | 1 -> (aax,aay+1,aaz),(abx,aby-1,abz)
	      | _ -> (aax,aay,aaz+1),(abx,aby,abz-1)
	in

        let h1 = 
          hrr0_v ap bm angMom_c (totAngMom_a+1) (totAngMom_b-1) totAngMom_c 
        in
        let f = (Coordinate.coord center_ab xyz) in
        if (abs_float f < cutoff) then h1 else
        let h2 = 
          hrr0_v angMom_a bm angMom_c totAngMom_a (totAngMom_b-1) totAngMom_c 
        in Array.map2 (fun h1 h2 -> h1 +. h2 *. f) h1 h2

  and hrr_v angMom_a angMom_b angMom_c angMom_d
      totAngMom_a totAngMom_b totAngMom_c totAngMom_d =

    match (totAngMom_b, totAngMom_d) with
    | (_,0) -> if (totAngMom_b = 0) then
		vrr_v 0 angMom_a angMom_c totAngMom_a totAngMom_c
	      else
		hrr0_v angMom_a angMom_b angMom_c totAngMom_a totAngMom_b totAngMom_c
    | (_,_) ->
      let (acx, acy, acz) = angMom_c
      and (adx, ady, adz) = angMom_d in
      let cp, dm, xyz =
        match angMom_d with
        | (_,0,0) -> (acx+1, acy, acz), (adx-1, ady, adz), 0
        | (_,_,0) -> (acx, acy+1, acz), (adx, ady-1, adz), 1
        | _       -> (acx, acy, acz+1), (adx, ady, adz-1), 2
      in
      let h1 = 
        hrr_v angMom_a angMom_b cp dm totAngMom_a totAngMom_b (totAngMom_c+1) (totAngMom_d-1)
      and h2 =
        hrr_v angMom_a angMom_b angMom_c dm totAngMom_a totAngMom_b totAngMom_c (totAngMom_d-1)
      in
      Array.init ncoef (fun k -> h1.(k) +. h2.(k) *. (Coordinate.coord center_cd.(k) xyz)) 
  in
  hrr_v
    (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 *)

  begin
    match Contracted_shell.(totAngMom shell_a, totAngMom shell_b,
                            totAngMom shell_c, totAngMom shell_d) with
    | Angular_momentum.(S,S,S,S) ->
      contracted_class.(0) <-
        Array.fold_left
          (fun accu shell_ab -> accu +.
            Array.fold_left (fun accu shell_cd ->
              let coef_prod =
                shell_ab.Shell_pair.coef *. shell_cd.Shell_pair.coef
              in
              (** Screening on the product of coefficients *)
              try
                if (abs_float coef_prod) < 1.e-3*.cutoff then
                  raise NullQuartet;

                let expo_pq_inv =
                  shell_ab.Shell_pair.expo_inv +. shell_cd.Shell_pair.expo_inv
                in
                let center_pq =
                  Coordinate.(shell_ab.Shell_pair.center |- shell_cd.Shell_pair.center)
                in
                let norm_pq_sq =
                  Coordinate.dot center_pq center_pq
                in

                let zero_m_array = 
                  zero_m ~maxm:0 ~expo_pq_inv ~norm_pq_sq
                in

                accu +. coef_prod *. zero_m_array.(0)
              with NullQuartet -> accu
            ) 0. shell_q
          ) 0. shell_p

    | _ -> 
      Array.iter (fun shell_ab ->
          let norm_coef_scale_p = shell_ab.Shell_pair.norm_coef_scale in
          let b = shell_ab.Shell_pair.j in
          let common =
            Array.map (fun shell_cd ->
                let coef_prod =
                  shell_ab.Shell_pair.coef *. shell_cd.Shell_pair.coef
                in
                let expo_pq_inv =
                  shell_ab.Shell_pair.expo_inv +. shell_cd.Shell_pair.expo_inv
                in
                let center_pq =
                  Coordinate.(shell_ab.Shell_pair.center |- shell_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

                let d = shell_cd.Shell_pair.j in

                (zero_m_array, shell_cd.Shell_pair.expo_inv,
                Contracted_shell.expo shell_d d, shell_cd.Shell_pair.center_ab,
                center_pq,coef_prod)
              ) shell_q
            |> Array.to_list
            |> List.filter (fun (zero_m_array, expo_inv, d, center_cd,
                  center_pq,coef_prod) -> abs_float coef_prod >= 1.e-4 *. cutoff)
            |> Array.of_list
          in
          let zero_m_array = Array.map (fun (zero_m_array, expo_inv, d, center_cd,
                                            center_pq,coef_prod) -> zero_m_array) common
          and expo_inv = Array.map (fun (zero_m_array, expo_inv, d, center_cd,
                                        center_pq,coef_prod) -> expo_inv ) common
          and d = Array.map (fun (zero_m_array, expo_inv, d, center_cd,
                                  center_pq,coef_prod) -> d) common
          and center_cd = Array.map (fun (zero_m_array, expo_inv, d, center_cd,
                                          center_pq,coef_prod) -> center_cd) common
          and center_pq = Array.map (fun (zero_m_array, expo_inv, d, center_cd,
                                          center_pq,coef_prod) -> center_pq) common
          and coef_prod = Array.map (fun (zero_m_array, expo_inv, d, center_cd,
                                          center_pq,coef_prod) -> coef_prod) common
          in
          (* Transpose zero_m_array 
          *)
          let zero_m_array =
            let result = Array.init (maxm+1) (fun _ ->
              Array.make (Array.length coef_prod) 0.)
            in
            for m=0 to maxm do
              for k=0 to (Array.length coef_prod-1) do
               result.(m).(k) <- zero_m_array.(k).(m)
              done;
            done;
            result
          in

            (* Compute the integral class from the primitive shell quartet *)
          let map_1d = Array.init maxm (fun _ -> Zmap.create (4*maxm)) in
          let map_2d = Array.init maxm (fun _ -> Zmap.create (Array.length class_indices)) in
            let norm = 
                let norm_coef_scale_q = shell_q.(0).Shell_pair.norm_coef_scale in
                Array.map (fun v1 ->
                  Array.map (fun v2 -> v1 *. v2) norm_coef_scale_q
                ) norm_coef_scale_p
                |> Array.to_list
                |> Array.concat
            in
            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
                let integral = 
                    hvrr_two_e_vector (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, d)
                      (shell_ab.Shell_pair.expo_inv, expo_inv)
                      (shell_ab.Shell_pair.center_ab, center_cd, center_pq)
                      coef_prod map_1d map_2d
                in
                let x = Array.fold_left (+.) 0. integral in
                contracted_class.(i) <- contracted_class.(i) +. x *. norm.(i)
              ) class_indices
        ) shell_p

  end;

  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