#include "piccard.hpp"

namespace Kernel{

  Piccard::Piccard(){
    previous_solution=nullptr;
    new_solution=nullptr;
    l=nullptr;
    Pl=nullptr;
  }

  void Piccard::init(const Geometry* geometry_,double** pumps_){
    geometry=geometry_;
    pumps=pumps_;
    horizontal_problem.init(geometry);
    overland.init(geometry);
    all_vertical_richards.init(geometry,pumps);
    if(l!=nullptr) delete[] l;
    if(Pl!=nullptr) delete[] Pl;
    l=new double[geometry->nX];
    Pl=new double[geometry->nX];
    error_x=new double[geometry->nX];
  }

  void
  Piccard::run(){
    #ifdef MYDEBUG
    cout<<"  [Piccard::run] start"<<endl;
    #endif
    double error=0;
    //Copy s_in.hsat in l
    memcpy(l,previous_solution->hsat,sizeof(double)*geometry->nX);
    //Compute Pl from s_in.hsat and s_in.P

    compute_Pl(previous_solution->P);

    double n1_init_hydr=norm1(previous_solution->hydr,geometry->nX);
    double n1_init_hov=norm1(previous_solution->hov,geometry->nX);

    //Compute hydr from s_in.hydr, s_in.hsat and Pl
    horizontal_problem.previous_hydr=previous_solution->hydr;
    horizontal_problem.l=previous_solution->hsat;
    horizontal_problem.Pl=Pl;
    horizontal_problem.n1_init_hydr=n1_init_hydr;
    horizontal_problem.error_x=error_x;
    horizontal_problem.run();
    swap(new_solution->hydr,horizontal_problem.hydr);
    error+=horizontal_problem.total_error;

    //Compute Overland
    overland.dt=dt;
    overland.init_hov=previous_solution->hov;
    overland.previous_hov=previous_solution->hov;
    overland.P=previous_solution->P;
    overland.l=previous_solution->hsat;
    overland.Pl=Pl;
    overland.hydr=new_solution->hydr;
    overland.n1_init_hov=n1_init_hov;
    overland.error_x=error_x;
    overland.run();
    swap(new_solution->hov,overland.hov);
    error+=overland.total_error;

    //----------------------------------------------
    // Apply AllVerticalRichads algorithm to obtain
    // - P
    // - hsat
    //---------------------------------------------

    all_vertical_richards.dt=dt;
    all_vertical_richards.init_P=previous_solution->P;
    all_vertical_richards.previous_P=previous_solution->P;
    all_vertical_richards.hydr=new_solution->hydr;
    all_vertical_richards.l=previous_solution->hsat;
    all_vertical_richards.Pl=Pl;
    all_vertical_richards.Psoil=overland.Psoil;
    //Specify output (may can change during the run of all_vertical_richards)
    all_vertical_richards.hsat=new_solution->hsat;
    all_vertical_richards.P=new_solution->P;
    all_vertical_richards.error_x=error_x;
    all_vertical_richards.init_indice_x_Richards();
    all_vertical_richards.run();
    new_solution->hsat=all_vertical_richards.hsat;
    new_solution->P=all_vertical_richards.P;
    size_t k=0;
    double previous_error=numeric_limits<double>::infinity();
  //  Debug::pause();

    while(error>=tolerence_Piccard and k<max_iterations_Piccard and (abs(previous_error-error)>tolerence_Piccard/100 or error<oscilation_Piccard)){
      previous_error=error;
      error=0;
      ++k;
      #ifdef MYDEBUG
      if(Debug::level>1) cout<<"  [Piccard::run] k = "<<k<<endl;
      #endif

      compute_l(l,all_vertical_richards.hsat,error);

      #ifdef MYDEBUG
      if(Debug::level>1) cout<<"  [Piccard::run] error (l) = \033[31m"<<error<<"\033[0m"<<endl;
      #endif

      compute_Pl(all_vertical_richards.P);
      horizontal_problem.previous_hydr=new_solution->hydr;
      horizontal_problem.l=l;
      horizontal_problem.Pl=Pl;
      horizontal_problem.run();
      swap(new_solution->hydr,horizontal_problem.hydr);

      //new_solution->hydr=horizontal_problem.hydr;

      error+=horizontal_problem.total_error;

      #ifdef MYDEBUG
      if(Debug::level>1) cout<<"  [Piccard::run] error (l+hori) = \033[31m"<<error<<"\033[0m"<<endl;
      #endif

      //Compute Overland
      overland.previous_hov=new_solution->hov;
      overland.P=new_solution->P;
      overland.l=l;
      overland.Pl=Pl;
      overland.hydr=new_solution->hydr;
      overland.run();
      swap(new_solution->hov,overland.hov);
      error+=overland.total_error;

      #ifdef MYDEBUG
      if(Debug::level>1) cout<<"  [Piccard::run] error (l+hori+overland) = \033[31m"<<error<<"\033[0m"<<endl;
      #endif

      //Voir calcul indice_x_Richards
      all_vertical_richards.init_P=previous_solution->P; //P_0
      all_vertical_richards.previous_P=all_vertical_richards.P; //P_{k-1}
      all_vertical_richards.hydr=new_solution->hydr;
      all_vertical_richards.l=l;
      all_vertical_richards.Pl=Pl;
      all_vertical_richards.Psoil=overland.Psoil;
      //Specify output (may can change during the run of all_vertical_richards)
      all_vertical_richards.hsat=new_solution->hsat;
      all_vertical_richards.P=new_solution->P;
      all_vertical_richards.update_indice_x_Richards();

      all_vertical_richards.run();
      new_solution->hsat=all_vertical_richards.hsat;
      new_solution->P=all_vertical_richards.P;
      //we get P_k : all_vertical_richards.P
      if(!all_vertical_richards.has_converged){
        has_converged=false;
        return;
      }

    }
    if(error>=tolerence_Piccard){
      #ifdef MYDEBUG
      if(Debug::level>1) cout<<"  [Piccard]::run not converge"<<endl;
      exit(0);
      #endif
      has_converged=false;
      //Voir nettoyage memoire
      return;
    }
    #ifdef MYDEBUG
    if(Debug::level>1) cout<<"  [Piccard]::run converge"<<endl;
    #endif
    swap(l,new_solution->l);
    swap(Pl,new_solution->Pl);
    has_converged=true;
    //Voir nettoyage memoire
    return;
  }

  void
  Piccard::compute_l(double* h,double* hsat,double& error) {
    double e=0;
    for(size_t ix=0;ix<geometry->nX;++ix){
      double a=h[ix];
      double b=max(hsat[ix],a);
      l[ix]=b;
      double t=b-a;
      e+=t*t;
    }
    error+=sqrt(e);
  }

  void
  Piccard::compute_Pl(double** P){
    for(size_t ix=0;ix<geometry->nX;++ix){
      double* Px=P[ix];
      if(l[ix]==geometry->hsoil[ix]){
        Pl[ix]=Px[geometry->nZ[ix]-1];
      }
      else{
        size_t a=(l[ix]-geometry->hbot[ix])/geometry->dZ[ix];
        double p1=Px[a];
        double p2=Px[a+1];
        Pl[ix]=p1+(p2-p1)/geometry->dZ[ix]*(l[ix]-geometry->Z[ix][a]);
      }
    }
  }
}