/*********************************************************************************
* Copyright (C) 2015 Alexey V. Akimov
*
* This file is distributed under the terms of the GNU General Public License
* as published by the Free Software Foundation, either version 2 of
* the License, or (at your option) any later version.
* See the file LICENSE in the root directory of this distribution
* or <http://www.gnu.org/licenses/>.
*
*********************************************************************************/

#include "System.h"
#include "units.h"
#include "random.h"
#include "DIIS.h"
#include "Density_Builder.h"

/*********************************************************************************
 
*********************************************************************************/

double p_up(double e, double e_up, double de){
  double res = 0.0;
  if(e<e_up){ res = 0.0; }
  else{ 
    double argg = (e-e_up)/de; 
    if(argg<=100){ res = exp(argg); }
    else{ res = exp(100.0); }
  }
  return res;
}

double p_dn(double e, double e_dn, double de){
  double res = 0.0;
  if(e>e_dn){ res = 0.0; }
  else{ 
    double argg = -(e-e_dn)/de; 
    if(argg<=100){ res = exp(argg); }
    else{ res = exp(100.0); }
  }
  return res;
//  return (e>e_dn)?0.0:exp(-(e-e_dn)/de);
}

double p_ef(double e, double ef, double de){
  double res = 0.0;
  double argg = (e - ef)/de;
  if(argg<-100){ res = 1.0; }
  else if(argg>100.0){ res = 0.0; }
  else{ res = 1.0/(1.0+exp(argg)); }
  return res;
}



void Chebyshev_coeff(vector<double>& C, double (*f)(double x, double y, double z), double ef, double de, int N){
// According to Numerical Recipes

// Computes expansion coefficients of scalar Fermi function
// C  - must be initialized and contains N elements: C[0]... C[N-1]
// double (*f)(double x, double y, double z) -is a scalar function which we are approximating
// ef - trial parameter (Fermi energy, upper or lower eigenvalue)
// de - energy width parameter (smaller it is, the higher the accuracy, but is slower calculation)
// np - degree of polynomial expansion

  float np = N;

  for(int j=0;j<N;j++){  C[j] = 0.0;   }// for k


  for(int k=0;k<N;k++){                 // over all points

    double x_k = cos((k+0.5)*M_PI/np);   // k-th zero of polynomial T_N

    // Function (Fermi) at the x_k point
    double f_k = f(x_k, ef, de) * (2.0/np);

    double t_curr = 0.0;
    double t_prev1, t_prev2;
      
    for(int j=0;j<N;j++){

      if(j==0){  t_curr = 1.0; } // T_0
      else if(j==1){  t_prev1 = t_curr; t_curr = x_k; } // T_1
      else{                                             // T_j
        t_prev2 = t_prev1; t_prev1 = t_curr;
        t_curr = 2.0*x_k * t_prev1 - t_prev2; 
      }

      C[j] += f_k * t_curr;

    }// for all coefficients
  }// for j


//  cout<<"Chebyshev coefficients\n";
//  for(int k=0;k<=np;k++){  cout<<"k= "<<k<<"  C["<<k<<"]= "<<C[k]<<endl;   }// for k


}

double Chebyshev_fit(MATRIX& H, MATRIX& P, double (*f)(double _x, double _y, double _z), double ef, double de, int np){
// Compute density matrix using Chebyshev polynomials
// Return trace of the density matrix, for it should be equal to the number of electrons

  int N = H.num_of_cols; // assume square matrix

  // Compute coefficients
  vector<double> c_k(np+1,0.0);   Chebyshev_coeff(c_k, f, ef,de,np);


  // Recursive definition of Chebyshev matrices
  P = 0.0;
  MATRIX T_k(N,N);     
  MATRIX T_prev1(N,N); 
  MATRIX T_prev2(N,N); 
  
  for(int k=0;k<=np;k++){

    if(k==0){  T_k.Init_Unit_Matrix(1.0);}
    else if(k==1){  T_prev1 = T_k; T_k = H; }
    else{
      T_prev2 = T_prev1;  T_prev1 = T_k;
      T_k = 2.0*H*T_prev1 - T_prev2; 
    }


    P += c_k[k] * T_k; 
    if(k==0){  P -= 0.5*c_k[0]*T_k; }    // T_k is simply unity matrix in this case

  }// for k

  return P.tr();    

}

int System::foe(Control_Parameters& prms,int do_guess){
// method - specifies additional level of approximation
// method = "indo"  - INDO
// method = "cndo2" - CNDO/2
// method = "hf"    - HF
// method = "eht"   - EHT
// method = "eht0"  - EHT with minimalistic extra-stuff

// spin_method = "resticted"
// spin_method = "unrestricted"

// do_guess = 1 - yes, for scf or initialization of MD
// do_guess = 0 - no, for following steps of MD - for efficiency

// configuration - is the index of the defined configurations (charge, from, to)
// configuration == 0 <==>  ground state (default)


  
  int iter = 0;
  int a,b,c,d,ii,jj,i,j,k,kk,n;
  double Eelec_prev,dE,den_err;
  std::string eigen_method; 
  std::string core_method;


  //----------- Control parameters ---------
  std::string method = prms.hamiltonian;
  std::string spin_method = prms.spin_method;
  double etol = prms.etol;
  double den_tol = prms.den_tol;  
  int Niter = prms.Niter;
//  int use_sad = prms.use_sad;  // SAD - summ of atomic densities, for initial guess

  dE = 2.0*etol; 

  //=========================== Guess and initialization ===========================================
  //================================================================================================
  //================================================================================================
  //================================================================================================
  //================================================================================================

  if(method=="eht"){
    if(verbose_level>=1){
      cout<<"Performing EHT calculations\n. See:\n";
      cout<<"??\n";

    }
//    use_sad = 0;
//    prms.use_diis = 0;
//    prms.use_damping = 0;
//    prms.use_level_shift = 0;
//    cout<<"WARNING!!!  For EHT the options: use_sad, use_diis, use_damping, use_level_shift are set to 0\n";

    iter = 1;
//    dE = 0.0;
    eigen_method = "generalized"; 
    core_method = "eht";    
  }

  else if(method=="cndo"){
    if(verbose_level>=1){
      cout<<"Performing CNDO calculations\n. See:\n";
      cout<<"??\n";
    }
    eigen_method = "standard";
    core_method = "cndo";    
  }
  else if(method=="cndo2"){
    if(verbose_level>=1){
      cout<<"Performing CNDO/2 calculations\n. See:\n";
      cout<<"[1] Pople, J.A.; Segal, G.A J. Chem. Phys. 1966 44, 3289\n";
    }
    eigen_method = "standard";
    core_method = "cndo2";    
  }
  else if(method=="indo"){
    if(verbose_level>=1){
      cout<<"Performing INDO calculations\n. See:\n";
      cout<<"[1] Pople, J.A.; Beveridge, D.L.; Dobosh, P.A. J. Chem. Phys. 1967 47, 2026\n";
      cout<<"[2] Pople, J.A.; Segal, G.A J. Chem. Phys. 1966 44, 3289\n";
    }
    eigen_method = "standard";      
    core_method = "indo";
  }
  else if(method=="hf"){
    if(verbose_level>=1){
      cout<<"Performing HF calculations\n.";
    }
    eigen_method = "generalized";
    core_method = "indo";
  }
  else{
      cout<<"In System::indo()\n";
      cout<<"Error: method = "<<method<<" is not supported in this function\n";
      cout<<"Available options are:\n";
      cout<<"\ncndo\ncndo2\nindo\neht\nhf\n";
  }


//  exit(0);

  //------------------------ Initialization ----------------------------
  if(do_guess){ 

    //----------------- Get initial (core) Fock matrix --------------
    int tmp_eht_sce_formula = prms.eht_sce_formula;
    prms.eht_sce_formula = 0;
        
    // this also updates the overlap matrix in AO basis
    update_core(prms);

    *Fao_alp = *Hao;
    *Fao_bet = *Hao;
    
    // Reset eht_sce_formula to original value
    prms.eht_sce_formula = tmp_eht_sce_formula;


    //---------------- Get guess electron density --------------------
    // FMO is initialized and involved only if the guess_type is "fmo". Later we will make "sad" and the standard cases
    // to be the specific cases of FMO
    // Once FMO_fmo is called, the matrices are allocated
    if(prms.guess_type=="sad"){ FMO_sad(prms,eigen_method);     }
    else if(prms.guess_type=="fmo"){  FMO_gen(prms,eigen_method); }  // general FMO - for now it will work
    else{
    
      if(prms.spin_method=="unrestricted"){
        Fock_to_P(Norb, Nocc_alp, 1, Nocc_alp, eigen_method, prms.pop_opt, Fao_alp, Sao, C_alp, E_alp, bands_alp, occ_alp, P_alp);
        Fock_to_P(Norb, Nocc_bet, 1, Nocc_bet, eigen_method, prms.pop_opt, Fao_bet, Sao, C_bet, E_bet, bands_bet, occ_bet, P_bet);
        *P = *P_alp + *P_bet;
      }
      else if(prms.spin_method=="restricted"){
        Fock_to_P(Norb, Nocc, 2, 2.0*Nocc, eigen_method, prms.pop_opt, Fao, Sao, C, E, bands, occ, P);
      }

    }// else

    // Symmetrize alpha and beta density matrices:
//    *P_alp = 0.5 * (*P);
//    *P_bet = 0.5 * (*P);
    

    if(0){
      cout<<"Total composed density matrix\n";
      cout<<"P_alp=\n"<<*P_alp<<endl;
      cout<<"P_bet=\n"<<*P_bet<<endl;
    }

  }// do_guess

  else{

    update_overlap_matrix(prms);

  }



  update_Mull_orb_pop(P, Sao, Mull_orb_pop_gross,Mull_orb_pop_net);
  update_Mull_charges(Mull_orb_pop_gross, Mull_orb_pop_net, Zeff, at_orbitals, Mull_charges_gross, Mull_charges_net);
  Mull_orb_pop_gross0 = Mull_orb_pop_gross;
  Mull_orb_pop_net0 = Mull_orb_pop_net;

//exit(0);

  cout<<"Guess bands\n";
  if(prms.guess_type=="fmo"){

    map<int,pair<int,int> > mapping;
    vector< pair<int,double> > comb_bands_alp, s_comb_bands_alp;
    vector< pair<int,double> > comb_occ_alp, s_comb_occ_alp;

    int indx = 0;
    for(k=0;k<FMO_num_subsets;k++){
      for(int kk=0;kk<FMO_bands_alp[k].size();kk++){
        comb_bands_alp.push_back(pair<int,double>(indx, FMO_bands_alp[k][kk].second));
        comb_occ_alp.push_back(pair<int,double>(indx, FMO_occ_alp[k][kk].second));
        mapping[indx] = pair<int,int>(k,kk);
      }

    }// for k

    // Order all (merged) bands
    merge_sort(comb_bands_alp, s_comb_bands_alp);

    // Show all bands
    int sz = comb_bands_alp.size();

    for(i=0;i<sz;i++){
      int indx = s_comb_bands_alp[i].first; // global MO index
      cout<<" band# = "<<i
          <<" fragment# = "<<mapping[indx].first
          <<" local mo index = "<<mapping[indx].second
          <<" global mo index ="<<indx
          <<" energy(eV) = "<<s_comb_bands_alp[i].second/eV
          <<" energy(a.u.) = "<<s_comb_bands_alp[i].second
          <<" occupation = "<<FMO_occ_alp[mapping[indx].first][mapping[indx].second].second<<endl;
    }// for i

      //---------------------------------------------------------------------------------------------
      // Compute and print MO overlap - the MOs belonging to different fragments should be orthogonal
      cout<<"MO overlap\n";
      for(k=0;k<FMO_num_subsets;k++){
        for(i=0;i<FMO_norb[k];i++){

          for(kk=0;kk<FMO_num_subsets;kk++){
            for(j=0;j<FMO_norb[kk];j++){

              double ovlp = 0.0; // <I|J>,  I = (k,i), J = (kk,j)

              for(a=0;a<FMO_norb[k];a++){
                for(b=0;b<FMO_norb[kk];b++){
                 
                  int ki = FMO_subsets[k][a];
                  int kkj = FMO_subsets[kk][b];

                  ovlp += FMO_C_alp[k]->M[a*FMO_norb[k]+i] * FMO_C_alp[k]->M[b*FMO_norb[k]+j] * Sao->M[ki*Norb + kkj];

                }// for b
              }// for a              

              cout<<ovlp<<"  ";

            }// for j
          }// for kk
 
          cout<<endl;
   
        }// for i
      }// for k
      //---------------------------------------------------------------------------------------------


  }
  else if(prms.guess_type=="sad"){ }
  else{
    show_bands(spin_method);
  }


  if(0){
    cout<<"Mulliken charges at guess:\n";
    for(i=0;i<Nnucl;i++){   cout<<"i= "<<i<<"  q= "<<Mull_charges_gross[i]<<endl;  }
  }

//  exit(0);


//  den_err = max_den();
//  cout<<"Starting density error = "<<den_err<<endl;
  den_err = 2.0*den_tol;
  N_diis = 0;

  int stop_diis = 0;
//  if(use_diis){  update_diis_matrices(spin_method); } // call after density matrix is updated!!!
  int damp_hist = 0;

  int start_damp = 0;

  int start_diis = 0;
  int diis_delay = 0;

  DIIS* diis;
  DIIS* diis_alp;
  DIIS* diis_bet;

  MATRIX* err_alp;
  MATRIX* err_bet;


  if(prms.use_diis){

    err_alp = new MATRIX(Norb,Norb);
    err_bet = new MATRIX(Norb,Norb);
  
    if(prms.spin_method=="unrestricted"){
      diis_alp = new DIIS(prms.diis_max,Norb);
      diis_bet = new DIIS(prms.diis_max,Norb);
    }

  }// use_diis

//  if(method=="eht0"){
//    Eelec = Eelec_prev = energy_elec0(method,spin_method);
//  }
//  else{
    if(prms.guess_type=="fmo"){
      Eelec = 0.0;
      for(k=0;k<FMO_num_subsets;k++){
        Eelec += ::energy_elec(Norb,FMO_P_alp[k],FMO_Fao_alp[k],FMO_Fao_alp[k]); // for EHT!!!
        Eelec += ::energy_elec(Norb,FMO_P_bet[k],FMO_Fao_bet[k],FMO_Fao_bet[k]); // for EHT!!!
      }// for k
    }
    else{
      Eelec = energy_elec(method,spin_method);    
    }// !fmo
    
    Eelec_prev = Eelec;

//    Eelec = Eelec_prev = energy_elec(method,spin_method); //!!!! 
    cout<<"Energy = "<<Eelec<<endl;
//  }
  
  *P_alp_old = *P_alp;
  *P_bet_old = *P_bet;
  *P_old = *P;
  


  //=========================== Now enter main SCF cycle ===========================================
  //================================================================================================
  //================================================================================================
  //================================================================================================
  //================================================================================================
  ofstream f1("energy.txt",ios::out);

//exit(0);

  if(verbose_level>=0){
    cout<<"----------------------- Entering main SCF cycle for RHF calculations --------------------\n";
  }


  //dE = 2.0*etol; 

  while(iter<Niter && fabs(dE)>etol  /*&& den_err>den_tol*/){

    if(do_guess){ 
      if(iter>10){ start_diis = 1; start_damp = 1; }
      if(iter<4){ damp_hist = 0;}
    }
    else{
      if(iter>10){ start_diis = 1; start_damp = 1; }
    }

//    cout<<"start_damp = "<<start_damp<<endl;
    

    if(verbose_level>=1){   cout<<"===============Iteration# "<<iter<<" =====================\n";   }

    // Guess density
    if(prms.use_damping && start_damp){
 
      // Extrapolate density matrix for Fock matrix construction:
      *P_alp = *P_alp_old*prms.damping_const + *P_alp*(1.0 - prms.damping_const); 
      *P_bet = *P_bet_old*prms.damping_const + *P_bet*(1.0 - prms.damping_const); 
      *P = *P_alp + *P_bet;
    
//      cout<<"mixing densities\n";
    }

    if(prms.use_damping){
      *P_alp_old = *P_alp;
      *P_bet_old = *P_bet;
      *P_old = *P;
    }



    //------------------ Build Hamiltonian (Fock matrix ) on the guess density --------------------------
    // Update properties that depend on density matrix 
    // DIIS Step 5: Determine new Fock matrix
    update_Mull_orb_pop(P, Sao, Mull_orb_pop_gross,Mull_orb_pop_net);
    update_Mull_charges(Mull_orb_pop_gross, Mull_orb_pop_net, Zeff, at_orbitals, Mull_charges_gross, Mull_charges_net);

//exit(0);

      if(method=="cndo"||method=="cndo2"||method=="indo"){

        if(spin_method=="restricted"){     build_Fock_cndo_indo();    }
        else if(spin_method=="unrestricted"){      build_uFock_cndo_indo();   }// unrestricted

      }// cndo || cndo2 || indo
      else if(method=="eht"){

        if(spin_method=="restricted"){     build_Fock_eht();      }
        else if(spin_method=="unrestricted"){    build_uFock_eht(prms);     } // may work on fragment Fock matrices!!!
 
      }// eht
      else if(method=="hf"){     build_Fock_hf();   }
      else{;;}

//exit(0);

    //----------------- Compute error for given initial (guess) electron density ---------------
    // DIIS Step 6: Return to DIIS Step 1.
    if(prms.use_diis && start_diis){

      // DIIS Step 1: Compute error matrices
      // Here one should use the same density that was used to construct Fock matix
      *err_alp = (*Fao_alp) * (*P_alp) * (*Sao) - (*Sao) * (*P_alp) * (*Fao_alp);
      *err_bet = (*Fao_bet) * (*P_bet) * (*Sao) - (*Sao) * (*P_bet) * (*Fao_bet);

         // Convert errors into MO basis
      *err_alp = ((*C_alp).T()) * *err_alp * (*C_alp);
      *err_bet = ((*C_bet).T()) * *err_bet * (*C_bet);



//      cout<<"*err_alp= "<<((*err_alp).T() * (*err_alp)).tr()<<endl;

      // DIIS Step 2: Store error matrix and current Fock matrix
      diis_alp->add_diis_matrices(Fao_alp, err_alp);
      diis_bet->add_diis_matrices(Fao_bet, err_bet);  


      if(diis_delay >= diis_alp->N_diis_max){

      // DIIS Setp 3: Compute extrapolated Fock matrix and replace current Fock matrix by the extrapolated matrix
      diis_alp->extrapolate_matrix(Fao_alp_tmp);
      diis_bet->extrapolate_matrix(Fao_bet_tmp);

      *Fao_alp = *Fao_alp_tmp;
      *Fao_bet = *Fao_bet_tmp;      

      }

      diis_delay++;

    }// use_diis && start_diis



    //---------- Obtain a new density for this iteration -------------------------
    // DIIS Step 4: Diagonalize the Fock matrix F' (extrapolated), determine new density matrix
    if(prms.guess_type=="fmo"){

      for(k=0;k<FMO_num_subsets;k++){

        MATRIX* s; s = new MATRIX(FMO_norb[k],FMO_norb[k]);
        pop_submatrix(Sao, s, FMO_subsets[k]);

        Fock_to_P(FMO_norb[k],FMO_nocc_alp[k], 1, FMO_nocc_alp[k], eigen_method, prms.pop_opt,
                 FMO_Fao_alp[k], s, FMO_C_alp[k], FMO_E_alp[k], FMO_bands_alp[k], FMO_occ_alp[k], FMO_P_alp[k]);

        Fock_to_P(FMO_norb[k],FMO_nocc_bet[k], 1, FMO_nocc_bet[k], eigen_method, prms.pop_opt,
                 FMO_Fao_bet[k], s, FMO_C_bet[k], FMO_E_bet[k], FMO_bands_bet[k], FMO_occ_bet[k], FMO_P_bet[k]);

        delete s;
        
//        cout<<"Fragment k="<<k<<"\n"<<*FMO_E_alp[k]<<endl;

      }// for k

    }// if guess_type == fmo
 
    else{


    if(prms.spin_method=="unrestricted"){

      Fock_to_P(Norb, Nocc_alp, 1, Nocc_alp, eigen_method, prms.pop_opt, Fao_alp, Sao, C_alp, E_alp, bands_alp, occ_alp, P_alp);
      Fock_to_P(Norb, Nocc_bet, 1, Nocc_bet, eigen_method, prms.pop_opt, Fao_bet, Sao, C_bet, E_bet, bands_bet, occ_bet, P_bet);
      *P = *P_alp + *P_bet;

    }// unrestricted

    else if(prms.spin_method=="restricted"){
      Fock_to_P(Norb, Nocc, 2, 2.0*Nocc, eigen_method, prms.pop_opt, Fao, Sao, C, E, bands, occ, P);
    }

    if(verbose_level>1){
      show_bands(spin_method);
    }


    }// else:  guess_type !=fmo
    


    

    double ovlp_nrm = 0.0;            
    // Calculation of total electronic energy
    if(prms.guess_type=="fmo"){
      Eelec = 0.0;
      for(k=0;k<FMO_num_subsets;k++){
        Eelec += ::energy_elec(FMO_norb[k],FMO_P_alp[k],FMO_Fao_alp[k],FMO_Fao_alp[k]); // for EHT!!!
        Eelec += ::energy_elec(FMO_norb[k],FMO_P_bet[k],FMO_Fao_bet[k],FMO_Fao_bet[k]); // for EHT!!!
      }// for k

      //---------------------------------------------------------------------------------------------
      // Compute and print MO overlap - the MOs belonging to different fragments should be orthogonal
      cout<<"MO overlap\n";
      for(k=0;k<FMO_num_subsets;k++){
        for(i=0;i<FMO_norb[k];i++){

          for(kk=0;kk<FMO_num_subsets;kk++){
            for(j=0;j<FMO_norb[kk];j++){

              double ovlp = 0.0; // <I|J>,  I = (k,i), J = (kk,j)

              for(a=0;a<FMO_norb[k];a++){
                for(b=0;b<FMO_norb[kk];b++){
                 
                  int ki = FMO_subsets[k][a];
                  int kkj = FMO_subsets[kk][b];

                  ovlp += FMO_C_alp[k]->M[a*FMO_norb[k]+i] * FMO_C_alp[kk]->M[b*FMO_norb[kk]+j] * Sao->M[ki*Norb + kkj];

                }// for b
              }// for a              

              cout<<ovlp<<"  ";

              if(kk!=k){
                ovlp_nrm += ovlp*ovlp;
              }

            }// for j
          }// for kk
 
          cout<<endl;
   
        }// for i
      }// for k
      //---------------------------------------------------------------------------------------------


    }
    else{
      Eelec = energy_elec(method,spin_method);    
    }// !fmo

    dE = Eelec - Eelec_prev;
    Eelec_prev = Eelec;
    

    if(verbose_level>=1){
      cout<<"Total electronic energy = "<<Eelec<<endl;
      cout<<"Change of total electronic energy, dE = "<<dE<<endl;
    }

    f1 << iter<<"  "<<dE<<"  "<<Eelec<<"  "<<ovlp_nrm<<endl;

    iter++;    

   

  }// while iter<Niter


  f1.close();
//  exit(0);

  if(fabs(dE)>etol /*&& den_err>den_tol*/){
    cout<<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
    cout<<"!!!!! Error: Convergence is not achieved after "<<Niter<<" iterations\n dE = "<<dE<<" !!!!!\n";
    cout<<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
    return 111;
    exit(0);
  }


  //===================== Post-processing using converged wavefunction =============================
  //================================================================================================
  //================================================================================================
  //================================================================================================
  //================================================================================================




  // Now compute vertical excitation energies
  double E_ground = Eelec;
  vector< pair<int,double> > occ_alp_grnd = occ_alp;
  vector< pair<int,double> > occ_bet_grnd = occ_bet;

  // !!! === Compute vertical excitations === !!!
  for(a=0;a<=prms.num_excitations;a++){  // +1 , so that the last calculation is for the ground state density

    cout<<"excitation "<<a<<"  \n";
    occ_alp = occ_alp_grnd;
    occ_bet = occ_bet_grnd;

    b = 0; 
    if(a==prms.num_excitations){  b = 0; }
    else{ b = a; }

    excite(Norb, prms.excitations[b], Nocc_alp, occ_alp, Nocc_bet, occ_bet); // ground state excitation
    cout<<"Excitation: "<<prms.excitations[b].from_orbit[0]<<"  "<<prms.excitations[b].from_spin[0]<<"  -->  "
                        <<prms.excitations[b].to_orbit[0]<<"  "<<prms.excitations[b].to_spin[0]<<endl;




    show_bands(spin_method);


    // Update density matrix
    compute_density_matrix(occ_alp, C_alp, P_alp);
    compute_density_matrix(occ_bet, C_bet, P_bet);
    *P = *P_alp + *P_bet;


    if(prms.excitations_opt=="scf"){  // Update Fock matrix also

      // Update density-dependent properties
      update_Mull_orb_pop(P, Sao, Mull_orb_pop_gross,Mull_orb_pop_net);
      update_Mull_charges(Mull_orb_pop_gross, Mull_orb_pop_net, Zeff, at_orbitals, Mull_charges_gross, Mull_charges_net);
    
    
    
      // ... including Fock matrix
      if(method=="cndo"||method=="cndo2"||method=="indo"){
    
        if(spin_method=="restricted"){     build_Fock_cndo_indo();    }
        else if(spin_method=="unrestricted"){      build_uFock_cndo_indo();   }// unrestricted
    
      }// cndo || cndo2 || indo
      else if(method=="eht"){
    
        if(spin_method=="restricted"){     build_Fock_eht();      }
        else if(spin_method=="unrestricted"){    build_uFock_eht(prms);     } // may work on fragment Fock matrices!!!
    
      }// eht
      else if(method=="hf"){     build_Fock_hf();   }
      else{;;}

    }// excitations_option == scf

    else{ // excitaions only affect density matrix, not the Fock matrix

    }

    prms.excitations[b].Energy = energy_elec(method,spin_method) - E_ground;
    cout<<"E-E0= "<<prms.excitations[b].Energy<<endl;    


  }// for a


//exit(0);


  //---------- Compute nuclear and total energies -----------
  Enucl = 0.0;
  for(a=0;a<Nnucl;a++){
    for(b=a+1;b<Nnucl;b++){
      double rab = (R[a] - R[b]).length();

      double zeff_a = Zeff[a];
      double zeff_b = Zeff[b];

      Enucl += zeff_a*zeff_b/rab;

    }
  }

  Etot = Enucl + Eelec; // total energy


  Epair = 0.0;
  if(method=="eht"){

//    Epair = pair_terms();
    Etot = Etot + Epair;
  }



  if(verbose_level>=0){
    cout<<"Convergence is achieved in "<<(iter-1)<<" iterations\n";
    cout<<"Total electronic energy = "<<Eelec<<" a.u.; "<<Eelec/eV<<" eV"<<endl;
    cout<<"Total nuclear-nuclear interaction energy = "<<Enucl<<" a.u.; "<<Enucl/eV<<" eV"<<endl;
    cout<<"Total pair energy = "<<Epair<<" a.u.; "<<Epair/eV<<" eV"<<endl;
    cout<<"Total system energy = "<<Etot<<" a.u.; "<<Etot/eV<<" eV"<<endl;
  }
  if(verbose_level>=0){ 
    cout<<"Converged bands\n";
//    if(prms.guess_type=="fmo"){
//      for(k=0;k<FMO_num_subsets;k++){
//        ::show_bands(FMO_norb[k], FMO_nocc_alp[k], FMO_bands_alp[k], FMO_occ_alp[k]);
//      }// for k
//    }
    if(prms.guess_type=="fmo"){
   
      map<int,pair<int,int> > mapping;
      vector< pair<int,double> > comb_bands_alp, s_comb_bands_alp;
      vector< pair<int,double> > comb_occ_alp, s_comb_occ_alp;
   
      int indx = 0;
      for(k=0;k<FMO_num_subsets;k++){
        for(int kk=0;kk<FMO_bands_alp[k].size();kk++){
          comb_bands_alp.push_back(pair<int,double>(indx, FMO_bands_alp[k][kk].second));
//          comb_occ_alp.push_back(pair<int,double>(indx, FMO_occ_alp[k][kk].second));
          mapping[indx] = pair<int,int>(k,kk);
          indx++;
        }
   
      }// for k
   
      // Order all (merged) bands
      merge_sort(comb_bands_alp, s_comb_bands_alp);
   
      // Show all bands
      int sz = comb_bands_alp.size();
   
      for(i=0;i<sz;i++){
        int indx = s_comb_bands_alp[i].first; // global MO index
        cout<<" band# = "<<i
            <<" fragment# = "<<mapping[indx].first
            <<" local mo index = "<<mapping[indx].second
            <<" global mo index ="<<indx
            <<" energy(eV) = "<<s_comb_bands_alp[i].second/eV
            <<" energy(a.u.) = "<<s_comb_bands_alp[i].second
            <<" occupation = "<<FMO_occ_alp[mapping[indx].first][mapping[indx].second].second<<endl;
      }// for i
    }

    else{
      show_bands(spin_method);
    }
  }

//  exit(0);

  //------------- Update converged MOs ----------------------
  if(verbose_level>=2){
    cout<<"MO coefficients (MOs in columns, AOs in rows)\n";
    if(spin_method=="restricted"){
      cout<<*C<<endl;
    }
    else if(spin_method=="unrestricted"){
      cout<<"C_alp:\n"<<*C_alp<<endl;
      cout<<"C_bet:\n"<<*C_bet<<endl;
    }

  }

  //=============== FOE stuff ========================

  // Power method for finding largest eigenvalue
  // Initialize vector
  MATRIX v(Norb,1); v = 0.0;

  
  MATRIX Sinv(Norb,Norb); Sao->Inverse(Sinv);

  MATRIX Id(Norb,Norb); Id.Init_Unit_Matrix(1.0);

  MATRIX* lambd; lambd = new MATRIX(Norb,Norb);
  MATRIX* coeff; coeff = new MATRIX(Norb,Norb);

  solve_eigen(Sao, &Id, lambd, coeff, 0);

  cout<<"Overlap eigenvalues = \n"<<*lambd<<endl;

  for(i=0;i<Norb;i++){ lambd->M[i*Norb+i] = 1.0/sqrt(lambd->M[i*Norb+i]); }


  MATRIX Sinv_half(Norb,Norb);
  Sinv_half = (*coeff) * (*lambd) * ((*coeff).T());

  cout<<"Exact S^-1/2 = \n"<<Sinv_half<<endl;

  cout<<"S^-1/2 * S * S^-1/2 = \n"<<Sinv_half * (*Sao) * Sinv_half<<endl;


  MATRIX s(Norb,Norb);
  s = *Sao - Id;

  MATRIX Sinv_half_app(Norb,Norb); 
  Sinv_half_app = Id - 0.5*s + 0.375*s*s - (15.0/48.0)*s*s*s; 

  cout<<"Approximate S^-1/2 = \n"<<Sinv_half_app<<endl;

  cout<<"S^-1/2 * S * S^-1/2 = \n"<<Sinv_half_app * (*Sao) * Sinv_half_app<<endl;

  
  MATRIX Heff(Norb,Norb);

  double f = 10.0; // some shift to make all eigenvalues positive - must be larger for larger systems!

  //============================ Minimal eigenvalue =================================
  Heff = -Sinv_half * (*Fao_alp) * Sinv_half + f*Id; // minimal eigenvalue


  // Set it random
  double nrm = 0.0;

  for(i=0;i<Norb;i++){  v.M[i] = uniform(0.0,1.0); }
  nrm = (1.0/sqrt( (v.T()*v).M[0]) );
  
  int m = 0;
  do{
    // Normalize
    v = v * nrm;

    // Iteration and norm  
    v = (Heff * v);
    nrm = (1.0/sqrt( (v.T()*v).M[0]) );
    m++;

  }while(m<200);


  cout<<"Eigenvector for minimal eigenvector = "<<nrm*v<<endl;
  double e_dn = -nrm*nrm*(v.T() * Heff * v).M[0] + f;
  cout<<"Eigenvalue = "<<e_dn<<endl; // minimal eigenvalue


  //============================ Maximal eigenvalue =================================

  Heff = Sinv_half * (*Fao_alp) * Sinv_half + f*Id; // maximal eigenvalue


  // Set it random
  for(i=0;i<Norb;i++){  v.M[i] = uniform(0.0,1.0); }
  nrm = (1.0/sqrt( (v.T()*v).M[0]) );
  
  m = 0;
  do{

    // Normalize
    for(i=0;i<Norb;i++){  v.M[i] *= nrm; }

    // Iteration and norm  
    v = (Heff * v);
    nrm = (1.0/sqrt( (v.T()*v).M[0]) );
    m++;

  }while(m<200);

  cout<<"Eigenvector for maximal eigenvalue = "<<nrm*v<<endl;
//  cout<<"Eigenvalue = "<<-nrm*nrm*(v.T() * Heff * v).M[0] + f<<endl; // minimal eigenvalue
  double e_up = nrm*nrm*(v.T() * Heff * v).M[0] - f;
  cout<<"Maximal Eigenvalue = "<<e_up<<endl; // maximal eigenvalue




  // Just in case - to mitigate effects of round off
//  e_up *= 1.1;
//  e_dn *= 1.1;

//  double e_up = 0.5*(e_up_min + e_up_min);


  // 

  // Use bisection method to find upper eigenvalue
  MATRIX P_tmp(Norb,Norb);
  double err1 = 1.0;
  int iter1 = 0;

/*

  while(!((e_up_max-e_up_min)<0.001 && err1<1e-8) && iter1<100){

    err1 = fabs(Chebyshev_fit(Heff, P_tmp, p_up, e_up, de, np));

    if(err1>0.0){ e_up_min = e_up; e_up = 0.5*(e_up_min + e_up_max); }
    else{ e_up_max = e_up; e_up = 0.5*(e_up_min + e_up_max); }

//    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" e_up= "<<e_up<<" e_up_max= "<<e_up_max<<" e_up_min= "<<e_up_min<<endl;

    iter1++;
  }
    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" e_up= "<<e_up<<" e_up_max= "<<e_up_max<<" e_up_min= "<<e_up_min<<endl;


  double e_dn_max = 10.5;
  double e_dn_min = -10.5;
  double e_dn = 0.0;



//    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" e_dn= "<<e_dn<<" e_dn_max= "<<e_dn_max<<" e_dn_min= "<<e_dn_min<<endl;

  err1 = 1.0;
  iter1 = 0;

  while(!((e_dn_max-e_dn_min)<0.001 && err1<1e-8) && iter1<100){

    err1 = fabs(Chebyshev_fit(Heff, P_tmp, p_dn, e_dn, de, np));

    if(err1>0.0){ e_dn_max = e_dn; e_dn = 0.5*(e_dn_min + e_dn_max); }
    else{ e_dn_min = e_dn; e_dn = 0.5*(e_dn_min + e_dn_max); }

//    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" e_dn= "<<e_dn<<" e_dn_max= "<<e_dn_max<<" e_dn_min= "<<e_dn_min<<endl;

    iter1++;
  }
    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" e_dn= "<<e_dn<<" e_dn_max= "<<e_dn_max<<" e_dn_min= "<<e_dn_min<<endl;


  // Assume scaling is found 
  e_up = 4.0;
  e_dn = -4.0;

*/

  // Scale Hamiltonian:
//  MATRIX Id(Norb,Norb); Id.Init_Unit_Matrix(1.0);


// Below we work with orthogonalized orbitals (Lowdin)

//  e_up *= 2.0;
//  e_dn *= 2.0;

  double ef_max = 1.5*e_up;
  double ef_min = 1.5*e_dn; 
  double ef = 0.5*(ef_min + ef_max);

  Heff = Sinv_half * (*Fao_alp) * Sinv_half;

  Heff -= Id*e_dn;
  Heff =  Heff * (2.0/(e_up-e_dn));
  Heff -= Id;

  cout<<Heff<<endl;


//  de = 0.1;
  double de = 0.025;
  int np = 30;

  err1 = 1.0;
  iter1 = 0;

// This is because we rescaled energy range to [-1, 1]
  cout<<"Number of electrons ef = -1: "<<Chebyshev_fit(Heff, P_tmp, p_ef, -1.0, de, np)<<endl;
  cout<<"Number of electrons ef =  1: "<<Chebyshev_fit(Heff, P_tmp, p_ef,  1.0, de, np)<<endl;


//  cout<<"Number of electrons ef = ef_min: "<<Chebyshev_fit(Heff, P_tmp, p_ef, ef_min, de, np)<<endl;
//  cout<<"Number of electrons ef = ef_max: "<<Chebyshev_fit(Heff, P_tmp, p_ef, ef_max, de, np)<<endl;



  while(!((ef_max-ef_min)<0.01 && err1<1e-8) && iter1<100){

    err1 = Chebyshev_fit(Heff, P_tmp, p_ef, ef, de, np) - Nelec/2;

    if(err1>0.0){ ef_max = ef; ef = 0.5*(ef_min + ef_max); }
    else{ ef_min = ef; ef = 0.5*(ef_min + ef_max); }

//    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" ef= "<<ef<<" ef_max= "<<ef_max<<" ef_min= "<<ef_min<<endl;

    iter1++;
  }
    cout<<"iter= "<<iter1<<" err1 = "<<err1<<" ef= "<<ef<<" ef_max= "<<ef_max<<" ef_min= "<<ef_min<<endl;

  cout<<"Rescaled back Fermi energy = "<<0.5*(ef+ 1.0)*(e_up-e_dn) + e_dn<<endl;

  cout<<"Density matrix in Lowdin-orthoganilezed orbitals: \n"<<P_tmp<<endl;

  cout<<"Density matrix in AO orbitals: \n"<<Sinv_half * P_tmp * Sinv_half<<endl;

  cout<<"Reference density matrix: \n"<<*P_alp<<endl;

  



  //==================================================



  return 0;
}


