/* ======================================================================== *\
!
! *
! * This file is part of MARS, the MAGIC Analysis and Reconstruction
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appear in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! * 
!
!
!   Author(s): Abelardo Moralejo 11/2003 <mailto:moralejo@pd.infn.it>
!
!   Copyright: MAGIC Software Development, 2000-2003
!
!
\* ======================================================================== */

/////////////////////////////////////////////////////////////////////////////
//
// MStereoPar
//
// Storage Container for shower parameters estimated using the information
// from two telescopes (presently for MC studies)
//
// 
/////////////////////////////////////////////////////////////////////////////
#include "MStereoPar.h"
#include <fstream>

#include "MLog.h"
#include "MLogManip.h"

#include "MHillas.h"
#include "MMcEvt.hxx"
#include "MGeomCam.h"


ClassImp(MStereoPar);

using namespace std;

// --------------------------------------------------------------------------
//
// Default constructor.
//
MStereoPar::MStereoPar(const char *name, const char *title)
{
    fName  = name  ? name  : "MStereoPar";
    fTitle = title ? title : "Stereo image parameters";


}


// --------------------------------------------------------------------------
//
void MStereoPar::Reset()
{
    fCoreX = 0.;
    fCoreY = 0.;
    fSourceX = 0.;
    fSourceY = 0.;
}


// --------------------------------------------------------------------------
//
//  Calculation of shower parameters
//
void MStereoPar::Calc(const MHillas &hillas1, const MMcEvt &mcevt1, const MGeomCam &mgeom1, const Float_t ct1_x, const Float_t ct1_y, const MHillas &hillas2, const MMcEvt &mcevt2, const MGeomCam &mgeom2, const Float_t ct2_x, const Float_t ct2_y)
{
    //
    // Get the direction corresponding to the c.o.g. of the image on 
    // the camera
    //

    Float_t ct1_cosx_a;
    Float_t ct1_cosy_a;
    Float_t ct1_cosz_a; // Direction from ct1 to the shower c.o.g.

    Camera2direction(1e3*mgeom1.GetCameraDist(), mcevt1.GetTelescopePhi(), mcevt1.GetTelescopeTheta(), hillas1.GetMeanX(), hillas1.GetMeanY(), &ct1_cosx_a, &ct1_cosy_a, &ct1_cosz_a);

    //
    // Now we get another (arbitrary) point along the image long axis,
    // fMeanX + cosdelta, fMeanY + sindelta, and calculate the direction 
    // to which it corresponds.
    //
    
    Float_t ct1_cosx_b;
    Float_t ct1_cosy_b;
    Float_t ct1_cosz_b;

    Camera2direction(1e3*mgeom1.GetCameraDist(), mcevt1.GetTelescopePhi(), mcevt1.GetTelescopeTheta(), hillas1.GetMeanX()+hillas1.GetCosDelta(), hillas1.GetMeanY()+hillas1.GetSinDelta(), &ct1_cosx_b, &ct1_cosy_b, &ct1_cosz_b);

    //
    // The vectorial product of the latter two vectors is a vector 
    // perpendicular to the plane which contains the shower axis and 
    // passes through the telescope center (center of reflector). 
    // The vectorial product of that vector and (0,0,1) is a vector on
    // the horizontal plane pointing from the telescope center to the 
    // shower core position on the z=0 plane (ground).
    //

    Float_t ct1_coreVersorX = ct1_cosz_a*ct1_cosx_b - ct1_cosx_a*ct1_cosz_b;
    Float_t ct1_coreVersorY = ct1_cosz_a*ct1_cosy_b - ct1_cosy_a*ct1_cosz_b;

    //
    // Now we calculate again the versor, now assuming that the source 
    // direction is paralel to the telescope axis (camera position 0,0)  
    // This increases the precision of the core determination if the showers
    // actually come from that direction (like for gammas from a point source)

    Camera2direction(1e3*mgeom1.GetCameraDist(), mcevt1.GetTelescopePhi(), mcevt1.GetTelescopeTheta(), 0., 0., &ct1_cosx_b, &ct1_cosy_b, &ct1_cosz_b);

    Float_t ct1_coreVersorX_best = ct1_cosz_a*ct1_cosx_b - ct1_cosx_a*ct1_cosz_b;
    Float_t ct1_coreVersorY_best = ct1_cosz_a*ct1_cosy_b - ct1_cosy_a*ct1_cosz_b;
    
    //
    // Now the second telescope
    //

    Float_t ct2_cosx_a;
    Float_t ct2_cosy_a;
    Float_t ct2_cosz_a; // Direction from ct2 to the shower c.o.g.


    Camera2direction(1e3*mgeom2.GetCameraDist(), mcevt2.GetTelescopePhi(), mcevt2.GetTelescopeTheta(), hillas2.GetMeanX(), hillas2.GetMeanY(), &ct2_cosx_a, &ct2_cosy_a, &ct2_cosz_a);

    Float_t ct2_cosx_b;
    Float_t ct2_cosy_b;
    Float_t ct2_cosz_b;

    Camera2direction(1e3*mgeom2.GetCameraDist(), mcevt2.GetTelescopePhi(), mcevt2.GetTelescopeTheta(), hillas2.GetMeanX()+hillas2.GetCosDelta(), hillas2.GetMeanY()+hillas2.GetSinDelta(), &ct2_cosx_b, &ct2_cosy_b, &ct2_cosz_b);


    Float_t ct2_coreVersorX = ct2_cosz_a*ct2_cosx_b - ct2_cosx_a*ct2_cosz_b;
    Float_t ct2_coreVersorY = ct2_cosz_a*ct2_cosy_b - ct2_cosy_a*ct2_cosz_b;


    Camera2direction(1e3*mgeom2.GetCameraDist(), mcevt2.GetTelescopePhi(), mcevt2.GetTelescopeTheta(), 0., 0., &ct2_cosx_b, &ct2_cosy_b, &ct2_cosz_b);

    Float_t ct2_coreVersorX_best = ct2_cosz_a*ct2_cosx_b - ct2_cosx_a*ct2_cosz_b;
    Float_t ct2_coreVersorY_best = ct2_cosz_a*ct2_cosy_b - ct2_cosy_a*ct2_cosz_b;
    
    //
    // Estimate core position:
    //
    Float_t t = ct1_x - ct2_x - ct2_coreVersorX/ct2_coreVersorY*(ct1_y-ct2_y);
    t /= (ct2_coreVersorX/ct2_coreVersorY*ct1_coreVersorY - ct1_coreVersorX);

    fCoreX = ct1_x + t * ct1_coreVersorX;
    fCoreY = ct1_y + t * ct1_coreVersorY;

    // fCoreX, fCoreY, fCoreX2, fCoreY2 will have the same units 
    // as ct1_x, ct1_y, ct2_x, ct2_y


    //
    // Now the estimated core position assuming the source is located in 
    // the center of the camera:
    //
    t = ct1_x - ct2_x - ct2_coreVersorX_best / 
	ct2_coreVersorY_best*(ct1_y-ct2_y);
    t /= (ct2_coreVersorX_best/ct2_coreVersorY_best*ct1_coreVersorY_best - 
	  ct1_coreVersorX_best);

    fCoreX2 = ct1_x + t * ct1_coreVersorX_best;
    fCoreY2 = ct1_y + t * ct1_coreVersorY_best;

    //
    // Be careful, the coordinates in MMcEvt.fCoreX,fCoreY are actually 
    // those of the vector going *from the shower core to the telescope*.
    // Ours are those of the vector which goes from telescope 1 to the 
    // core estimated core.
    //

    /////////////////////////////////////////////////////////////////////
    //
    // Now estimate the source location on the camera by intersecting 
    // major axis of the ellipses. This assumes both telescopes are 
    // pointing paralel! We introduce the camera scale to account for
    // the use of telescopes with different focal distances. 
    //

    Float_t scale1 = mgeom1.GetConvMm2Deg();
    Float_t scale2 = mgeom2.GetConvMm2Deg();

    t = scale2*hillas2.GetMeanY() - scale1*hillas1.GetMeanY() +
	(scale1*hillas1.GetMeanX() - scale2*hillas2.GetMeanX()) * 
	hillas2.GetSinDelta() / hillas2.GetCosDelta();

    t /= (hillas1.GetSinDelta() - 
	  hillas2.GetSinDelta()/hillas2.GetCosDelta()*hillas1.GetCosDelta());

    fSourceX = scale1*hillas1.GetMeanX() + t * hillas1.GetCosDelta();
    fSourceY = scale1*hillas1.GetMeanY() + t * hillas1.GetSinDelta();

    //
    // Squared angular distance from reconstructed source position to 
    // camera center.
    //
    fTheta2 = fSourceX*fSourceX+fSourceY*fSourceY;

    //
    // Get the direction corresponding to the intersection of axes
    //

    Float_t source_direction[3];

    Camera2direction(1e3*mgeom1.GetCameraDist(), mcevt1.GetTelescopePhi(), mcevt1.GetTelescopeTheta(), fSourceX/scale1, fSourceY/scale1, &(source_direction[0]), &(source_direction[1]), &(source_direction[2]));


    //
    // Calculate impact parameters
    //

    Float_t scalar = (fCoreX-ct1_x)*source_direction[0] +
	(fCoreY-ct1_y)*source_direction[1];
    fCT1Impact = sqrt( (fCoreX-ct1_x)*(fCoreX-ct1_x) +
		       (fCoreY-ct1_y)*(fCoreY-ct1_y) -
		       scalar * scalar );

    scalar = (fCoreX-ct2_x)*source_direction[0] +
	(fCoreY-ct2_y)*source_direction[1];
    fCT2Impact = sqrt( (fCoreX-ct2_x)*(fCoreX-ct2_x) +
		       (fCoreY-ct2_y)*(fCoreY-ct2_y) -
		       scalar * scalar );

    //
    // Now calculate i.p. assuming source is point-like and placed in
    // the center of the camera.
    //
    scalar = (fCoreX2-ct1_x)*(-sin(mcevt1.GetTelescopeTheta())*
			     cos(mcevt1.GetTelescopePhi()))  +
      (fCoreY2-ct1_y)*(-sin(mcevt1.GetTelescopeTheta())*
		      sin(mcevt1.GetTelescopePhi()));

    fCT1Impact2 = sqrt( (fCoreX2-ct1_x)*(fCoreX2-ct1_x) +
		       (fCoreY2-ct1_y)*(fCoreY2-ct1_y) -
		       scalar * scalar );

    scalar = (fCoreX2-ct2_x)*(-sin(mcevt2.GetTelescopeTheta())*
			     cos(mcevt2.GetTelescopePhi()))  +
      (fCoreY2-ct2_y)*(-sin(mcevt2.GetTelescopeTheta())*
		      sin(mcevt2.GetTelescopePhi()));

    fCT2Impact2 = sqrt( (fCoreX2-ct2_x)*(fCoreX2-ct2_x) +
		       (fCoreY2-ct2_y)*(fCoreY2-ct2_y) -
		       scalar * scalar );

 
    SetReadyToSave();
} 

// --------------------------------------------------------------------------
//
// Transformation of coordinates, from a point on the camera x, y , to
// the director cosines of the corresponding direction, in the system of 
// coordinates in which X-axis is North, Y-axis is west, and Z-axis 
// points to the zenith. The transformation has been taken from TDAS 01-05,
// although the final system of coordinates is not the same defined there,
// but the one defined in Corsika (for convenience). 
//
// rc is the distance from the reflector center to the camera. CTphi and 
// CTtheta indicate the telescope orientation. The angle CTphi is the 
// azimuth of the vector going along the telescope axis from the camera 
// towards the reflector, measured from the North direction anticlockwise 
// ( being West: phi=pi/2, South: phi=pi, East: phi=3pi/2 )
//
// rc and x,y must be given in the same units!
//
  

void MStereoPar::Camera2direction(Float_t rc, Float_t CTphi, Float_t CTtheta, Float_t x, Float_t y, Float_t* cosx, Float_t* cosy, Float_t* cosz)
{
    //
    // We convert phi to the convention defined in TDAS 01-05
    //
    Float_t sinphi = sin(2*TMath::Pi()-CTphi);
    Float_t cosphi = cos(CTphi);
    Float_t costheta = cos(CTtheta);
    Float_t sintheta = sin(CTtheta);

    Float_t xc = x/rc;
    Float_t yc = y/rc;

    Float_t norm = 1/sqrt(1+xc*xc+yc*yc);

    Float_t xref = xc * norm;
    Float_t yref = yc * norm;
    Float_t zref = -1 * norm;

    *cosx =  xref * sinphi + yref * costheta*cosphi - zref * sintheta*cosphi;
    *cosy = -xref * cosphi + yref * costheta*sinphi - zref * sintheta*sinphi;
    *cosz =                  yref * sintheta        + zref * costheta; 

    //  Now change from system A of TDAS 01-05 to Corsika system:

    *cosy *= -1;
    *cosz *= -1; 

}
