open Lacaml.D
open Simulation
open Constants
open Util


type t =
  {
    fock     : Mat.t ;
    core     : Mat.t ;
    coulomb  : Mat.t ;
    exchange : Mat.t ;
  }

module Ao = AOBasis



let make ~density ?(threshold=Constants.epsilon) ao_basis =
  let m_P  = density
  and m_T  = Lazy.force ao_basis.Ao.kin_ints |> KinInt.matrix 
  and m_V  = Lazy.force ao_basis.Ao.eN_ints  |> NucInt.matrix
  and m_G  = Lazy.force ao_basis.Ao.ee_ints  
  in
  let nBas = Mat.dim1 m_T
  in

  let m_Hc = Mat.add m_T m_V 
  and m_J = Array.make_matrix nBas nBas 0.
  and m_K = Array.make_matrix nBas nBas 0.
  in

  for sigma = 1 to nBas do
    let m_Ksigma = m_K.(sigma-1) in
    for nu = 1 to nBas do
      let m_Jnu = m_J.(nu-1) in
      for lambda = 1 to nBas do
        let pJ = m_P.{lambda,sigma}
        and pK = 0.5 *. m_P.{lambda,nu}
        in
        match (abs_float pJ > threshold , abs_float pK > threshold, nu < sigma) with
        | (false, false, _) -> ()
        | (true , true , true) ->
            begin
              for mu = 1 to nu do
                let integral = 
                  ERI.get_phys m_G mu lambda nu sigma
                in
                if (integral <> 0.) then begin
                  m_Jnu.(mu-1) <- m_Jnu.(mu-1) +.  pJ *. integral;
                  m_Ksigma.(mu-1) <- m_Ksigma.(mu-1) -. pK *. integral
                end
              done;
              for mu = nu+1 to sigma do
                m_Ksigma.(mu-1) <- m_Ksigma.(mu-1) -. pK *.
                              ERI.get_phys m_G mu lambda nu sigma
              done
            end
        | (true , true , false) ->
           begin
              for mu = 1 to sigma do
                let integral = 
                  ERI.get_phys m_G mu lambda nu sigma
                in
                if (integral <> 0.) then begin
                  m_Jnu.(mu-1) <- m_Jnu.(mu-1) +.  pJ *. integral;
                  m_Ksigma.(mu-1) <- m_Ksigma.(mu-1) -. pK *. integral
                end
              done;
              for mu = sigma+1 to nu do
                m_Jnu.(mu-1) <- m_Jnu.(mu-1) +. pJ *.
                              ERI.get_phys m_G mu lambda nu sigma
              done
            end
        | (false, true , _) ->
            for mu = 1 to sigma do
              m_Ksigma.(mu-1) <- m_Ksigma.(mu-1) -. pK *.
                            ERI.get_phys m_G mu lambda nu sigma
            done
        | (true , false, _) ->
            for mu = 1 to nu do
              m_Jnu.(mu-1) <- m_Jnu.(mu-1) +.  pJ *. 
                            ERI.get_phys m_G mu lambda nu sigma
            done
      done
    done;
    for mu = 1 to sigma-1 do
      m_K.(mu-1).(sigma-1) <- m_Ksigma.(mu-1);
    done
  done;
  for nu = 1 to nBas do
    let m_Jnu = m_J.(nu-1) in
    for mu = 1 to nu-1 do
      m_J.(mu-1).(nu-1) <- m_Jnu.(mu-1)
    done
  done;

  let m_J =  Mat.of_array m_J
  and m_K =  Mat.of_array m_K
  in
  { fock = Mat.add m_Hc (Mat.add m_J m_K) ;
    core = m_Hc ; coulomb = m_J ; exchange = m_K }



let op ~f f1 f2 =
  let m_Hc = f1.core
  and m_J = f f1.coulomb f2.coulomb
  and m_K = f f1.exchange f2.exchange
  in
  {
    fock     = Mat.add m_Hc (Mat.add m_J m_K);
    core     = m_Hc;
    coulomb  = m_J;
    exchange = m_K;
  }


let add = op ~f:(fun a b -> Mat.add a b)
let sub = op ~f:(fun a b -> Mat.sub a b)


let pp_fock ppf a =
  Format.fprintf ppf "@[<2>";
  Format.fprintf ppf "@[ Fock matrix:@[<2>@[%a@]@.]@]" pp_matrix a.fock;
  Format.fprintf ppf "@[ Core Hamiltonian:@[<2>@[%a@]@.]@]" pp_matrix a.core;
  Format.fprintf ppf "@[ Coulomb matrix:@[<2>@[%a@]@.]@]" pp_matrix a.coulomb;
  Format.fprintf ppf "@[ Exchange matrix:@[<2>@[%a@]@.]@]" pp_matrix a.exchange;
  Format.fprintf ppf "@]"