#include "particleBeam.h"
#include "mathematicalConstants.h"
#include "PhysicalConstants.h"
//#include <string>
#include <stdio.h>
#include <algorithm>

#include "environmentVariables.h"



particleBeam::particleBeam()  {
  // rij_transportMoments_.resize(6);
  // unsigned dim=0;
  // unsigned k;
  // for ( k=0; k < 6; k++){
  //   rij_transportMoments_.at(k).resize(++dim); 
  // }
  P0Transport_ = 0.0;
  particleRepresentationOk_ = false;
  momentRepresentationOk_ = false;
}

void particleBeam::clear() {
  unsigned k,j;
  goodPartic_.clear();
  rij_.raz();
  // for ( k=0; k < 6; k++){
  //   for ( j=0; j <= k; j++) {
  //     ( rij_transportMoments_.at(k) ).at(j) = 0.0; 
  //   }
  // }
  P0Transport_ = 0.0;
  particleRepresentationOk_ = false;
  momentRepresentationOk_ = false;
}

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

const transportMoments&   particleBeam::getTransportMoments() const  { 
    return rij_;
}
double particleBeam::getP0Transport() const { 
  return P0Transport_;
}
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::setFromBeamParameters(double x, double xp, double y, double yp, double dl, double del, double P0)
  // {
  //   clear();
  //   rij_ = transportMoments(x, xp, y, yp, dl, del);
  //   P0Transport_ = P0;
  //   momentRepresentationOk_ = true;
  // }


bool  particleBeam::setFromTransport(string elLab, const nomdElements elem)
{
  string elementLabel = elLab;
  // transformer le label en majuscules ; je ne suis pas sur que ca 
  // marchera a tous les coups (glm)
  std::transform(elementLabel.begin(), elementLabel.end(), elementLabel.begin(), (int (*)(int))std::toupper);

  cout << " particleBeam::setFromTransport on cherche element " << elementLabel << endl;
  string buf;
  ifstream infile;
  string nameIn = WORKINGAREA + "transport.output";
  infile.open(nameIn.c_str(), ios::in);
  if (!infile) {
    cerr << " particleBeam::setFromTransport : error re-opening transport output stream " << nameIn << endl;
    return false;
  }
  //  else cout << " particleBeam::setFromTransport() : ouverture du fichier " << nameIn << endl; 

  string::size_type nn = string::npos;
  while ( getline(infile, buf) ) {
    //      cout << " buf= "  << buf << endl;
    nn = buf.find(elementLabel);
    //     cout << " string::npos= " << string::npos << " nn= " << nn << endl;
    if ( nn != string::npos ) {
      //	  cout << " particleBeam::setFromTransport : element " << elementLabel << " trouve " << endl;
      break;
    }
  }

  if ( nn == string::npos ) {
    cerr << " particleBeam::setFromTransport : element " << elementLabel << " non trouve dans le fichier  " << nameIn << endl;
    return false;
  }
  if (elem.getElementType() == bend ) {
    getline(infile, buf);
    getline(infile, buf);
  }
  readTransportMoments(infile);
  //  impressionDesMoments();
  infile.close();
  momentRepresentationOk_ = true;
  return true;
}


bool particleBeam::setFromParmela(unsigned numeroElement, double referencefrequency) {
  unsigned  k;
  FILE* filefais;
  string nomfilefais = WORKINGAREA + "parmdesz";
  cout << " nom fichier desz : " << nomfilefais << endl;
  filefais = fopen(nomfilefais.c_str(), "r");

  if ( filefais == (FILE*)0 ) {
    cerr << " particleBeam::setFromParmela() erreur a l'ouverture du fichier" << nomfilefais  << endl;; 
    return false;
  }
  else cout << " particleBeam::setFromParmela() : ouverture du fichier " << nomfilefais << endl; 

  parmelaParticle partic;
  std::vector<parmelaParticle> faisceau;

  cout << " particleBeam::setFromParmela : numeroElement = " << numeroElement << endl;
  numeroElement--;



  int testNombrePartRef =0;
  double phaseRef;

  while( partic.readFromParmelaFile(filefais) > 0 ) {
    if ( partic.ne == numeroElement )
      {
	faisceau.push_back(partic);

	if ( partic.np == 1 ) {
	  // en principe on est sur la particule de reference
	  if ( fabs(partic.xx) > EPSILON || fabs(partic.yy) > EPSILON || fabs(partic.xxp) > EPSILON  || fabs(partic.yyp) > EPSILON) {
	    printf(" ATTENTION part. reference douteuse  \n");
	    partic.imprim();
	  }
	  phaseRef = partic.phi;
	  TRIDVECTOR  posRef(partic.xx,partic.yy,0.0);
	  TRIDVECTOR betagammaRef(partic.xxp*partic.begamz, partic.yyp*partic.begamz, partic.begamz);
	  referenceParticle_ = bareParticle(posRef, betagammaRef);
	  testNombrePartRef++;
	  if ( testNombrePartRef != 1 ) {
	    cerr << " TROP DE PART. DE REF : " << testNombrePartRef << " !! " << endl;
	    return false;
	  }
	}
      }
  }

  if ( faisceau.size() == 0) 
    {
      cerr << " particleBeam::setFromParmela echec lecture  element " << numeroElement+1 << endl;
      return false;
    }
  
  // facteur  c/ 360. pour calculer (c dphi) / (360.freq) 
  // avec freq en Mhz et dphi en degres et résultat en cm: 
  double FACTEUR =  83.3333;  // ameliorer la precision



  // pour l'instant on choisit un centroid nul;
  centroid_ = vector<double>(6,0.0);

  goodPartic_.clear();
  goodPartic_.resize(faisceau.size(), bareParticle());
  double x,xp,y,yp;
  double betagammaz;
  double betaz;
  double deltaz;
  double dephas;
  double g;
  TRIDVECTOR  pos;
  TRIDVECTOR betagamma;
  // contrairement a ce qu'indique la notice PARMELA, dans parmdesz, les xp et yp 
  // sont donnes en radians
  for ( k=0; k < faisceau.size(); k++) {
    x=faisceau.at(k).xx;
    xp=faisceau.at(k).xxp;
    y=faisceau.at(k).yy;
    yp=faisceau.at(k).yyp;

    // dephasage par rapport a la reference  
    dephas = faisceau.at(k).phi - phaseRef; // degrés
    g = faisceau.at(k).wz/ERESTMeV;
    betagammaz = faisceau.at(k).begamz;
    betaz = betagammaz/(g+1.0);
    deltaz = FACTEUR * betaz * dephas / referencefrequency;
    x += xp * deltaz;
    y += yp * deltaz;
    pos.setComponents(x,y,deltaz);
    betagamma.setComponents(xp*betagammaz, yp*betagammaz, betagammaz);
    goodPartic_.at(k) = bareParticle(pos,betagamma);
  }
  particleRepresentationOk_ = true;
  return true;
}


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);
      cout << " part. numero " << k << "  x= " << xx << " y= " << yy  << " z= " << zz << 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) );

  // initialisation des moments
  razDesMoments();

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

    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 );
    //    ( rij_transportMoments_.at(j) ).at(j) = sqrt(( rij_transportMoments_.at(j) ).at(j) * facmoy );
  }

  for ( j = 0; j < 6; j++) {
    auxj =  ( matrice.at(j) ).at(j);
    //    auxj =  ( rij_transportMoments_.at(j) ).at(j);
    for (m=0; m < j; m++) {
      auxm = ( matrice.at(m) ).at(m);
      (  matrice.at(j) ).at(m) *= facmoy/(auxj * auxm);
      //      auxm = ( rij_transportMoments_.at(m) ).at(m);
      // ( rij_transportMoments_.at(j) ).at(m) *= facmoy/(auxj * auxm);
    }
  }
    
  // 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;

  // ( rij_transportMoments_.at(1) ).at(1)  *= uniteAngle;
  // ( rij_transportMoments_.at(3) ).at(3)  *= uniteAngle;
   
  P0Transport_ = 1.0e-3*ERESTMeV*P_reference_MeV_sur_c;

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

  momentRepresentationOk_ = true;
}

void particleBeam::impressionDesMoments() const {
  rij_.impression();
  // unsigned j,m;
  // cout << " impression des moments " << endl;
  // for ( j = 0; j < 6; j++)
  //   {
  //     for (m=0; m <= j; m++) 
  // 	{
  // 	  cout  << ( rij_transportMoments_.at(j) ).at(m) << " ";
  // 	}
  //     cout << endl;
  //   }
}

void particleBeam::razDesMoments() {
  rij_.raz();
  // // initialisation des moments
  // unsigned j,m;
  // for ( j = 0; j < rij_transportMoments_.size(); j++)
  //   {
  //     for (m=0; m <= j; m++) 
  // 	{
  // 	  ( rij_transportMoments_.at(j) ).at(m) = 0.0; // element r_jm
  // 	}
  //   }
}


void particleBeam::readTransportMoments(ifstream& inp) {
rij_.readFromTransportOutput(inp);
  // string  bidString;
  // double bidon;

  // // initialisation des moments
  // razDesMoments();

  // inp >> bidon >>  bidString >>  bidon >>  ( rij_transportMoments_.at(0) ).at(0) >> bidString;
  // inp >> bidon >> ( rij_transportMoments_.at(1) ).at(1) >>  bidString >> ( rij_transportMoments_.at(1) ).at(0);
  // inp >> bidon >> ( rij_transportMoments_.at(2) ).at(2) >>  bidString >> ( rij_transportMoments_.at(2) ).at(0)  >> ( rij_transportMoments_.at(2) ).at(1);
  // inp >> bidon >> ( rij_transportMoments_.at(3) ).at(3) >>  bidString >> ( rij_transportMoments_.at(3) ).at(0)  >> ( rij_transportMoments_.at(3) ).at(1) >> ( rij_transportMoments_.at(3) ).at(2);

  // inp >> bidon >> ( rij_transportMoments_.at(4) ).at(4) >>  bidString >> ( rij_transportMoments_.at(4) ).at(0)  >> ( rij_transportMoments_.at(4) ).at(1) >> ( rij_transportMoments_.at(4) ).at(2) >> ( rij_transportMoments_.at(4) ).at(3);

  // inp >> bidon >> ( rij_transportMoments_.at(5) ).at(5) >>  bidString >> ( rij_transportMoments_.at(5) ).at(0)  >> ( rij_transportMoments_.at(5) ).at(1) >> ( rij_transportMoments_.at(5) ).at(2) >> ( rij_transportMoments_.at(5) ).at(3) >> ( rij_transportMoments_.at(5) ).at(4);

}

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

void particleBeam::donneesDessinEllipseXxp(vector<double>& xcor, vector<double>& ycor) {
  int k;
  double x,y;

  if ( !momentRepresentationOk_ ) return;

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

  double xm = ( rij_.getMatrix().at(0) ).at(0);
  double ym = ( rij_.getMatrix().at(1) ).at(1);
  double r  = ( rij_.getMatrix().at(1) ).at(0);

  // double xm = ( rij_transportMoments_.at(0) ).at(0);
  // double ym = ( rij_transportMoments_.at(1) ).at(1);
  // double r  = ( rij_transportMoments_.at(1) ).at(0);

  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 ) 
    {
      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
    {
      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);
	}
	
    }
}