//This class determines the overall geometry of the tracker taking
//into account all rotations and alignment of sensors wrt the centre
//of the phantom. it can be used to obtain the position of a detector
//centre, corner or strip in either world or the detector co-ordinates

//The constructor for this class is overloaded such that the class
//can be constructed from the default detector positions and rotations
//that are hard coded here, or taken from the .mac file or from an root file.


//necessary to include full path for when this class in included outside geant4 i.e.
//in the tracking routine
#include "TrackerGeometry.hh"
#include <fstream>
#include <iostream>
#include <cstring>
#include <sstream>
#include <TGraph.h>
#include <TBranch.h>


//Default constructor
TrackerGeometry::TrackerGeometry()
{
    //Take existing setup/detector parameters from input file or class
	InitParameters();
    //Calculate new parameters relevant to tracker
	ComputeParameters();
}

//Constructor using input .mac file
TrackerGeometry::TrackerGeometry(std::string input_macro_file)
{
    //Take existing setup/detector parameters from input file or class
	InitParameters(input_macro_file);
    //Calculate new parameters relevant to tracker
	ComputeParameters();
}

//Constructor using input .root file (not used by geant4)
TrackerGeometry::TrackerGeometry(TTree * geom_tree)
{
    //Take existing setup/detector parameters from input file or class
	InitParameters(geom_tree);
    //Calculate new parameters relevant to tracker
	ComputeParameters();
}

TrackerGeometry::~TrackerGeometry()
{
    
}

//Initialise parameters to default values
void TrackerGeometry::InitParameters()
{
    //This function obtains the detector and tracker geometry
    
    //Det. geometry parameters
    s_pitch = 0.0908;           //Strip pitch in mm (PRaVDA det.)
    //s_pitch = 0.08;             //Strip pitch in mm (ALiBaVa det.)
    s_length = 96.0;            //Length of strips in mm, slightly longer than width (pitch*strips)
    strips = 1024;              //No. of strips on each detector
    
    //tracker geometry parameters
    halfPhantomSizeZ = 37.5;          //default
    halfCalorimeterSizeZ = 125.0;     //default
    
    inter_plane_dist = 10.0;          //default
    inter_module_dist = 100.0;        //default
    phantom_gap = 80.0;               //default
    phantom_zpos = 0.0;               //default
    
    //Use fixed rotational misalignment set in geant4 .mac files
    stereo_x1 = -0.1194 * TMath::DegToRad();
    stereo_u1 = 60.1194 * TMath::DegToRad();
    stereo_v1 = -60.1194 * TMath::DegToRad();
    
    stereo_x2 = -0.1194 * TMath::DegToRad();
    stereo_u2 = 60.1194 * TMath::DegToRad();
    stereo_v2 = -59.8806 * TMath::DegToRad();
    
    stereo_x3 = 0.1194 * TMath::DegToRad();
    stereo_u3 = 59.8806 * TMath::DegToRad();
    stereo_v3 = -60.1194 * TMath::DegToRad();
    
    stereo_x4 = 0.1194 * TMath::DegToRad();
    stereo_u4 = 59.8806 * TMath::DegToRad();
    stereo_v4 = -59.8806 * TMath::DegToRad();
    
    //Use default stereo angles
    /*stereo_x1 = 0.0 * TMath::DegToRad();
    stereo_u1 = 60.0 * TMath::DegToRad();
    stereo_v1 = -60. * TMath::DegToRad();
    
    stereo_x2 = 0.0 * TMath::DegToRad();
    stereo_u2 = 60.0 * TMath::DegToRad();
    stereo_v2 = -60.0 * TMath::DegToRad();
    
    stereo_x3 = 0.0 * TMath::DegToRad();
    stereo_u3 = 60.0 * TMath::DegToRad();
    stereo_v3 = -60.0 * TMath::DegToRad();
    
    stereo_x4 = 0.0 * TMath::DegToRad();
    stereo_u4 = 60.0 * TMath::DegToRad();
    stereo_v4 = -60.0 * TMath::DegToRad();*/
    
    //Printout for debug
    /*std::cout << "\nReading default geom params.." << std::endl;
    std::cout << "s_pitch = " << s_pitch << std::endl;
    std::cout << "s_length = " << s_length << std::endl;
    std::cout << "strips = " << strips << std::endl;
    
    std::cout << "halfPhantomSizeZ = " << halfPhantomSizeZ << std::endl;
    std::cout << "halfCalorimeterSizeZ = " << halfCalorimeterSizeZ << std::endl;
    
    std::cout << "plane_dist = " << inter_plane_dist << std::endl;
    std::cout << "module_dist = " << inter_module_dist << std::endl;
    std::cout << "phantom_gap = " << phantom_gap << std::endl;
    
    std::cout << "x1_stereo angle = " << stereo_x1 << std::endl;
    std::cout << "u1_stereo angle = " << stereo_u1 << std::endl;
    std::cout << "v1_stereo angle = " << stereo_v1 << std::endl;
    
    std::cout << "x2_stereo angle = " << stereo_x2 << std::endl;
    std::cout << "u2_stereo angle = " << stereo_u2 << std::endl;
    std::cout << "v2_stereo angle = " << stereo_v2 << std::endl;
    
    std::cout << "x3_stereo angle = " << stereo_x3 << std::endl;
    std::cout << "u3_stereo angle = " << stereo_u3 << std::endl;
    std::cout << "v3_stereo angle = " << stereo_v3 << std::endl;
    
    std::cout << "x4_stereo angle = " << stereo_x4 << std::endl;
    std::cout << "u4_stereo angle = " << stereo_u4 << std::endl;
    std::cout << "v4_stereo angle = " << stereo_v4 << std::endl;*/
}

//this function overloads the previous one to allow it to obtain
//the parameter values from a .mac file input for geant4.
void TrackerGeometry::InitParameters(std::string input_macro_file)
{
    //This function obtains the detector and tracker geometry
    
    //*** Read in the value of the tracking/detector geometry variables from input .mac file ***//
    
    const int MAX_CHARS_PER_LINE = 1024;
    const int MAX_WORDS_PER_LINE = 50;
    const char* const DELIMITER = " ";
    
    // create a file-reading object
    ifstream fin;
    fin.open(input_macro_file.c_str()); // open proton_no_vis.mac as default
    if (!fin.good()){exit(-1);}
    
    std::cout << "Reading TrackerGeometry Parameters from " << input_macro_file.c_str() << std::endl;
    
    std::string n_strips = "/det/n_strips";
    std::string strip_pitch = "/det/strip_pitch";
    std::string strip_length = "/det/strip_length";
    
    std::string hPhantomSizeZ = "/geom/halfPhantomSizeZ";
    std::string hCalorimeterSizeZ = "/geom/halfCalorimeterSizeZ";
    
    std::string plane_d = "/geom/plane_dist";
    std::string module_d = "/geom/module_dist";
    std::string phantom_g = "/geom/phantom_gap";
		std::string phantom_z = "/geom/phantom_z";
    
    std::string x1_stereo = "/det/x1_Sensor/theta";
    std::string u1_stereo = "/det/u1_Sensor/theta";
    std::string v1_stereo = "/det/v1_Sensor/theta";
    
    std::string x2_stereo = "/det/x2_Sensor/theta";
    std::string u2_stereo = "/det/u2_Sensor/theta";
    std::string v2_stereo = "/det/v2_Sensor/theta";
    
    std::string x3_stereo = "/det/x3_Sensor/theta";
    std::string u3_stereo = "/det/u3_Sensor/theta";
    std::string v3_stereo = "/det/v3_Sensor/theta";
    
    std::string x4_stereo = "/det/x4_Sensor/theta";
    std::string u4_stereo = "/det/u4_Sensor/theta";
    std::string v4_stereo = "/det/v4_Sensor/theta";
    
    // read each line of the file
    while (!fin.eof())
    {
        // read an entire line into memory
        char buf[MAX_CHARS_PER_LINE];
        fin.getline(buf, MAX_CHARS_PER_LINE);
        
        // parse the line into blank-delimited words
        int n = 0; // a for-loop index
        
        // array to store memory addresses of the words in buf
        const char* token[MAX_WORDS_PER_LINE] = {}; // initialize to 0
        
        // parse the line
        token[0] = strtok(buf, DELIMITER); // first token
        if (token[0]) // zero if line is blank
        {
            for (n = 1; n < MAX_WORDS_PER_LINE; n++)
            {
                token[n] = strtok(0, DELIMITER); // subsequent words
                if (!token[n]) break; // no more words
            }
        }
        
        
        // process the words on a line and extract the parameters
        for (int i = 0; i < n; i++) // n = #of words
        {
            std::string s1 = token[0]; //convert to string for find fn
            
            //Do not read lines that are commented out, also prevents variables being overwritten
            //if e.g. the same variable is on two consecutive lines with the commented out one second
            if(s1.find("#") != std::string::npos) {break;}
            
            //If string/token at the start of a line matches variable name then variable is equal to the next token
            
            if(s1.find(n_strips) != std::string::npos)  { strips = atof(token[1]); }
            else if(s1.find(strip_pitch) != std::string::npos)  { s_pitch = atof(token[1]); }
            else if(s1.find(strip_length) != std::string::npos)  { s_length = atof(token[1]); }
            
            else if(s1.find(hPhantomSizeZ) != std::string::npos)  { halfPhantomSizeZ = atof(token[1]); }
            else if(s1.find(hCalorimeterSizeZ) != std::string::npos) { halfCalorimeterSizeZ = atof(token[1]); }
            
            else if(s1.find(plane_d) != std::string::npos)   { inter_plane_dist = atof(token[1]); }
            else if(s1.find(module_d) != std::string::npos)  { inter_module_dist = atof(token[1]); }
            else if(s1.find(phantom_g) != std::string::npos) { phantom_gap = atof(token[1]); }
            else if(s1.find(phantom_z) != std::string::npos) { phantom_zpos = atof(token[1]); }
            
            else if(s1.find(x1_stereo) != std::string::npos) { stereo_x1 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(u1_stereo) != std::string::npos) { stereo_u1 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(v1_stereo) != std::string::npos) { stereo_v1 = atof(token[1]) * TMath::DegToRad(); }
            
            else if(s1.find(x2_stereo) != std::string::npos) { stereo_x2 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(u2_stereo) != std::string::npos) { stereo_u2 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(v2_stereo) != std::string::npos) { stereo_v2 = atof(token[1]) * TMath::DegToRad(); }
            
            else if(s1.find(x3_stereo) != std::string::npos) { stereo_x3 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(u3_stereo) != std::string::npos) { stereo_u3 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(v3_stereo) != std::string::npos) { stereo_v3 = atof(token[1]) * TMath::DegToRad(); }
            
            else if(s1.find(x4_stereo) != std::string::npos) { stereo_x4 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(u4_stereo) != std::string::npos) { stereo_u4 = atof(token[1]) * TMath::DegToRad(); }
            else if(s1.find(v4_stereo) != std::string::npos) { stereo_v4 = atof(token[1]) * TMath::DegToRad(); }
            
            break;  //prevents looping through all words on line after value is found
        }
    }
    
    //Printout for debug
    std::cout << "\nReading geom params from .mac file.." << std::endl;
    std::cout << "s_pitch = " << s_pitch << std::endl;
    std::cout << "s_length = " << s_length << std::endl;
    std::cout << "strips = " << strips << std::endl;
    
    std::cout << "halfPhantomSizeZ = " << halfPhantomSizeZ << std::endl;
    std::cout << "halfCalorimeterSizeZ = " << halfCalorimeterSizeZ << std::endl;
    
    std::cout << "plane_dist = " << inter_plane_dist << std::endl;
    std::cout << "module_dist = " << inter_module_dist << std::endl;
    std::cout << "phantom_gap = " << phantom_gap << std::endl;
		std::cout << "phantom_zpos = " << phantom_zpos << std::endl;
    
    std::cout << "x1_stereo angle = " << stereo_x1 << std::endl;
    std::cout << "u1_stereo angle = " << stereo_u1 << std::endl;
    std::cout << "v1_stereo angle = " << stereo_v1 << std::endl;
    
    std::cout << "x2_stereo angle = " << stereo_x2 << std::endl;
    std::cout << "u2_stereo angle = " << stereo_u2 << std::endl;
    std::cout << "v2_stereo angle = " << stereo_v2 << std::endl;
    
    std::cout << "x3_stereo angle = " << stereo_x3 << std::endl;
    std::cout << "u3_stereo angle = " << stereo_u3 << std::endl;
    std::cout << "v3_stereo angle = " << stereo_v3 << std::endl;
    
    std::cout << "x4_stereo angle = " << stereo_x4 << std::endl;
    std::cout << "u4_stereo angle = " << stereo_u4 << std::endl;
    std::cout << "v4_stereo angle = " << stereo_v4 << std::endl;
    std::cout << "\n" << std::endl;
    
}

//this function overloads the previous one to allow it to obtain
//the parameter values from the .root file output from geant4.
void TrackerGeometry::InitParameters(TTree * t_geom)
{
    //This function obtains the detector and tracker geometry
    
    // List of branches
    TBranch        *b_noOfSensorStrips;   //!
    TBranch        *b_teleStripPitch;   //!
    TBranch        *b_sensorStripLength;   //!
    TBranch        *b_inter_plane_dist;   //!
    TBranch        *b_inter_module_dist;   //!
    TBranch        *b_phantom_gap;   //!
    TBranch        *b_halfPhantomSizeZ;   //!
    TBranch        *b_halfCalorimeterSizeZ;   //!
    TBranch        *b_x1_SensorTheta;   //!
    TBranch        *b_u1_SensorTheta;   //!
    TBranch        *b_v1_SensorTheta;   //!
    TBranch        *b_x2_SensorTheta;   //!
    TBranch        *b_u2_SensorTheta;   //!
    TBranch        *b_v2_SensorTheta;   //!
    TBranch        *b_x3_SensorTheta;   //!
    TBranch        *b_u3_SensorTheta;   //!
    TBranch        *b_v3_SensorTheta;   //!
    TBranch        *b_x4_SensorTheta;   //!
    TBranch        *b_u4_SensorTheta;   //!
    TBranch        *b_v4_SensorTheta;   //!
    
    t_geom->SetBranchAddress("noOfSensorStrips", &strips, &b_noOfSensorStrips);
    t_geom->SetBranchAddress("teleStripPitch", &s_pitch, &b_teleStripPitch);
    t_geom->SetBranchAddress("sensorStripLength", &s_length, &b_sensorStripLength);
    t_geom->SetBranchAddress("inter_plane_dist", &inter_plane_dist, &b_inter_plane_dist);
    t_geom->SetBranchAddress("inter_module_dist", &inter_module_dist, &b_inter_module_dist);
    t_geom->SetBranchAddress("phantom_gap", &phantom_gap, &b_phantom_gap);
    t_geom->SetBranchAddress("halfPhantomSizeZ", &halfPhantomSizeZ, &b_halfPhantomSizeZ);
    t_geom->SetBranchAddress("halfCalorimeterSizeZ", &halfCalorimeterSizeZ, &b_halfCalorimeterSizeZ);
    t_geom->SetBranchAddress("x1_SensorTheta", &stereo_x1, &b_x1_SensorTheta);
    t_geom->SetBranchAddress("u1_SensorTheta", &stereo_u1, &b_u1_SensorTheta);
    t_geom->SetBranchAddress("v1_SensorTheta", &stereo_v1, &b_v1_SensorTheta);
    t_geom->SetBranchAddress("x2_SensorTheta", &stereo_x2, &b_x2_SensorTheta);
    t_geom->SetBranchAddress("u2_SensorTheta", &stereo_u2, &b_u2_SensorTheta);
    t_geom->SetBranchAddress("v2_SensorTheta", &stereo_v2, &b_v2_SensorTheta);
    t_geom->SetBranchAddress("x3_SensorTheta", &stereo_x3, &b_x3_SensorTheta);
    t_geom->SetBranchAddress("u3_SensorTheta", &stereo_u3, &b_u3_SensorTheta);
    t_geom->SetBranchAddress("v3_SensorTheta", &stereo_v3, &b_v3_SensorTheta);
    t_geom->SetBranchAddress("x4_SensorTheta", &stereo_x4, &b_x4_SensorTheta);
    t_geom->SetBranchAddress("u4_SensorTheta", &stereo_u4, &b_u4_SensorTheta);
    t_geom->SetBranchAddress("v4_SensorTheta", &stereo_v4, &b_v4_SensorTheta);
    t_geom->GetEntry(0);
    
    
    //Printout for debug
    /*std::cout << "\nReading geom params from .mac file.." << std::endl;
     std::cout << "s_pitch = " << s_pitch << std::endl;
     std::cout << "s_length = " << s_length << std::endl;
     std::cout << "strips = " << strips << std::endl;
     
     std::cout << "halfPhantomSizeZ = " << halfPhantomSizeZ << std::endl;
     std::cout << "halfCalorimeterSizeZ = " << halfCalorimeterSizeZ << std::endl;
     
     std::cout << "plane_dist = " << inter_plane_dist << std::endl;
     std::cout << "module_dist = " << inter_module_dist << std::endl;
     std::cout << "phantom_gap = " << phantom_gap << std::endl;
     
     std::cout << "x1_stereo angle = " << stereo_x1 << std::endl;
     std::cout << "u1_stereo angle = " << stereo_u1 << std::endl;
     std::cout << "v1_stereo angle = " << stereo_v1 << std::endl;
     
     std::cout << "x2_stereo angle = " << stereo_x2 << std::endl;
     std::cout << "u2_stereo angle = " << stereo_u2 << std::endl;
     std::cout << "v2_stereo angle = " << stereo_v2 << std::endl;
     
     std::cout << "x3_stereo angle = " << stereo_x3 << std::endl;
     std::cout << "u3_stereo angle = " << stereo_u3 << std::endl;
     std::cout << "v3_stereo angle = " << stereo_v3 << std::endl;
     
     std::cout << "x4_stereo angle = " << stereo_x4 << std::endl;
     std::cout << "u4_stereo angle = " << stereo_u4 << std::endl;
     std::cout << "v4_stereo angle = " << stereo_v4 << std::endl;
     std::cout << "\n" << std::endl;*/

}

//This function computes the tracker geometry
void TrackerGeometry::ComputeParameters()
{
    det_width = strips*s_pitch;
    //distance from origin to corner of det.
    diag = ( sqrt( det_width*det_width + s_length*s_length ) )*0.5;
    //angle between vector from origin (det. centre) to det. corner and x axis
    origin_angle = atan( (s_length*0.5) / (det_width*0.5) );
    
    //******************************************* Compute positions of geometry in z ***************************************************//
    
    //Phantom is placed at isocentre all other geometry is relative to this
    //phantom_zpos = 0.0;
    phantom_centre_d = phantom_zpos;
    phantom_average_d = phantom_zpos - 0.5*halfPhantomSizeZ;
    phantom_surface_d = phantom_zpos - halfPhantomSizeZ;
    
    //Compensator
    compensator_zpos = phantom_zpos -0.5*phantom_gap - inter_plane_dist*5 - inter_module_dist - halfPhantomSizeZ;
    
    //Incoming tracker
    x1_zpos = phantom_zpos - 0.5*phantom_gap - inter_plane_dist*4 - inter_module_dist;
    u1_zpos = phantom_zpos - 0.5*phantom_gap - inter_plane_dist*3 - inter_module_dist;
    v1_zpos = phantom_zpos - 0.5*phantom_gap - inter_plane_dist*2 - inter_module_dist;
    
    x2_zpos = phantom_zpos - 0.5*phantom_gap - inter_plane_dist*2;
    u2_zpos = phantom_zpos - 0.5*phantom_gap - inter_plane_dist;
    v2_zpos = phantom_zpos - 0.5*phantom_gap;
    
    //Incoming truth planes fixed at 1mm after last tracker plane from phantom and 1mm before phantom surface
    TP1_zpos = phantom_zpos - 0.5*phantom_gap + 1.0;    //1mm after v2 plane
    TP2_zpos = phantom_zpos - halfPhantomSizeZ - 1.0;   //1mm from phantom surface
    
    //Outgoing tracker
    x3_zpos = phantom_zpos + 0.5*phantom_gap;
    u3_zpos = phantom_zpos + 0.5*phantom_gap + inter_plane_dist;
    v3_zpos = phantom_zpos + 0.5*phantom_gap + inter_plane_dist*2;
    
    //Outgoing truth planes fixed at 1mm from phantom surface and 1mm before next tracker plane after phantom
    TP3_zpos = phantom_zpos + halfPhantomSizeZ + 1;     //1mm from phantom surface
    TP4_zpos = phantom_zpos + 0.5*phantom_gap - 1.0;    //1mm before x3 plane
    
    x4_zpos = phantom_zpos + 0.5*phantom_gap + inter_plane_dist*2 + inter_module_dist;
    u4_zpos = phantom_zpos + 0.5*phantom_gap + inter_plane_dist*3 + inter_module_dist;
    v4_zpos = phantom_zpos + 0.5*phantom_gap + inter_plane_dist*4 + inter_module_dist;
    
    //Range telescope
    calorimeter_zpos = phantom_zpos + 0.5*phantom_gap + inter_plane_dist*5 + inter_module_dist + halfCalorimeterSizeZ;
    
    //******************************************* x1 plane of the tracker ***************************************************//
    
    // (x,y,z) positions of bottom left corner wrt phantom centre (world co-ordinates)
    A_x1.SetX(-1*diag*cos(origin_angle - stereo_x1));
    A_x1.SetY(-1*diag*sin(origin_angle - stereo_x1));
    A_x1.SetZ(x1_zpos);

    // (x,y,z) positions of top left corner wrt phantom centre (world co-ordinates)
    B_x1.SetX(-1*diag*cos(origin_angle + stereo_x1));
    B_x1.SetY(diag*sin(origin_angle + stereo_x1));
    B_x1.SetZ(x1_zpos);

    // (x,y,z) positions of top right corner wrt phantom centre (world co-ordinates)
    C_x1.SetX(diag*cos(origin_angle - stereo_x1));
    C_x1.SetY(diag*sin(origin_angle - stereo_x1));
    C_x1.SetZ(x1_zpos);

    // (x,y,z) positions of bottom right corner wrt phantom centre (world co-ordinates)
    D_x1.SetX(diag*cos(origin_angle + stereo_x1));
    D_x1.SetY(-1*diag*sin(origin_angle + stereo_x1));
    D_x1.SetZ(x1_zpos);
    
    //******************************************* u1 plane of the tracker ***************************************************//
    
    A_u1.SetX(-1*diag*cos(origin_angle - stereo_u1));
    A_u1.SetY(-1*diag*sin(origin_angle - stereo_u1));
    A_u1.SetZ(u1_zpos);
    
    B_u1.SetX(-1*diag*cos(origin_angle + stereo_u1));
    B_u1.SetY(diag*sin(origin_angle + stereo_u1));
    B_u1.SetZ(u1_zpos);
    
    C_u1.SetX(diag*cos(origin_angle - stereo_u1));
    C_u1.SetY(diag*sin(origin_angle - stereo_u1));
    C_u1.SetZ(u1_zpos);
    
    D_u1.SetX(diag*cos(origin_angle + stereo_u1));
    D_u1.SetY(-1*diag*sin(origin_angle + stereo_u1));
    D_u1.SetZ(u1_zpos);
    
    //******************************************* v1 plane of the tracker ***************************************************//
    
    A_v1.SetX(-1*diag*cos(origin_angle - stereo_v1));
    A_v1.SetY(-1*diag*sin(origin_angle - stereo_v1));
    A_v1.SetZ(v1_zpos);
    
    B_v1.SetX(-1*diag*cos(origin_angle + stereo_v1));
    B_v1.SetY(diag*sin(origin_angle + stereo_v1));
    B_v1.SetZ(v1_zpos);
    
    C_v1.SetX(diag*cos(origin_angle - stereo_v1));
    C_v1.SetY(diag*sin(origin_angle - stereo_v1));
    C_v1.SetZ(v1_zpos);
    
    D_v1.SetX(diag*cos(origin_angle + stereo_v1));
    D_v1.SetY(-1*diag*sin(origin_angle + stereo_v1));
    D_v1.SetZ(v1_zpos);
    
    //******************************************* x2 plane of the tracker ***************************************************//
    
    A_x2.SetX(-1*diag*cos(origin_angle - stereo_x2));
    A_x2.SetY(-1*diag*sin(origin_angle - stereo_x2));
    A_x2.SetZ(x2_zpos);
    
    B_x2.SetX(-1*diag*cos(origin_angle + stereo_x2));
    B_x2.SetY(diag*sin(origin_angle + stereo_x2));
    B_x2.SetZ(x2_zpos);
    
    C_x2.SetX(diag*cos(origin_angle - stereo_x2));
    C_x2.SetY(diag*sin(origin_angle - stereo_x2));
    C_x2.SetZ(x2_zpos);
    
    D_x2.SetX(diag*cos(origin_angle + stereo_x2));
    D_x2.SetY(-1*diag*sin(origin_angle + stereo_x2));
    D_x2.SetZ(x2_zpos);
    
    //******************************************* u2 plane of the tracker ***************************************************//
    
    A_u2.SetX(-1*diag*cos(origin_angle - stereo_u2));
    A_u2.SetY(-1*diag*sin(origin_angle - stereo_u2));
    A_u2.SetZ(u2_zpos);

    B_u2.SetX(-1*diag*cos(origin_angle + stereo_u2));
    B_u2.SetY(diag*sin(origin_angle + stereo_u2));
    B_u2.SetZ(u2_zpos);
    
    C_u2.SetX(diag*cos(origin_angle - stereo_u2));
    C_u2.SetY(diag*sin(origin_angle - stereo_u2));
    C_u2.SetZ(u2_zpos);
    
    D_u2.SetX(diag*cos(origin_angle + stereo_u2));
    D_u2.SetY(-1*diag*sin(origin_angle + stereo_u2));
    D_u2.SetZ(u2_zpos);
    
    //******************************************* v2 plane of the tracker ***************************************************//
    
    A_v2.SetX(-1*diag*cos(origin_angle - stereo_v2));
    A_v2.SetY(-1*diag*sin(origin_angle - stereo_v2));
    A_v2.SetZ(v2_zpos);
    
    B_v2.SetX(-1*diag*cos(origin_angle + stereo_v2));
    B_v2.SetY(diag*sin(origin_angle + stereo_v2));
    B_v2.SetZ(v2_zpos);
    
    C_v2.SetX(diag*cos(origin_angle - stereo_v2));
    C_v2.SetY(diag*sin(origin_angle - stereo_v2));
    C_v2.SetZ(v2_zpos);
    
    D_v2.SetX(diag*cos(origin_angle + stereo_v2));
    D_v2.SetY(-1*diag*sin(origin_angle + stereo_v2));
    D_v2.SetZ(v2_zpos);
    
    //******************************************* x3 plane of the tracker ***************************************************//
    
    A_x3.SetX(-1*diag*cos(origin_angle - stereo_x3));
    A_x3.SetY(-1*diag*sin(origin_angle - stereo_x3));
    A_x3.SetZ(x3_zpos);
    
    B_x3.SetX(-1*diag*cos(origin_angle + stereo_x3));
    B_x3.SetY(diag*sin(origin_angle + stereo_x3));
    B_x3.SetZ(x3_zpos);
    
    C_x3.SetX(diag*cos(origin_angle - stereo_x3));
    C_x3.SetY(diag*sin(origin_angle - stereo_x3));
    C_x3.SetZ(x3_zpos);
    
    D_x3.SetX(diag*cos(origin_angle + stereo_x3));
    D_x3.SetY(-1*diag*sin(origin_angle + stereo_x3));
    D_x3.SetZ(x3_zpos);
    
    //******************************************* u3 plane of the tracker ***************************************************//
    
    A_u3.SetX(-1*diag*cos(origin_angle - stereo_u3));
    A_u3.SetY(-1*diag*sin(origin_angle - stereo_u3));
    A_u3.SetZ(u3_zpos);
    
    B_u3.SetX(-1*diag*cos(origin_angle + stereo_u3));
    B_u3.SetY(diag*sin(origin_angle + stereo_u3));
    B_u3.SetZ(u3_zpos);
    
    C_u3.SetX(diag*cos(origin_angle - stereo_u3));
    C_u3.SetY(diag*sin(origin_angle - stereo_u3));
    C_u3.SetZ(u3_zpos);
    
    D_u3.SetX(diag*cos(origin_angle + stereo_u3));
    D_u3.SetY(-1*diag*sin(origin_angle + stereo_u3));
    D_u3.SetZ(u3_zpos);
    
    //******************************************* v3 plane of the tracker ***************************************************//
    
    A_v3.SetX(-1*diag*cos(origin_angle - stereo_v3));
    A_v3.SetY(-1*diag*sin(origin_angle - stereo_v3));
    A_v3.SetZ(v3_zpos);
    
    B_v3.SetX(-1*diag*cos(origin_angle + stereo_v3));
    B_v3.SetY(diag*sin(origin_angle + stereo_v3));
    B_v3.SetZ(v3_zpos);
    
    C_v3.SetX(diag*cos(origin_angle - stereo_v3));
    C_v3.SetY(diag*sin(origin_angle - stereo_v3));
    C_v3.SetZ(v3_zpos);
    
    D_v3.SetX(diag*cos(origin_angle + stereo_v3));
    D_v3.SetY(-1*diag*sin(origin_angle + stereo_v3));
    D_v3.SetZ(v3_zpos);
    
    //******************************************* x4 plane of the tracker ***************************************************//
    
    A_x4.SetX(-1*diag*cos(origin_angle - stereo_x4));
    A_x4.SetY(-1*diag*sin(origin_angle - stereo_x4));
    A_x4.SetZ(x4_zpos);
    
    B_x4.SetX(-1*diag*cos(origin_angle + stereo_x4));
    B_x4.SetY(diag*sin(origin_angle + stereo_x4));
    B_x4.SetZ(x4_zpos);
    
    C_x4.SetX(diag*cos(origin_angle - stereo_x4));
    C_x4.SetY(diag*sin(origin_angle - stereo_x4));
    C_x4.SetZ(x4_zpos);
    
    D_x4.SetX(diag*cos(origin_angle + stereo_x4));
    D_x4.SetY(-1*diag*sin(origin_angle + stereo_x4));
    D_x4.SetZ(x4_zpos);
    
    //******************************************* u4 plane of the tracker ***************************************************//
    
    A_u4.SetX(-1*diag*cos(origin_angle - stereo_u4));
    A_u4.SetY(-1*diag*sin(origin_angle - stereo_u4));
    A_u4.SetZ(u4_zpos);
    
    B_u4.SetX(-1*diag*cos(origin_angle + stereo_u4));
    B_u4.SetY(diag*sin(origin_angle + stereo_u4));
    B_u4.SetZ(u4_zpos);
    
    C_u4.SetX(diag*cos(origin_angle - stereo_u4));
    C_u4.SetY(diag*sin(origin_angle - stereo_u4));
    C_u4.SetZ(u4_zpos);
    
    D_u4.SetX(diag*cos(origin_angle + stereo_u4));
    D_u4.SetY(-1*diag*sin(origin_angle + stereo_u4));
    D_u4.SetZ(u4_zpos);
    
    //******************************************* v4 plane of the tracker ***************************************************//
    
    A_v4.SetX(-1*diag*cos(origin_angle - stereo_v4));
    A_v4.SetY(-1*diag*sin(origin_angle - stereo_v4));
    A_v4.SetZ(v4_zpos);
    
    B_v4.SetX(-1*diag*cos(origin_angle + stereo_v4));
    B_v4.SetY(diag*sin(origin_angle + stereo_v4));
    B_v4.SetZ(v4_zpos);
    
    C_v4.SetX(diag*cos(origin_angle - stereo_v4));
    C_v4.SetY(diag*sin(origin_angle - stereo_v4));
    C_v4.SetZ(v4_zpos);
    
    D_v4.SetX(diag*cos(origin_angle + stereo_v4));
    D_v4.SetY(-1*diag*sin(origin_angle + stereo_v4));
    D_v4.SetZ(v4_zpos);
    
    //call PrintGeometry() for debug
    //PrintGeometry();
}


void TrackerGeometry::PrintGeometry()
{
    std::cout << "\nTrackerGeometry object created with the following parameters.." << std::endl;
    std::cout << "Detectors have: " << strips << " strips with a pitch of: " << s_pitch << " mm" << std::endl;
    std::cout << "The length of the strips (detector) is: " << s_length << " mm, and the width of the detector is: " << det_width << " mm" << std::endl;
    
    std::cout << "\nThe tracker and phantom are positioned along the z-axis as follows..\n" << std::endl;
    
    std::cout << "The x1 det plane is at: " << x1_zpos << " mm" << std::endl;
    std::cout << "The u1 det plane is at: " << u1_zpos << " mm" << std::endl;
    std::cout << "The v1 det plane is at: " << v1_zpos << " mm" << std::endl;
    
    std::cout << "The x2 det plane is at: " << x2_zpos << " mm" << std::endl;
    std::cout << "The u2 det plane is at: " << u2_zpos << " mm" << std::endl;
    std::cout << "The v2 det plane is at: " << v2_zpos << " mm" << std::endl;
    
    std::cout << "The x3 det plane is at: " << x3_zpos << " mm" << std::endl;
    std::cout << "The u3 det plane is at: " << u3_zpos << " mm" << std::endl;
    std::cout << "The v3 det plane is at: " << v3_zpos << " mm" << std::endl;
    
    std::cout << "The x4 det plane is at: " << x4_zpos << " mm" << std::endl;
    std::cout << "The u4 det plane is at: " << u4_zpos << " mm" << std::endl;
    std::cout << "The v4 det plane is at: " << v4_zpos << " mm\n" << std::endl;
    
    std::cout << "Truth plane 1 is at: " << TP1_zpos << " mm" << std::endl;
    std::cout << "Truth plane 2 is at: " << TP2_zpos << " mm" << std::endl;
    std::cout << "Truth plane 3 is at: " << TP3_zpos << " mm" << std::endl;
    std::cout << "Truth plane 4 is at: " << TP4_zpos << " mm\n" << std::endl;
    
    std::cout << "The phantom surface plane is at: " << phantom_surface_d << " mm" << std::endl;
    std::cout << "The phantom average plane is at: " << phantom_average_d << " mm" << std::endl;
    std::cout << "The phantom centre plane is at: " << phantom_centre_d << " mm\n" << std::endl;
    
    std::cout << "Det. centres have default positions at (0,0,z) mm" << std::endl;
    std::cout << "Rotation for u-planes in clockwise direction"<< std::endl;
    std::cout << "Rotation for v-planes in anti-clockwise direction"<< std::endl;
    std::cout << "Detector corners labelled as follows..\n"<< std::endl;
    
    std::cout << "B ******************* C" << std::endl;
    std::cout << "  *                 *  " << std::endl;
    std::cout << "  *                 *  " << std::endl;
    std::cout << "  *                 *  " << std::endl;
    std::cout << "  *        *        *  " << std::endl;
    std::cout << "  *                 *  " << std::endl;
    std::cout << "  *                 *  " << std::endl;
    std::cout << "  *                 *  " << std::endl;
    std::cout << "A ******************* D" << std::endl;
    
    //*************************************************** x1 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the x1 plane.." << std::endl;
    std::cout << "The position of A_x1 = (" << A_x1.X() << "," << A_x1.Y() << "," << A_x1.Z() << ") mm" << std::endl;
    std::cout << "The position of B_x1 = (" << B_x1.X() << "," << B_x1.Y() << "," << B_x1.Z() << ") mm" << std::endl;
    std::cout << "The position of C_x1 = (" << C_x1.X() << "," << C_x1.Y() << "," << C_x1.Z() << ") mm" << std::endl;
    std::cout << "The position of D_x1 = (" << D_x1.X() << "," << D_x1.Y() << "," << D_x1.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_x1 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** u1 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the u1 plane.." << std::endl;
    std::cout << "The position of A_u1 = (" << A_u1.X() << "," << A_u1.Y() << "," << A_u1.Z() << ") mm" << std::endl;
    std::cout << "The position of B_u1 = (" << B_u1.X() << "," << B_u1.Y() << "," << B_u1.Z() << ") mm" << std::endl;
    std::cout << "The position of C_u1 = (" << C_u1.X() << "," << C_u1.Y() << "," << C_u1.Z() << ") mm" << std::endl;
    std::cout << "The position of D_u1 = (" << D_u1.X() << "," << D_u1.Y() << "," << D_u1.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_u1 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** v1 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the v1 plane.." << std::endl;
    std::cout << "The position of A_v1 = (" << A_v1.X() << "," << A_v1.Y() << "," << A_v1.Z() << ") mm" << std::endl;
    std::cout << "The position of B_v1 = (" << B_v1.X() << "," << B_v1.Y() << "," << B_v1.Z() << ") mm" << std::endl;
    std::cout << "The position of C_v1 = (" << C_v1.X() << "," << C_v1.Y() << "," << C_v1.Z() << ") mm" << std::endl;
    std::cout << "The position of D_v1 = (" << D_v1.X() << "," << D_v1.Y() << "," << D_v1.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_v1 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    
    //*************************************************** x2 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the x2 plane.." << std::endl;
    std::cout << "The position of A_x2 = (" << A_x2.X() << "," << A_x2.Y() << "," << A_x2.Z() << ") mm" << std::endl;
    std::cout << "The position of B_x2 = (" << B_x2.X() << "," << B_x2.Y() << "," << B_x2.Z() << ") mm" << std::endl;
    std::cout << "The position of C_x2 = (" << C_x2.X() << "," << C_x2.Y() << "," << C_x2.Z() << ") mm" << std::endl;
    std::cout << "The position of D_x2 = (" << D_x2.X() << "," << D_x2.Y() << "," << D_x2.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_x2 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** u2 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the u2 plane.." << std::endl;
    std::cout << "The position of A_u2 = (" << A_u2.X() << "," << A_u2.Y() << "," << A_u2.Z() << ") mm" << std::endl;
    std::cout << "The position of B_u2 = (" << B_u2.X() << "," << B_u2.Y() << "," << B_u2.Z() << ") mm" << std::endl;
    std::cout << "The position of C_u2 = (" << C_u2.X() << "," << C_u2.Y() << "," << C_u2.Z() << ") mm" << std::endl;
    std::cout << "The position of D_u2 = (" << D_u2.X() << "," << D_u2.Y() << "," << D_u2.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_u2 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** v2 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the v2 plane.." << std::endl;
    std::cout << "The position of A_v2 = (" << A_v2.X() << "," << A_v2.Y() << "," << A_v2.Z() << ") mm" << std::endl;
    std::cout << "The position of B_v2 = (" << B_v2.X() << "," << B_v2.Y() << "," << B_v2.Z() << ") mm" << std::endl;
    std::cout << "The position of C_v2 = (" << C_v2.X() << "," << C_v2.Y() << "," << C_v2.Z() << ") mm" << std::endl;
    std::cout << "The position of D_v2 = (" << D_v2.X() << "," << D_v2.Y() << "," << D_v2.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_v2 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    
    //*************************************************** x3 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the x3 plane.." << std::endl;
    std::cout << "The position of A_x3 = (" << A_x3.X() << "," << A_x3.Y() << "," << A_x3.Z() << ") mm" << std::endl;
    std::cout << "The position of B_x3 = (" << B_x3.X() << "," << B_x3.Y() << "," << B_x3.Z() << ") mm" << std::endl;
    std::cout << "The position of C_x3 = (" << C_x3.X() << "," << C_x3.Y() << "," << C_x3.Z() << ") mm" << std::endl;
    std::cout << "The position of D_x3 = (" << D_x3.X() << "," << D_x3.Y() << "," << D_x3.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_x3 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** u3 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the u3 plane.." << std::endl;
    std::cout << "The position of A_u3 = (" << A_u3.X() << "," << A_u3.Y() << "," << A_u3.Z() << ") mm" << std::endl;
    std::cout << "The position of B_u3 = (" << B_u3.X() << "," << B_u3.Y() << "," << B_u3.Z() << ") mm" << std::endl;
    std::cout << "The position of C_u3 = (" << C_u3.X() << "," << C_u3.Y() << "," << C_u3.Z() << ") mm" << std::endl;
    std::cout << "The position of D_u3 = (" << D_u3.X() << "," << D_u3.Y() << "," << D_u3.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_u3 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** v3 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the v3 plane.." << std::endl;
    std::cout << "The position of A_v3 = (" << A_v3.X() << "," << A_v3.Y() << "," << A_v3.Z() << ") mm" << std::endl;
    std::cout << "The position of B_v3 = (" << B_v3.X() << "," << B_v3.Y() << "," << B_v3.Z() << ") mm" << std::endl;
    std::cout << "The position of C_v3 = (" << C_v3.X() << "," << C_v3.Y() << "," << C_v3.Z() << ") mm" << std::endl;
    std::cout << "The position of D_v3 = (" << D_v3.X() << "," << D_v3.Y() << "," << D_v3.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_v3 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    
    //*************************************************** x4 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the x4 plane.." << std::endl;
    std::cout << "The position of A_x4 = (" << A_x4.X() << "," << A_x4.Y() << "," << A_x4.Z() << ") mm" << std::endl;
    std::cout << "The position of B_x4 = (" << B_x4.X() << "," << B_x4.Y() << "," << B_x4.Z() << ") mm" << std::endl;
    std::cout << "The position of C_x4 = (" << C_x4.X() << "," << C_x4.Y() << "," << C_x4.Z() << ") mm" << std::endl;
    std::cout << "The position of D_x4 = (" << D_x4.X() << "," << D_x4.Y() << "," << D_x4.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_x4 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** u4 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the u4 plane.." << std::endl;
    std::cout << "The position of A_u4 = (" << A_u4.X() << "," << A_u4.Y() << "," << A_u4.Z() << ") mm" << std::endl;
    std::cout << "The position of B_u4 = (" << B_u4.X() << "," << B_u4.Y() << "," << B_u4.Z() << ") mm" << std::endl;
    std::cout << "The position of C_u4 = (" << C_u4.X() << "," << C_u4.Y() << "," << C_u4.Z() << ") mm" << std::endl;
    std::cout << "The position of D_u4 = (" << D_u4.X() << "," << D_u4.Y() << "," << D_u4.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_u4 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*************************************************** v4 plane of the tracker *************************************************************//
    
    //Position of det. corners wrt origin (world co-ordinates)
    std::cout << "\nFor the v4 plane.." << std::endl;
    std::cout << "The position of A_v4 = (" << A_v4.X() << "," << A_v4.Y() << "," << A_v4.Z() << ") mm" << std::endl;
    std::cout << "The position of B_v4 = (" << B_v4.X() << "," << B_v4.Y() << "," << B_v4.Z() << ") mm" << std::endl;
    std::cout << "The position of C_v4 = (" << C_v4.X() << "," << C_v4.Y() << "," << C_v4.Z() << ") mm" << std::endl;
    std::cout << "The position of D_v4 = (" << D_v4.X() << "," << D_v4.Y() << "," << D_v4.Z() << ") mm" << std::endl;
    std::cout << "The angle of rotation = " << stereo_v4 * TMath::RadToDeg() << " deg.\n" << std::endl;
    
    //*****************************************************************************************************************************************//
    
}

TVector3 TrackerGeometry::get_strip_origin(TVector3 det_origin, int strip_no, double stereo_angle)
{
    TVector3 strip_origin;
    
    //obtain position of first strip (det. corner) using
    //polar co-ordinates wrt the det. centre.
    float r_origin_x = det_origin.X();
    float r_origin_y = det_origin.Y();
    
    //obtain position of the chosen strip wrt to the first strip (det. corner)
    //using polar co-ordinates.
    float r_x_strips = (strip_no*s_pitch - 0.5*s_pitch) * cos(stereo_angle);
    float r_y_strips = (-1*strip_no*s_pitch + 0.5*s_pitch) * sin(stereo_angle);
    
    //using the position of the first strip convert from det. co-ordinates to world
    //co-ordinates
    strip_origin.SetX(r_x_strips + r_origin_x);
    strip_origin.SetY(r_y_strips + r_origin_y);
    strip_origin.SetZ(det_origin.Z());
    
    return strip_origin;
}

//return co-ordinates at the top of strip using co-ordinates of bottom
TVector3 TrackerGeometry::build_strip(TVector3 btm, double stereo_angle)
{
    TVector3 top;
    top.SetXYZ(btm.X() + s_length*sin(stereo_angle), btm.Y() + s_length*cos(stereo_angle), btm.Z());
    return top;
}

//used for drawing geometry and hits, returns three lines for a strip that represent the strip edges and centre
void TrackerGeometry::draw_strip(TLine * x_low, TLine * x_mid, TLine * x_high, float x, float y, float z, double stereo_angle)
{
    TVector3 top, btm;
    
    btm.SetXYZ(x, y, z);
    top.SetXYZ(btm.X() + s_length*sin(stereo_angle), btm.Y() + s_length*cos(stereo_angle), btm.Z());
    x_mid->SetX1(btm.X());
    x_mid->SetY1(btm.Y());
    x_mid->SetX2(top.X());
    x_mid->SetY2(top.Y());
    
    btm.SetXYZ(x - 0.5*s_pitch*cos(stereo_angle), y + 0.5*s_pitch*sin(stereo_angle), z);
    top.SetXYZ(btm.X() + s_length*sin(stereo_angle), btm.Y() + s_length*cos(stereo_angle), btm.Z());
    x_low->SetX1(btm.X());
    x_low->SetY1(btm.Y());
    x_low->SetX2(top.X());
    x_low->SetY2(top.Y());
    
    btm.SetXYZ(x + 0.5*s_pitch*cos(stereo_angle), y - 0.5*s_pitch*sin(stereo_angle), z);
    top.SetXYZ(btm.X() + s_length*sin(stereo_angle), btm.Y() + s_length*cos(stereo_angle), btm.Z());
    x_high->SetX1(btm.X());
    x_high->SetY1(btm.Y());
    x_high->SetX2(top.X());
    x_high->SetY2(top.Y());
}

//Transform 3D hit location (x,y) into 1D det. co-ordinate (u or v)
double TrackerGeometry::world_2_det_transform(TVector3 det_origin, TVector3 det_opp_origin, TVector3 hit)
{
    //u or v axis constructed between two rotated detector corners
    TVector3 det_axis = det_opp_origin - det_origin;
    //Transform 3D vectors to 2D ie must make sure that hit vector
    //has same z component as detector corners so that it's magnitude
    //and angle wrt to det_axis vector corresponds to x/y components only.
    hit.SetZ(det_origin.Z());
    //vector from origin to hit location.
    TVector3 dist_to_hit = hit - det_origin;
    //Change from 2D position to 1D ie distance of hit along det axis
    //1D used since detector is only segmented in one dimension.
    double r = dist_to_hit.Mag()*cos(det_axis.Angle(dist_to_hit));
    
    //Use for debug
    
    //std::cout << "hit = (" << hit.x() << "," << hit.y() << "," << hit.z() << ")" << std::endl;
    
    return r;
}

//Checks if a hit is inside a given strip
bool TrackerGeometry::IsInsideStrip(TVector3 h, TVector3 origin, double stereo_angle)
{
    
    //see here for how to do it analytically:
    //http://math.stackexchange.com/questions/190111/how-to-check-if-a-point-is-inside-a-rectangle
    //probably not much faster than using the IsInside() function that belongs to the TGraph object in root
    //which probably uses a similar algorithm
    
    TVector3 top, btm, A,B,C,D;
    
    btm.SetXYZ(origin.X() - 0.5*s_pitch*cos(stereo_angle), origin.Y() + 0.5*s_pitch*sin(stereo_angle), origin.Z());
    top.SetXYZ(btm.X() + s_length*sin(stereo_angle), btm.Y() + s_length*cos(stereo_angle), btm.Z());
    A.SetXYZ(btm.X(),btm.Y(),btm.Z());
    B.SetXYZ(top.X(),top.Y(),top.Z());
    
    btm.SetXYZ(origin.X() + 0.5*s_pitch*cos(stereo_angle), origin.Y() - 0.5*s_pitch*sin(stereo_angle), origin.Z());
    top.SetXYZ(btm.X() + s_length*sin(stereo_angle), btm.Y() + s_length*cos(stereo_angle), btm.Z());
    D.SetXYZ(btm.X(),btm.Y(),btm.Z());
    C.SetXYZ(top.X(),top.Y(),top.Z());
    
    bool IsInside = true;
    float _X[5];
    float _Y[5];
    TGraph * g;
    
    _X[0] = A.X();
    _Y[0] = A.Y();
    _X[1] = B.X();
    _Y[1] = B.Y();
    _X[2] = C.X();
    _Y[2] = C.Y();
    _X[3] = D.X();
    _Y[3] = D.Y();
    _X[4] = A.X();
    _Y[4] = A.Y();
    
    g = new TGraph(5,_X,_Y);
    if( !g->IsInside( h.X(),h.Y() ) ) { IsInside = false; }
    delete g;
    
	return IsInside;
}

TVector3 TrackerGeometry::get_average_strip_pos(TVector3 det_origin, std::vector<int> strips, double stereo_angle)
{
    TVector3 average(0,0,0);
    for(size_t i=0; i<strips.size(); i++){average += get_strip_origin(det_origin, strips[i], stereo_angle);}
    return TVector3( average.x()/strips.size(), average.y()/strips.size(), average.z()/strips.size());
}

//Find average pos between two points. if z is the same it will remain constant
TVector3 TrackerGeometry::get_average_pos(TVector3 p1, TVector3 p2)
{
    return TVector3( 0.5*(p1.x() + p2.x()), 0.5*(p1.y() + p2.y()), 0.5*(p1.z() + p2.z()));
}

//Find average pos from list of points. if z is the same it will remain constant
TVector3 TrackerGeometry::get_average_pos(std::vector<TVector3> points)
{
    TVector3 average(0,0,0);
    for(size_t i=0; i<points.size(); i++){average += points[i];}
    return TVector3( average.x()/points.size(), average.y()/points.size(), average.z()/points.size());
}

TTree* TrackerGeometry::WriteParameters()
{
	TTree * geomTree = new TTree( "trackerGeom" , "trackerGeom" );
    
     double noOfSensorStrips = 1024;
    //Set branches in trackerGeom tree
    geomTree->Branch( "noOfSensorStrips" , &noOfSensorStrips );
    geomTree->Branch( "teleStripPitch" , &s_pitch );
    geomTree->Branch( "sensorStripLength" , &s_length );
    geomTree->Branch( "inter_plane_dist" , &inter_plane_dist );
    geomTree->Branch( "inter_module_dist" , &inter_module_dist );
    geomTree->Branch( "phantom_gap" , &phantom_gap );
    geomTree->Branch( "halfPhantomSizeZ" , &halfPhantomSizeZ );
    geomTree->Branch( "halfCalorimeterSizeZ" , &halfCalorimeterSizeZ );
    
    geomTree->Branch( "x1_SensorTheta" , &stereo_x1 );
    geomTree->Branch( "u1_SensorTheta" , &stereo_u1 );
    geomTree->Branch( "v1_SensorTheta" , &stereo_v1 );
    geomTree->Branch( "x2_SensorTheta" , &stereo_x2 );
    geomTree->Branch( "u2_SensorTheta" , &stereo_u2 );
    geomTree->Branch( "v2_SensorTheta" , &stereo_v2 );
    geomTree->Branch( "TP1_Theta" , &stereo_x1 );
    geomTree->Branch( "TP2_Theta" , &stereo_x1 );
    geomTree->Branch( "TP3_Theta" , &stereo_x1 );
    geomTree->Branch( "TP4_Theta" , &stereo_x1 );
    geomTree->Branch( "x3_SensorTheta" , &stereo_x3 );
    geomTree->Branch( "u3_SensorTheta" , &stereo_u2 );
    geomTree->Branch( "v3_SensorTheta" , &stereo_v3 );
    geomTree->Branch( "x4_SensorTheta" , &stereo_x4 );
    geomTree->Branch( "u4_SensorTheta" , &stereo_u4 );
    geomTree->Branch( "v4_SensorTheta" , &stereo_v4 );
    
    geomTree->Fill();

	return geomTree;
}