///
/// KRATTA get module location in spherical coordinate system 
///
/// Compile:
/// 	g++ getAnglesForModule.C -o getAnglesForModule
/// Run:
///    ./getAnglesForModule
///	Run:
///     root main2.C
///

#include <iostream>
#include <string>
#include <cmath>
#include <math.h>

double  MY_M_PI = 3.14159265358979323846; ///Because one can't use M_PI const in root macro.

using namespace std;

class KrattaModulePosCalc
{
	public:
		
		static const int    fModulesInRow       =   7;
		static const double fGeoModuleAngle     =   5.257;              // [deg] Szerokosc katowa modulu
		static const double fGeoColumnAngle     =   6.00;               // [deg] Odstep miedzy modulami w pierwszej warstwie (rozstaw modolow na stole).
		
		static const double x0 =  0.0; 		// [cm] ///Zmienna inicjalizujaca
		static const double y0 =  0.0; 		// [cm] ///Zmienna inicjalizujaca
		static const double z0 = 40.2;		// [cm] ///Odleglosc KRATTY od poczatku ukladu wsp.
		static const double Kratta_Theta_Y      =   42.0;				// [deg] Kat pod ktorym ustawiony zostal detektor KRATTA liczac od osi OZ
		static const int    fVerboseLevel       =   0;
		
		double moduleLocationCartesian[35][3]; //[modulNo][x,y,z];
		double moduleLocationSpherRad[35][3]; //[modulNo][r,thetaRad,phiRad];
		double moduleLocationSpherDeg[35][3]; //[modulNo][r,thetaDeg,phiDeg];
		
		KrattaModulePosCalc()
		{

			temp_x = 0.0;
			temp_y = 0.0;
			temp_z = 0.0;
			
			temp_theta_rad = 0.0;
			temp_phi_rad = 0.0;
			temp_r = 0.0;
			
			temp_theta_deg = 0.0;
			temp_phi_deg = 0.0;

			calculateLocationForEachModule();
			
		}

		double getX( int moduleNo)       { return moduleLocationCartesian[moduleNo][0]; }	
		double getY( int moduleNo)       { return moduleLocationCartesian[moduleNo][1]; }	
		double getZ( int moduleNo)       { return moduleLocationCartesian[moduleNo][2]; }	
		
		double getR( int moduleNo)       { return moduleLocationSpherRad [moduleNo][0]; }	
		double getThetaRad( int moduleNo){ return moduleLocationSpherRad [moduleNo][1]; }
		double getPhiRad( int moduleNo)  { return moduleLocationSpherRad [moduleNo][2]; }	
		
		double getThetaDeg( int moduleNo){ return moduleLocationSpherDeg [moduleNo][1]; }
		double getPhiDeg( int moduleNo)  { return moduleLocationSpherDeg [moduleNo][2]; }
		
	private: 
	
		void calculateLocationForEachModule( void )
		{
			for(int i = 0; i< 35; i++)
			{
				calculateLocationForModule(i);
				moduleLocationCartesian[i][0] = temp_x;
				moduleLocationCartesian[i][1] = temp_y;
				moduleLocationCartesian[i][2] = temp_z;
				
				moduleLocationSpherRad[i][0] = temp_r;
				moduleLocationSpherRad[i][1] = temp_theta_rad;
				moduleLocationSpherRad[i][2] = temp_phi_rad;
				
				moduleLocationSpherDeg[i][0] = temp_r;
				moduleLocationSpherDeg[i][1] = temp_theta_deg;
				moduleLocationSpherDeg[i][2] = temp_phi_deg;
			}
		}
	
		void calculateLocationForModule( int moduleNo)
		{
			///Okreslenie pozycji modulu w KRATTA
			int mRow = (int) moduleNo / fModulesInRow;
			int mCol =  moduleNo % fModulesInRow;
			
			///Wyliczenie katow rotacji:
			double thetaDeg = Kratta_Theta_Y - ( (mCol - 3) * fGeoColumnAngle );	
			double theta    = thetaDeg*MY_M_PI/180.;
			if( fVerboseLevel > 0) cout << "theta = " << theta << endl;
			
			double phiDeg =  ( (mRow + 0.5 ) * fGeoModuleAngle );	
			double phi    = phiDeg*MY_M_PI/180.;
			if( fVerboseLevel > 0) cout << "phi = " << phi << endl;
			
			///Wyliczenie pozycji modulu po rotacjach X'= [Rox][Roy]X
			double xx =  cos(theta)*x0            - sin(theta)*sin(phi)*y0   + sin(theta)*cos(phi)*z0;
			double yy =         0.0*x0            +          cos(theta)*y0   +            sin(phi)*z0;
			double zz = -sin(theta)*x0            - cos(theta)*sin(phi)*y0   + cos(theta)*cos(phi)*z0;	
			
			temp_x = xx;
			temp_y = yy;
			temp_z = zz;			
			if( fVerboseLevel > 0) cout << "Wartosc po obrocie R1 w ukladzie kartezjanskim: \t[" << temp_x << "," << temp_y << "," << temp_z <<  "]" << endl;
			
			///Wyliczenie reprezentacji katowej (w radianach i stopniach)
			temp_r     = sqrt ( xx*xx + yy*yy + zz*zz );
			temp_theta_rad = acos ( yy/temp_r );
			temp_phi_rad   = atan ( xx/zz );
			if( fVerboseLevel > 0) cout << "Wartosc koncowa w ukladzie sferycznym (rad):\t[" << temp_r << ", " << temp_theta_rad << ", " << temp_phi_rad <<  "]" << endl;
			
			temp_theta_deg = temp_theta_rad*180./MY_M_PI;
			temp_phi_deg   = temp_phi_rad*180./MY_M_PI;
			if( fVerboseLevel > 0) cout << "Wartosc koncowa w ukladzie sferycznym (st.):\t[" << temp_r << ", " << temp_theta_deg << ", " << temp_phi_deg <<  "]" << endl;
			
			return;
		}
		
		double temp_theta_deg;
		double temp_phi_deg;
		
		double temp_theta_rad;
		double temp_phi_rad;
		double temp_r;
		
		double temp_x;
		double temp_y;
		double temp_z;
 
 
};

void printModuleId( int moduleNo)
{
	static const int fModulesInRow = 7;
	int mRow = (int) moduleNo / fModulesInRow;
	int mCol =  moduleNo % fModulesInRow;
		
	cout << "Module_"<< moduleNo <<"["<<  (char)(mCol + 65) <<"-"<< mRow + 1 <<"]" << endl;
	
	return;
}

/*
 * Jak uzywac klasy:
 * 
 * 1. Trzeba zadeklarowac jej jedna instancje. 
 *    Konstruktor wyliczy wszystkie katy, i zapisze je w tablicy.
 * 2. Potem mozna je odczytywac uzywajac przeznaczonych do tego celu funkcji.
 *    Dzieki temu nie wykonywane sa juz obliczenia i czas dzialania jest szybki
 * 
 */
int main2 ( )
{

	double xp=0;
	double yp=0;
	double zp=0;
	
	double xt=0;
	double yt=0;
	double zt=0;
	
	double odstepMiedzyModulamiWRzedzie = 0.0;

	
	TH3D *h3 = new TH3D("h3", "Kratta", 100, 0., 50., 100,-50., 50.,100, -30., 30.);
	h3->SetXTitle("X"); 
	h3->SetYTitle("Y"); 
	h3->SetZTitle("Z"); 
	
	KrattaModulePosCalc kc;
	
	for (int i = 0; i < 35; i++)
	{
		
		odstepMiedzyModulamiWRzedzie = 0.0;
		printModuleId(i);
		
		xt = kc.getX(i);
		yt = kc.getY(i);
		zt = kc.getZ(i);
		
		cout <<"Polozenie katowe modulu "<< i <<  " [rad]:\t[" << kc.getR(i) << "," << kc.getThetaRad(i) << "," << kc.getPhiRad(i) <<  "]" << endl;
		cout <<"Polozenie katowe modulu "<< i <<  " [st.]:\t[" << kc.getR(i) << "," << kc.getThetaDeg(i) << "," << kc.getPhiDeg(i) <<  "]" << endl;
		
		odstepMiedzyModulamiWRzedzie = sqrt((xt-xp)*(xt-xp)+(yt-yp)*(yt-yp)+(zt-zp)*(zt-zp) );
		cout << "Odstep od poprzedniego modulu " << odstepMiedzyModulamiWRzedzie << "." << endl;
		xp=xt;
		yp=yt;
		zp=zt;
		h3->Fill(zt, xt, yt); ///By latwiej moc obracach histogramem przestawiono osie: z->x, x->y, y->z
	}
	h3->Draw();
	return 0;
}