/* ======================================================================== *\
!
! *
! * 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): Roger Firpo   04/2004 <mailto:jlopez@ifae.es>
!   Author(s): Javier López  05/2004 <mailto:jlopez@ifae.es>
!
!   Copyright: MAGIC Software Development, 2000-2004
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
// MSrcPosFromStars
//
// Task to set the position of the source using the positions of the stars
// in the field of view of the source
//
//  Output Containers:
//    MSrcPosCam
//
//////////////////////////////////////////////////////////////////////////////


#include "MSrcPosFromStars.h"

#include <TList.h>

#include "MSrcPosCam.h"
#include "MStarLocalCam.h"
#include "MStarLocalPos.h"

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

#include "MParList.h"

ClassImp(MSrcPosFromStars);

using namespace std;

static const TString gsDefName  = "MSrcPosFromStars";
static const TString gsDefTitle = "task to calculate the position of the source using the position of stars";

// -------------------------------------------------------------------------
//
// Default constructor. 
//
MSrcPosFromStars::MSrcPosFromStars(Float_t first, Float_t second, const char *name, const char *title)
    : fStars(NULL), fNumStars(2), fDistanceFirstStar(first), fDistanceSecondStar(second)
{
  fName  = name  ? name  : gsDefName.Data();
  fTitle = title ? title : gsDefTitle.Data();

}

// -------------------------------------------------------------------------
//
Int_t MSrcPosFromStars::PreProcess(MParList *pList)
{

  if(!MSrcPlace::PreProcess(pList))
    return kFALSE;
  
  fStars = (MStarLocalCam*)pList->FindObject(AddSerialNumber("MStarLocalCam"));
  if (!fStars)
    {
      *fLog << err << AddSerialNumber("MStarLocalCam") << " not found ... aborting" << endl;
      return kFALSE;
    }
  
  return kTRUE;
}

// --------------------------------------------------------------------------
//
// 
Int_t MSrcPosFromStars::ComputeNewSrcPosition()
{

  if (fDistanceFirstStar == 0. || fDistanceSecondStar == 0.)
    {
      if (fStars->GetNumStars() > 0)
        {

          //Look for the star closer to the center of the camera
          TIter Next(fStars->GetList());
          MStarLocalPos* star;
          Float_t mindist = 600; //mm
          UInt_t starnum = 0;
          Int_t select = -1;
          while ((star=(MStarLocalPos*)Next())) 
            {
              Float_t dist = TMath::Sqrt(star->GetMeanX()*star->GetMeanX() +
                                         star->GetMeanY()*star->GetMeanY());
              if (dist < mindist)
                {
                  mindist = dist;
                  select = starnum;
                }
              
              starnum++;
            }

          if (select < 0)
            {
              *fLog << err << "Not found star closer to center" << endl;
              return kFALSE;
            }
          
          MStarLocalPos& selecStar = (*fStars)[select];

          if (selecStar.GetChiSquareNdof() > 0. && selecStar.GetChiSquareNdof() < 10.)
            {
              
              Float_t selecStarPosX = selecStar.GetMeanX();
              Float_t selecStarPosY = selecStar.GetMeanY();
        
              GetOutputSrcPosCam()->SetXY(selecStarPosX,selecStarPosY);
            }
        }
    }
  else if (fStars->GetNumStars() == fNumStars)
    {
      
      MStarLocalPos& firstStar  = (*fStars)[0];
      MStarLocalPos& secondStar = (*fStars)[1];

      if (firstStar.GetChiSquareNdof() > 0. && firstStar.GetChiSquareNdof() < 10. &&
          secondStar.GetChiSquareNdof() > 0. && secondStar.GetChiSquareNdof() < 10.)
        {
      
          Float_t firstStarPosX = firstStar.GetMeanX();
          Float_t firstStarPosY = firstStar.GetMeanY();
          
          Float_t secondStarPosX = secondStar.GetMeanX();
          Float_t secondStarPosY = secondStar.GetMeanY();
          
          Float_t distanceStars = sqrt(pow(firstStarPosX - secondStarPosX,2) + pow(firstStarPosY - secondStarPosY,2));
          Float_t sin_alpha = (secondStarPosY - firstStarPosY) / distanceStars;
          Float_t cos_alpha = (secondStarPosX - firstStarPosX) / distanceStars;
          
          Float_t x = (pow(fDistanceFirstStar,2) - pow(fDistanceSecondStar,2) + pow(distanceStars,2)) / (2 * distanceStars);
          
          Float_t arg = 4 * pow(distanceStars,2) * pow(fDistanceFirstStar,2) - pow(pow(fDistanceFirstStar,2) - pow(fDistanceSecondStar,2) + pow(distanceStars,2),2);
          
          if (arg >= 0.)
            {
              
              Float_t y = sqrt(arg) / (2 * distanceStars);
              
              Float_t xc1 = firstStarPosX + x * cos_alpha - y * sin_alpha;
              Float_t yc1 = firstStarPosY + x * sin_alpha + y * cos_alpha;
              
              Float_t xc2 = firstStarPosX + x * cos_alpha + y * sin_alpha;
              Float_t yc2 = firstStarPosY + x * sin_alpha - y * cos_alpha;
              
              if (sqrt(xc1*xc1 + yc1*yc1) < sqrt(xc2*xc2 + yc2*yc2))
                GetOutputSrcPosCam()->SetXY(xc1,yc1);
              else
                GetOutputSrcPosCam()->SetXY(xc2,yc2);
            }
          else
            *fLog << warn << GetName() << " negative argument ["  << arg << "] for sqrt()" << endl;
          
        }
    }
  
  return kTRUE;
}