#include "MDriveCom.h"

#include <iostream>

#include <TObjArray.h>

#include "MAstro.h"
#include "MCosy.h"
#include "MString.h"
#include "Ring.h"
#include "Led.h"

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

#define FACT

using namespace std;

bool MDriveCom::ReadAngle(TString &str, Double_t &ret)
{
    Char_t  sgn;
    Int_t   d, len;
    UInt_t  m;
    Float_t s;

    // Skip whitespaces before %c and after %f
    int n=sscanf(str.Data(), " %c %d %d %f %n", &sgn, &d, &m, &s, &len);

    if (n!=4 || (sgn!='+' && sgn!='-'))
        return false;

    str.Remove(0, len);

    ret = MAstro::Dms2Deg(d, m, s, sgn);
    return true;
}

bool MDriveCom::ReadPosition(TString &str, Double_t &d1, Double_t &d2)
{
    if (!ReadAngle(str, d1))
        return false;

    if (!ReadAngle(str, d2))
        return false;

    return true;
}

bool MDriveCom::CommandRADEC(TString &str)
{
    Double_t ra, dec;
    if (!ReadPosition(str, ra, dec))
    {
        gLog << err << "ERROR - Reading position from RADEC" << endl;
        return false;
    }
    if (!str.IsNull())
    {
        gLog << err << "ERROR - Too many bytes in command RADEC" << endl;
        return false;
    }

    gLog << all << "CC-COMMAND " << MTime(-1) << " RADEC " << ra << "h " << dec << "d" << endl;

    ra *= 15; // h -> deg

    RaDec rd(ra, dec);

    //cout << "MDriveCom - TRACK... start." << endl;
    if (fQueue)
        fQueue->PostMsg(WM_TRACK, &rd, sizeof(rd));
    //cout << "MDriveCom - TRACK... done." << endl;
    return true;
}

bool MDriveCom::CommandGRB(TString &str)
{
    Double_t ra, dec;
    if (!ReadPosition(str, ra, dec))
    {
        gLog << err << "ERROR - Reading position from GRB" << endl;
        return false;
    }
    if (!str.IsNull())
    {
        gLog << err << "ERROR - Too many bytes in command GRB" << endl;
        return false;
    }

    gLog << all << "CC-COMMAND " << MTime(-1) << " GRB " << ra << "h " << dec << "d '" << str << "'" << endl;

    ra *= 15; // h -> deg

    RaDec rd[2] = { RaDec(ra, dec), RaDec(ra, dec) };

    //cout << "MDriveCom - TRACK... start." << endl;
    if (fQueue)
        fQueue->PostMsg(WM_GRB, &rd, sizeof(rd));
    //cout << "MDriveCom - TRACK... done." << endl;
    return true;
}

bool MDriveCom::CommandZDAZ(TString &str)
{
    Double_t zd, az;
    if (!ReadPosition(str, zd, az))
    {
        gLog << err << "ERROR - Reading position from ZDAZ" << endl;
        return false;
    }

    if (!str.IsNull())
    {
        gLog << err << "ERROR - Too many bytes in command ZDAZ" << endl;
        return false;
    }

    gLog << all << "CC-COMMAND " << MTime(-1) << " ZDAZ " << zd << "deg " << az << "deg" << endl;

    ZdAz za(zd, az);

    //cout << "MDriveCom - POSITION... start." << endl;
    if (fQueue)
        fQueue->PostMsg(WM_POSITION, &za, sizeof(za));
    //cout << "MDriveCom - POSITION... done." << endl;
    return true;
}

bool MDriveCom::CommandCELEST(TString &str)
{
    str = str.Strip(TString::kBoth);
    if (str.IsNull())
    {
        gLog << err << "ERROR - CELEST command empty." << endl;
        return false;
    }

    Int_t id, len;
    Float_t offset, angle;
    Int_t n=sscanf(str.Data(), "%d %f %f %n", &id, &offset, &angle, &len);
    if (n!=3)
    {
        gLog << warn << "WARNING - Not enough argmuents (CELEST)." << endl;
        return kCONTINUE;
    }

    str.Remove(0, len);
    str = str.Strip(TString::kBoth);

    if (!str.IsNull())
    {
        gLog << err << "ERROR - Too many bytes in command CELEST" << endl;
        return false;
    }

    gLog << all << "CC-COMMAND " << MTime(-1) << " CELEST ID=" << id << " WobbleOffset=" << offset << "deg WobbleAngle=" << angle << "deg" << endl;

    if (id==0)
    {
        gLog << err << "ERROR - Tracking the sun IS STRICLY FORBIDDEN - ignored." << endl;
        return false;
    }

    if (id<0 || id>9)
    {
        gLog << err << "ERROR - Unknown id " << id << " (must be between 1 and 9)." << endl;
        return false;
    }

    Double_t data[3] = { (double)id, offset, angle };

    //cout << "MDriveCom - CELEST... start." << endl;
    if (fQueue)
        fQueue->PostMsg(WM_CELEST, data, sizeof(Double_t)*3);
    //cout << "MDriveCom - CELEST... done." << endl;
    return true;
}

bool MDriveCom::CommandMOON(TString &str)
{
    str = str.Strip(TString::kBoth);
    if (str.IsNull())
    {
        gLog << err << "ERROR - MOON command empty." << endl;
        return false;
    }

    Int_t len;
    Float_t wobble, offset;
    Int_t n=sscanf(str.Data(), "%f %f %n", &wobble, &offset, &len);
    if (n!=2)
    {
        gLog << warn << "WARNING - Not enough argmuents (MOON)." << endl;
        return kCONTINUE;
    }

    str.Remove(0, len);
    str = str.Strip(TString::kBoth);

    if (!str.IsNull())
    {
        gLog << err << "ERROR - Too many bytes in command MOON" << endl;
        return false;
    }

    gLog << all << "CC-COMMAND " << MTime(-1) << " MOON WobbleOffset=" << wobble << "deg ShadowOffset=" << offset << "deg" << endl;

    Double_t data[2] = { wobble, offset };

    //cout << "MDriveCom - MOON... start." << endl;
    if (fQueue)
        fQueue->PostMsg(WM_MOON, data, sizeof(Double_t)*2);
    //cout << "MDriveCom - MOON... done." << endl;
    return true;
}

bool MDriveCom::CommandPREPS(TString &str)
{
    str = str.Strip(TString::kBoth);
    if (str.IsNull())
    {
        gLog << err << "ERROR - No identifier for preposition (PREPS) given." << endl;
        return false;
    }
    if (str.First(' ')>=0)
    {
        gLog << err << "ERROR - PREPS command syntax error (contains whitespaces)." << endl;
        return false;
    }

    str.ToLower();

    gLog << all << "CC-COMMAND " << MTime(-1) << " PREPS '" << str << "'" << endl;

    //cout << "MDriveCom - TRACK... start." << endl;
    if (fQueue)
        fQueue->PostMsg(WM_PREPS, (void*)str.Data(), str.Length()+1);
    //cout << "MDriveCom - TRACK... done." << endl;
    return true;
}

bool MDriveCom::CommandTPOINT(TString &str)
{
    gLog << all << "CC-COMMAND " << MTime(-1) << " TPOIN " << str << endl;

    TObjArray *arr = str.Tokenize(' ');

    if (arr->GetEntries()!=2)
    {
        delete arr;
        gLog << err << "ERROR - Wrong number of arguments in TPOIN command" << endl;
        return false;
    }

    delete arr;

    if (fQueue)
        fQueue->Proc(WM_STARGTPOINT, (void*)str.Data());

    return true;
}

bool MDriveCom::CommandSTGMD(TString &str)
{
    gLog << all << "CC-COMMAND " << MTime(-1) << " STGMD " << str << endl;

    bool on = str=="ON";

    if (fQueue)
        fQueue->Proc(WM_STARGMODE, &on);

    return true;
}

bool MDriveCom::CommandLEDS(TString &str)
{
    gLog << all << "CC-COMMAND " << MTime(-1) << " LEDS " << str << endl;

    str = str.Strip(TString::kBoth);
    if (str.IsNull())
    {
        gLog << err << "ERROR - LEDS command empty." << endl;
        return false;
    }

    Int_t len;
    Long_t u[2];
    Int_t n=sscanf(str.Data(), "%ld %ld %n", &u[0], &u[1], &len);
    if (n!=2)
    {
        gLog << warn << "WARNING - Not enough argmuents (LEDS)." << endl;
        return kCONTINUE;
    }

    str.Remove(0, len);
    str = str.Strip(TString::kBoth);

    if (!str.IsNull())
    {
        gLog << err << "ERROR - Too many bytes in command LEDS" << endl;
        return false;
    }


    if (fQueue)
        fQueue->Proc(WM_LEDS, u);

    return true;
}

bool MDriveCom::CommandARM(TString &str)
{
    str = str.Strip(TString::kBoth);
    if (str.IsNull())
    {
        gLog << err << "ERROR - No identifier for ARM command." << endl;
        return false;
    }
    if (str.First(' ')>=0)
    {
        gLog << err << "ERROR - ARM command syntax error (contains whitespaces)." << endl;
        return false;
    }

    str.ToLower();
    if (str!="lock" && str!="unlock")
    {
        gLog << err << "ERROR - ARM command syntax error (neither LOCK nor UNLOCK)." << endl;
        return false;
    }

    gLog << all << "CC-COMMAND " << MTime(-1) << " ARM '" << str << "'" << endl;

    bool lock = str=="lock";

    if (fQueue)
        fQueue->PostMsg(WM_ARM, &lock, sizeof(lock));
    return true;
}

bool MDriveCom::InterpreteCmd(TString cmd, TString str)
{
    if (cmd==(TString)"WAIT" && str.IsNull())
    {
        //cout << "MDriveCom - WAIT... start." << endl;
        gLog << all << "CC-COMMAND " << MTime(-1) << " WAIT" << endl;
        if (fQueue)
            fQueue->PostMsg(WM_WAIT);
        //cout << "MDriveCom - WAIT... done." << endl;
        return true;
    }

    if (cmd==(TString)"STOP!" && str.IsNull())
    {
        //cout << "MDriveCom - STOP!... start." << endl;
        gLog << all << "CC-COMMAND " << MTime(-1) << " STOP!" << endl;
        if (fQueue)
            fQueue->PostMsg(WM_STOP);
        //cout << "MDriveCom - STOP!... done." << endl;
        return true;
    }

    if (cmd==(TString)"RADEC")
        return CommandRADEC(str);

    if (cmd==(TString)"GRB")
        return CommandGRB(str);

    if (cmd==(TString)"ZDAZ")
        return CommandZDAZ(str);

    if (cmd==(TString)"CELEST")
        return CommandCELEST(str);

    if (cmd==(TString)"MOON")
        return CommandMOON(str);

    if (cmd==(TString)"PREPS")
        return CommandPREPS(str);

    if (cmd==(TString)"TPOIN")
        return CommandTPOINT(str);

    if (cmd==(TString)"ARM")
        return CommandARM(str);

    if (cmd==(TString)"STGMD")
        return CommandSTGMD(str);

    if (cmd==(TString)"LEDS")
        return CommandLEDS(str);

    if (cmd==(TString)"KEEP_ALIVE")
    {
        gLog << dbg << MTime(-1) << " KEEP_ALIVE" << endl;
        return true;
    }

    if (cmd.IsNull() && str.IsNull())
    {
        gLog << all << "CC-COMMAND " << MTime(-1) << " Empty command (single '\\n') received." << endl;
        return false;
    }

    gLog << err << "CC-COMMAND " << MTime(-1) << " Syntax error: '" << cmd << "':'" << str << "'" << endl;
    return false;
}

void MDriveCom::Print(TString &str, Double_t deg) const
{
    Char_t sgn;
    UShort_t d, m, s;

    MAstro::Deg2Dms(deg, sgn, d, m, s);

    str += MString::Format("%c %03d %02d %03d ", sgn, d, m, s);
}

bool MDriveCom::SendReport(UInt_t stat, Double_t mjd, RaDec rd, double ha, ZdAz so, ZdAz is, ZdAz er, Bool_t armed, Int_t stargmd, UInt_t pdo3)
{
    // rd [rad]
    // so [rad]
    // is [deg]
    // er [rad]
    rd *= kRad2Deg;
    so *= kRad2Deg;
#ifdef FACT
    er *= kRad2Deg*3600;
#else
    er *= kRad2Deg;
#endif

    rd.Ra(rd.Ra()/15);

    // Set status flag
    if (stat&kError)
        SetStatus(0);
    if (stat&kStopped)
        SetStatus(1);
    if (stat&kStopping || stat&kMoving)
        SetStatus(3);
    if (stat&kTracking)
        SetStatus(4);

    TString str;
    Print(str, rd.Ra());    // Ra
    Print(str, rd.Dec());   // Dec
    Print(str, ha);         // HA
    //str += MString::Format("%12.6f ", mjd==0 ? MTime(-1).GetMjd() : mjd); // mjd
    str += MString::Format("%12.6f ", mjd); // mjd
    Print(str, so.Zd());
    Print(str, so.Az());
    Print(str, is.Zd());
    Print(str, is.Az());
    str += MString::Format("%08.3f ", er.Zd());
    str += MString::Format("%08.3f ", er.Az());
    str += armed ? "1 " : "0 ";
    str += MString::Format("%d ", stargmd);           // Starguider mode: 0=none, 1=starguider, 2=starguider off
    str += MString::Format("%x ", pdo3);              

    return SendRep("DRIVE-REPORT", str.Data(), kFALSE);
}

bool MDriveCom::SendStatus(const char *stat)
{
    return SendRep("DRIVE-STATUS", stat, kFALSE);
}

bool MDriveCom::SendStargReport(UInt_t stat, ZdAz miss, ZdAz nompos, Ring center, Int_t num, Int_t n, Double_t bright, Double_t mjd, Int_t numleds, Int_t numrings)
{
    // miss   [deg]
    // nompos [deg]
    const MTime t(-1);

    miss *= 60;        // [arcmin]

    // Set status flag
    if (stat&kError)
        SetStatus(0);
    if (stat&kStandby)
        SetStatus(2);
    if (stat&kMonitoring)
        SetStatus(4);
    
    TString str;
    str += MString::Format("%05.3f ", miss.Zd());       //[arcmin]
    str += MString::Format("%05.3f ", miss.Az());       //[arcmin]
    Print(str, nompos.Zd());                            //[deg]
    Print(str, nompos.Az());                            //[deg]
    str += MString::Format("%05.1f ",   center.GetX()); //
    str += MString::Format("%05.1f ",   center.GetY()); //
    str += MString::Format("%04d ",   n);               // number of correleated stars
    str += MString::Format("%03.1f ",  bright);         // arbitrary sky brightness
    str += MString::Format("%12.6f ", t.GetMjd());      // mjd
    str += MString::Format("%d ", numleds);             // number of detected leds
    str += MString::Format("%d ", numrings);            // number of detected rings
    str += MString::Format("%04d ", num);               // number of detected stars

    return SendRep("STARG-REPORT", str, kTRUE);
}

bool MDriveCom::SendTPoint(bool stat, char type, Float_t mag, const char *name, const AltAz &za0, const ZdAz &za1,
                           const TVector2 &xy, Float_t dzd, Float_t daz, const MTime &t,
                           const Ring &center, const Led &star, Int_t numleds, Int_t numrings,
                           Int_t numstars, Int_t numcor, Float_t bright)
{
    SetStatus(stat);

    TString str = type;
    str += MString::Format(" %8.4f ", za0.Az());
    str += MString::Format("%8.4f ", za0.Alt());
    str += MString::Format("%8.4f ", fmod(za1.Az()+360, 360));
    str += MString::Format("%8.4f ", 90-za1.Zd());
    str += MString::Format("%8.4f ", xy.X());
    str += MString::Format("%8.4f ", xy.Y());
    str += MString::Format("%8.4f ", dzd);
    str += MString::Format("%8.4f ", daz);
    str += MString::Format("%12.6f ", t.GetMjd());
    str += MString::Format("%f ", center.GetMag());
    str += MString::Format("%f ", star.GetMag());
    str += MString::Format("%f ", center.GetX());
    str += MString::Format("%f ", center.GetY());
    str += MString::Format("%f ", star.GetX());
    str += MString::Format("%f ", star.GetY());
    str += numleds;
    str += " ";
    str += numrings;
    str += " ";
    str += numstars;
    str += " ";
    str += numcor;
    str += " ";
    str += bright;
    str += " ";
    str += mag;
    str += " ";
    str += name;

    return SendRep("TPOINT-REPORT", str, kTRUE);
}