/* ======================================================================== *\
!
! *
! * 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 :
//   MStarLocalCam[MStarLocalCam], MStarLocalCamSource[MStarLocalCam]
//
//  Output Containers :
//   MSkyCamTrans, MSrcPosCam 
//
/////////////////////////////////////////////////////////////////////////////
#include <TList.h>
#include <TSystem.h>

#include <fstream>

#include "MTelAxisFromStars.h"

#include "MParList.h"
#include "MSrcPosCam.h"

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

#include "MStarLocalCam.h"
#include "MStarLocalPos.h"
#include "MSkyCamTrans.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 correlated Gauss fit
    // type 0 : result from the weighted average
    // type 1 : result from the uncorrelated Gauss fit
    fInputType = 2;
}

// --------------------------------------------------------------------------
//
// Destructor
//
MTelAxisFromStars::~MTelAxisFromStars()
{
}

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

Int_t MTelAxisFromStars::PreProcess(MParList *pList)
{
  
   fStarLocalCam = (MStarLocalCam*)pList->FindObject("MStarLocalCam", "MStarLocalCam");
   if (!fStarLocalCam)
   {
       *fLog << err << "MTelAxisFromStars::PreProcess; container 'MStarLocalCam' not found... aborting." << endl;
       return kFALSE;
   }

   
   fSourceLocalCam = (MStarLocalCam*)pList->FindObject("MSourceLocalCam", "MStarLocalCam");
   if (!fSourceLocalCam)
   {
       *fLog << err << "MTelAxisFromStars::PreProcess; container 'MSourceLocalCam' not found... continue  " << endl;
   }


    fSrcPos = (MSrcPosCam*)pList->FindCreateObj(AddSerialNumber("MSrcPosCam"));
    if (!fSrcPos)
    {
        *fLog << err << "MTelAxisFromStars::PreProcess;  MSrcPosCam not found...  aborting" << endl;
        return kFALSE;
    }

    fSkyCamTrans = (MSkyCamTrans*)pList->FindCreateObj(AddSerialNumber("MSkyCamTrans"));
    if (!fSkyCamTrans)
    {
        *fLog << err << "MTelAxisFromStars::PreProcess;  MSkyCamTrans not found...  aborting" << endl;
        return kFALSE;
    }


    return kTRUE;
}

// --------------------------------------------------------------------------
//
// Set the type of the input
//
//     type = 0        calculated star positions (by averaging)
//     type = 1        fitted star positions (by uncorrelated Gauss fit)
//     type = 2        fitted star positions (by correlated   Gauss fit)
//
void MTelAxisFromStars::SetInputType(Int_t type)
{
  *fLog << all << "MTelAxisFromStars::SetInputType; input type is set equal to : " 
        << type << endl;

  fInputType = type;
}

// --------------------------------------------------------------------------
//
// Fix the scale factor fLambda
//
//
void MTelAxisFromStars::FixScaleFactorAt(Double_t lambda)
{
  *fLog << all << "MTelAxisFromStars::FixScaleFactorAt; scale factor will be fixed at : " 
        << lambda << endl;

  fFixedScaleFactor = lambda;
}


// --------------------------------------------------------------------------
//
// Fix rotation angle fAlfa
//
//
void MTelAxisFromStars::FixRotationAngleAt(Double_t alfa)
{
  *fLog << all << "MTelAxisFromStars::FixRotationAngleAt; rotation angle will be fixed at : " 
        << alfa << endl;

  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 MStarLocalCam 
  Int_t fNumStars = fStarLocalCam->GetNumStars();

  if (fNumStars <= 0)
    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;

  MStarLocalPos *star = 0;
  TIter next(fStarLocalCam->GetList());
  Int_t ix = 0;

  // loop over all stars
  while ( (star = (MStarLocalPos*)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(); 
      exy[0][0][ix]= star->GetSigmaMinorAxisCalc()*star->GetSigmaMinorAxisCalc(); 
      exy[0][1][ix]  =  0.0;
      exy[1][0][ix]  =  0.0;
      exy[1][1][ix]= star->GetSigmaMajorAxisCalc()*star->GetSigmaMajorAxisCalc(); 
    }

    else if (fInputType == 1)
    {
      // values from uncorrelatd Gauss fit
      bxy[0][ix]   = star->GetMeanXFit(); 
      bxy[1][ix]   = star->GetMeanYFit(); 
      exy[0][0][ix]= star->GetSigmaMinorAxisFit()*star->GetSigmaMinorAxisFit(); 
      exy[0][1][ix]  =  0.0;
      exy[1][0][ix]  =  0.0;
      exy[1][1][ix]= star->GetSigmaMajorAxisFit()*star->GetSigmaMajorAxisFit(); 
    }

    else if (fInputType == 2)
    {
      // values from correlatd Gauss fit
      bxy[0][ix]   = star->GetMeanXCGFit(); 
      bxy[1][ix]   = star->GetMeanYCGFit(); 

      // this is the error matrix for (MeanXCGFit, MeanYCGFit);
      // this is the error matrix which should be used
      exy[0][0][ix]  =  star->GetXXErrCGFit(); 
      exy[0][1][ix]  =  star->GetXYErrCGFit();
      exy[1][0][ix]  =  star->GetXYErrCGFit();
      exy[1][1][ix]  =  star->GetYYErrCGFit();

      // this is the error matrix constructed from SigmaXCGFit and SigmaYCGFit;
      // it is used because the errors above are too small, at present
      //exy[0][0][ix]  =  star->GetSigmaXCGFit() * star->GetSigmaXCGFit(); 
      //exy[0][1][ix]  =  star->GetCorrXYCGFit() * 
      //                  star->GetSigmaXCGFit() * star->GetSigmaYCGFit();
      //exy[1][0][ix]  =  exy[0][1][ix];
      //exy[1][1][ix]  =  star->GetSigmaYCGFit() * star->GetSigmaYCGFit();
    }

    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)
  {
    *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 estimated position, obtained by transforming the expected
  // position (0, 0), into SrcPosCam

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


  //--------------------------------------
  // 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 MStarLocalCam 
  // container (with the name "MSourceLocalCam") 
  Int_t fNumStarsSource = fSourceLocalCam->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);

    MStarLocalPos *starSource = 0;
    TIter nextSource(fSourceLocalCam->GetList());
    ix = 0;
    while ( (starSource = (MStarLocalPos*)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 MStarLocalPos container
    TIter setnextSource(fSourceLocalCam->GetList());
    ix = 0;
    while ( (starSource = (MStarLocalPos*)setnextSource()) )
    {
      if (fInputType == 1)
      {
        starSource->SetFitValues(100.0, 100.0,   bsxy[0][ix],    bsxy[1][ix],
              sqrt(esxy[0][0][ix]), sqrt(esxy[1][1][ix]), fChi2, fNdof); 
      }
      else if (fInputType == 2)
      {
        Double_t corr = esxy[0][1][ix]/ sqrt( esxy[0][0][ix] * esxy[1][1][ix] );
        starSource->SetCGFitValues(100.0, 100.0, bsxy[0][ix], bsxy[1][ix],
	      sqrt(esxy[0][0][ix]), sqrt(esxy[1][1][ix]), corr,
              esxy[0][0][ix], esxy[0][1][ix],  esxy[1][1][ix], 
              fChi2, fNdof); 
      }

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



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

  Double_t cosal = cos(alfa);
  Double_t sinal = 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 / kRad2Deg;  // 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/kRad2Deg )
          alfa = -5.0/kRad2Deg;
        else if ( alfa > 5.0/kRad2Deg )
          alfa = 5.0/kRad2Deg;
        
        dalfa = alfa - alfaold;

        alfaold = alfa;

        sinal = sin(alfa);
        cosal = 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 * kRad2Deg;

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

// --------------------------------------------------------------------------
//
//
Int_t MTelAxisFromStars::PostProcess()
{

    return kTRUE;
}


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












