/* ======================================================================== *\
!
! *
! * 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): Wolfgang Wittek 07/2004 <mailto:wittek@mppmu.mpg.de>
!
!   Copyright: MAGIC Software Development, 2000-2004
!
!
\* ======================================================================== */

/////////////////////////////////////////////////////////////////////////////
//
//  MTelAxisFromStars
//
//  This task 
//  - determines the transformation from expected positions of stars
//    in the camera to measured positions of these stars in the camera
//  - applies this transformation to expected positions of other objects 
//    to obtain the estimated positions of these objects in the camera
//  - puts the estimated positions into the relevant containers
//
//  Input Containers :
//   MStarCam[MStarCam], MStarCamSource[MStarCam]
//
//  Output Containers :
//   MSkyCamTrans, MSrcPosCam 
//
/////////////////////////////////////////////////////////////////////////////
#include <TMath.h>
#include <TList.h>
#include <TSystem.h>

#include <fstream>

#include "MTelAxisFromStars.h"

#include "MParList.h"


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

#include "MReportDrive.h"
#include "MPointingPos.h"
#include "MSrcPosCam.h"

#include "MStarCam.h"
#include "MStarPos.h"
#include "MSkyCamTrans.h"
#include "MStarCamTrans.h"

ClassImp(MTelAxisFromStars);

using namespace std;

// --------------------------------------------------------------------------
//
// Constructor
//
MTelAxisFromStars::MTelAxisFromStars(const char *name, const char *title)
{
    fName  = name  ? name  : "MTelAxisFromStars";
    fTitle = title ? title : "Calculate source position from star positions";

    // if scale factor fLambda should NOT be fixed set fFixdScaleFactor to 
    // -1.0; otherwise set it to the value requested
    fFixedScaleFactor   = 1.0;

    // if rotation angle fAlfa should NOT be fixed set fFixdRotationAngle to 
    // -1.0; otherwise set it to the requested value
    fFixedRotationAngle = 0.0;

    // default type of input is :    the result of the Gauss fit
    // type 0 : result from the weighted average
    // type 1 : result from the Gauss fit
    fInputType = 1;

    // default value of fAberr
    // the value 1.07 is valid if the expected position (with aberration)
    // in the camera is calculated as the average of the positions obtained
    // from the reflections at the individual mirrors
    fAberr = 1.07;

}

// --------------------------------------------------------------------------
//
// Destructor
//
MTelAxisFromStars::~MTelAxisFromStars()
{
  delete fStarCamTrans;
}

// --------------------------------------------------------------------------
//
// Set links to containers
//

Int_t MTelAxisFromStars::PreProcess(MParList *pList)
{
   fDrive = (MReportDrive*)pList->FindObject(AddSerialNumber("MReportDrive"));
   if (!fDrive)
   {
       *fLog << err << AddSerialNumber("MReportDrive") 
             << " not found... aborting." << endl;
       return kFALSE;
   }
 
  
   fStarCam = (MStarCam*)pList->FindObject("MStarCam", "MStarCam");
   if (!fStarCam)
   {
       *fLog << err << "MStarCam not found... aborting." << endl;
       return kFALSE;
   }

   
   fSourceCam = (MStarCam*)pList->FindObject("MSourceCam", "MStarCam");
   if (!fSourceCam)
   {
       *fLog << warn << "MSourceCam[MStarCam] not found... continue  " << endl;
   }


   //-----------------------------------------------------------------
    fSrcPos = (MSrcPosCam*)pList->FindCreateObj(AddSerialNumber("MSrcPosCam"), "MSrcPosCam");
    if (!fSrcPos)
        return kFALSE;

    fPntPos = (MSrcPosCam*)pList->FindCreateObj(AddSerialNumber("MSrcPosCam"), "MPntPosCam");
    if (!fPntPos)
        return kFALSE;

    fPointPos = (MPointingPos*)pList->FindCreateObj(AddSerialNumber("MPointingPos"), "MPointingPos");
    if (!fPointPos)
        return kFALSE;

    fSourcePos = (MPointingPos*)pList->FindCreateObj(AddSerialNumber("MPointingPos"), "MSourcePos");
    if (!fSourcePos)
        return kFALSE;

    fSkyCamTrans = (MSkyCamTrans*)pList->FindCreateObj(AddSerialNumber("MSkyCamTrans"));
    if (!fSkyCamTrans)
        return kFALSE;


   //-----------------------------------------------------------------
   // book an MStarCamTrans object
   // this is needed when calling one of the member functions of MStarCamTrans

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

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

   //-----------------------------------------------------------------
    fStarCamTrans = new MStarCamTrans(*geom, *obs);

   *fLog << all   << "MTelAxisFromStars::Preprocess; the optical aberration factor is set equal to : " 
         << fAberr ;

   *fLog << all   << "MTelAxisFromStars::Preprocess; input type is set equal to : " 
         << fInputType ;
   if (fInputType == 0)
     *fLog << " (calculated star positions)" << endl;
   else
     *fLog << " (fitted star positions)" << endl;

   *fLog << all << "MTelAxisFromStars::Preprocess; scale factor will be fixed at : " 
         << fFixedScaleFactor << endl;

   *fLog << all << "MTelAxisFromStars::Preprocess; rotation angle will be fixed at : " 
         << fFixedRotationAngle << endl;

    return kTRUE;
}

// --------------------------------------------------------------------------
//
// Set optical aberration factor
//
//   fAberr   is the ratio between 
//            the distance from the camera center with optical aberration and
//            the distance from the camera center with an ideal imaging
//
//   fAberr = r_real/r_ideal 
//
void MTelAxisFromStars::SetOpticalAberr(Double_t aberr)
{
  fAberr = aberr;
}

// --------------------------------------------------------------------------
//
// Set the type of the input
//
//     type = 0        calculated star positions (by averaging)
//     type = 1        fitted star positions (by Gauss fit)
//
void MTelAxisFromStars::SetInputType(Int_t type)
{
  fInputType = type;
}

// --------------------------------------------------------------------------
//
// Fix the scale factor fLambda
//
//
void MTelAxisFromStars::FixScaleFactorAt(Double_t lambda)
{
  fFixedScaleFactor = lambda;
}


// --------------------------------------------------------------------------
//
// Fix rotation angle fAlfa
//
//
void MTelAxisFromStars::FixRotationAngleAt(Double_t alfa)
{
  fFixedRotationAngle = alfa;    // [degrees]
}


// --------------------------------------------------------------------------
//
// Process
//
//           call FindSkyCamTrans    to find the Sky-Camera transformation
//           call TransSkyCam        to transform some sky directions
//                                   into the camera system
//           put the estimated source position into MSrcPosCam
//

Int_t MTelAxisFromStars::Process()
{
  //Int_t run = fRun->GetRunNumber();
  //*fLog << "MTelAxisFromStars::Process; run = " << run << endl;

  //--------------------------------------
  // Define the input for FindSkyCamTrans
  //

  // get the expected (axy[0], axy[1]) and the measured positions 
  // (bxy[0], bxy[1]) of stars in the camera from MStarCam 
  Int_t fNumStars = fStarCam->GetNumStars();

  if (fNumStars <= 0){
      *fLog << err << "No stars found!" << endl;
      return kTRUE;
  }

  TArrayD  axy[2];
  axy[0].Set(fNumStars);
  axy[1].Set(fNumStars);

  TArrayD  bxy[2];
  bxy[0].Set(fNumStars);
  bxy[1].Set(fNumStars);

  // error matrix of bxy
  TArrayD  exy[2][2];
  exy[0][0].Set(fNumStars);
  exy[0][1].Set(fNumStars);
  exy[1][0].Set(fNumStars);
  exy[1][1].Set(fNumStars);

  // transformation parameters
  Double_t fLambda;
  Double_t fAlfa;
  Double_t fA[2][2];
  Double_t fD[2];
  Double_t fErrD[2][2];
  Int_t    fNumIter;
  Int_t    fNdof;
  Double_t fChi2;
  Double_t fChi2Prob;

  MStarPos *star = 0;
  TIter next(fStarCam->GetList());
  Int_t ix = 0;

  // loop over all stars
  while ( (star = (MStarPos*)next()) )
  {
    axy[0][ix]   = star->GetXExp(); 
    axy[1][ix]   = star->GetYExp(); 

    if (fInputType == 0)
    {
      // values from averaging
      bxy[0][ix]     = star->GetMeanXCalc(); 
      bxy[1][ix]     = star->GetMeanYCalc(); 

      // this is the error matrix for (MeanXCalc, MeanYCalc);
      // this is the error matrix which should be used
      exy[0][0][ix]  =  star->GetXXErrCalc(); 
      exy[0][1][ix]  =  star->GetXYErrCalc();
      exy[1][0][ix]  =  star->GetXYErrCalc();
      exy[1][1][ix]  =  star->GetYYErrCalc();

      //exy[0][0][ix]  = star->GetSigmaXCalc()*star->GetSigmaXCalc(); 
      //exy[0][1][ix]  =  0.0;
      //exy[1][0][ix]  =  0.0;
      //exy[1][1][ix]  = star->GetSigmaYCalc()*star->GetSigmaYCalc(); 
    }

    else if (fInputType == 1)
    {
      // values from Gauss fit
      bxy[0][ix]   = star->GetMeanXFit(); 
      bxy[1][ix]   = star->GetMeanYFit(); 

      // this is the error matrix for (MeanXFit, MeanYFit);
      // this is the error matrix which should be used
      exy[0][0][ix]  =  star->GetXXErrFit(); 
      exy[0][1][ix]  =  star->GetXYErrFit();
      exy[1][0][ix]  =  star->GetXYErrFit();
      exy[1][1][ix]  =  star->GetYYErrFit();

      // this is the error matrix constructed from SigmaXFit and SigmaYFit;
      // it is used because the errors above are too small, at present
      //exy[0][0][ix]  =  star->GetSigmaXFit() * star->GetSigmaXFit(); 
      //exy[0][1][ix]  =  star->GetCorrXYFit() * 
      //                  star->GetSigmaXFit() * star->GetSigmaYFit();
      //exy[1][0][ix]  =  exy[0][1][ix];
      //exy[1][1][ix]  =  star->GetSigmaYFit() * star->GetSigmaYFit();
    }

    else
    {
      *fLog << err << "MTelAxisFromStars::Process;  type of input is not defined" 
            << endl;
      return kFALSE;
    }   

    // don't include stars with undefined error
    Double_t deter =   exy[0][0][ix]*exy[1][1][ix]
                     - exy[0][1][ix]*exy[1][0][ix];

    //*fLog << "ix ,deter, xx, xy, yy = " << ix << ":    "
    //      << deter << ",  " << exy[0][0][ix] << ",  "
    //      << exy[0][1][ix] << ",  " << exy[1][1][ix] << endl;
    if (deter <= 0.0)
      continue;

    //*fLog << "MTelAxisFromStars : " << endl;
    //*fLog << "      ix, XExp, YExp, XFit, YFit, SigmaX2, SigmaXY, SigmaY2 = " 
    //      << ix << " :  " 
    //      << axy[0][ix] << ",  " << axy[1][ix] << ",  "
    //      << bxy[0][ix] << ",  " << bxy[1][ix] << ",  "
    //      << exy[0][0][ix] << ",  " << exy[0][1][ix] << ",  " 
    //      << exy[1][1][ix] << endl;

    ix++;
  }

  //--------------------------------------
  // Find the transformation from expected positions (axy[1], axy[2]) 
  // to measured positions (bxy[1], bxy[2]) in the camera

  Int_t fNStars = ix;

  if (ix < fNumStars)
  {
    // reset the sizes of the arrays
    Int_t fNStars = ix;
    axy[0].Set(fNStars);
    axy[1].Set(fNStars);

    bxy[0].Set(fNStars);
    bxy[1].Set(fNStars);

    exy[0][0].Set(fNStars);
    exy[0][1].Set(fNStars);
    exy[1][0].Set(fNStars);
    exy[1][1].Set(fNStars);
  }

  Bool_t fitOK;
  if (fNStars < 1)
  {
    //*fLog << "MTelAxisFromStars::Process; no star for MTelAxisFromStars"
    //      << endl;
    fitOK = kFALSE;
  }
  else
  {
    fitOK =  FindSkyCamTrans(axy,      bxy,   exy,   
               fFixedRotationAngle,    fFixedScaleFactor, fLambda,
               fAlfa  ,  fA,    fD,    fErrD,
               fNumIter, fNdof, fChi2, fChi2Prob);
  }

  if (!fitOK  &&  fNStars >= 1)
  {
    *fLog << err 
          << "MTelAxisFromStars::Process;  Fit to find transformation from star to camera system failed" 
          << endl;
    
    //if (fNStars > 0)
    //{
    //  *fLog << err 
    //        << "    fNumIter, fNdof, fChi2, fChi2Prob = " << fNumIter
    //        << ",  " << fNdof << ",  " << fChi2 << ",  " << fChi2Prob << endl;
    //}

    return kTRUE;
  }


  //--------------------------------------
  // Put the transformation parameters into the MSkyCamTrans container

  fSkyCamTrans->SetParameters(fLambda,   fAlfa,    fA,    fD,    fErrD,
                              fNumStars, fNumIter, fNdof, fChi2, fChi2Prob);
  fSkyCamTrans->SetReadyToSave();

  //--------------------------------------
  // Put the camera position (X, Y) 
  //         obtained by transforming the camera center (0, 0)
  // into MPntPosCam[MSrcPosCam]

  fPntPos->SetXY(fD[0], fD[1]);
  fPntPos->SetReadyToSave();       


  //--------------------------------------
  // Put the corrected pointing position into MPointingPos 
  //
  SetPointingPosition(fStarCamTrans, fDrive, fSkyCamTrans, fPointPos);


  //--------------------------------------
  // Put the estimated position of the source into SrcPosCam
  //
  // get the source direction from MReportDrive
  // Note : this has to be changed for the wobble mode, where the source 
  //        isn't in the center of the camera
  Double_t decsource = fDrive->GetDec();
  Double_t rasource  = fDrive->GetRa();
  //
  Double_t radrive = fDrive->GetRa();
  Double_t hdrive  = fDrive->GetHa();
  Double_t hsource = hdrive + radrive - rasource;
  fSourcePos->SetSkyPosition(rasource, decsource, hsource);

  SetSourcePosition(fStarCamTrans, fPointPos, fSourcePos, fSrcPos);

  *fLog << "after calling SetSourcePosition : , X, Y ,fD = " 
        << fSrcPos->GetX() << ",  " << fSrcPos->GetY() << ",  "
        << fD[0] << ",  " << fD[1] << endl;

  //--------------------------------------
  // Apply the transformation to some expected positions (asxy[1], asxy[2]) 
  // to obtain estimated positions (bsxy[1], bsxy[2]) in the camera 
  //      and their error matrices esxy[2][2]

  // get the expected positions (asxy[1], asxy[2]) from another MStarCam 
  // container (with the name "MSourceCam") 
  Int_t fNumStarsSource = 0; 

  if (fSourceCam)
    fNumStarsSource = fSourceCam->GetNumStars();

  //*fLog << "MTelAxisFromStars::Process;  fNumStarsSource = " 
  //      << fNumStarsSource << endl;

  if (fNumStarsSource > 0)
  {
    TArrayD  asxy[2];
    asxy[0].Set(fNumStarsSource);
    asxy[1].Set(fNumStarsSource);

    TArrayD  bsxy[2];
    bsxy[0].Set(fNumStarsSource);
    bsxy[1].Set(fNumStarsSource);

    TArrayD  esxy[2][2];
    esxy[0][0].Set(fNumStarsSource);
    esxy[0][1].Set(fNumStarsSource);
    esxy[1][0].Set(fNumStarsSource);
    esxy[1][1].Set(fNumStarsSource);

    MStarPos *starSource = 0;
    TIter nextSource(fSourceCam->GetList());
    ix = 0;
    while ( (starSource = (MStarPos*)nextSource()) )
    {
      asxy[0][ix]  = starSource->GetXExp(); 
      asxy[1][ix]  = starSource->GetYExp(); 

      ix++;
    }

    TransSkyCam(fLambda, fA, fD, fErrD, asxy, bsxy,  esxy);   

    // put the estimated positions into the MStarCam container
    // with name "MSourceCam"
    TIter setnextSource(fSourceCam->GetList());
    ix = 0;
    while ( (starSource = (MStarPos*)setnextSource()) )
    {
       Double_t corr = esxy[0][1][ix] / 
                       TMath::Sqrt( esxy[0][0][ix] * esxy[1][1][ix] );
       if (fInputType == 1)
       {
         starSource->SetFitValues(100.0, 100.0, bsxy[0][ix], bsxy[1][ix],
	       TMath::Sqrt(esxy[0][0][ix]), TMath::Sqrt(esxy[1][1][ix]), corr,
               esxy[0][0][ix],       esxy[0][1][ix],       esxy[1][1][ix], 
               fChi2, fNdof); 
       }
       else
       {
         starSource->SetCalcValues(100.0, 100.0, bsxy[0][ix], bsxy[1][ix],
	       TMath::Sqrt(esxy[0][0][ix]), TMath::Sqrt(esxy[1][1][ix]), corr,
               esxy[0][0][ix],       esxy[0][1][ix],       esxy[1][1][ix]); 
       }

      ix++;
    }

  }

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

  return kTRUE;
}



//---------------------------------------------------------------------------
//
// FindSkyCamTrans
//
// This routine determines the transformation 
//
//                                      ( cos(alfa)   -sin(alfa) )
//      b = lambda * A * a + d      A = (                        )
//             ^     ^       ^          ( sin(alfa)    cos(alfa) )
//             |     |       |
//           scale rotation shift 
//          factor matrix
//
// from sky coordinates 'a' (projected onto the camera) to camera 
// coordinates 'b', using the positions of known stars in the camera. 
// The latter positions may have been determined by analysing the 
// DC currents in the different pixels.
//
// Input  : a[2]    x and y coordinates of stars projected onto the camera;
//                  they were obtained from (RA, dec) of the stars and
//                  (ThetaTel, PhiTel) and the time of observation;
//                  these are the 'expected positions' of stars in the camera
//          b[2]    'measured positions' of these stars in the camera;
//                  they may have been obtained from the DC currents
//          e[2][2] error matrix of b[2]
//          fixedrotationangle  value [in degrees] at which rotation angle 
//                              alfa should be fixed; -1 means don't fix
//          fixedscalefactor    value at which scale factor lambda  
//                              should be fixed; -1 means don't fix
//
// Output : lambda, alfadeg, A[2][2], d[2]   fit results;
//                               parameters describing the transformation 
//                               from 'expected positions' to the 'measured 
//                               positions' in the camera
//          errd[2][2]           error matrix of d[2]
//          fNumIter             number of iterations
//          fNdoF                number of degrees of freedom
//          fChi2                chi-square value
//          fChi2Prob            chi-square probability
//
// The units are assumed to be 
//     [degrees]  for  alfadeg
//     [mm]       for  a, b, d
//     [1]        for  lambda                       

Bool_t MTelAxisFromStars::FindSkyCamTrans(
   TArrayD a[2],      TArrayD b[2],     TArrayD e[2][2], 
   Double_t &fixedrotationang, Double_t &fixedscalefac,  Double_t &lambda,  
   Double_t &alfadeg, Double_t A[2][2], Double_t d[2],   Double_t errd[2][2],
   Int_t &fNumIter,   Int_t &fNdof,     Double_t &fChi2, Double_t &fChi2Prob)
{
  Int_t fNumStars = a[0].GetSize();

  //*fLog << "MTelAxisFromStars::FindSkyCamTrans; expected and measured positions :"
  //        << endl;
  for (Int_t ix=0; ix<fNumStars; ix++)
  {
    //*fLog << "    ix, a[0], a[1], b[0], b[1], errxx, errxy, erryy = " 
    //        << ix << " :   " 
    //        << a[0][ix] << ",  " << a[1][ix] << ";    " 
    //        << b[0][ix] << ",  " << b[1][ix] << ";    " 
    //        << e[0][0][ix] << ",  " << e[0][1][ix] << ",  "
    //        << e[1][1][ix] << endl;
  }


  //-------------------------------------------
  // fix some parameters if the number of degrees of freedom is too low
  // (<= 0.0)

  Double_t fixedscalefactor   = fixedscalefac;
  Double_t fixedrotationangle = fixedrotationang;

  // calculate number of degrees of freedom
  fNdof = 2 * fNumStars - 4;
  if (fixedscalefactor != -1.0)
     fNdof += 1;
  if (fixedrotationangle != -1.0)
     fNdof += 1;

  // if there is only 1 star fix both rotation angle and scale factor
  if (fNumStars == 1)
  {
     if (fixedscalefactor == -1.0)
     {  
       fixedscalefactor = 1.0;
       *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; scale factor is fixed at "
	     << fixedscalefactor << endl;
     }
     if (fixedrotationangle == -1.0)
     {
       fixedrotationangle = 0.0;
       *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; rotation angle is fixed at "
	     << fixedrotationangle << endl;
     }
  }
  // otherwise fix only 1 parameter if possible
  else if (fNdof < 0)
  {
    if (fNdof < -2)
    {
      *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; number of degrees of freedom is too low : " 
            << fNdof << ";  fNumStars = " << fNumStars << endl;
      return kFALSE;
    }
    else if (fNdof == -2)
    {
      if (fixedscalefactor == -1.0  &&  fixedrotationangle == -1.0)
      {
        fixedscalefactor   = 1.0;
        fixedrotationangle = 0.0;
        *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; scale factor and rotation angle are fixed at " 
              << fixedscalefactor << "  and  " << fixedrotationangle 
              << " respectively" << endl;
      }
      else
      {
        *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; number of degrees of freedom is too low : " 
              << fNdof << ";  fNumStars = " << fNumStars << endl;
        return kFALSE;
      }
    }
    else if (fNdof == -1)
    {
      if (fixedrotationangle == -1.0)
      {
        fixedrotationangle = 0.0;
        *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; rotation angle is fixed at "
	      << fixedrotationangle << endl;
      }
      else if (fixedscalefactor == -1.0)
      {
        fixedscalefactor = 1.0;
        *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; scale factor is fixed at "
	      << fixedscalefactor << endl;
      }
      else
      {
        *fLog << warn << "MTelAxisFromStars::FindSkyCamTrans; number of degrees of freedom is too low : " 
              << fNdof << ";  fNumStars = " << fNumStars<< endl;
        return kFALSE;
      }
    }
  }

  // recalculate number of degrees of freedom
  fNdof = 2 * fNumStars - 4;
  if (fixedscalefactor != -1.0)
     fNdof += 1;
  if (fixedrotationangle != -1.0)
     fNdof += 1;

  if (fNdof < 0)
    return kFALSE;
  //-------------------------------------------


  // get first approximation of scaling factor
  if (fixedscalefactor != -1.0)
    lambda = fixedscalefactor;
  else
    lambda = 1.0;

  Double_t lambdaold = lambda;
  Double_t dlambda = 0.0;

  // get first approximation of rotation angle
  Double_t alfa = 0.0;
  if (fixedrotationangle != -1.0)
    alfa = fixedrotationangle / TMath::RadToDeg();

  Double_t alfaold   = alfa;
  // maximum allowed change of alfa in 1 iteration step (5 degrees)
  Double_t dalfamax = 5.0 / TMath::RadToDeg();
  Double_t dalfa = 0.0;

  Double_t cosal = TMath::Cos(alfa);
  Double_t sinal = TMath::Sin(alfa);

  A[0][0] =  cosal;
  A[0][1] = -sinal;
  A[1][0] =  sinal;
  A[1][1] =  cosal;


  Double_t absdold2    = 10000.0;
  Double_t fChangeofd2 = 10000.0;


  TArrayD Aa[2];
  Aa[0].Set(fNumStars);
  Aa[1].Set(fNumStars);


  Double_t sumEbminlamAa[2];

  TArrayD Ebminlambracd[2];
  Ebminlambracd[0].Set(fNumStars);
  Ebminlambracd[1].Set(fNumStars);

  TArrayD EAa[2];
  EAa[0].Set(fNumStars);
  EAa[1].Set(fNumStars);

  // invert the error matrices
  TArrayD  c[2][2];
  c[0][0].Set(fNumStars);
  c[0][1].Set(fNumStars);
  c[1][0].Set(fNumStars);
  c[1][1].Set(fNumStars);

  for (Int_t ix=0; ix<fNumStars; ix++)
  {
    Double_t XX = e[0][0][ix]; 
    Double_t XY = e[0][1][ix]; 
    Double_t YY = e[1][1][ix]; 

    // get inverse of error matrix
    Double_t determ = XX*YY - XY*XY;
    c[0][0][ix]  =  YY / determ; 
    c[0][1][ix]  = -XY / determ; 
    c[1][0][ix]  = -XY / determ;
    c[1][1][ix]  =  XX / determ; 
  }



  // calculate sum of inverted error matrices
  Double_t determsumc;
  Double_t sumc[2][2];
  sumc[0][0] = 0.0;
  sumc[0][1] = 0.0;
  sumc[1][0] = 0.0;
  sumc[1][1] = 0.0;

  for (Int_t ix=0; ix<fNumStars; ix++)
  {
       sumc[0][0]  += c[0][0][ix]; 
       sumc[0][1]  += c[0][1][ix]; 
       sumc[1][0]  += c[1][0][ix]; 
       sumc[1][1]  += c[1][1][ix]; 
  }
  determsumc = sumc[0][0]*sumc[1][1] - sumc[0][1]*sumc[1][0];

  // calculate inverse of sum of inverted error matrices
  Double_t sumcinv[2][2];
  sumcinv[0][0]  =  sumc[1][1] / determsumc; 
  sumcinv[0][1]  = -sumc[0][1] / determsumc; 
  sumcinv[1][0]  = -sumc[1][0] / determsumc;
  sumcinv[1][1]  =  sumc[0][0] / determsumc; 

  //*fLog << "sumcinv = " << sumcinv[0][0] << ",  " << sumcinv[0][1] 
  //      << ",  " << sumcinv[1][1] << endl;


  // minimize chi2 by iteration *****   start   **********************
  
  // stop iteration when change in |d|*|d| is less than 'told2'
  //                and  change in alfa    is less than 'toldalfa'
  //                and  change in lambda  is less than 'toldlambda'
  //                 or  chi2              is less than 'tolchi2'
  Double_t told2 = 0.3*0.3;  // [mm*mm]; 1/100 of an inner pixel diameter
  Double_t toldalfa = 0.01 / TMath::RadToDeg();  // 0.01 degrees 
  Double_t toldlambda = 0.00006;   // uncertainty of 1 mm of distance 
                                   // between camera and reflector
  Double_t tolchi2 = 1.e-5;

  Int_t fNumIterMax  = 100;
  fNumIter           =   0;

  for (Int_t i=0; i<fNumIterMax; i++)
  {
    fNumIter++;

    // get next approximation of d  ------------------
    for (Int_t ix=0; ix<fNumStars; ix++)
    {
        Aa[0][ix] = A[0][0] * a[0][ix]  +  A[0][1]*a[1][ix];
        Aa[1][ix] = A[1][0] * a[0][ix]  +  A[1][1]*a[1][ix];

        //*fLog << "ix, Aa = " << ix << " :  " << Aa[0][ix] << ",  "
        //      << Aa[1][ix] << endl;
    }

    sumEbminlamAa[0] = 0.0;
    sumEbminlamAa[1] = 0.0;

    for (Int_t ix=0; ix<fNumStars; ix++)
    {
      sumEbminlamAa[0] +=   c[0][0][ix] * (b[0][ix] - lambda*Aa[0][ix])
                          + c[0][1][ix] * (b[1][ix] - lambda*Aa[1][ix]);

      sumEbminlamAa[1] +=   c[1][0][ix] * (b[0][ix] - lambda*Aa[0][ix])
                          + c[1][1][ix] * (b[1][ix] - lambda*Aa[1][ix]);
    }

    //*fLog << "sumEbminlamAa = " << sumEbminlamAa[0] << ",  "
    //      << sumEbminlamAa[1] << endl;

    d[0] =      sumcinv[0][0] * sumEbminlamAa[0]
	      + sumcinv[0][1] * sumEbminlamAa[1] ;

    d[1] =      sumcinv[1][0] * sumEbminlamAa[0] 
              + sumcinv[1][1] * sumEbminlamAa[1] ;

    Double_t absdnew2 = d[0]*d[0] + d[1]*d[1]; 
    fChangeofd2 = absdnew2 - absdold2;

    //*fLog << "fNumIter : " << fNumIter 
    //      << "; alfa, lambda, d[0], d[1], absdold2, absdnew2 = " << endl; 
    //*fLog << alfa << ",  " << lambda << ",  " << d[0] << ",  " << d[1] 
    //      << ",  " << absdold2 << ",  " << absdnew2 << endl;
     

    if ( fabs(fChangeofd2) < told2   &&  fabs(dalfa) < toldalfa  &&
         fabs(dlambda)     < toldlambda ) 
    {
      //*fLog << "Iteration stopped because of small changes : fChangeofd2, dalfa, dlambda = "
      //      << fChangeofd2 << ",  " << dalfa << ",  "  << dlambda << endl;
      break;
    }
    absdold2 = absdnew2;

    // get next approximation of matrix A  ----------------
    if (fFixedRotationAngle == -1.0)
    {     
        for (Int_t ix=0; ix<fNumStars; ix++)
        {
          Ebminlambracd[0][ix] =   
             c[0][0][ix] * ( b[0][ix] - lambda*Aa[0][ix] - d[0] )
           + c[0][1][ix] * ( b[1][ix] - lambda*Aa[1][ix] - d[1] );

          Ebminlambracd[1][ix] =   
             c[1][0][ix] * ( b[0][ix] - lambda*Aa[0][ix] - d[0] )
           + c[1][1][ix] * ( b[1][ix] - lambda*Aa[1][ix] - d[1] );

          //*fLog << "ix, Ebminlambracd = " << ix << " :  "
          //      << Ebminlambracd[0][ix] << ",  "
          //      << Ebminlambracd[1][ix] << endl;
        }

        // stop iteration if fChi2 is small enough
        fChi2 = 0.0;
        for (Int_t ix=0; ix<fNumStars; ix++)
        {
          fChi2 +=  (b[0][ix]-lambda*Aa[0][ix]-d[0] ) * Ebminlambracd[0][ix]
                  + (b[1][ix]-lambda*Aa[1][ix]-d[1] ) * Ebminlambracd[1][ix];
        }
        if ( fChi2 < tolchi2 ) 
        {
          //*fLog << "iteration stopped because of small fChi2 :  "
          //      << fChi2 << endl;
          break;
        }


        Double_t dchi2dA[2][2];
        dchi2dA[0][0] = 0.0; 
        dchi2dA[0][1] = 0.0;
        dchi2dA[1][0] = 0.0;
        dchi2dA[1][1] = 0.0;
		
        for (Int_t ix=0; ix<fNumStars; ix++)
        {
          dchi2dA[0][0] += Ebminlambracd[0][ix] * a[0][ix];
          dchi2dA[0][1] += Ebminlambracd[0][ix] * a[1][ix];
          dchi2dA[1][0] += Ebminlambracd[1][ix] * a[0][ix];
          dchi2dA[1][1] += Ebminlambracd[1][ix] * a[1][ix];
        }

        //*fLog << "dchi2dA = " << dchi2dA[0][0] << ",  " << dchi2dA[0][1]
        //      << ",  " << dchi2dA[1][0] << ",  " << dchi2dA[1][1] << endl;

        // *********  1st derivative (d chi2) / (d alfa) ************
        Double_t dchi2dalfa =   -2.0*lambda * 
                              ( - sinal*(dchi2dA[0][0]+dchi2dA[1][1])
                                + cosal*(dchi2dA[1][0]-dchi2dA[0][1]) );
        

        //Double_t dalfa1st = - fChi2 / dchi2dalfa;

        //*fLog << "fChi2, dchi2dalfa = " << fChi2 << ",  " 
        //      << dchi2dalfa << endl;
        //*fLog << "proposed change of alfa using 1st derivative = " 
        //      << dalfa1st << endl;

        // *********  2nd derivative (d2 chi2) / (d alfa2) ******
        Double_t term1 = 0.0;
        Double_t term2 = 0.0;
        Double_t term3 = 0.0;
        Double_t term4 = 0.0;

        for (Int_t ix=0; ix<fNumStars; ix++)
        {
          term1 += a[0][ix]*c[0][0][ix]*a[0][ix] + a[1][ix]*c[1][0][ix]*a[0][ix]
	         + a[0][ix]*c[0][1][ix]*a[1][ix] + a[1][ix]*c[1][1][ix]*a[1][ix];

          term2 += a[0][ix]*c[1][0][ix]*a[0][ix] - a[1][ix]*c[0][0][ix]*a[0][ix]
	         + a[0][ix]*c[1][1][ix]*a[1][ix] - a[1][ix]*c[0][1][ix]*a[1][ix];

          term3 = a[0][ix]*c[0][0][ix]*a[1][ix] + a[1][ix]*c[1][0][ix]*a[1][ix]
	         - a[0][ix]*c[0][1][ix]*a[0][ix] - a[1][ix]*c[1][1][ix]*a[0][ix];

          term4 += a[0][ix]*c[1][0][ix]*a[1][ix] - a[1][ix]*c[0][0][ix]*a[1][ix]
	         - a[0][ix]*c[1][1][ix]*a[0][ix] + a[1][ix]*c[0][1][ix]*a[0][ix];
        }

        Double_t d2chi2dalfa2 = 
                  - 2.0*lambda * ( - cosal*(dchi2dA[0][0]+dchi2dA[1][1])
                                   - sinal*(dchi2dA[1][0]-dchi2dA[0][1]) )
	   + 2.0*lambda*lambda * ( sinal*sinal * term1 - sinal*cosal * term2
		  	         + sinal*cosal * term3 - cosal*cosal * term4);

        // Gauss-Newton step
        Double_t dalfa2nd = - dchi2dalfa / d2chi2dalfa2;

        //*fLog << "proposed change of alfa using 2st derivative = " 
        //    << dalfa2nd << endl;

        //dalfa = dalfa1st;
        dalfa = dalfa2nd;

        // ******************************************


        // restrict change of alfa
        if ( fabs(dalfa) > dalfamax )
        {
          dalfa = TMath::Sign( dalfamax, dalfa ); 
        }
        alfa = alfaold + dalfa;

        if ( alfa < -5.0/TMath::RadToDeg() )
          alfa = -5.0/TMath::RadToDeg();
        else if ( alfa > 5.0/TMath::RadToDeg() )
          alfa = 5.0/TMath::RadToDeg();
        
        dalfa = alfa - alfaold;

        alfaold = alfa;

        sinal = TMath::Sin(alfa);
        cosal = TMath::Cos(alfa);

        A[0][0] =  cosal;
        A[0][1] = -sinal;
        A[1][0] =  sinal;
        A[1][1] =  cosal;

        //*fLog << "alfa-alfaold = " << dalfa << endl;
        //*fLog << "new alfa = " << alfa << endl;
    }


    // get next approximation of lambda  ----------------
    if (fFixedScaleFactor == -1.0)
    {
      for (Int_t ix=0; ix<fNumStars; ix++)
      {
        Aa[0][ix] = A[0][0]*a[0][ix] + A[0][1]*a[1][ix];
        Aa[1][ix] = A[1][0]*a[0][ix] + A[1][1]*a[1][ix];

        EAa[0][ix] =   
           c[0][0][ix] * Aa[0][ix]  +  c[0][1][ix] * Aa[1][ix];
        EAa[1][ix] =   
           c[1][0][ix] * Aa[0][ix]  +  c[1][1][ix] * Aa[1][ix];

        //*fLog << "ix, Aa = " << ix << " :  " << Aa[0][ix] << ",  "
        //      << Aa[1][ix] << endl;

        //*fLog << "ix, EAa = " << ix << " :  " << EAa[0][ix] << ",  "
        //      << EAa[1][ix] << endl;
      }

      Double_t num   = 0.0;
      Double_t denom = 0.0;
      for (Int_t ix=0; ix<fNumStars; ix++)
      {
        num    +=   (b[0][ix]-d[0]) * EAa[0][ix] 
                  + (b[1][ix]-d[1]) * EAa[1][ix];

        denom  +=    Aa[0][ix]      * EAa[0][ix]  
                  +  Aa[1][ix]      * EAa[1][ix]; 

        //*fLog << "ix : b-d = " << ix << " :  " << b[0][ix]-d[0]
	//      << ",  " << b[1][ix]-d[1] << endl;

        //*fLog << "ix : Aa = " << ix << " :  " << Aa[0][ix]
	//      << ",  " << Aa[1][ix] << endl;
      }

      lambda = num / denom;

      if ( lambda < 0.9 )
        lambda = 0.9;
      else if ( lambda > 1.1 )
        lambda = 1.1;

      dlambda = lambda - lambdaold;
      lambdaold = lambda;

      //*fLog << "num, denom, lambda, dlambda = " << num
      //      << ",  " << denom << ",  " << lambda << ",  "
      //      << dlambda << endl;
    }

  }
  //-------   end of iteration *****************************************

  alfadeg = alfa * TMath::RadToDeg();

  // calculate error matrix of d[2]
  errd[0][0] = sumcinv[0][0];
  errd[0][1] = sumcinv[0][1];
  errd[1][0] = sumcinv[1][0];
  errd[1][1] = sumcinv[1][1];

  // evaluate quality of fit

  // calculate chi2
  for (Int_t ix=0; ix<fNumStars; ix++)
  {
    Ebminlambracd[0][ix] =   
       c[0][0][ix] * ( b[0][ix] - lambda*Aa[0][ix] - d[0] )
     + c[0][1][ix] * ( b[1][ix] - lambda*Aa[1][ix] - d[1] );

    Ebminlambracd[1][ix] =   
       c[1][0][ix] * (b[0][ix] - lambda*Aa[0][ix] - d[0] )
     + c[1][1][ix] * (b[1][ix] - lambda*Aa[1][ix] - d[1] );
  }

  fChi2 = 0.0;
  for (Int_t ix=0; ix<fNumStars; ix++)
  {
    fChi2 +=  (b[0][ix] - lambda*Aa[0][ix] - d[0] ) * Ebminlambracd[0][ix]
            + (b[1][ix] - lambda*Aa[1][ix] - d[1] ) * Ebminlambracd[1][ix];
  }

  fChi2Prob = TMath::Prob(fChi2, fNdof);

  //*fLog << "MTelAxisFromStars::FindSkyCamTrans :" << endl;
  //*fLog <<  "   fNumStars, fChi2, fNdof, fChi2Prob, fNumIter, fChangeofd2, dalfa, dlambda = "
  //      << fNumStars << ",  " << fChi2 << ",  " << fNdof << ",  " 
  //      << fChi2Prob << ",  " 
  //      << fNumIter << ",  "  << fChangeofd2 << ",  " << dalfa << ",  "
  //      << dlambda << endl;
  //*fLog << "    lambda, alfadeg, d[0], d[1] = " << lambda << ",  " 
  //      << alfadeg   << ",  " << d[0] << ",  " << d[1] << endl;

  return kTRUE;
}

// --------------------------------------------------------------------------
//
// Apply transformation (lambda, A, d)
//       to the        expected  positions (a[1], a[2]) 
//       to obtain the estimated positions (b[1], b[2])
//
//       e[2][2]       is the error matrix of b[2]

void MTelAxisFromStars::TransSkyCam(
    Double_t &lambda, Double_t A[2][2], Double_t d[2], Double_t errd[2][2],
    TArrayD a[2],     TArrayD b[2],     TArrayD e[2][2])   
{
  Int_t numpos = a[0].GetSize();
  if (numpos <= 0)
    return;

  //*fLog << "MTelAxisFromStars::TransSkyCam; lambda, A, d = " 
  //      << lambda << ";    " << A[0][0] << ",  " << A[0][1] << ",  "
  //      << A[1][1] << ";   " << d[0] << ",  " << d[1] << endl;   

  //*fLog << "MTelAxisFromStars::TransSkyCam; expected and estimated positions :"
  //      << endl;

  for (Int_t ix=0; ix<numpos; ix++)
  {
    //    *fLog << "MTelAxisFromStars;  ix = " << ix << endl;

      b[0][ix] = lambda * (A[0][0]*a[0][ix] + A[0][1]*a[1][ix]) + d[0];
      b[1][ix] = lambda * (A[1][0]*a[0][ix] + A[1][1]*a[1][ix]) + d[1];

      e[0][0][ix] = errd[0][0];
      e[0][1][ix] = errd[0][1];
      e[1][0][ix] = errd[1][0];
      e[1][1][ix] = errd[1][1];

      // *fLog << "    ix, a[0], a[1], b[0], b[1], errxx, errxy, erryy = " 
      //      << ix << " :   " 
      //      << a[0][ix] << ",  " << a[1][ix] << ";    " 
      //      << b[0][ix] << ",  " << b[1][ix] << ";    " 
      //      << e[0][0][ix] << ",  " << e[0][1][ix] << ",  "
      //      << e[1][1][ix] << endl;
  }
}

// --------------------------------------------------------------------------
//
// SetPointingPosition
//
//    put the corrected pointing direction into MPointingPos[MPointingPos];
//    this direction corresponds to the position (0,0) in the camera
//

Bool_t MTelAxisFromStars::SetPointingPosition(MStarCamTrans *fstarcamtrans,
       MReportDrive *fdrive, MSkyCamTrans *ftrans, MPointingPos *fpointpos)
{
  Double_t decdrive = fdrive->GetDec();
  Double_t hdrive   = fdrive->GetHa();
  Double_t radrive  = fdrive->GetRa();

  // this is the estimated position (with optical aberration) in the camera 
  // corresponding to the direction in MReportDrive
  Double_t Xpoint = (ftrans->GetShiftD())[0];    
  Double_t Ypoint = (ftrans->GetShiftD())[1];    

  // get the sky direction corresponding to the position (0,0) in the camera
  Double_t decpoint = 0.0;
  Double_t hpoint   = 0.0;
  fstarcamtrans->CelCamToCel0(decdrive, hdrive, 
                         Xpoint/fAberr, Ypoint/fAberr, decpoint, hpoint);
  Double_t rapoint = radrive - hpoint + hdrive;
  fpointpos->SetSkyPosition(rapoint, decpoint, hpoint);

  // get the local direction corresponding to the position (0,0) in the camera
  Double_t thetadrive = fdrive->GetNominalZd();
  Double_t phidrive   = fdrive->GetNominalAz();
  Double_t thetapoint = 0.0;
  Double_t phipoint   = 0.0;
  fstarcamtrans->LocCamToLoc0(thetadrive, phidrive, 
               Xpoint/fAberr, Ypoint/fAberr, thetapoint, phipoint); 
  fpointpos->SetLocalPosition(thetapoint, phipoint);
  fpointpos->SetReadyToSave();

  //*fLog << "SetPointingPosition : decdrive, hdrive, radrive Xpoint, Ypoint = "
  //      << decdrive << ",  " << hdrive << ",  " << radrive << ",  "
  //      << Xpoint << ",  " << Ypoint << endl;

  //*fLog << "SetPointingPosition : thetadrive, phidrive, thetapoint, phipoint = "       
  //      << thetadrive << ",  " << phidrive << ",  " << thetapoint << ",  "
  //      << phipoint << endl;

  return kTRUE;
}

// --------------------------------------------------------------------------
//
// SetSourcePosition
//
//    put the estimated position of the source in the camera into 
//    MSrcPosCam[MSrcPosCam]
//
//    and the estimated local direction of the source into
//    MSourcePos[MPointingPos]
//

Bool_t MTelAxisFromStars::SetSourcePosition(MStarCamTrans *fstarcamtrans,
       MPointingPos *fpointpos, MPointingPos *fsourcepos, MSrcPosCam *fsrcpos)
{
  // get the corrected pointing direction
  // corresponding to the position (0,0) in the camera
  Double_t decpoint = fpointpos->GetDec();
  Double_t hpoint   = fpointpos->GetHa();

  // get the sky direction of the source
  Double_t decsource = fsourcepos->GetDec();
  Double_t hsource   = fsourcepos->GetHa();

  // get the estimated position (Xsource, Ysource) of the source in the camera;
  // this is a position for an ideal imaging, without optical aberration
  Double_t Xsource = 0.0;
  Double_t Ysource = 0.0;
  fstarcamtrans->Cel0CelToCam(decpoint,  hpoint, 
                              decsource, hsource, Xsource, Ysource);
  fsrcpos->SetXY(Xsource*fAberr, Ysource*fAberr);
  fsrcpos->SetReadyToSave();

  // get the estimated local direction of the source
  Double_t thetapoint = fpointpos->GetZd();
  Double_t phipoint   = fpointpos->GetAz();
  Double_t thetasource = 0.0;
  Double_t phisource   = 0.0;
  fstarcamtrans->Loc0CamToLoc(thetapoint, phipoint, 
                              Xsource, Ysource, thetasource, phisource);
  fsourcepos->SetLocalPosition(thetasource, phisource);
  fsourcepos->SetReadyToSave();

  //*fLog << "SetSourcePosition : decpoint, hpoint, decsource, hsource, Xsource, Ysource = "
  //      << decpoint << ",  " << hpoint << ",  " << decsource << ",  "
  //      << hsource  << ",  " << Xsource << ",  " << Ysource << endl;
  //*fLog << "SetSourcePosition : thetapoint, phipoint, thetasource, phisource = "
  //      << thetapoint << ",  " << phipoint << ",  " << thetasource << ",  "
  //      << phisource << endl;

  return kTRUE;
}

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