#include <fstream.h>
#include <iostream.h>
#include <iomanip.h>

#include <TF1.h>
#include <TH1.h>
#include <TH2.h>
#include <TGraph.h>

#include <TList.h>
#include <TMinuit.h>

#include <TView.h>
#include <TLine.h>
#include <TMarker.h>
#include <TCanvas.h>

#include "coord.h"
#include "MBending.h"

//
// Sekans = 1/cos
//

class Set : public TObject
{
    friend ifstream &operator>>(ifstream &fin, Set &set);
private:
    Double_t fStarAz;
    Double_t fStarEl;

    Double_t fRawAz;
    Double_t fRawEl;

public:
    Set(Double_t sel=0, Double_t saz=0, Double_t rel=0, Double_t raz=0) :
        fStarAz(saz*TMath::Pi()/180),
        fStarEl(sel*TMath::Pi()/180),
        fRawAz(raz*TMath::Pi()/180),
        fRawEl(rel*TMath::Pi()/180)
    {
    }

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

         Double_t dist = acos(d);

         return dist * 180 / TMath::Pi();
    }

    void operator=(Set &set)
    {
        fStarAz = set.fStarAz;
        fStarEl = set.fStarEl;
        fRawAz  = set.fRawAz;
        fRawEl  = set.fRawEl;
    }

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

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

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

    void AdjustEl(Double_t del) { fStarEl += del*TMath::Pi()/180; }
    void AdjustAz(Double_t daz) { fStarAz += daz*TMath::Pi()/180; }

    void Adjust(const MBending &bend)
    {
        AltAz p = bend(GetStarAltAz());
        fStarEl = p.Alt();
        fStarAz = p.Az();
    }
    void AdjustBack(const MBending &bend)
    {
        AltAz p = bend.CorrectBack(GetRawAltAz());
        fRawEl = p.Alt();
        fRawAz = p.Az();
    }
};

ifstream &operator>>(ifstream &fin, Set &set)
{
    Double_t v[4];
    fin >> v[0];
    fin >> v[1];
    fin >> v[2];
    fin >> v[3];

    Double_t dummy;
    fin>>dummy;
    fin>>dummy;
    fin>>dummy;

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

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

    return fin;
}

TList gCoordinates;

void fcn(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag)
{
    f = 0;

    MBending bend;
    bend.SetParameters(par); // Set Parameters [deg] to MBending

    for (int i=0; i<gCoordinates.GetSize(); i++)
    {
        Set set = *(Set*)gCoordinates.At(i);

        set.Adjust(bend);

        Double_t err = 1;
        Double_t res = set.GetResidual()/err;

        f += res*res;
    }

    f /= gCoordinates.GetSize()*gCoordinates.GetSize();
}

void DrawMarker(TVirtualPad *pad, Double_t r0, Double_t phi0, Double_t r1, Double_t phi1)
{
    TView *view = pad->GetView();

    if (!view)
    {
        cout << "No View!" << endl;
        return;
    }

    TMarker mark0;
    TMarker mark1;
    mark0.SetMarkerStyle(kStar);
    mark1.SetMarkerStyle(kStar);
    mark1.SetMarkerColor(kRed);

    r0 /= 90;
    r1 /= 90;
    phi0 *= TMath::Pi()/180;
    phi1 *= TMath::Pi()/180;

    Double_t x0[3] = { r0*cos(phi0), r0*sin(phi0), 0};
    Double_t x1[3] = { r1*cos(phi1), r1*sin(phi1), 0};

    Double_t y0[3], y1[3];

    view->WCtoNDC(x0, y0);
    view->WCtoNDC(x1, y1);

    mark0.DrawMarker(y0[0], y0[1]);
    mark1.DrawMarker(y1[0], y1[1]);
}

void DrawPolLine(TVirtualPad *pad, Double_t r0, Double_t phi0, Double_t r1, Double_t phi1)
{
    TView *view = pad->GetView();

    if (!view)
    {
        cout << "No View!" << endl;
        return;
    }
    /*
    if (r0<0)
    {
        r0 = -r0;
        phi0 += 180;
    }
    if (r1<0)
    {
        r1 = -r1;
        phi1 += 180;
    }

    phi0 = fmod(phi0+360, 360);
    phi1 = fmod(phi1+360, 360);

    if (phi1-phi0<-180)
        phi1+=360;
    */
    TLine line;
    line.SetLineWidth(2);
    line.SetLineColor(kBlue);

    Double_t p0 = phi0<phi1?phi0:phi1;
    Double_t p1 = phi0<phi1?phi1:phi0;

    if (phi0>phi1)
    {
        Double_t d = r1;
        r1 = r0;
        r0 = d;
    }

    r0 /= 90;
    r1 /= 90;

    Double_t dr = r1-r0;
    Double_t dp = p1-p0;

    Double_t x0[3] = { r0*cos(p0*TMath::Pi()/180), r0*sin(p0*TMath::Pi()/180), 0};

    for (double i=p0+10; i<p1+10; i+=10)
    {
        if (i>p1)
            i=p1;

        Double_t r = dr/dp*(i-p0)+r0;
        Double_t p = TMath::Pi()/180*i;

        Double_t x1[3] = { r*cos(p), r*sin(p), 0};

        Double_t y0[3], y1[3];

        view->WCtoNDC(x0, y0);
        view->WCtoNDC(x1, y1);

        line.DrawLine(y0[0], y0[1], y1[0], y1[1]);

        x0[0] = x1[0];
        x0[1] = x1[1];
    }
}

void DrawSet(TVirtualPad *pad, Set &set, Float_t scale=-1)
{
    Double_t r0   = set.GetRawZd();
    Double_t phi0 = set.GetRawAz();
    Double_t r1   = set.GetStarZd();
    Double_t phi1 = set.GetStarAz();

    if (r0<0)
    {
        r0 = -r0;
        phi0 += 180;
    }
    if (r1<0)
    {
        r1 = -r1;
        phi1 += 180;
    }

    phi0 = fmod(phi0+360, 360);
    phi1 = fmod(phi1+360, 360);

    if (phi1-phi0<-180)
        phi1+=360;

    if (scale<0 || scale>1000)
        scale = -1;

    if (scale>0)
    {
        Double_t d = r1-r0;
        r0 += scale*d;
        r1 -= scale*d;
        d = phi1-phi0;
        phi0 += scale*d;
        phi1 -= scale*d;

        DrawPolLine(pad, r0, phi0, r1, phi1);
        DrawMarker(pad,  r0, phi0, r1, phi1);
    }
    else
        DrawMarker(pad,  r1, phi1, 0 ,0);
}

void tpointfit()
{
    cout << "Starting..." << endl;

    gCoordinates.SetOwner();

    TH1F hres1("Res1", "  Residuals before correction     ", 10, 0,   4);
    TH1F hres2("Res2", "  Residuals after correction     ",  40, 0,   4);

    hres1.SetXTitle("\\Delta [\\circ]");
    hres1.SetYTitle("Counts");

    hres2.SetXTitle("\\Delta [\\circ]");
    hres2.SetYTitle("Counts");

    TGraph gdaz;
    TGraph gdzd;
    TGraph gaz;
    TGraph gzd;

    gdaz.SetTitle(" \\Delta Az vs. Zd ");
    gdzd.SetTitle(" \\Delta Zd vs. Az ");

    gaz.SetTitle(" \\Delta Az vs. Az ");
    gzd.SetTitle(" \\Delta Zd vs. Zd ");

    ifstream fin("tpoint.txt");

    while (fin && fin.get()!='\n');
    while (fin && fin.get()!='\n');
    while (fin && fin.get()!='\n');
    if (!fin)
    {
        cout << "File not found!" << endl;
        return;
    }

    while (1)
    {
        Set set;

        fin >> set;  // Read data from file [deg], it is stored in [rad]

        if (!fin)
            break;

        gCoordinates.Add(new Set(set));
    }

    cout << "Found " << gCoordinates.GetSize() << " sets of coordinates." << endl;

    TMinuit minuit(16);  //initialize TMinuit with a maximum of 5 params
    minuit.SetPrintLevel(-1);
    minuit.SetFCN(fcn);

    MBending bending;
    bending.SetMinuitParameters(minuit, 16); // Init Parameters [deg]

    //minuit.FixParameter(0);  //(1) IA
    //minuit.FixParameter(1);  //(1) IE
    minuit.FixParameter(2);  //(2) NPAE
    //minuit.FixParameter(3);  //(1) CA
    minuit.FixParameter(4);  //(2) AN
    //minuit.FixParameter(5);  //(1) AW

    minuit.FixParameter(6);  // NRX
    minuit.FixParameter(7);  // NRY
    minuit.FixParameter(8);  // CRX
    minuit.FixParameter(9);  // CRY
    minuit.FixParameter(10); // ECES
    minuit.FixParameter(11); //(2) ACES
    minuit.FixParameter(12); // ECEC
    minuit.FixParameter(13); //(2) ACEC
    minuit.FixParameter(14); // MAGIC1
    minuit.FixParameter(15); // MAGIC2


    //minuit.Command("SHOW PARAMETERS");
    //minuit.Command("SHOW LIMITS");
    cout << endl;

    Int_t ierflg = 0;
    ierflg = minuit.Migrad();
    cout << "Migrad returns " << ierflg << endl;
    // minuit.Release(2);
    ierflg = minuit.Migrad();
    cout << "Migrad returns " << ierflg << endl << endl;

    //
    // Get Fit Results
    //
    bending.GetMinuitParameters(minuit);
    bending.PrintMinuitParameters(minuit);
    bending.Save("bending_magic.txt");


    //
    // Make a copy of all list entries
    //
    TList list;
    list.SetOwner();
    for (int i=0; i<gCoordinates.GetSize(); i++)
        list.Add(new Set(*(Set*)gCoordinates.At(i)));

    //
    // Correct for Offsets only
    //
    double par[16];
    bending.GetParameters(par);
    for (int i=2; i<16; i++) par[i]=0;
    MBending b2;
    b2.SetParameters(par);

    //
    // Calculate correction and residuals
    //
    for (int i=0; i<gCoordinates.GetSize(); i++)
    {
        Set &set0 = *(Set*)gCoordinates.At(i);

        ZdAz za = set0.GetStarZdAz()*kRad2Deg;

        //
        // Correct for offsets only
        //
        Set set1(set0);
        set1.Adjust(b2);

        hres1.Fill(set1.GetResidual());

        set0.Adjust(bending);
        hres2.Fill(set0.GetResidual());

        Double_t dz = fmod(set0.GetDAz()+720, 360);
        if (dz>180)
            dz -= 360;

        static int j=0;
        gdzd.SetPoint(j, za.Az(), set0.GetDZd());
        gaz.SetPoint( j, za.Az(), dz);
        gdaz.SetPoint(j, za.Zd(), dz);
        gzd.SetPoint( j, za.Zd(), set0.GetDZd());
        j++;
    }

    //
    // Check for overflows
    //
    const Stat_t ov = hres2.GetBinContent(hres2.GetNbinsX()+1);
    if (ov>0)
        cout << "WARNING: " << ov << " overflows in residuals." << endl;

    //
    // Print all data sets for which the backward correction is
    // twice times worse than the residual gotten from the
    // bending correction itself
    //
    cout << endl;
    cout << "Checking backward correction (raw-->star):" << endl;
    for (int i=0; i<gCoordinates.GetSize(); i++)
    {
        Set set0(*(Set*)list.At(i));
        Set set1(*(Set*)list.At(i));

        set0.AdjustBack(bending);
        set1.Adjust(bending);

        Double_t res0 = set0.GetResidual();
        Double_t res1 = set1.GetResidual();

        Double_t diff = fabs(res0-res1);

        if (diff<hres2.GetMean()*0.66)
            continue;

        cout << "DBack: " << setw(7) << set0.GetStarZd() << " " << setw(8) << set0.GetStarAz() << ":  ";
        cout << "ResB="<< setw(7) << res0*60 << "  ResF=" << setw(7) << res1*60 << "  |ResB-ResF|=" << setw(7) << diff*60 << " arcmin" << endl;
    }
    cout << "OK." << endl;

    //
    // Print out the residual before and after correction in several
    // units
    //
    cout << endl;
    cout << gCoordinates.GetSize() << " data sets." << endl << endl;
    cout << "Spead before: " << hres1.GetMean() << "deg  +-  " << hres1.GetRMS() << "deg" << endl;
    cout << "Spead after:  " << hres2.GetMean() << "deg  +-  " << hres2.GetRMS() << "deg" << endl;
    cout << "Spead before: " << (int)(hres1.GetMean()*60+.5) << "'  +-  " << (int)(hres1.GetRMS()*60+.5) << "'" << endl;
    cout << "Spead before: " << (int)(hres1.GetMean()*60*60/23.4+.5) << "pix  +-  " << (int)(hres1.GetRMS()*60*60/23.4+.5) << "pix" << endl;
    cout << "Spead after:  " << (int)(hres2.GetMean()*60+.5) << "'  +-  " << (int)(hres2.GetRMS()*60+.5) << "'" << endl;
    cout << "Spead after:  " << (int)(hres2.GetMean()*60*60/23.4+.5) << "pix  +-  " << (int)(hres2.GetRMS()*60*60/23.4+.5) << "pix" << endl;
    cout << "Spead after:  " << (int)(hres2.GetMean()*16384/360+.5) << "SE  +-  " << (int)(hres2.GetRMS()*16384/360+.5) << "SE" << endl;
    cout << endl;



    TCanvas *c1=new TCanvas("c1", "c");
    c1->Divide(2,2);

    c1->cd(2);
    gdaz.DrawClone("A*");

    c1->cd(1);
    gaz.DrawClone("A*");

    c1->cd(3);
    gdzd.DrawClone("A*");

    c1->cd(4);
    gzd.DrawClone("A*");



    c1=new TCanvas("c2", "c", 800, 800);
    c1->Divide(2,2);

    c1->cd(2);
    hres1.SetLineColor(kRed);
    hres1.DrawCopy();

    c1->cd(4);
    hres2.SetLineColor(kBlue);
    hres2.DrawCopy();

    c1->cd(1);
    gPad->SetTheta(90);
    gPad->SetPhi(90);
    TH2F h2res1("Res2D1", "   Arb. Residuals before correction (scaled)     ", 32, 0, 360,  10, 0, 90);
    h2res1.DrawCopy("surf1pol");
    gPad->Modified();
    gPad->Update();
    for (int i=0; i<gCoordinates.GetSize(); i++)
        DrawSet(gPad, *(Set*)list.At(i));//, 10./hres1.GetMean());

    c1->cd(3);
    gPad->SetTheta(90);
    gPad->SetPhi(90);
    h2res1.SetTitle("   Arb. Residuals after correction (scaled)     ");
    h2res1.DrawCopy("surf1pol");
    gPad->Modified();
    gPad->Update();
    for (int i=0; i<gCoordinates.GetSize(); i++)
        DrawSet(gPad, *(Set*)gCoordinates.At(i), 10./hres2.GetMean());

}