/* ======================================================================== *\
!
! *
! * 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>
!              Abelardo Moralejo 1/2005 <mailto:moralejo@pd.infn.it>
!
!   Copyright: MAGIC Software Development, 2000-2004
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
// 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:
//   - wobble mode missing   /// NOTE, A. Moralejo: I see no need for special code
//
//   - 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 "MParList.h"

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

#include "MRawRunHeader.h"
#include "MObservatory.h"
#include "MSrcPosCam.h"
#include "MAstroSky2Local.h"
#include "MPointingPos.h"
#include "MGeomCam.h"
#include "MVector3.h"

ClassImp(MSrcPosCalc);

using namespace std;

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

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


    fSourcePos = (MPointingPos*)pList->FindObject("MSourcePos", "MPointingPos");
    if (!fSourcePos)
    {
        *fLog << warn << "MSourcePos [MPointPos] not found... The source position" << endl;
        *fLog << warn << "set in MSrcPosCam (camera center if not set explicitely) will" << endl;
        *fLog << warn << "be left unchanged, same for all events." << endl;
        return kSKIP;
    }

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

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

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

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

    // 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 MSrcPosCalc::Process()
{
  //     *fLog << dbg << "Camera center : Zd=" << fPointPos->GetZd() << " Az=" << fPointPos->GetAz() << endl;
  //     *fLog << dbg << "Camera center : RA=" << fPointPos->GetRa() << " Dec=" << fPointPos->GetDec() << endl;
  //     *fLog << dbg << "MJD: " << fTime->GetMjd() << ", time [ms]: " << fTime->GetTime() << endl;

  MVector3 pos, pos0;  // pos: source position;  pos0: camera center

  if (fSourcePos)
    {
      // 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.

      pos.SetRaDec(fSourcePos->GetRaRad(), fSourcePos->GetDecRad());

      // 	*fLog << dbg << "Sky position of source: Ra=" << fSourcePos->GetRa() << 
      // 	  " Dec=" << fSourcePos->GetDec() << endl;
    }

  // 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!

  pos0.SetRaDec(fPointPos->GetRaRad(), fPointPos->GetDecRad());
  pos0 *= conv;

  //     *fLog << dbg << "From MAstroSky2Local, without precession and nutation corrections:" << endl;
  //     *fLog << dbg << "- Camera center (2) from RA,dec and time : Zd=" << pos0.Theta()*180./TMath::Pi() 
  //  	  << " Az=" << pos0.Phi()*180./TMath::Pi() << endl;
  //     *fLog << dbg << "- Local position of source: Zd=" << pos.Theta()*TMath::RadToDeg() << " Az=" << pos.Phi()*TMath::RadToDeg() << endl;


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

  fSrcPosCam->SetXY(v);

  //     v *= fGeom->GetConvMm2Deg();
  //     *fLog << dbg << "X=" << v.X() << " deg,  Y=" << v.Y() << " deg" << endl;
  //     *fLog << *fTime << endl;
  //     *fLog << endl;

  return kTRUE;
}