(* Algorithme de localisation des orbitales *)

(* SOMMAIRE *)

(* I. Définition des types *)

(* II : Définition des fonctions pour la localisation *)

    (* 1. Définitions de base nécessaire pour la suite *)
    (* 2. Fonction de calcul de la matrice des angles selon la méthode de Edminston *)
    (* 3.  Fonction calcul de la matrice des angles selon la méthode de Boys façon Edminston *)
    (* 4. Fonction de calcul de la matrice des angles selon la méthode de Boys *)
    (* 5. Pattern matching :  calcul de alpha et du critere D *)
    (* 6. Norme d'une matrice *)
    (* 7.  Détermination de alpha_max et ses indices i et j.
           Si alpha max > pi/2 on soustrait pi/2 à la matrice des alphas de manière récursive
           Si alpha max < pi/2 on ajoute pi/2 à la matrice des alphas de manière récursive *)
    (* 8. Matrice de rotation 2 par 2 *)
    (* 9. Matrice de rotation n par n avec un angle fixe  *)
    (* 10. Fonction d'extraction des 2 vecteurs propres i et j de la matrice des OMs pour les mettres dans la matrice Ksi (n par 2)
           pour appliquer R afin d'effectuer la rotation des orbitales *) (* {1,2} -> 1ere ligne, 2e colonne *)
    (* 11. Fonction de calcul de ksi~ (matrice n par 2), nouvelle matrice par application de la matrice de rotation dans laquelle
           on obtient les deux orbitales que l'on va réinjecter dans la matrice Phi*)
    (* 12. Fonction pour la création de matrice intermédiaire pour supprimer i j et ajouter i~ et j~ *)
    (* 13. Matrice intérmédiaire où les orbitales i et j ont été supprimées et remplacées par des 0, par soustraction de la matrice Phi
           par la matrice *)

(* III. Localisation des OMs *)

    (* 1. Boucle de calcul de la nouvelle matrice des coefficient après n rotation d'orbitales  *)
    (* 2. Fonction de localisation *)

(* IV : Fonctions pour la séparation et le rassemblement des matrices *)

    (* 1. Fonction de création d'une liste d'entier à partir d'un vecteur de float*)
    (* 2. Fonction créant une liste à partir des éléments manquant d'une autre liste, dans l'intervalle [1 ; n_mo] *)
    (* 3. Fonction de séparation d'une matrice en 2 sous matrice, la première matrice correspondant aux colonnes de la matrice dont le numéro est présent
          dans la liste et la seconde à celles dont le numéro de colonne n'est pas présent dans la liste *)
    (* 4. Liste des OMs occupées *)
    (* 5. Liste des OMs de coeur *)
    (* 6. Liste des OMs pi au sein des OMs virtuelles *)
    (* 7. Fonction de rassemblement de 2 matrices *)

(* V. Calcul *)

    (* 1. Séparation des OMs -> m_occ : OMs occupées; m_vir : OMs vacantes *)
    (* 2. Application d'une légère rotation d'un angle fixé aux OMs pour améliorer la convergence *) (* !!! Réglage de l'angle de rotation !!!  *)
    (* 3. Légende *)
    (* 4. Rotation des orbitales : localise localisation new_X / "methode" / epsilon = 1. si <1 diminue l'angle des rotations / nb itération / critère de convergence *) (* !!!  Paramètres du calcul !!!  *)
    (* 5. Rassemblement des occupées et des virtuelles *)

let methode = "BOYS";;
let iteration = 100000;;
let cc = 10e-3;;
let epsilon = 1.;;
(*let list_split_vir2 = [];;*)

open Lacaml.D

(* I. Définition des types *)

type alphaij = {
  alpha_max : float;
  indice_ii : int;
  indice_jj : int;
}

type alphad = {
  m_alpha : Mat.t;
  d : float;
}

let localize mo_basis =

  let simulation = MOBasis.simulation mo_basis in
  let nocc =
      let elec = Simulation.electrons simulation
          in
          Electrons.n_alfa elec
  in
  let ao_basis = MOBasis.ao_basis mo_basis in
  let n_core =
    (Nuclei.small_core @@ Simulation.nuclei @@ MOBasis.simulation mo_basis) / 2
  in
  (* II : Définition des fonctions pour la localisation *)

  (* 1. Définitions de base nécessaire pour la suite *)
  let m_C = MOBasis.mo_coef mo_basis in
  let n_ao = Mat.dim1 m_C in
  let pi = acos(-1.) in

  (* 2. Fonction de calcul de la matrice des angles selon la méthode de Edminston *)
  let f_alpha m_C =
      
      let ee_ints = AOBasis.ee_ints ao_basis in
      let n_mo = Mat.dim2 m_C in
      (*let t0 = Sys.time () in*)
   
      let m_b12 = Mat.init_cols n_mo n_mo (fun i j -> 0.) in
      let m_a12 = Mat.init_cols n_mo n_mo (fun i j -> 0.) in
      let v_d = Vec.init n_mo (fun i -> 0.) in
   
      (* Tableaux temporaires *)
      let m_pqr =
          Bigarray.(Array3.create Float64 fortran_layout n_ao n_ao n_ao)
      in
      let m_qr_i = Mat.create (n_ao*n_ao) n_mo in
      let m_ri_j = Mat.create (n_ao*n_mo) n_mo in
      let m_ij_k = Mat.create (n_mo*n_mo) n_mo in
   
      Array.iter (fun s ->
         (* Grosse boucle externe sur s *)
         Array.iter (fun r ->
           Array.iter (fun q ->
             Array.iter (fun p ->
               m_pqr.{p,q,r} <- ERI.get_phys ee_ints p q r s
             ) (Util.array_range 1 n_ao)
           ) (Util.array_range 1 n_ao)
         ) (Util.array_range 1 n_ao);
      
       (* Conversion d'un tableau a 3 indices en une matrice nao x nao^2 *)
       let m_p_qr =
         Bigarray.reshape (Bigarray.genarray_of_array3 m_pqr) [| n_ao ; n_ao*n_ao |]
         |> Bigarray.array2_of_genarray
       in
      
       let m_qr_i =
         (* (qr,i) = <i r|q s> = \sum_p <p r | q s> C_{pi} *)
         gemm ~transa:`T ~c:m_qr_i m_p_qr m_C
       in
      
       let m_q_ri =
         (* Transformation de la matrice (qr,i) en (q,ri) *)
         Bigarray.reshape_2 (Bigarray.genarray_of_array2 m_qr_i) n_ao (n_ao*n_mo)
       in
      
       let m_ri_j =
         (* (ri,j) = <i r | j s> = \sum_q <i r | q s> C_{bj} *)
         gemm ~transa:`T ~c:m_ri_j m_q_ri m_C
       in
      
       let m_r_ij =
         (* Transformation de la matrice (ri,j) en (r,ij) *)
         Bigarray.reshape_2 (Bigarray.genarray_of_array2 m_ri_j) n_ao (n_mo*n_mo)
       in
      
       let m_ij_k =
         (* (ij,k) = <i k | j s> = \sum_r <i r | j s> C_{rk} *)
         gemm ~transa:`T ~c:m_ij_k m_r_ij m_C
       in
      
       let m_ijk  =
         (* Transformation de la matrice (ei,j) en (e,ij) *)
         Bigarray.reshape (Bigarray.genarray_of_array2 m_ij_k) [| n_mo ; n_mo ; n_mo |]
         |> Bigarray.array3_of_genarray
       in
      
       Array.iter (fun j ->
           Array.iter (fun i ->
             m_b12.{i,j} <- m_b12.{i,j} +. m_C.{s,j} *. (m_ijk.{i,i,i} -. m_ijk.{j,i,j});
             m_a12.{i,j} <- m_a12.{i,j} +. m_ijk.{i,i,j} *. m_C.{s,j}  -.
             0.25 *. ( (m_ijk.{i,i,i} -. m_ijk.{j,i,j}) *. m_C.{s,i} +.
                       (m_ijk.{j,j,j} -. m_ijk.{i,j,i}) *. m_C.{s,j})
                 ) (Util.array_range 1 n_mo);
                  v_d.{j} <- v_d.{j} +.  m_ijk.{j,j,j} *. m_C.{s,j}
         ) (Util.array_range 1 n_mo)
    ) (Util.array_range 1 n_ao);
   
    (*let t1 = Sys.time () in
    Printf.printf "t = %f s\n%!" (t1 -. t0);*)
    
    (Mat.init_cols n_mo n_mo ( fun i j ->
    if i= j 
        then 0.
    else if  +. 0.25 *. atan2 m_b12.{i,j} m_a12.{i,j} <= 0. 
        then  pi /. 4. +. 0.25 *. atan2 m_b12.{i,j} m_a12.{i,j}
        else -. pi /. 4. +. 0.25 *. atan2 m_b12.{i,j} m_a12.{i,j})
    ,Vec.sum v_d)
  in
(* 3.  Fonction calcul de la matrice des angles selon la méthode de Boys façon Edminston *)
  let f_alpha_boys m_C = 
      
      let multipoles = AOBasis.multipole ao_basis in
      let n_mo = Mat.dim2 m_C
      in
      let phi_x_phi =
          Multipole.matrix_x multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in    
      let phi_y_phi =
          Multipole.matrix_y multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in
      let phi_z_phi =
          Multipole.matrix_z multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in      

      let m_b12= 
          let b12 g =  Mat.init_cols n_mo n_mo ( fun i j ->
                                            (g.{i,i} -. g.{j,j}) *. g.{i,j})

          in                                
          Mat.add (b12 phi_x_phi) ( Mat.add (b12 phi_y_phi) (b12 phi_z_phi))
      in 
      let m_a12 =
          let a12 g  = Mat.init_cols n_mo n_mo ( fun i j -> 
                        g.{i,j} *. g.{i,j} -. 0.25 *. ((g.{i,i} -. g.{j,j}) *. (g.{i,i} -. g.{j,j})))
          in
          Mat.add (a12 phi_x_phi) ( Mat.add (a12 phi_y_phi) (a12 phi_z_phi))
      in
      (Mat.init_cols n_mo n_mo ( fun i j -> 
          if i=j 
              then 0.
          else if  +. 0.25 *. atan2 m_b12.{i,j} m_a12.{i,j} >= 0.
              then pi /. 4. -. 0.25 *. atan2 m_b12.{i,j} m_a12.{i,j}
              else -. pi /. 4. -. 0.25 *. atan2 m_b12.{i,j} m_a12.{i,j})
        ,Vec.sum(Vec.init n_mo ( fun i -> (phi_x_phi.{i,i})**2. +. (phi_y_phi.{i,i})**2. +. (phi_z_phi.{i,i})**2.)))
  in
(* 4. Fonction de calcul de la matrice des angles selon la méthode de Boys *)

  let f_theta m_C = 
      let multipoles = AOBasis.multipole ao_basis in
      let n_mo = Mat.dim2 m_C
      in
      let phi_x_phi =
          Multipole.matrix_x multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in    
      let phi_y_phi =
          Multipole.matrix_y multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in
      let phi_z_phi =
          Multipole.matrix_z multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in
      let phi_x2_phi =
          Multipole.matrix_x2 multipoles 
          |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in    
      let phi_y2_phi =
            Multipole.matrix_y2 multipoles 
            |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in
      let phi_z2_phi =
            Multipole.matrix_z2 multipoles 
            |> MOBasis.mo_matrix_of_ao_matrix ~mo_coef:m_C
      in
      let m_b_0 =
          let b_0 g h = Mat.init_cols n_mo n_mo (fun i j ->
              h.{i,i} +. h.{j,j} -. (g.{i,i}**2. +. g.{j,j}**2.))
          in
          Mat.add (b_0 phi_x_phi phi_x2_phi) (Mat.add (b_0 phi_y_phi phi_y2_phi) (b_0 phi_z_phi phi_z2_phi))  
      in
      let m_beta = 
          let beta g = Mat.init_cols n_mo n_mo (fun i j ->
              (g.{i,i} -. g.{j,j})**2. -. 4. *. g.{i,j}**2.)
          in
          Mat.add (beta phi_x_phi) (Mat.add (beta phi_y_phi) (beta phi_z_phi))
      in 
           (*Util.debug_matrix "m_beta" m_beta;*)

      let m_gamma = 
          let gamma g = Mat.init_cols n_mo n_mo (fun i j ->
              4. *. g.{i,j} *. (g.{i,i} -. g.{j,j}) )
          in
           Mat.add (gamma phi_x_phi) (Mat.add (gamma phi_y_phi) (gamma phi_z_phi))
      in 
           (*Util.debug_matrix "m_gamma" m_gamma;*)

      let m_theta = Mat.init_cols n_mo n_mo (fun i j ->
          if i = j 
              then 0.
              else +. 0.25 *. atan2 m_gamma.{i,j} m_beta.{i,j})
      in
           (*Util.debug_matrix "m_theta" m_theta;*)

      let m_critere_B = Mat.init_cols n_mo n_mo (fun i j ->
          m_b_0.{i,j} +. 0.25 *. ((1. -. cos(4. *. m_theta.{i,j})) *. m_beta.{i,j} +. sin(4. *. m_theta.{i,j}) *. m_gamma.{i,j}))
      in
      m_theta, Vec.sum(Mat.as_vec m_critere_B)
  in

  (* 5. Pattern matching :  calcul de alpha et du critere D *)

  let m_alpha_d methode m_C = 
    
      let alpha methode =
          match methode with 
              | "Boys_er"
              | "BOYS_ER"
              | "boys_er" -> let alpha_boys , d_boys  = f_alpha_boys m_C in {m_alpha = alpha_boys; d = d_boys}
              | "Boys"
              | "boys"
              | "BOYS" -> let theta_boys, b_boys = f_theta m_C in {m_alpha = theta_boys; d = 1. /. b_boys}
              | "ER"
              | "er" -> let alpha_er , d_er = f_alpha m_C in {m_alpha = alpha_er; d = d_er}
              | _ -> invalid_arg "Unknown method, please enter Boys or ER"

      in 
      alpha methode
  in
  (* 6. Norme d'une matrice *)

  let norme m = 
      
      let vec_m = Mat.as_vec m in
      let vec2 = Vec.sqr vec_m
      in
      sqrt(Vec.sum vec2)
  in 

  (* 7.  Détermination de alpha_max et ses indices i et j.
  Si alpha max > pi/2 on soustrait pi/2 à la matrice des alphas de manière récursive
  Si alpha max < pi/2 on ajoute pi/2 à la matrice des alphas de manière récursive *)
  let rec new_m_alpha m_alpha m_C n_rec_alpha=

      let n_mo = Mat.dim2 m_C in
      let alpha_m =
        
    (*Printf.printf "%i\n%!" n_rec_alpha;*)
        
          if n_rec_alpha == 0 
              then m_alpha 
              else 
                  Mat.init_cols n_mo n_mo (fun i j -> 
                  if  (m_alpha.{i,j}) >= (pi /. 2.) 
                      then (m_alpha.{i,j} -. ( pi /. 2.))
                  else if m_alpha.{i,j} <= -. pi /. 2.
                      then (m_alpha.{i,j} +. ( pi /. 2.))
                      else m_alpha.{i,j} )
      in 
        
      (*Util.debug_matrix "alpha_m" alpha_m;*)
        
      (* Détermination de l'emplacement du alpha max *)
      let max_element alpha_m = 

        Mat.as_vec alpha_m
        |> iamax
      in
     
      (* indice i et j du alpha max *)
      let indice_ii, indice_jj =

          let max = max_element alpha_m 
          in
          (max - 1) mod n_mo +1, (max - 1) / n_mo +1
      in
    
      (* Valeur du alpha max*)
      let alpha alpha_m  = 
        
          let i = indice_ii in
          let j = indice_jj
          in
        
        (*Printf.printf "%i %i\n%!" i j;*)
        
          alpha_m.{i,j}
            
      in
      let alpha_max = alpha alpha_m in
    
      (*Printf.printf "%f\n%!" alpha_max;*)
    
      if alpha_max < pi /. 2.
          then {alpha_max; indice_ii; indice_jj}
          else new_m_alpha alpha_m m_C (n_rec_alpha-1)
  in

  (* 8. Matrice de rotation 2 par 2 *)
  let f_R alpha  =

      Mat.init_cols 2 2 (fun i j -> 
          if i=j 
              then cos alpha
          else if i>j 
              then sin alpha 
              else -. sin alpha )
  in

  (* 9. Matrice de rotation n par n avec un angle fixe  *)
  let rotate m_C angle=
 
      let n_mo =  Mat.dim2 m_C in

      (* Antisymétrisation et 0 sur la diagonale*)
      let m_angle angle=

          Mat.init_cols n_mo n_mo (fun i j ->
              if i = j 
                  then 0.
              else if i>j 
                  then -. angle
                  else angle)
      in

                (*Util.debug_matrix "m" m;*)
      let m = m_angle angle in
      (* Mise au carré -> X² *)
      let mm = gemm m m in

                (*Util.debug_matrix "mm" mm;*)

      (* Diagonalisation de X² -> diag = vecteur contenant les eigenvalues (-tau²),
      m_angle2 -> Matrice avec les eigenvectors (W) *)
      let diag = syev ~vectors:true mm in
      (* Passage de -tau² à tau² *)
      let square_tau= Vec.abs diag in
      (* Passage de tau² à tau *)
      let tau = Vec.sqrt square_tau in 
      (* Calcul de cos tau à partir du vecteur tau *)
      let cos_tau = Vec.cos tau in
      (* Matrice cos tau *)
      let m_cos_tau =
 
          Mat.init_cols n_mo n_mo (fun i j ->
              if i=j
                  then cos_tau.{i}
                  else 0.)
      in

                    (*Util.debug_matrix "m_cos_tau" m_cos_tau;*)

      (* Calcul de sin tau à partir du vecteur tau *)
      let sin_tau = Vec.sin tau in 
      (* Matrice de sin tau *)
      let m_sin_tau =

          Mat.init_cols n_mo n_mo (fun i j ->
              if i=j 
                  then sin_tau.{i} 
                  else 0.) 
      in

                    (*Util.debug_matrix "m_sin_tau" m_sin_tau;*)

      (* Calcul de la transposée de W (m_angles2) *)
      let transpose_mm = Mat.transpose_copy mm in

                    (*Util.debug_matrix "transpose_mm" transpose_mm;*)

      (* Calcul du vecteur tau^-1 *)
      let tau_1 =

          Vec.init n_mo (fun i ->
              if tau.{i}<= 0.001
                  then 1.
                  else 1. /. tau.{i})
      in
      (* Calcul de la matrice tau^⁻1 *)
      let m_tau_1 = 

          Mat.init_cols n_mo n_mo (fun i j -> 
              if i=j 
                  then tau_1.{i} 
                  else 0.) 
      in

                     (*Util.debug_matrix "m_tau_1" m_tau_1;*)

      (* gemm mm (gemm m_cos_tau transpose_mm) -> W cos(tau) Wt *)
      let a = gemm mm (gemm m_cos_tau transpose_mm) in

                  (*Util.debug_matrix "a" a;*)

      (* gemm mm (gemm m_tau_1 ( gemm m_sin_tau (gemm transpose_mm m))) -> W tau^-1 sin(tau) Wt X *)
      let b = gemm mm (gemm m_tau_1 ( gemm m_sin_tau (gemm transpose_mm m))) in

                    (*Util.debug_matrix "b" b;*)

      (* Somme de 2 matrices, ici -> a + b -> R *)
      let m_r = Mat.add a b (* Matrice de rotation R *)
      in
      gemm m_C m_r
  in

  (* 10. Fonction d'extraction des 2 vecteurs propres i et j de la matrice des OMs pour les mettres dans la matrice Ksi (n par 2)
  pour appliquer R afin d'effectuer la rotation des orbitales *) (* {1,2} -> 1ere ligne, 2e colonne *)
  let f_Ksi indice_i indice_j m_C  =

      let n_ao = Mat.dim1 m_C 
      in
      Mat.init_cols n_ao 2 (fun i j -> 
          if j=1 
              then m_C.{i,indice_i} 
              else m_C.{i,indice_j} )
  in

  (* 11. Fonction de calcul de ksi~ (matrice n par 2), nouvelle matrice par application de la matrice de rotation dans laquelle
  on obtient les deux orbitales que l'on va réinjecter dans la matrice Phi*)
  let f_Ksi_tilde m_R m_Ksi m_C = 
      
      gemm m_Ksi m_R 
  in

  (* 12. Fonction pour la création de matrice intermédiaire pour supprimer i j et ajouter i~ et j~ *)
  let f_k mat indice_i indice_j m_C =
 
      let n_mo = Mat.dim2 m_C in
      let n_ao = Mat.dim1 m_C
      in
      Mat.init_cols n_ao n_mo (fun i j -> 
          if j=indice_i 
              then mat.{i,1}
          else if j=indice_j 
              then mat.{i,2}
          else 0.)
  in

  (* 13. Matrice intérmédiaire où les orbitales i et j ont été supprimées et remplacées par des 0, par soustraction de la matrice Phi
  par la matrice *)
  let f_interm m_C m_Psi = 

      Mat.sub m_C m_Psi
  in

(* III. Localisation des OMs *)

  (* 1. Boucle de calcul de la nouvelle matrice des coefficient après n rotation d'orbitales  *)
  let rec localisation m_C methode epsilon n prev_critere_D prev_m_alpha cc max_D=

    (*Printf.printf "%i\n%!" n;*)
    (*Printf.printf "%f\n%!" epsilon;*)
    (*Util.debug_matrix "m_C" m_C;
    Util.debug_matrix "new_alpha_m" prev_m_alpha;*)

      if n == 0 
        then m_C
        else

           (* Fonction de calcul de la nouvelle matrice de coef après rotation d'un angle alpha *)
           let new_m_C m_C methode m_alpha epsilon =
        
               (* alphaij contient le alpha max ainsi que ses indices i et j *)
               let n_rec_alpha = 10 in (* Nombre ditération max pour réduire les valeurs de alpha *)
               let alphaij = new_m_alpha m_alpha m_C n_rec_alpha in

               (* Valeur de alpha max après calcul *) (* Epsilon = Pas <1. , 1. -> normal, sinon Pas plus petit *)
               let alpha = (alphaij.alpha_max) *. epsilon in
        
               (*Printf.printf "%f\n%!" alpha;*)

               (* Indice i et j du alpha max après calcul *)
               let indice_i = alphaij.indice_ii in
               let indice_j = alphaij.indice_jj in

               (*Printf.printf "%i %i\n%!" indice_i indice_j;*)
        
               (* Matrice de rotation *)
               let m_R = f_R alpha in

               (*Util.debug_matrix "m_R" m_R;*)
        
               (* Matrice qui va subir la rotation *)
               let m_Ksi = f_Ksi indice_i indice_j m_C in

               (*Util.debug_matrix "m_Ksi" m_Ksi;*)

               (* Matrice ayant subit la rotation *)
               let m_Ksi_tilde = f_Ksi_tilde m_R m_Ksi m_C in

               (*Util.debug_matrix "m_Ksi_tilde" m_Ksi_tilde;*)

               (* Matrice pour supprimerles coef des orbitales i et j dans la matrice des coef *)
               let m_Psi = f_k m_Ksi indice_i indice_j m_C in

               (*Util.debug_matrix "m_Psi" m_Psi;*)

               (* Matrice pour ajouter les coef des orbitales i~ et j~ dans la matrice des coef *)
               let m_Psi_tilde = f_k m_Ksi_tilde indice_i indice_j m_C in

               (*Util.debug_matrix "m_Psi_tilde" m_Psi_tilde;*)

               (* Matrice avec les coef des orbitales i et j remplacés par 0 *)
               let m_interm = f_interm m_C m_Psi 

               (*Util.debug_matrix "m_interm" m_interm;*)
           in 
           (* Matrice après rotation *)
           (Mat.add m_Psi_tilde m_interm, alpha)
        
           in
           (* Extraction de la nouvelle matrice des coef et du alpha_max *)
           let m_new_m_C, alpha_max = new_m_C m_C methode prev_m_alpha epsilon in

           (* Fonction de pattern matching en fonction de la méthode *)
           let alphad = m_alpha_d methode m_new_m_C in
               
           (* Matrice des alphas *)
           let m_alpha = alphad.m_alpha in
    
           (* Critères possibles de localisation -> norme_alpha, moyenne_alpha, alpha, diff *)
    
           (* Norme de m_alpha, moyenne et max  *)
    
           let norme_alpha = norme m_alpha in
           let moyenne_alpha = norme_alpha /. (float_of_int((Mat.dim1 m_C)* (Mat.dim2 m_C))) in
           let alpha = alpha_max /. epsilon in
    
    
           (* D critère à maximiser *)
           let critere_D = alphad.d in
           let diff = max_D -. critere_D in
    
           (*Printf.printf "%f\n%!" diff;*)
           Printf.printf "%i %f %f %f %f %f\n%!" n max_D critere_D alpha norme_alpha moyenne_alpha;
   
           let max_D =
               if diff > 0.
                   then max_D
                   else critere_D
           in
           (*Util.debug_matrix "new_alpha_m" m_alpha;*)
           (*Util.debug_matrix "m_new_m_C" m_new_m_C;*)


           (* Critère de conv pour sortir *)
           if alpha_max**2.  < cc**2.
               then m_new_m_C
               else localisation m_new_m_C methode epsilon (n-1) critere_D m_alpha cc max_D
  in

  (* 2. Fonction de localisation *)
  let localise localisation m_C methode epsilon n cc =

      let alphad = m_alpha_d methode m_C in
      let prev_m_alpha = alphad.m_alpha in 
      let prev_critere_D = 0. in
      let max_D = 0.
      in
      localisation m_C methode epsilon n prev_critere_D prev_m_alpha cc max_D
  in

(* IV : Fonctions pour la séparation et le rassemblement des matrices *)

  (* 1. Fonction de création d'une liste d'entier à partir d'un vecteur de float*)
  let int_list vec = 

      let float_list = Vec.to_list vec in
      let g a = int_of_float(a)
      in
      List.map g float_list
  in

  (* 2. Fonction créant une liste à partir des éléments manquant d'une autre liste, dans l'intervalle [1 ; n_mo] *)
  let miss_elem mat list = 

      let n_mo = Mat.dim2 mat in
      let vec = Vec.init (n_mo) (fun i ->
          if List.mem i list
              then 0.
              else float_of_int(i))
      in
      let list_int = int_list vec    
      in
      List.filter (fun x -> x > 0) list_int
  in

  (* 3. Fonction de séparation d'une matrice en 2 sous matrice, la première matrice correspondant aux colonnes de la matrice dont le numéro est présent
  dans la liste et la seconde à celles dont le numéro de colonne n'est pas présent dans la liste *)
  let split_mat mat list =

      let vec_of_mat = Mat.to_col_vecs mat in
      let f a = vec_of_mat.(a-1) in
      let vec_list_1 = List.map f list in
      let list_2 = miss_elem mat list in
      let vec_list_2 = List.map f list_2
      in
      (Mat.of_col_vecs_list vec_list_1,Mat.of_col_vecs_list vec_list_2)
  in

  (* 4. Liste des OMs occupées *)
  let list_occ = 

      let vec_occ = Vec.init (nocc) (fun i -> float_of_int(i))
      in
      int_list vec_occ
  in

  (* 5. Liste des OMs de coeur *)
  let list_core = 
      let vec_core = Vec.init (n_core) (fun i -> float_of_int(i))
      in int_list vec_core
  in

  (* 6. Liste des OMs pi au sein des OMs virtuelles *)
  let list_pi mat = 
      let n_ao = Mat.dim1 m_C in
      let n_mo = Mat.dim2 m_C in
      let m_pi = Mat.init_cols n_ao n_mo ( fun i j -> 
          if m_C.{i,j}**2. < 10e-8**2.
              then 0.
              else m_C.{i,j})
      in
        (*Printf.printf "%i\n" nocc;
        Util.debug_matrix "m_pi" m_pi;*)
      let vec = Mat.to_col_vecs m_pi in
      let vec_dot = Vec.init (n_mo-nocc) (fun i -> dot vec.(0) vec.(i-1+nocc))
      in
      let vec_pi = Vec.init (n_mo-nocc) (fun i ->
          if vec_dot.{i} = 0.
              then float_of_int(i)
              else 0.)
      in  
      let list_pi = int_list vec_pi in
      let rec remove x list =
          match list with
            | [] -> []
            | var :: tail -> if var = x
                       then remove x tail
                       else var :: (remove x tail)
      in 
      remove 0 list_pi
  in    
  (* 7. Fonction de rassemblement de 2 matrices *)
  let assemble m_occ m_vir =

      let v_occ = Mat.to_col_vecs m_occ in
      let v_vir = Mat.to_col_vecs m_vir in
      let m = Array.append v_occ v_vir 
      in
      Mat.of_col_vecs m
  in

(* V. Calcul *)

  (* 1. Séparation des OMs -> m_occ : OMs occupées; m_vir : OMs vacantes, puis des orbitales de coeur : m_core et occupées : m_occu *)
  let m_occ , m_vir = split_mat m_C list_occ in
       (* Util.debug_matrix "m_vir" m_vir;*)
  let m_core, m_occu = split_mat m_occ list_core in
  let dim_m_core = Mat.dim1 m_core in
  
  let list_split_vir2 = list_pi m_vir in

  let m_vir2, m_vir1 = split_mat m_vir list_split_vir2 in
       (* Util.debug_matrix "m_vir1" m_vir1;
        Util.debug_matrix "m_vir2" m_vir2;*)
  let dim_m_vir2 = Mat.dim1 m_vir2 in
  (*Util.debug_matrix "m_vir2" m_vir2;*)
  (* 2. Application d'une légère rotation d'un angle fixé aux OMs pour améliorer la convergence *)
  let new_core = 
    if dim_m_core = 0 
        then m_core 
        else rotate m_core 0.001 
  in
  let new_occu = rotate m_occu 0.001 in

  let new_vir2 = 
    if dim_m_vir2 = 0
        then m_vir2
        else rotate m_vir2 0.001
  in
  let new_vir1 = rotate m_vir1 0.001 in

  (* 3. Légende *)
  let texte = "Methode : " ^ methode ^ "; Nombre iterarion : " ^ string_of_int(iteration) ^ "; cc : " ^ string_of_float(cc)^string_of_float(epsilon) in
  let legende = "n max_D Critere_D alpha_max norme_alpha moyenne_alpha"
  in
  Printf.printf "%s\n" "";
  Printf.printf "%s\n" "Localisation des orbitales";
  Printf.printf "%s\n" "";
  Printf.printf "%s\n" texte;
  Printf.printf "%s\n" "";
  Printf.printf "%s\n" legende; 
  Printf.printf "%s\n" "";

  (* 4. Rotation des orbitales : localise localisation new_X / "methode" / epsilon = 1. si <1 diminue l'angle des rotations / nb itération / critère de convergence *)
  Printf.printf "%s\n" "Localisation orbitales de coeurs, 1/4";
  let loc_m_core = 
    if dim_m_core= 0 
        then new_core 
        else localise localisation new_core methode epsilon iteration cc
  in
  Printf.printf "%s\n" "";
  Printf.printf "%s\n" "Localisation orbitales occupées, 2/4";
  let loc_m_occu = localise localisation new_occu methode epsilon iteration cc
  in
  Printf.printf "%s\n" "";
  Printf.printf "%s\n" "Localisation orbitales virtuelles pi, 3/4";
  let loc_m_vir2 = 
    if dim_m_vir2 = 0
        then new_vir2
        else localise localisation new_vir2 methode epsilon iteration cc
  in
  Printf.printf "%s\n" "";
  Printf.printf "%s\n" "Localisation orbitales virtuelles sigma, 4/4";
  let loc_m_vir1 = localise localisation new_vir1 methode epsilon iteration cc
  in

  (* 5. Rassemblement des occupées et des virtuelles *)
  let m_assemble_occ = assemble loc_m_core loc_m_occu
  in
  let m_assemble_vir = assemble loc_m_vir2 loc_m_vir1
  in
  let m_assemble_loc = assemble m_assemble_occ m_assemble_vir
      in 
      m_assemble_loc