// -------------------------------------------------------------------------
// -----                    FOPIField source file                  -----
// -----                Created 20.12.2011 by P. Buehler           -----
// -------------------------------------------------------------------------

#include <cmath>

#include "FairRun.h"
#include "FairRuntimeDb.h"
#include "PndConstPar.h"
#include "FOPIField.h"

#include <iomanip>
#include <iostream>

using namespace std;

// -----   Default constructor   -------------------------------------------
FOPIField::FOPIField() 
: fXmin(-120.),   
  fXmax( 120.),
  fYmin(-120.),
  fYmax( 120.),
  fZmin(-500.),
  fZmax( 500.),
  falign(kFALSE)
{
	fType = 0;
	taroff = 0;
	fldval = 0.6;
}
// -------------------------------------------------------------------------
FOPIField::FOPIField(Double_t mfvalnorm)
: fXmin(-120.),   
  fXmax( 120.),
  fYmin(-120.),
  fYmax( 120.),
  fZmin(-500.),
  fZmax( 500.),
  falign(kFALSE)
{
	fType = 0;
	taroff = 0;
	fldval = mfvalnorm;
}
// -------------------------------------------------------------------------
FOPIField::FOPIField(const char* name, Double_t xMin, 
		Double_t xMax, Double_t yMin,
		Double_t yMax, Double_t zMin,
		Double_t zMax)
: FairField(name),
  fXmin(xMin),
  fXmax(xMax),
  fYmin(yMin),
  fYmax(yMax),
  fZmin(zMin),
  fZmax(zMax),
  falign(kFALSE)

{
	fType = 0;
	taroff = 0;
}
// -------------------------------------------------------------------------



// --------   Constructor from PndFieldPar   -------------------------------
FOPIField::FOPIField(PndConstPar* fieldPar) 
: FairField(),
  fXmin(0),
  fXmax(0),
  fYmin(0),
  fYmax(0),
  fZmin(0),
  fZmax(0),
  falign(kFALSE)
{
	if ( ! fieldPar ) {
		cerr << "-W- FOPIField::FOPIField: empty parameter container!"<< endl;
		fType= -1;
	}
	else {
		fXmin = fieldPar->GetXmin();
		fXmax = fieldPar->GetXmax();
		fYmin = fieldPar->GetYmin();
		fYmax = fieldPar->GetYmax();
		fZmin = fieldPar->GetZmin();
		fZmax = fieldPar->GetZmax();
		fType = fieldPar->GetType();
	}
	taroff = 0;
}
// -------------------------------------------------------------------------



// -----   Destructor   ----------------------------------------------------
FOPIField::~FOPIField() { }
// -------------------------------------------------------------------------



// -----   Set alignment   -------------------------------------------------
void FOPIField::SetAlignment(Double_t x, Double_t y,  
		Double_t z, Double_t phi,
		Double_t theta, Double_t psi){

	trans = new TGeoTranslation(x,y,z);
	rot   = new TGeoRotation("rot",phi, theta, psi);
	fTransformation = TGeoCombiTrans(*trans,*rot);
	falign=kTRUE;

}

// -----   Set field region   ----------------------------------------------
void FOPIField::SetFieldRegion(Double_t xMin, Double_t xMax, 
		Double_t yMin, Double_t yMax,
		Double_t zMin, Double_t zMax) {
	fXmin = xMin;
	fXmax = xMax;
	fYmin = yMin;
	fYmax = yMax;
	fZmin = zMin;
	fZmax = zMax;

}

// -----   Alignment -------------------------------------------------------

void FOPIField::Alignment(Double_t x, Double_t y, Double_t z) {
	Double_t xyz[3];
	xyz[0] = x;
	xyz[1] = y;
	xyz[2] = z;
	Double_t newpoint[3];
	fTransformation.MasterToLocal(xyz,newpoint);
	fnpx = newpoint[0];
	fnpy = newpoint[1];
	fnpz = newpoint[2];
}    



// -----   Get x component of field   --------------------------------------
void FOPIField::CompField(Double_t x,Double_t y,Double_t z) {

	// according to FOPI flib/fhmagfld.f
	// define some constants  
	Double_t rzp[21] = {0,507,-0.1175,-2.265e-4,905,-0.529,6.678e-5,3.85e-2,
			4.14e-5,-1.46e-7,1.3e-10,1.2e-2,2.44e-6,-5.3e-8,-0.312,1.63e-3,
			-1.03e-6,3.93e-2,2.86e-5,-1.21e-7,1.27e-10};
	Double_t zzp[21] = {0,1645,1754,1062,4.0e6,4.0e-4};
	Double_t zrf[4]  = {0,2.0,-7.18e-4,-8.2e-8};
	Double_t rp[21]  = {0,1.76e-4, 0.75,1280,174,0.008,700,150,942,-0.363,
			-2.13e-4,0.5,6000,0.7,0.75,1000};
	Double_t zp[3]   = {0,1645,0};

	// compute magnetic field
	Double_t rho = sqrt(pow(x,2)+pow(y,2));
	Double_t b0 = fldval;
	Double_t zn = 10.*(z + taroff);        // z=0  <->  center magnet
	Double_t rn = 10.*rho;
	Double_t zw = 0.;

	if ((rn > 1200) && (zn > 2000)) rn=1200.;

	// axial field component
	Double_t b1 = rzp[1] + rzp[2]*rn + rzp[3]*pow(rn,2);
	Double_t b2 = rzp[4] + rzp[5]*rn + rzp[6]*pow(rn,2);
	Double_t b3 = ((rzp[7] + rzp[8]*rn + rzp[9]*pow(rn,2) + rzp[10]*pow(rn,3)))/0.6;
	Double_t ze1 = zzp[2] +rzp[11]*rn+ rzp[12]*pow(rn,2) +rzp[13]*pow(rn,3);
	Double_t ze2 = zzp[3] +rzp[14]*rn+ rzp[15]*pow(rn,2) +rzp[16]*pow(rn,3);
	if (zn < 0.) {
		ze1=-ze1;
		ze2=-ze2;
		b3 = (rzp[17] + rzp[18]*rn + rzp[19]*pow(rn,2) + rzp[20]*pow(rn,3))/0.6;
		if (zn > -1600) {
			b2 = b2*(1-zn*rn/zzp[4]);
		} else {
			b2 = b2*(1+rn*zzp[5]);
		}
	}
	Double_t zr1 = (zn-ze1)/b1;
	Double_t zr2 = (zn - ze2)/b2;
	if (zn < 0) {
		zr1=-zr1;
		zr2=-zr2;
	}
	if (abs(zr1) > 50) zr1=50;
	if (abs(zr2) > 50) zr2=50;

	// Tails im Randfeld
	Double_t ec= zrf[1] + zrf[2]*rn + zrf[3]*pow(rn,2);
	Double_t fz = 0.;
	if (zr1 > ec) {
		Double_t ex = exp(ec);
		Double_t b4 = (1+ex)/ex -ec;
		Double_t b5 = (b4+ec)/(1+ex);
		fz = (b4+zr1)/b5;
	} else {
		fz = (1+exp(zr1));
	}
	fz  = b0/fz;
	Double_t fbz = fz + b0*b3*exp(-pow(zr2,2));

	// radial field component
	Double_t brf = b0/0.6*(rp[1]*rn +rp[2]*exp((rn-rp[3])/rp[4]));
	brf = brf+rp[5]*exp(-pow((rn-rp[6])/rp[7],2));
	Double_t a1 = rp[8]+rp[9]*rn+rp[10]*pow(rn,2);
	if (a1 < 50.) a1=50.;

	Double_t zer1=zp[1]+a1*rp[11]*(1-rn/rp[12]);
	Double_t zer2=zp[1]-a1*rp[13];
	Double_t zer3=zer1;
	Double_t zer4=zp[1]-a1*rp[14];

	Double_t a2=((zer2-zp[1])*zer2 + pow(a1,2));
	Double_t a3=((zer4-zp[1])*zer4 + pow(a1,2));
	if(a2 == 0. || a3 == 0.) {
		fBx = 0.;
		fBy = 0.;
		fBz = 10.*fldval;
		return;
	} else {
		a2=((zer2-zp[1])*(pow(a1,2)))/a2;
		a3=(zer4-zp[1])*(pow(a1,2))/a3;
	}

	zw = zn;
	if (zn < 0) zw=-zn;

	Double_t fbr = 0.;
	if (abs((zw-zp[1])/a1) < 20) {
		fbr = brf*exp(-pow((zw-zp[1])/a1,2));
	} else {
		fbr = brf;
	}

	if (zn > 0 && zw > zer1) {
		fbr=brf*exp(pow((zer1-zp[1])/a1,2)-2*(zer1-zp[1])/pow(a1,2)*(zw-zp[1]));
	} else if (zn > 0 && zw < zer2) {
		fbr=brf*exp(pow((zer2-zp[1])/a1,2)-2*(zer2-zp[1])*
				(zw-zp[1])/pow(a1,2)-pow((zw-zer2)/rp[15],2));
	} else if (zn < 0 && zw > zer3) {
		fbr=brf*exp(pow((zer3-zp[1])/a1,2)-2*(zer3-zp[1])/pow(a1,2)*(zw-zp[1]));
	} else if  (zn < 0 && zw < zer4) {
		fbr=brf*exp(pow((zer4-zp[1])/a1,2)-2*(zer4-zp[1])/
				pow(a1,2)*(zw-zp[1])-pow((zw-zer4)/rp[15],2));
	} else {
		fbr = brf*exp(-pow((zw-zp[1])/a1,2));
	}

	if (zw == -zn) fbr=-fbr;

	// set field values
	if (rho != 0.) {
		fBx = fbr*x/rho;
		fBy = fbr*y/rho;
	} else {
		fBx = 0.;
		fBy = 0.;
	}
	fBz = fbz;

	// convert from Tesla to kG
	fBx = 10.*fBx;
	fBy = 10.*fBy;
	fBz = 10.*fBz;

	// Alignment
	if (falign){
		Double_t rotate[3];
		Double_t tmp[3];
		tmp[0]=fBx;
		tmp[1]=fBy;
		tmp[2]=fBz;
		rot->MasterToLocal(tmp,rotate);
		fBx=rotate[0];
		fBy=rotate[1];
		fBz=rotate[2];
	}
	// fprintf(stderr,"< I - FOPIField> H(%f/%f/%f): %f %f %f\n",
	//   x,y,z,fBx,fBy,fBz);

}

// -----   Get Newpoint    -------------------------------------------------

Double_t FOPIField::GetNewPointX(Double_t x, Double_t y, Double_t z){
	this->Alignment(x,y,z);
	return fnpx;
}
Double_t FOPIField::GetNewPointY(Double_t x, Double_t y, Double_t z){
	this->Alignment(x,y,z);
	return fnpx;
}
Double_t FOPIField::GetNewPointZ(Double_t x, Double_t y, Double_t z){
	this->Alignment(x,y,z);
	return fnpz;
} 

// -----   Get x component of field   --------------------------------------
Double_t FOPIField::GetBx(Double_t x, Double_t y, Double_t z) {
	if ( x < fXmin  ||  x > fXmax  ||
			y < fYmin  ||  y > fYmax  ||
			z < fZmin  ||  z > fZmax ) return 0.;

	this->CompField(x,y,z);
	return fBx;

}
// -------------------------------------------------------------------------



// -----   Get y component of field   --------------------------------------
Double_t FOPIField::GetBy(Double_t x, Double_t y, Double_t z) {
	if ( x < fXmin  ||  x > fXmax  ||
			y < fYmin  ||  y > fYmax  ||
			z < fZmin  ||  z > fZmax ) return 0.;

	this->CompField(x,y,z);
	return fBy;

}
// -------------------------------------------------------------------------



// -----   Get z component of field   --------------------------------------
Double_t FOPIField::GetBz(Double_t x, Double_t y, Double_t z) {
	if ( x < fXmin  ||  x > fXmax  ||
			y < fYmin  ||  y > fYmax  ||
			z < fZmin  ||  z > fZmax ) return 0.;

	this->CompField(x,y,z);
	return fBz;

}
// -------------------------------------------------------------------------



// -----   Screen output   -------------------------------------------------
void FOPIField::Print() {
	cout << "======================================================" << endl;
	cout << "----  " << fTitle << " : " << fName << endl;
	cout << "----" << endl;
	cout << "----  Field type    : FOPI" << endl;
	cout << "----" << endl;
	cout << "----  Field regions : " << endl;
	cout << "----        x = " << setw(4) << fXmin << " to " << setw(4)
    		   << fXmax << " cm" << endl;
	cout << "----        y = " << setw(4) << fYmin << " to " << setw(4)
    		   << fYmax << " cm" << endl;
	cout << "----        z = " << setw(4) << fZmin << " to " << setw(4)
    		   << fZmax << " cm" << endl;
	cout << " " << endl;
	cout << "======================================================" << endl;
}
// -------------------------------------------------------------------------
// ---------   Fill the parameters   --------------------------------------------
void FOPIField::FillParContainer()
{
	TString MapName=GetName();
	cout << "FOPIField::FillParContainer() " << endl;
	FairRun *fRun=FairRun::Instance();	
	FairRuntimeDb *rtdb=fRun->GetRuntimeDb();
	Bool_t kParameterMerged=kTRUE;
	PndConstPar* Par = (PndConstPar*) rtdb->getContainer("PndConstPar");
	Par->SetParameters(this);
	Par->setInputVersion(fRun->GetRunId(),1);
	Par->setChanged();

}

void FOPIField::GetFieldValue(const Double_t point[3], Double_t* bField)
{
	if ( point[0] < fXmin  ||  point[0] > fXmax  ||
			point[1] < fYmin  ||  point[1] > fYmax  ||
			point[2] < fZmin  ||  point[2] > fZmax )
	{
		bField[0]=0.;
		bField[1]=0.;
		bField[2]=0.;
	}
	else
	{

		if (falign)
		{
			Double_t npoint[3];
			for(int i = 1; i < 3; ++i)
			{
				npoint[i]=point[i];
			}
			this->Alignment(npoint[0], npoint[1], npoint[2]);
			this->CompField(fnpx,fnpy,fnpz);
		}
		else
		{
			this->CompField(point[0],point[1],point[2]);
		}
		bField[0]=fBx;
		bField[1]=fBy;
		bField[2]=fBz;
	}
}



ClassImp(FOPIField)