#include "MDriveCom.h"

#include <iostream>

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

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))
    {
        cout << "ERROR - Reading position from RADEC" << endl;
        return false;
    }
    if (!str.IsNull())
    {
        cout << "ERROR - Too many bytes in command RADEC" << endl;
        return false;
    }

    cout << "CC-COMMAND " << MTime(-1) << " RADEC " << 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_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))
    {
        cout << "ERROR - Reading position from GRB" << endl;
        return false;
    }
    if (!str.IsNull())
    {
        cout << "ERROR - Too many bytes in command GRB" << endl;
        return false;
    }

    cout << "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))
    {
        cout << "ERROR - Reading position from ZDAZ" << endl;
        return false;
    }

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

    cout << "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::CommandPREPS(TString &str)
{
    str = str.Strip(TString::kBoth);
    if (str.IsNull())
    {
        cout << "ERROR - No identifier for preposition (PREPS) given." << endl;
        return false;
    }
    if (str.First(' ')>=0)
    {
        cout << "ERROR - PREPS command syntax error (contains whitespaces)." << endl;
        return false;
    }

    str.ToLower();

    cout << "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)
{
    cout << "CC-COMMAND " << MTime(-1) << " TPOIN " << str << endl;

    if (fQueue)
        fQueue->PostMsg(WM_TPOINT);//, (void*)str.Data(), str.Length()+1);

    return true;
}

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

    if (fQueue)
        fQueue->PostMsg(WM_STARGMODE);//, (void*)str.Data(), str.Length()+1);

    return true;
}

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

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

    cout << "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;
        cout << "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;
        cout << "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)"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.IsNull() && str.IsNull())
    {
        cout << "CC-COMMAND " << MTime(-1) << " Empty command (single '\\n') received." << endl;
        return false;
    }

    cout << "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, RaDec rd, ZdAz so, ZdAz is, ZdAz er, Bool_t armed)
{
    // rd [rad]
    // so [rad]
    // is [deg]
    // er [rad]
    const MTime t(-1);

    rd *= kRad2Deg;
    so *= kRad2Deg;
    er *= kRad2Deg;

    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, 0);          // HA
    str += MString::Format("%12.6f ", t.GetMjd()); // 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 ";

    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(UInt_t stat)
{
    //const MTime t(-1);

    SetStatus(stat);
    /*
     TString str;
     str += name;  // star name
     str += " ";
     str += nominalaz;
     str += nominalel;
     str += realaz;
     str += realel;
     str += nomra;
     str += nomdec;
     str += diffaz;
     str += diffel;
     str += mjd;
     str += numleds;
     str += artmag;
     */
    return SendRep("TPOINT-REPORT", "", kTRUE);
}