#include "TPointStar.h"

#include <TMath.h>

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

#include "MPointing.h"

ClassImp(TPointStar);

using namespace std;

void TPointStar::Init(const char *name, const char *title)
{
    fName  = name  ? name  : "TPointStar";
    fTitle = title ? title : "A set of TPoints";
}

TPointStar::TPointStar(Double_t sel, Double_t saz, Double_t rel, Double_t raz) :
    fStarAz(saz*TMath::DegToRad()),
    fStarEl(sel*TMath::DegToRad()),
    fRawAz(raz*TMath::DegToRad()),
    fRawEl(rel*TMath::DegToRad()), fMag(-25)
{
    Init();
}

Double_t TPointStar::GetDEl() const     { return (fRawEl-fStarEl)*TMath::RadToDeg(); }
Double_t TPointStar::GetDZd() const     { return -GetDEl(); }
Double_t TPointStar::GetDAz() const     { return (fRawAz-fStarAz)*TMath::RadToDeg(); }
Double_t TPointStar::GetStarEl() const  { return fStarEl*TMath::RadToDeg(); }
Double_t TPointStar::GetStarZd() const  { return 90.-fStarEl*TMath::RadToDeg(); }
Double_t TPointStar::GetStarAz() const  { return fStarAz*TMath::RadToDeg(); }
Double_t TPointStar::GetRawEl() const   { return fRawEl*TMath::RadToDeg(); }
Double_t TPointStar::GetRawAz() const   { return fRawAz*TMath::RadToDeg(); }
Double_t TPointStar::GetRawZd() const   { return 90.-fRawEl*TMath::RadToDeg(); }

ZdAz  TPointStar::GetStarZdAz() const   { return ZdAz(TMath::Pi()/2-fStarEl, fStarAz); }
AltAz TPointStar::GetStarAltAz() const  { return AltAz(fStarEl, fStarAz); }

ZdAz  TPointStar::GetRawZdAz() const    { return ZdAz(TMath::Pi()/2-fRawEl, fRawAz); }
AltAz TPointStar::GetRawAltAz() const   { return AltAz(fRawEl, fRawAz); }

void TPointStar::AdjustEl(Double_t del) { fStarEl += del*TMath::DegToRad(); }
void TPointStar::AdjustAz(Double_t daz) { fStarAz += daz*TMath::DegToRad(); }

void TPointStar::Adjust(const MPointing &bend)
{
    AltAz p = bend(GetStarAltAz());
    fStarEl = p.Alt();
    fStarAz = p.Az();
}

void TPointStar::AdjustBack(const MPointing &bend)
{
    AltAz p = bend.CorrectBack(GetRawAltAz());
    fRawEl = p.Alt();
    fRawAz = p.Az();
}

Double_t TPointStar::GetResidual(Double_t *err) const
    {
      /*
       TVector3 v1, v2;
       v1.SetMagThetaPhi(1, TMath::Pi()/2-fRawEl, fRawAz);
       v2.SetMagThetaPhi(1, TMath::Pi()/2-fStarEl, fStarAz);

       return v1.Angle(v2)*TMath::RadToDeg();
       */

        const Double_t del = fRawEl-fStarEl;
        const Double_t daz = fRawAz-fStarAz;

        /*
        const Double_t dphi2 = daz/2.;
        const Double_t cos2  = cos(dphi2)*cos(dphi2);
        const Double_t sin2  = sin(dphi2)*sin(dphi2);
        const Double_t d = cos(del)*cos2 - cos(fRawEl+fStarEl)*sin2;
        */
        const Double_t d  = cos(del) - cos(fRawEl)*cos(fStarEl)*(1.-cos(daz));

        if (err)
        {
            // Error of one pixel in the CCD
            const Double_t e1 = 32./3600*TMath::DegToRad()   * 0.5;

            // Error of one SE unit
            const Double_t e2 = 360./16384*TMath::DegToRad() * 0.5;

            const Double_t e11 =  sin(del)+cos(fRawEl)*sin(fStarEl)*(1-cos(daz));
            const Double_t e12 =  cos(fRawEl)*cos(fStarEl)*sin(daz);

            const Double_t e21 = -sin(del)+sin(fRawEl)*cos(fStarEl)*(1-cos(daz));
            const Double_t e22 = -cos(fRawEl)*cos(fStarEl)*sin(daz);

            const Double_t err1  = sqrt(1-d*d);
            const Double_t err2  = (e11*e11 + e12*e12)*e1*e1;
            const Double_t err3  = (e21*e21 + e22*e22)*e2*e2;

            *err = sqrt(err2+err3)/err1 * TMath::RadToDeg();
        }

        const Double_t dist = acos(d);
        return dist * TMath::RadToDeg();
    }

istream &operator>>(istream &fin, TPointStar &set)
{
    TString str;
    do
    {
        str.ReadLine(fin);
        if (!fin)
            return fin;
    } while (str[0]=='#');

    Float_t v[4], mag;
    Int_t n = sscanf(str.Data(), "%f %f %f %f %*f %*f %*f %*f %*f %*f %f", v, v+1, v+2, v+3, &mag);
    if (n<4)
    {
        gLog << err << "Read: ERROR - Not enough numbers" << endl;
        return fin;
    }
    set.fMag = n<5 ? -25 : mag;

    set.fStarAz = v[0]*TMath::DegToRad();
    set.fStarEl = v[1]*TMath::DegToRad();

    set.fRawAz  = v[2]*TMath::DegToRad();
    set.fRawEl  = v[3]*TMath::DegToRad();

    if (fin)
    {
        Double_t res, err;
        res = set.GetResidual(&err);
        gLog << inf << "Read: " << v[0] << " " << v[1] << "  :  " << v[2] << " " << v[3] << "  :  " << v[2]-v[0] << " " << v[3]-v[1] << "  :  " << res << " " << err << " " << err/res << endl;
    }

    return fin;
}

ostream &operator<<(ostream &out, TPointStar &set)
{
    out << Form("%8.3f", set.fStarAz*TMath::RadToDeg()) << " ";
    out << Form("%7.3f", set.fStarEl*TMath::RadToDeg()) << "   ";
    out << Form("%8.3f", set.fRawAz*TMath::RadToDeg()) << " ";
    out << Form("%7.3f", set.fRawEl*TMath::RadToDeg()) << "   ";
    out << Form("%6.3f", set.fMag);

    return out;
}