/* ======================================================================== *\
!
! *
! * 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): Thomas Bretz 3/2004 <mailto:tbretz@astro.uni-wuerzburg.de>
!   Author(s): Abelardo Moralejo 1/2005 <mailto:moralejo@pd.infn.it>
!
!   Copyright: MAGIC Software Development, 2000-2005
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
// MSrcPosCalc
//
// Calculate the current source position in the camera from the (possibly
// already corrected, by starguider) J2000 sky coordinates of the camera
// center (contained in MPointingPos), and the source J2000 sky
// coordinates contained in MSourcePos (of type MPointingPos as well). If
// no MSourcePos is found in the parameter list,  source position is
// assumed to be the same for all events, that specified in  MSrcPosCam
// (if it already existed in the parameter list), or (0,0), the center  of
// the camera, if no MSrcPosCam was present in the parameter list. In both
// cases, no calculation is necessary and then the PreProcess returns
// kSKIP so that the task is removed from the task list.
// 
// The conversion factor between the camera plain (mm) and the sky (deg)
// is taken from MGeomCam. The time is taken from MTime, and the
// coordinates of the observatory from MObservatory.
// 
// Input Container:
//   MPointingPos
//   MObservatory
//   MGeomCam
//   MTime
//   [MSourcePos] (of type MPointingPos)
//
// Output Container:
//   MSrcPosCam
//
// To be done:
//   - a switch between using sky-coordinates and time or local-coordinates
//     from MPointingPos for determine the rotation angle
/////  NOTE, A. Moralejo: the above is not possible if MAstroSky2Local does not 
/////  account for precession and nutation.
//
//   - the center of rotation need not to be the camera center
/////  NOTE, A. Moralejo: I don't see the need for special code either.
//
//////////////////////////////////////////////////////////////////////////////
#include "MSrcPosCalc.h"

#include <TVector2.h>

#include "MParList.h"

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

#include "MGeomCam.h"
#include "MObservatory.h"
#include "MPointingPos.h"
#include "MPointingDev.h"
#include "MSrcPosCam.h"
#include "MRawRunHeader.h"
#include "MMcCorsikaRunHeader.h"

#include "MAstro.h"
#include "MVector3.h"
#include "MAstroSky2Local.h"

ClassImp(MSrcPosCalc);

using namespace std;

// --------------------------------------------------------------------------
//
// Initialize fY and fY with 0
//
MSrcPosCalc::MSrcPosCalc(const char *name, const char *title)
    : fObservatory(NULL), fPointPos(NULL), fSourcePos(NULL), fDeviation(NULL),
    fSrcPosCam(NULL), fSrcPosAnti(NULL), fGeom(NULL), fTime(NULL), fMode(kDefault)
{
    fName  = name  ? name  : "MSrcPosCalc";
    fTitle = title ? title : "Calculates the source position in the camera";

    AddToBranchList("MTime.*");
    AddToBranchList("MPointingPos.*");
}

// --------------------------------------------------------------------------
//
//  delete fSourcePos if kIsOwner
//  set fSourcePos to NULL
//
void MSrcPosCalc::FreeSourcePos()
{
    if (fSourcePos && TestBit(kIsOwner))
        delete fSourcePos;

    fSourcePos = NULL;
}

// --------------------------------------------------------------------------
//
//  ra  [h]
//  dec [deg]
//
void MSrcPosCalc::SetSourcePos(Double_t ra, Double_t dec)
{
    FreeSourcePos();

    fSourcePos = new MPointingPos("MSourcePos");
    fSourcePos->SetSkyPosition(ra, dec);

    SetOwner();
}

// --------------------------------------------------------------------------
//
// Return ra/dec as string
//
TString MSrcPosCalc::GetRaDec(const MPointingPos &pos) const
{
    const TString rstr = MAstro::Angle2Coordinate(pos.GetRa());
    const TString dstr = MAstro::Angle2Coordinate(pos.GetDec());

    return Form("Ra=%sh Dec=%sdeg", rstr.Data(), dstr.Data());
}

// --------------------------------------------------------------------------
//
// Search and if necessary create MSrcPosCam in the parameter list. Search
// MSourcePos. If not found, do nothing else, and skip the task. If MSrcPosCam
// did not exist before and has been created here, it will contain as source
// position the camera center (0,0).
// In the case that MSourcePos is found, go ahead in searching the rest of
// necessary containers. The source position will be calculated for each
// event in Process.
//
Int_t MSrcPosCalc::PreProcess(MParList *pList)
{
    fSrcPosCam = (MSrcPosCam*)pList->FindCreateObj("MSrcPosCam");
    if (!fSrcPosCam)
        return kFALSE;

    fSrcPosAnti = (MSrcPosCam*)pList->FindCreateObj("MSrcPosCam", "MSrcPosAnti");
    if (!fSrcPosAnti)
        return kFALSE;

    if (!fSourcePos)
    {
        fSourcePos = (MPointingPos*)pList->FindObject("MSourcePos", "MPointingPos");
        if (!fSourcePos)
        {
            *fLog << inf;
            *fLog << "MSourcePos [MPointPos] not found... The source position" << endl;
            *fLog << "set in MSrcPosCam will be set to  (0/0)  or in the case" << endl;
            *fLog << "of  Monte Carlo set to the appropriate wobble position." << endl;
            return kTRUE;
        }
    }
    // FIXME: Maybe we have to call FreeSourcePos in PostProcess()?

    fGeom = (MGeomCam*)pList->FindObject("MGeomCam");
    if (!fGeom)
    {
        *fLog << err << "MGeomCam not found... aborting." << endl;
        return kFALSE;
    }

    fPointPos = (MPointingPos*)pList->FindObject("MPointingPos");
    if (!fPointPos)
    {
        *fLog << err << "MPointingPos not found... aborting." << endl;
        return kFALSE;
    }

    fDeviation = (MPointingDev*)pList->FindObject("MPointingDev");

    *fLog << inf;
    //*fLog << "Pointing Position: " << GetRaDec(*fPointPos)  << endl;
    *fLog << "Source Position: " << GetRaDec(*fSourcePos) << endl;

    // For the case ReInit is never called we try:
    fObservatory = (MObservatory*)pList->FindObject("MObservatory");
    fTime        = (MTime*)       pList->FindObject("MTime");
    fRunType     = MRawRunHeader::kRTNone;

    return kTRUE;
}

// --------------------------------------------------------------------------
//
// If fIsWobbleMode==kFALSE set source position to v and anto-source
// position to -v, if fIsWobbleMode==kTRUE vice versa.
//
void MSrcPosCalc::SetSrcPos(TVector2 v) const
{
    if (fMode==kWobble)
    {
        fSrcPosAnti->SetXY(v);
        v *= -1;
        fSrcPosCam->SetXY(v);
    }
    else
    {
        fSrcPosCam->SetXY(v);
        v *= -1;
        fSrcPosAnti->SetXY(v);
    }
}

// --------------------------------------------------------------------------
//
// Checking for file type. If the file type is Monte Carlo the
// source position is arbitrarily determined from the MC headers.
//
Bool_t MSrcPosCalc::ReInit(MParList *plist)
{
    if (fMode==kOffData)
    {
        SetSrcPos(TVector2());
        return kTRUE;
    }

    MRawRunHeader *run = (MRawRunHeader*)plist->FindObject("MRawRunHeader");
    if (!run)
    {
        *fLog << err << "MRawRunHeader not found... aborting." << endl;
        return kFALSE;
    }

    fRunType = run->GetRunType();

    if (fRunType!=MRawRunHeader::kRTMonteCarlo)
    {
        fObservatory = (MObservatory*)plist->FindObject("MObservatory");
        if (!fObservatory)
        {
            *fLog << err << "MObservatory not found... aborting." << endl;
            return kFALSE;
        }
        fTime = (MTime*)plist->FindObject("MTime");
        if (!fTime)
        {
            *fLog << err << "MTime not found... aborting." << endl;
            return kFALSE;
        }
        return kTRUE;
    }

    MMcCorsikaRunHeader *h = (MMcCorsikaRunHeader*)plist->FindObject("MMcCorsikaRunHeader");
    if (!h)
    {
        *fLog << err << "MMcCorsikaRunHeader not found... aborting." << endl;
        return kFALSE;
    }

    TVector2 v(0, 0);
    if (h->GetWobbleMode()>0.5)
        v.Set(120., 0.);
    if (h->GetWobbleMode()<-0.5)
        v.Set(-120., 0.);

    SetSrcPos(v);

    *fLog << inf;
    *fLog << "Source Position set to x=" << fSrcPosCam->GetX() << "mm ";
    *fLog << "y=" << fSrcPosCam->GetY() << "mm" << endl;

    return kTRUE;
}

// --------------------------------------------------------------------------
//
// Loc0LocToCam
//
// Input :   (theta0, phi0)   direction for the position (0,0) in the camera  
//           ( theta,  phi)   some other direction
// 
// Output :  (X, Y)      position in the camera corresponding to (theta, phi)
//
TVector2 MSrcPosCalc::CalcXYinCamera(const MVector3 &pos0, const MVector3 &pos) const
{
    const Double_t theta0 = pos0.Theta();
    const Double_t phi0   = pos0.Phi();

    const Double_t theta  = pos.Theta();
    const Double_t phi    = pos.Phi();

    //--------------------------------------------

    /* --- OLD ---
     const Double_t YC0 = TMath::Cos(theta0)*TMath::Tan(theta)*TMath::Cos(phi-phi0) - TMath::Sin(theta0);
     const Double_t YC1 = TMath::Cos(theta0) + TMath::Sin(theta0)*TMath::Tan(theta);
     const Double_t YC  = YC0 / YC1;

     const Double_t XC0 =  TMath::Cos(theta0) - YC*TMath::Sin(theta0);
     const Double_t XC  = -TMath::Sin(phi-phi0) * TMath::Tan(theta) * XC0;
     */

    /* --- NEW ---  Same as MSrcPosCorrect::CalcXYinCamera */
    const Double_t XC0 = TMath::Sin(theta)*TMath::Sin(phi-phi0);
    const Double_t XC1 = TMath::Cos(theta0)*TMath::Cos(theta);
    const Double_t XC2 = TMath::Sin(theta0)*TMath::Sin(theta)*TMath::Cos(phi-phi0);

    const Double_t YC0 = TMath::Sin(theta0)*TMath::Cos(theta);
    const Double_t YC1 = TMath::Cos(theta0)*TMath::Sin(theta)*TMath::Cos(phi-phi0);

    const Double_t XC  =  - XC0     / (XC1 + XC2);
    const Double_t YC  = (-YC0+YC1) / (XC1 + XC2);

    //--------------------------------------------
    return TVector2(XC, YC);
}

// --------------------------------------------------------------------------
//
// Derotate fX/fY by the current rotation angle, set MSrcPosCam
//
Int_t MSrcPosCalc::Process()
{
    if (fRunType==MRawRunHeader::kRTMonteCarlo || !fSourcePos || !fTime || !fObservatory || fMode==kOffData)
    {
        // If this is MC data do not change source position
        if (fRunType==MRawRunHeader::kRTMonteCarlo)
            return kTRUE;

        // For real data reset source position to 0/0
        SetSrcPos();
        return kTRUE;
    }

    // Set Sky coordinates of source, taken from container "MSourcePos"
    // of type MPointingPos. The sky coordinates must be J2000, as the
    // sky coordinates of the camera center that we get from the container
    // "MPointingPos" filled by the Drive.
    MVector3 pos;  // pos: source position
    pos.SetRaDec(fSourcePos->GetRaRad(), fSourcePos->GetDecRad());

    // Convert sky coordinates of source to local coordinates. Warning! These are not the "true" local
    // coordinates, since this transformation ignores precession and nutation effects.
    const MAstroSky2Local conv(*fTime, *fObservatory);
    pos *= conv;

    // Set sky coordinates of camera center in pos0, and then convert to
    // local. Same comment as above. These coordinates differ from the true
    // local coordinates of the camera center that one could get from
    // "MPointingPos", calculated by the Drive: the reason is that the Drive
    // takes into account precession and nutation corrections, while
    // MAstroSky2Local (as of Jan 27 2005 at least) does not. Since we just
    // want to get the source position on the camera from the local
    // coordinates of the center and the source, it does not matter that
    // the coordinates contained in pos and pos0 ignore precession and
    // nutation... since the shift would be the same in both cases. What
    // would be wrong is to set in pos0 directly the local coordinates
    // found in MPointingPos!
    MVector3 pos0;  // pos0: camera center
    pos0.SetRaDec(fPointPos->GetRaRad(), fPointPos->GetDecRad());
    pos0 *= conv;

    TVector2 vx;
    if (fDeviation)
    {
        //vx = CalcXYinCamera(pos0, pos)*fGeom->GetCameraDist()*1000;
        pos0.SetZdAz(pos0.Theta()-fDeviation->GetDevZdRad(),
                     pos0.Phi()  -fDeviation->GetDevAzRad());
    }

    // Calculate source position in camera, and convert to mm:
    TVector2 v = CalcXYinCamera(pos0, pos)*fGeom->GetCameraDist()*1000;

    if (fDeviation)
        v -= fDeviation->GetDevXY();

    SetSrcPos(v);

    return kTRUE;
}

// --------------------------------------------------------------------------
//
// Convert a coordinate stored in str into a double, stored in ret.
// Returns kTRUE on success, otherwise kFALSE
//
// Allowed formats are:
//     12.5
//    -12.5
//    +12.5
//  -12:30:00.0
//   12:30:00.0
//  +12:30:00.0
//
Bool_t MSrcPosCalc::GetCoordinate(TString str, Double_t &ret) const
{
    str = str.Strip(TString::kBoth);

    if (str.First(':')<0)
    {
        ret = str.Atof();
        return kTRUE;
    }

    if (MAstro::Coordinate2Angle(str, ret))
        return kTRUE;

    *fLog << err << GetDescriptor() << endl;
    *fLog << "Interpretation of Coordinate '" << str << "' not successfull... abort." << endl;
    *fLog << "Corrdinate must have the format: '-12:30:00.0', '12:30:00.0' or '+12:30:00.0'" << endl;
    return kFALSE;
}

// --------------------------------------------------------------------------
//
// Read the setup from a TEnv, eg:
//   MSrcPosCalc.SourceRa:  ra
//   MSrcPosCalc.SourceDec: dec
//   MSrcPosCalc.SourcePos: ra dec
//
// For format of 'ra' and 'dec' see GetCoordinate()
//
// Coordinates are J2000.0
//
Int_t MSrcPosCalc::ReadEnv(const TEnv &env, TString prefix, Bool_t print)
{
    Double_t ra=0;
    Double_t dec=0;

    if (IsEnvDefined(env, prefix, "SourceRaDec", print))
    {
        TString str = GetEnvValue(env, prefix, "SourceRaDec", "");
        str = str.Strip(TString::kBoth);

        const Ssiz_t pos = str.First(' ');
        if (pos<0)
        {
            *fLog << err << GetDescriptor() << "SourceRaDec empty... abort." << endl;
            return kERROR;
        }

        if (!GetCoordinate(str(0, pos), ra))
            return kERROR;
        if (!GetCoordinate(str(pos+1, str.Length()), dec))
            return kERROR;

        SetSourcePos(ra, dec);
        return kTRUE;
    }

    Bool_t rc = kFALSE;
    if (IsEnvDefined(env, prefix, "SourceRa", print))
    {
        TString str = GetEnvValue(env, prefix, "SourceRa", "");

        if (!GetCoordinate(str, ra))
            return kERROR;

        rc = kTRUE;
    }

    if (IsEnvDefined(env, prefix, "SourceDec", print))
    {
        TString str = GetEnvValue(env, prefix, "SourceDec", "");

        if (!GetCoordinate(str, dec))
            return kERROR;

        rc = kTRUE;
    }

    if (!rc)
        return kFALSE;

    SetSourcePos(ra, dec);
    return kTRUE;
}
