#include "MMoonPointing.h"

#include <TMath.h>
#include <TFile.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*TMath::DegToRad())),
        fSinOffset(offset<=0?0:TMath::Sin(offset*TMath::DegToRad())),
        fCosAngle(0), fSinAngle(0)
    {
    }

    MPositionOffsetCalc(Double_t offset, Double_t angle) :
        fCosOffset(offset<=0?0:TMath::Cos(offset*TMath::DegToRad())),
        fSinOffset(offset<=0?0:TMath::Sin(offset*TMath::DegToRad())),
        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*TMath::DegToRad())),
        fSinOffset(offset<=0?0:TMath::Sin(offset*TMath::DegToRad())),
        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
    {
        if (fCosOffset==0 && fSinOffset==0)
        {
            if (sn)
                *sn = fSinAngle;
            if (cs)
                *cs = fCosAngle;
            return pos;
        }

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

        if (sinzd<=0)
            return ZdAz();

        const Double_t costheta = coszd*fCosOffset + sinzd*fSinOffset*fCosAngle;
        if (costheta*costheta >= 1)
            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();
        return;
    }

    if (fCos.Read("cosAngle")<=0)
    {
        MakeZombie();
        return;
    }

    if (fSin.Read("sinAngle")<=0)
    {
        MakeZombie();
        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 fcos = const_cast<TGraph2D&>(fCos).Interpolate(moon.Az(), moon.Zd());
    const Double_t fsin = const_cast<TGraph2D&>(fSin).Interpolate(moon.Az(), moon.Zd());
    if (fcos*fcos + fsin*fsin<0.98)
        return kFALSE;

    // Calculate Moon Shadow Position:
    Double_t sn=fsin;
    Double_t cs=fcos;

    const MPositionOffsetCalc calc1(fOffsetShadow, fcos, fsin);
    srcpos = calc1.Calc(moon, &sn, &cs);
    if (srcpos.Zd()==0)
        return kFALSE;

    // Calaculate Telescope pointing position:
    const MPositionOffsetCalc calc2(fOffsetWobble, cs, sn);
    pointpos = calc2.Calc(srcpos);
    if (pointpos.Zd()==0)
        return kFALSE;

    return kTRUE;
}


