//#include <../include/includefile.h>
#include <math.h>


void CalcICXY(const double zvertex, const double ichx, const double ichy, double *xcor, double *ycor, int whichCor = 3)
{
    // This code calculates the change in the X & Y positions of a photon hit in the IC. 
    // The inputs are the best electron vertex (zvertex) and the uncorrected IC X and Y (ichx & ichy).
    // There are three correction possibilities below (whichCor). I prefer the 3rd.
    // Note: to use with icpb variables, must convert between banks (ichb bank has uncorrected x & y) with 
    //{
    //    const int iICHB = (statc[iIC]-statc[iIC]%10000)/10000-1;    
    //}

    // I added these constants here so that the code is stand alone
    const double pi         = 3.14159265358979312;
    const double rad2deg    = 180./pi;
    const double deg2rad    = pi/180.;

    const double phi        = atan2(ichy, ichx);
    double zd               = 0;
    if (whichCor==1)
    {
        // Original depth correction wrt to vz
        zd                  = 0.0731*(73.111 -fabs(zvertex)); // From fits on 9 Jan 2014 from MHleveloff2p5_MH_pi0_HIeta_NODEPTH_20140109.root to finvMass_vz_RR_12 and zvertex_Dvz_RR_12
    } else if (whichCor==2)
    {
        // Depth correction based on IC geometry, linear approximation of angles
        double ichr         = sqrt(ichx*ichx + ichy*ichy); // cm
        double vzm          = fabs(zvertex); // cm
        double gth          = atan2(ichr, vzm); //rads
        const double r_in   = 1.36*3; // cm
        const double r_out  = 1.36*11; // cm
        const double theta_in   = atan(1.36*3/78.3) * rad2deg; // deg
        const double theta_out  = atan(1.36*11/78.3)* rad2deg; //deg
        double ICth         = (theta_in + (theta_out-theta_in)/(r_out-r_in) * (ichr-r_in)) * deg2rad; //rads
        double clusterdepth = 5.7; // cm
        zd                  = clusterdepth * sin(gth - ICth)/(tan(gth)*cos(ICth)); // cm
    } else if (whichCor==3)
    {
        // Depth correction based on IC geometry, discrete crystal angles - closest to exact geometry - preferred
        double ichr         = sqrt(ichx*ichx + ichy*ichy); //cm 
        double vzm          = fabs(zvertex); // cm
        double gth          = atan2(ichr, vzm); //rads
        const double g1X    = fabs(ichx); // cm
        const double g1Y    = fabs(ichy); // cm
        int nx              = (int) floor((g1X+.5*1.346)/1.346);
        int ny              = (int) floor((g1Y+.5*1.36)/1.36);
        double ICth         = atan( sqrt( pow(nx*1.346,2) + pow(ny*1.36,2) )/78.3); // rads
        double clusterdepth = 5.7; // cm

        zd                  = clusterdepth * sin(gth - ICth)/(tan(gth)*cos(ICth)); // cm
    } 
    const double dx  = -zd * cos(phi) * sqrt(ichx*ichx + ichy*ichy)/(fabs(zvertex) + zd); // cm
    const double dy  = -zd * sin(phi) * sqrt(ichx*ichx + ichy*ichy)/(fabs(zvertex) + zd); // cm

    *xcor = ichx + dx;
    *ycor = ichy + dy;
}