#include "MMoonPointing.h"

#include <TMath.h>
#include <TFile.h>

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

ClassImp(MMoonPointing);

using namespace std;

class MPositionOffsetCalc
{
private:
    Double_t fCosOffset;
    Double_t fSinOffset;

    Double_t fCosAngle;
    Double_t fSinAngle;

public:
    MPositionOffsetCalc(Double_t offset) :
        fCosOffset(offset==0 ? 0 : TMath::Cos(offset)),
        fSinOffset(offset==0 ? 0 : TMath::Sin(offset)),
        fCosAngle(0), fSinAngle(0)
    {
    }

    MPositionOffsetCalc(Double_t offset, Double_t angle) :
        fCosOffset(offset==0 ? 0 : TMath::Cos(offset)),
        fSinOffset(offset==0 ? 0 : TMath::Sin(offset)),
        fCosAngle(TMath::Cos(angle*TMath::DegToRad())),
        fSinAngle(TMath::Sin(angle*TMath::DegToRad()))
    {
    }

    MPositionOffsetCalc(Double_t offset, Double_t cos, Double_t sin) :
        fCosOffset(offset==0 ? 0 : TMath::Cos(offset)),
        fSinOffset(offset==0 ? 0 : TMath::Sin(offset)),
        fCosAngle(cos), fSinAngle(sin)
    {
    }

    void SetAngle(Double_t c, Double_t s) { fCosAngle=c; fSinAngle=s; }
    void SetAngle(Double_t a) { fCosAngle=TMath::Cos(a*TMath::DegToRad()); fSinAngle=TMath::Sin(a*TMath::DegToRad()); }

    ZdAz Calc(const ZdAz &pos, Double_t *sn=0, Double_t *cs=0) const
    {
        // This is to avoid unnecessary calculations
        // in case the offset is zero
        if (fCosOffset==0 && fSinOffset==0)
        {
            if (sn)
                *sn = -fCosAngle;
            if (cs)
                *cs =  fSinAngle;
            return pos;
        }

        const Double_t coszd = TMath::Cos(pos.Zd());
        const Double_t sinzd = TMath::Sin(pos.Zd());

        if (sinzd<=0)
        {
            gLog << warn << "WARNING - |Zd|>=90°" << endl;
            return ZdAz();
        }

        const Double_t costheta = coszd*fCosOffset + sinzd*fSinOffset*fCosAngle;
        if (costheta >= 1)
        {
            gLog << warn << "WARNING - cos(Zd) > 1." << endl;
            return ZdAz();
        }

        const Double_t sintheta = TMath::Sqrt(1 - costheta*costheta);

        const Double_t cosdeltaaz = (fCosOffset - coszd*costheta)/(sinzd*sintheta);
        const Double_t sindeltaaz = fSinAngle*fSinOffset/sintheta;

        if (sn)
            *sn = fSinAngle*coszd*sindeltaaz - cosdeltaaz*fCosAngle;
        if (cs)
            *cs = fSinAngle*sinzd/sintheta;

        return ZdAz(TMath::ACos(costheta), TMath::ATan2(sindeltaaz, cosdeltaaz) + pos.Az());
    }

    ZdAz GetOffset(const ZdAz &pos, Double_t *sn=0, Double_t *cs=0) const
    {
        return Calc(pos, sn, cs) - pos;
    }
};

MMoonPointing::MMoonPointing(const char *filename) : fOffsetShadow(-1), fOffsetWobble(-1)
{
    if (TString(filename).IsNull())
    {
        MakeZombie();
        return;
    }

    TFile f(filename);
    if (f.IsZombie())
    {
        MakeZombie();
        gLog << warn << "WARNING - Reading of " << filename << " failed." << endl;
        return;
    }

    if (fCos.Read("cosAngle")<=0)
    {
        MakeZombie();
        gLog << warn << "WARNING - Reading of TGraph2D 'cosAngle' from " << filename << " failed." << endl;
        return;
    }

    if (fSin.Read("sinAngle")<=0)
    {
        MakeZombie();
        gLog << warn << "WARNING - Reading of TGraph2D 'sinAngle' from " << filename << " failed." << endl;
        return;
    }

    fCos.SetDirectory(0);
    fSin.SetDirectory(0);
}

// position of the moon [rad]
// position of the Moon Shadow [rad]
// position of telescope pointing [rad]
Bool_t MMoonPointing::CalcPosition(const ZdAz &moon, ZdAz &srcpos, ZdAz &pointpos) const
{
    const Double_t az = fmod(moon.Az()+TMath::TwoPi(), TMath::TwoPi());

    const Double_t fcos = const_cast<TGraph2D&>(fCos).Interpolate(az, moon.Zd());
    const Double_t fsin = const_cast<TGraph2D&>(fSin).Interpolate(az, moon.Zd());

    // This is a sanity check for the case the Interpolation failed
    const Double_t norm = TMath::Hypot(fcos, fsin);
    if (norm<0.996 || norm>1.001)
    {
        gLog << warn << "WARNING - Local moon position Zd/Az=";
        gLog << moon.Zd()*TMath::RadToDeg() << "/" << moon.Az()*TMath::RadToDeg();
        gLog << " out of range of table for interpolation." << endl;
        gLog << "          fcos=" << fcos << endl;
        gLog << "          fsin=" << fsin << endl;
        gLog << "          norm=" << norm << endl;
        return kFALSE;
    }

    // Apply a small numerical improvement
    Double_t sn=fsin/norm;
    Double_t cs=fcos/norm;

    // Calculate moon shadow position
    const MPositionOffsetCalc calc1(fOffsetShadow, fcos, fsin);
    srcpos = calc1.Calc(moon, &sn, &cs);
    if (srcpos.Zd()==0)
    {
        gLog << warn << "WARNING - Calculation of moon shadow position failed." << endl;
        return kFALSE;
    }

    // Calaculate Telescope pointing position:
    const MPositionOffsetCalc calc2(fOffsetWobble, cs, sn);
    pointpos = calc2.Calc(srcpos);
    if (pointpos.Zd()==0)
    {
        gLog << warn << "WARNING - Calculation of moon shadow's wobble position failed." << endl;
        return kFALSE;
    }

    return kTRUE;
}