#include "particleBeam.h"
#include "mathematicalConstants.h"
#include "PhysicalConstants.h"
#include "mathematicalTools.h"
#include "mixedTools.h"

#include <stdio.h>
#include <algorithm>
#include <sstream>

using namespace std;

particleBeam::particleBeam()  {
  P0Transport_ = 0.0;
  particleRepresentationOk_ = false;
  momentRepresentationOk_ = false;
}

void particleBeam::clear() {
  goodPartic_.clear();
  rij_.raz();
  P0Transport_ = 0.0;
  particleRepresentationOk_ = false;
  momentRepresentationOk_ = false;
}

int particleBeam::getNbParticles() const {
  return goodPartic_.size();
}

const beam2Moments& particleBeam::getTransportMoments() const  { 
  return rij_;
}

double particleBeam::getSigmaTransportij(unsigned indexI, unsigned indexJ)  {
  if (  indexI > 5 ||  indexJ > 5 ) {
    cerr << " particleBeam::getSigmaTransportij() indices out of range  " << endl;
    return 0.0;
  }
  if ( !momentRepresentationOk_ ) {
    cerr << " particleBeam::getSigmaTransportij() beam is not in moment representation " << endl;
    return 0.0;
  }
 if ( indexI >= indexJ ) {
   return ( rij_.getMatrix().at(indexI) ).at(indexJ);
 } else {
   return ( rij_.getMatrix().at(indexJ) ).at(indexI);
 }
  
}

double particleBeam::getUnnormalizedEmittanceX() {
  double r = getSigmaTransportij(1,0);
  double rac = (1 - r*r);
  if ( rac <= 0.0 ) {
    return 0.0;
  }
  rac = sqrt(1 - r*r);
  return  dimensionalFactorFromTransportToGraphics(0)*getSigmaTransportij(0,0) * getSigmaTransportij(1,1) * rac; // en pi.mm.mrad
}

double particleBeam::getUnnormalizedTranspPhaseSpaceArea(unsigned TranspIndexAbs, unsigned TranspIndexOrd) {
  if (  TranspIndexAbs == TranspIndexOrd ) return 0.0;
  double r = getSigmaTransportij(TranspIndexAbs,TranspIndexOrd);
  double rac = (1 - r*r);
  if ( rac <= 0.0 ) {
    return 0.0;
  }
  rac = sqrt(1 - r*r);
  return  dimensionalFactorFromTransportToGraphics(TranspIndexAbs)*getSigmaTransportij(TranspIndexAbs,TranspIndexAbs) * 
             dimensionalFactorFromTransportToGraphics(TranspIndexOrd)*getSigmaTransportij(TranspIndexOrd,TranspIndexOrd) * rac; // en pi.mm.mrad
}



double particleBeam::getP0Transport() const { 
  return P0Transport_;
}

double particleBeam::referenceKineticEnergyMeV() const {
  if ( particleRepresentationOk_ ) {
    return (referenceParticle_.getGamma() -1.) * EREST_MeV;
  } else {
    double P0Norm = 1000.0 * P0Transport_ / EREST_MeV;
    double gamma = sqrt(1.0 +  P0Norm * P0Norm);
    return (gamma - 1.0) * EREST_MeV;
  }
}

void particleBeam::set2Moments(beam2Moments& moments) {
  rij_ = moments;
  momentRepresentationOk_ = true;
}

void particleBeam::setWithParticles(vector<double>& centroid, bareParticle& referencePart, vector<bareParticle>& particles) {
  cout << " particleBeam::setWithParticles taille vect. part. " << particles.size() << endl;
  centroid_.clear();
  centroid_ = centroid;
  referenceParticle_ = referencePart;
  goodPartic_.clear();
  goodPartic_ = particles;
  cout << " particleBeam::setWithParticles taille vect. part. ENREGISTRE " << goodPartic_.size() << endl;
  particleRepresentationOk_ = true;
}

bool particleBeam::particleRepresentationOk() const {
  return particleRepresentationOk_;
}
bool particleBeam::momentRepresentationOk() const {
  return momentRepresentationOk_;
}

void  particleBeam::addParticle( bareParticle p)
{
  goodPartic_.push_back(p);
}

const vector<bareParticle>& particleBeam::getParticleVector() const
{
  return goodPartic_;
}

vector<bareParticle>& particleBeam::getParticleVector() 
{
  return goodPartic_;
}

void particleBeam::getVariance(double& varx, double& vary, double& varz) const {
  unsigned int k;
  double x,y,z;
  double xav = 0.;
  double yav = 0.;
  double zav = 0.;
  double xavsq = 0.;
  double yavsq = 0.;
  double zavsq = 0.;

  TRIDVECTOR pos;


  for ( k = 0 ; k < goodPartic_.size(); k++) {
    pos = goodPartic_.at(k).getPosition();
    pos.getComponents(x,y,z);
    //      partic_[k].getXYZ(x,y,z);
    xav += x;
    xavsq += x*x;
    yav += y;
    yavsq += y*y;
    zav += z;
    zavsq += z*z;
  }

  double aginv = double (goodPartic_.size());
  aginv = 1.0/aginv;

  varx =  aginv * ( xavsq - xav*xav*aginv );
  vary =  aginv * ( yavsq - yav*yav*aginv );
  varz =  aginv * ( zavsq - zav*zav*aginv );
}


void particleBeam::printAllXYZ() const {
  cout << " dump du faisceau : " << endl;
  cout <<  goodPartic_.size() << " particules " << endl;
  unsigned int k;
  for ( k = 0 ; k < goodPartic_.size(); k++)
    {
      double xx,yy,zz;
      goodPartic_.at(k).getPosition().getComponents(xx,yy,zz);
      double betgamx, betgamy, betgamz;
      goodPartic_.at(k).getBetaGamma().getComponents(betgamx, betgamy, betgamz);
      cout << " part. numero " << k << "  x= " << xx << " y= " << yy  << " z= " << zz << "  betgamx= " << betgamx << " betgamy= " << betgamy  << " betgamz= " << betgamz << endl;
    }
}



void particleBeam::Zrange(double& zmin, double& zmax) const {
  double z;
  zmin = GRAND;
  zmax = -zmin;

  unsigned int k;
  for ( k = 0 ; k < goodPartic_.size(); k++)
    {
      z = goodPartic_.at(k).getZ();
      if ( z < zmin ) zmin = z;
      else if ( z > zmax) zmax = z;	    
    }
}



string particleBeam::FileOutputFlow() const {
  ostringstream sortie;
  unsigned int k;
  for ( k = 0 ; k < goodPartic_.size(); k++)
    {
      sortie << goodPartic_.at(k).FileOutputFlow() << endl;
    }
  sortie << endl;
  return sortie.str();
}

bool particleBeam::FileInput( ifstream& ifs) {
  bool test = true;
  string dum1, dum2;
  double dummy;
  if ( !( ifs >> dum1 >> dum2 >> dummy) ) return false;
 
  bareParticle pp;
  while ( pp.FileInput(ifs) )
    {
      addParticle( pp);
    }
  return test;
}

void particleBeam::buildMomentRepresentation() {

  unsigned k,j,m;
  double auxj, auxm;
  if ( !particleRepresentationOk_)
    {
      cerr << " particleBeam::buildMomentRepresentation() vecteur de particules invalide" << endl;
      return;
    }

  cout << " buildMomentRepresentation " << endl;
  //  printAllXYZ();

  double gref = referenceParticle_.getGamma() - 1.0;
  double P_reference_MeV_sur_c = sqrt( gref*(gref+2) );

  cout << " gref = " << gref << " P_reference_MeV_sur_c = " << P_reference_MeV_sur_c << endl;


  // initialisation des moments
  razDesMoments();

  // accumulation
  TRIDVECTOR pos;
  TRIDVECTOR begam;
  double gamma;
  double begamz;
  double g;
  double PMeVsc;
  double del;
  vector<double> part(6, 0.0);

    vector< vector<double> >& matrice = rij_.getMatrix();


  for (k=0; k < goodPartic_.size(); k++) {
    gamma = goodPartic_.at(k).getGamma();
    pos = goodPartic_.at(k).getPosition();
    begam= goodPartic_.at(k).getBetaGamma();
    begamz = begam.getComponent(2);
    g = gamma -1.0;
    PMeVsc = sqrt( g*(g+2) );
    del = 100.0 * ( PMeVsc -  P_reference_MeV_sur_c ) / P_reference_MeV_sur_c ; // en %

    part[0] = pos.getComponent(0);
    part[1] = begam.getComponent(0)/begamz;
    part[2] = pos.getComponent(1);
    part[3] = begam.getComponent(1)/begamz;
    part[4] = pos.getComponent(2);
    part[5] = del;

    for ( j = 0; j < 6; j++) {
      auxj = part.at(j) - centroid_.at(j);
      for (m=0; m <= j; m++) 
	{
	  auxm = part.at(m) - centroid_.at(m);

	  ( matrice.at(j) ).at(m) += auxj*auxm;
	  //	  ( rij_transportMoments_.at(j) ).at(m) += auxj*auxm;


	  //	      cout << " j= " << j << " m= " << m << " rjm= " << ( rij_transportMoments_.at(j) ).at(m) << endl;
	}
    }
  }


  // moyenne
  double facmoy = 1.0/double( goodPartic_.size() );
  for ( j = 0; j < 6; j++) {
        ( matrice.at(j) ).at(j) = sqrt(( matrice.at(j) ).at(j) * facmoy );
  }

  for ( j = 0; j < 6; j++) {
    auxj =  ( matrice.at(j) ).at(j);
    for (m=0; m < j; m++) {
      auxm = ( matrice.at(m) ).at(m);
      (  matrice.at(j) ).at(m) *= facmoy/(auxj * auxm);
    }
  }
    
  ////////////////// si C21 = 1 , transport plante ! a voir //////////
cout << " valeur initiale de  C21: " << ( matrice.at(1) ).at(0) << endl;
  if ( ( matrice.at(1) ).at(0) >0.999999  ) {
    ( matrice.at(1) ).at(0) = 0.999999;
    cout << " j'ai fait la correction C21: " << ( matrice.at(1) ).at(0) << endl;
  }
  

  // les longueurs sont en cm
  // les angles en radians, on passe en mrad;

  double uniteAngle = 1.0e+3;
  ( matrice.at(1) ).at(1)  *= uniteAngle;
  ( matrice.at(3) ).at(3)  *= uniteAngle;
  P0Transport_ = 1.0e-3*EREST_MeV*P_reference_MeV_sur_c;

  //  cout << " buildmomentrepresentation impression des moments " << endl;
  //  impressionDesMoments();

  momentRepresentationOk_ = true;
}

void particleBeam::impressionDesMoments() const {
  rij_.impression();
}

void particleBeam::razDesMoments() {
  rij_.raz();
}


// void particleBeam::readTransportMoments(ifstream& inp) {
// rij_.readFromTransportOutput(inp);
// }

// void particleBeam::readTransportMoments(stringstream& inp) {
// rij_.readFromTransportOutput(inp);
// }

double particleBeam::getXmaxRms() {
  if ( !momentRepresentationOk_ ) buildMomentRepresentation();
  return ( rij_.getMatrix().at(0) ).at(0);
  // return ( rij_transportMoments_.at(0) ).at(0);
}


void particleBeam::particlesPhaseSpaceData(vector<double>& xcor, vector<double>& ycor, vector<string>& legende, string namex, string namey) {

  unsigned indexAbs, indexOrd;
  indexAbs = pspaCoorIndexFromString(namex);
  indexOrd = pspaCoorIndexFromString(namey);
 
  particlesPhaseSpaceComponent(xcor, indexAbs);
  particlesPhaseSpaceComponent(ycor, indexOrd);
  legende.clear();
    legende.push_back( "phase space " + namex + "," + namey);
    legende.push_back( "particle number : " +   mixedTools::intToString(getNbParticles()));
}

void particleBeam::particlesPhaseSpaceComponent(vector<double>& coord, unsigned index) 
{
  if ( !particleRepresentationOk_ ) return;
  
  coord.clear();
  coord.resize(goodPartic_.size(), 0.0 );
  cout << " particleBeam::particlesPhaseSpaceComponent index = " << index << endl;

  if ( index <= 2 ) {
    for (unsigned i = 0; i < goodPartic_.size(); ++i) {
      coord.at(i) =  10.*goodPartic_.at(i).getPosition().getComponent(index);  // en mm
    }
    return;
  }
  
  if ( index >  2 && index < 5 ) {
    for (unsigned i = 0; i < goodPartic_.size(); ++i) {
      double begamz = goodPartic_.at(i).getBetaGamma().getComponent(2);
      if ( begamz != 0.0) {
	coord.at(i) =  1000.*goodPartic_.at(i).getBetaGamma().getComponent(index - 3)/begamz; // milliradians
      } else {
	coord.at(i) = 0.0;
      }
    }
    return;
  }

  if ( index == 5 ) {
    double gamma0 = referenceParticle_.getGamma();
    if ( gamma0 == 0.0 ) return;
    for (unsigned i = 0; i < goodPartic_.size(); ++i) {
      coord.at(i) =  100.*(goodPartic_.at(i).getGamma() - gamma0)/gamma0;  // en %
    }
    return;
  }
}

unsigned particleBeam::indexFromPspaToTransport(unsigned index) const {
  cout << " indexFromPspaToTransport entree : " << index << endl;
  switch ( index ) {
  case 0 : return  0; // x
  case 1 : return  2;  // y
  case 2 : return  4; // z -> l
  case 3 : return  1;  // xp
  case 4 : return  3;  // yp
  case 5 : return  5; // de/E
  default : { 
    cout << " particleBeam::indexFromPspaToTransport : coordinate index ERROR inital index :  "<< index << endl;
    return 99;
  }
  }
}

unsigned particleBeam::pspaCoorIndexFromString(string nameCoor) const {
  if ( nameCoor == "x" ) {
    return 0;
  } else if ( nameCoor == "y" ) {
    return 1;
  } else if ( nameCoor == "dz" ) {
    return 2;
  } else if ( nameCoor == "xp" ) {
    return 3;
  } else if ( nameCoor == "yp" ) {
    return 4;
  } else if ( nameCoor == "dE/E" ) {
    return 5;
  } else {
    return 99;
  }
}

unsigned particleBeam::transportCoorIndexFromString(string nameCoor) const {
  if ( nameCoor == "x" ) {
    return 0;
  } else if ( nameCoor == "y" ) {
    return 2;
  } else if ( nameCoor == "dz" ) {
    return 4;
  } else if ( nameCoor == "xp" ) {
    return 1;
  } else if ( nameCoor == "yp" ) {
    return 3;
  } else if ( nameCoor == "dE/E" ) {
    return 5;
  } else {
    return 99;
  }
}

//void particleBeam::donneesDessinEllipse(vector<double>& xcor, vector<double>& ycor, vector<string>& legende, unsigned indexAbs, unsigned indexOrd) {

void particleBeam::donneesDessinEllipse(vector<double>& xcor, vector<double>& ycor, vector<string>& legende, string namex, string namey) {
  int k;
  double x,y;
  if ( !momentRepresentationOk_ ) buildMomentRepresentation();

  //  if ( !momentRepresentationOk_ ) return;

  unsigned indexAbs, indexOrd;


  // index TRANSPORT
  indexAbs = transportCoorIndexFromString(namex);
  indexOrd = transportCoorIndexFromString(namey);
  cout << " namex= " << namex << " indexAbs= " << indexAbs << " namey= " << namey << " indexOrd= " << indexOrd << endl; 
    if ( indexAbs > 5 || indexOrd > 5 ) return;

  xcor.clear();
  ycor.clear();



  double dimensionalFactorX, dimensionalFactorY; // to mm, if necessary
  dimensionalFactorX = dimensionalFactorFromTransportToGraphics(indexAbs);
  dimensionalFactorY = dimensionalFactorFromTransportToGraphics(indexOrd);



  legende.clear();
  //  if ( indexAbs == 0 && indexOrd == 1 ) {
    string namx = transportVariableName(indexAbs);
    string namy = transportVariableName(indexOrd);
    double em = getUnnormalizedTranspPhaseSpaceArea(indexAbs,indexOrd) ;
    string  emitt = mixedTools::doubleToString(getUnnormalizedTranspPhaseSpaceArea(indexAbs,indexOrd));
    string xmax = namx + "max= ";
    string valXmax = mixedTools::doubleToString(dimensionalFactorX*getSigmaTransportij(indexAbs,indexAbs)); 
    string ymax = namy + "max= ";
    string valYmax = mixedTools::doubleToString(dimensionalFactorY*getSigmaTransportij(indexOrd,indexOrd)); // mm
    string correl = " correlation ";
    string valCorrel = mixedTools::doubleToString(getSigmaTransportij(1,0));
    string xunit = graphicTransportUnitName(indexAbs);
    string yunit = graphicTransportUnitName(indexOrd);
    legende.push_back( "emittance" + namx + "," + namy + ": " + emitt + " pi." + xunit + "." + yunit);
    legende.push_back( xmax + valXmax + xunit);
    legende.push_back( ymax + valYmax + yunit);
  // } else {
  //   legende.push_back(" text of legend not yet programmed ");
  // }

  cout << " index x" << indexAbs << " index y " << indexOrd << endl;
  double xm = dimensionalFactorX*( rij_.getMatrix().at(indexAbs) ).at(indexAbs);
  double ym = dimensionalFactorY*( rij_.getMatrix().at(indexOrd) ).at(indexOrd);
  double r;
  if ( indexOrd > indexAbs ) {
    r  = ( rij_.getMatrix().at(indexOrd) ).at(indexAbs);
  } else {
    r  = ( rij_.getMatrix().at(indexAbs) ).at(indexOrd);
  }

  cout <<  " racs11= " << xm << " racs22= " << ym << " r12= " << r << endl;


  int nbintv = 50;
  if ( xm == 0.0 ) return;
  double pas = 2.0 * xm / nbintv;

  //  cout << " r= " << r << endl;
  double rac = (1 - r*r);
  if ( rac > 0.0 ) 
    {
      cout << " cas rac > " << endl;
      rac = sqrt(1 - r*r);
      double alpha = -r / rac;
      double beta = xm / ( ym * rac);
      //  double gamma = ym / ( xm * rac );
      double epsil = xm * ym * rac;
      double fac1 = -1.0 / ( beta * beta);
      double fac2 = epsil/beta;
      double fac3 = -alpha/beta;
      double aux;
      for ( k=0; k < nbintv; k++)
	{
	  x = -xm + k*pas;
	  aux = fac1 * x * x + fac2;
	  //     cout << " aux2= " << aux << endl;
	  if ( aux <= 0.0 )
	    {
	      aux = 0.0;
	    }
	  else aux = sqrt(aux);
      
	  //        y = fac3*x;
	  y = fac3*x + aux;
	  xcor.push_back(x);
	  ycor.push_back(y);
	}

      for ( k=0; k <= nbintv; k++)
	{
	  x = xm - k*pas;
	  aux =  fac1 * x * x + fac2;
	  if ( aux <= 0.0 ) 
	    {
	      aux = 0.0;
	    }
	  else aux = sqrt(aux);
	  //   y = fac3*x;
	  y = fac3*x - aux;
	  xcor.push_back(x);
	  ycor.push_back(y);
	}
    }
  else
    // cas degenere
    {
      cout << " cas degenere " << endl;
      double fac = ym/xm;
      for ( k=0; k < nbintv; k++)
	{
	  x = -xm + k*pas;
	  y = fac*x;
	  xcor.push_back(x);
	  ycor.push_back(y);
	}
	
    }
}

void particleBeam::histogramme(unsigned int iabs,vector<double>& xcor,vector<int>& hist,double out[3])
{
  // out[0]= nbre de particules, out[1]= moyenne, out[2]= ecart-type
  vector<double> vshf;
  particlesPhaseSpaceComponent(vshf,iabs); 
  histogramInitialize(iabs,vshf,out);
  histogramPacking(out[2],vshf,xcor,hist);

  if(iabs == 5) {
    out[1] *= EREST_MeV; // moyenne en MeV
    out[2] *= EREST_keV; // ecart-type en KeV
  }
}

void particleBeam::histogramInitialize(unsigned int index,vector<double>& vshf,double out[3])
{
  double vmin= GRAND;
  double vmax= -vmin;
  double vmoy= 0.0;
  double ecatyp= 0.0;
  
  for (unsigned int k = 0; k < goodPartic_.size(); k++) {
    if (vshf[k] < vmin) vmin = vshf[k];
    else if (vshf[k] > vmax) vmax = vshf[k];
    vmoy += vshf[k];
    ecatyp += vshf[k]*vshf[k];
  }

  double sum= (float)goodPartic_.size();
  out[0]= sum; 
  vmoy /= sum;
  out[1]= vmoy;
  ecatyp /= sum;
  ecatyp = sqrt(abs(ecatyp-vmoy*vmoy));
  out[2]= ecatyp;

  if(index == 0) {
    cout << "position x -moyenne " << vmoy << " cm " << "-mini " << vmin << " cm " << "-maxi " << vmax << " cm " << "ecart type " << ecatyp << " cm" << endl;
  } 
  if(index == 1) {
    cout << "position y -moyenne " << vmoy << " cm " << "-mini " << vmin << " cm " << "-maxi " << vmax << " cm " << "ecart type " << ecatyp << " cm" << endl;
  } 
  if(index == 2) {
    cout << "position z -moyenne " << vmoy << " cm " << "-mini " << vmin << " cm " << "-maxi " << vmax << " cm " << "ecart type " << ecatyp << " cm" << endl;
  }
  if(index == 3) {
    cout << "divergence xp -moyenne " << vmoy << " mrad " << "-mini " << vmin << " mrad " << "-maxi " << vmax << " mrad " << "ecart type " << ecatyp << " mrad" << endl;
  } 
  if(index == 4) {
    cout << "divergence yp -moyenne " << vmoy << " mrad " << "-mini " << vmin << " mrad " << "-maxi " << vmax << " mrad " << "ecart type " << ecatyp << " mrad" << endl;
  } 
  if(index == 5) {
    double gmin = vmin-1.0;
    double gmax = vmax-1.0;
    cout << "energie cinetique -moyenne " << vmoy*EREST_MeV << " Mev " << "-mini " << gmin*EREST_MeV << " Mev " << "-maxi " << gmax*EREST_MeV << " Mev " << "ecart type " << ecatyp*EREST_MeV << " Kev" << endl;
  }

  for (unsigned int k = 0; k < goodPartic_.size(); k++) {
    vshf[k] -= vmoy;
  }
}

void particleBeam::histogramPacking(double ecatyp,vector<double>vshf,vector<double>&xcor,vector<int>& hist)
{
  // demi fenetre hfene= max(3.*ecatyp-vmoy,vmoy-3.*ecatyp);
  double hfene= 3.*ecatyp;
  // et pas de l'histogramme
  double hpas = hfene/25.;
  
  cout << "demi fenetre " << hfene << ", hpas= " << hpas << endl;

  double vmin = -hfene;
  double dfen = 2.*hfene;
  int ihist = dfen/hpas;
  double phist = ihist*hpas;
  double dpas = hpas-(dfen-phist);
  if(dpas <= hpas*1.e-03) {
    ihist++;
    phist= ihist*hpas;
  }
  double vmax= vmin+hpas*ihist;
  
  cout << "xAxisNumberOfBins= " << ihist <<", xAxisMinimum= " << vmin << ", xAxisMaximum= " << vmax << ", NParticules= " << vshf.size() << ", phist " << phist << endl;
  
  if(!xcor.empty()) xcor.clear();
  xcor.resize(ihist+1);  
  for (int i = 0; i <= ihist; ++i) {
    xcor[i] = vmin+i*hpas;
  }

  /////////////////////////////////////

  if(!hist.empty()) hist.clear();
  hist.resize(ihist,0);  
  for (unsigned int i = 0; i < vshf.size(); ++i) {
    double var= vshf[i]-vmin;
    if(var < 0 ) {
      cout<<"lesser that the minimum "<<var<<", ("<< i<<")"<< endl;
      hist.at(0)++;
    } else if(var >= phist) {
      cout<<"greater that the maximum "<<var<<", ("<< i<<")"<< endl;
      hist.at(ihist-1)++;
    } else {
      int kk= (int)floor(var/hpas);
      hist.at(kk)++;
    }
  }
}