/* ======================================================================== *\
!
! *
! * 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 8/2004 <mailto:tbretz@astro.uni-wuerzburg.de>
!
!   Copyright: MAGIC Software Development, 2000-2004
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
// MSrcPosFromModel
//
//
//////////////////////////////////////////////////////////////////////////////
#include "MSrcPosFromModel.h"

#include "MParList.h"

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

#include "MRawRunHeader.h"
#include "MPointing.h"
#include "MSrcPosCam.h"
#include "MAstroSky2Local.h"
#include "MPointingPos.h"
#include "MGeomCam.h"
#include "MReportDrive.h"

ClassImp(MSrcPosFromModel);

using namespace std;

// --------------------------------------------------------------------------
//
// Initialize fY and fY with 0
//
MSrcPosFromModel::MSrcPosFromModel(const char *name, const char *title)
{
    fName  = name  ? name  : "MSrcPosFromModel";
    fTitle = title ? title : "";

    fPoint0401 = new MPointing("bending0401.txt");
    fPoint0405 = new MPointing("bending0405.txt");
    fPointOld  = new MPointing("bending-old.txt");
    fPointNew  = new MPointing("bending0408.txt");
}

MSrcPosFromModel::~MSrcPosFromModel()
{
    delete fPoint0401;
    delete fPoint0405;
    delete fPointOld;
    delete fPointNew;
}

// --------------------------------------------------------------------------
//
// Search and if necessary create MSrcPosCam in the parameter list
//
Int_t MSrcPosFromModel::PreProcess(MParList *pList)
{
    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 << "MPointPos not found... aborting." << endl;
        return kFALSE;
    }

    fRun = (MRawRunHeader*)pList->FindObject("MRawRunHeader");
    if (!fRun)
    {
        *fLog << err << "MRawRunHeader not found... aborting." << endl;
        return kFALSE;
    }
/*
    fReport = (MReportDrive*)pList->FindObject("MReportDrive");
    if (!fReport)
    {
        *fLog << err << "MReportDrive not found... aborting." << endl;
        return kFALSE;
    }

    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;
    }

    fTime2 = (MTime*)pList->FindObject("MTimeDrive", "MTime");
    if (!fTime2)
    {
        *fLog << err << "MTimeDrive not found... aborting." << endl;
        return kFALSE;
    }
    */
    fSrcPos = (MSrcPosCam*)pList->FindCreateObj("MSrcPosCam");
    if (!fSrcPos)
        return kFALSE;

    return kTRUE;
}
/*
TVector2 MSrcPosFromModel::CalcXYinCamera(const ZdAz &pos0, const ZdAz &pos1) const
{
    MVector3 p0, p1;
    p0.SetZdAz(pos0.X(), pos0.Y());
    p1.SetZdAz(pos1.X(), pos1.Y());

    return CalcXYinCamera(p0, p1);
}

// --------------------------------------------------------------------------
//
// 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 MSrcPosFromModel::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();

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

    // pos0[3] = TMath::Cos(theta0)

    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;

    //--------------------------------------------
    return TVector2(XC, -YC);
}
*/
// --------------------------------------------------------------------------
//
// Derotate fX/fY by the current rotation angle, set MSrcPosCam
//
Int_t MSrcPosFromModel::Process()
{
    MPointing *conv1 = 0;
    MPointing *conv2 = 0;

    if (!conv1 && fRun->GetRunNumber()<26237)
    {
        conv1 = fPoint0401;
        conv2 = fPointOld;
    }
    if (!conv1 && fRun->GetRunNumber()<31684)
    {
        conv1 = fPoint0405;
        conv2 = fPointOld;
    }
    /*
     MVector3 sky;
     sky.SetRaDec(fPointPos->GetRaRad(), fPointPos->GetDecRad());
     MVector3 loc = MAstroSky2Local(*fTime2, *fObservatory)*sky;
     ZdAz za(loc.Theta(), loc.Phi());
     */

    // Get current pointing position from Drive system
    ZdAz za(fPointPos->GetZd(), fPointPos->GetAz());
    za *= TMath::DegToRad();

    // Get corresponding Shaftencoder positions for 'Used-Mode'
    const ZdAz za2 = conv1->Correct(za);  // [deg] --> [se]
    // Get corresponding Shaftencoder positions for 'Correct-Mode'
    const ZdAz za1 = conv2->Correct(za);  // [deg] --> [se]

    // The difference of the shaftencoder positions for both model
    // correspond in a first order approximation to the misspointing
    TVector2 v0 = TVector2(za2.Y()-za1.Y(), za2.X()-za1.X());

    // Convert misspointing to millimeters
    v0 *= TMath::RadToDeg()/fGeom->GetConvMm2Deg();

    // Set Source position
    fSrcPos->SetXY(v0);

    /*
    TVector2 v0 = CalcXYinCamera(za1, za0)*fGeom->GetCameraDist()*(-1000);
    TVector2 v1 = CalcXYinCamera(za0, za1)*fGeom->GetCameraDist()*(+1000);

    fSrcPos->SetXY(TVector2(0,0)); // v1

    za0 *= TMath::RadToDeg();
    za1 *= TMath::RadToDeg();

    *fLog << warn << endl;
    *fLog << "-1->  " << za0.X() << " " << za0.Y() << "   " << v0.X() << " " << v0.Y() << "   " << v0.X()*fGeom->GetConvMm2Deg() << " " << v0.Y()*fGeom->GetConvMm2Deg() << endl;
    *fLog << "-2->  " << za1.X() << " " << za1.Y() << "   " << v1.X() << " " << v1.Y() << "   " << v1.X()*fGeom->GetConvMm2Deg() << " " << v1.Y()*fGeom->GetConvMm2Deg() << endl;

    Double_t rho = fPointPos->RotationAngle(*fObservatory, *fTime2);
    *fLog << "-3->  " << rho*TMath::RadToDeg() << endl;

    v1=v1.Rotate(-rho);
    *fLog << "-4->  " << "       " << " " << "        " << "   " << v1.X() << " " << v1.Y() << "   " << v1.X()*fGeom->GetConvMm2Deg() << " " << v1.Y()*fGeom->GetConvMm2Deg() << endl;

    */
    /*
     *fLog << dbg << endl;
     *fLog << "Time: " << setprecision(12) << fTime2->GetMjd() << " " << *fTime2 << endl;
     *fLog << setprecision(6);
     */

    return kTRUE;
}