#include "MDriveCom.h"

#include <iostream>

#include "coord.h"
#include "MAstro.h"
#include "MCosy.h"
#include "MString.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;
    fQueue->PostMsg(WM_TRACK, &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;
    fQueue->PostMsg(WM_POSITION, &za, sizeof(za));
    //cout << "MDriveCom - POSITION... done." << endl;
    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;
        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;
        fQueue->PostMsg(WM_STOP);
        //cout << "MDriveCom - STOP!... done." << endl;
        return true;
    }

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

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

    if (cmd==(TString)"PREPS")
    {
        cout << "Prepos: " << str << endl;
        return true;
    }

    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);

    MString txt;
    str += txt.Print("%c %03d %02d %03d ", sgn, d, m, s);
}

bool MDriveCom::SendReport(UInt_t stat, RaDec rd, ZdAz so, ZdAz is, ZdAz er)
{
    // 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);

    MString txt;

    TString str;
    Print(str, rd.Ra());    // Ra
    Print(str, rd.Dec());   // Dec
    Print(str, 0);          // HA
    str += txt.Print("%12.6f ", t.GetMjd()); // mjd
    Print(str, so.Zd());
    Print(str, so.Az());
    Print(str, is.Zd());
    Print(str, is.Az());
    str += txt.Print("%08.3f ", er.Zd());
    str += txt.Print("%08.3f", er.Az());

    return Send("DRIVE-REPORT", str, kFALSE);
}

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

bool MDriveCom::SendStargReport(UInt_t stat, ZdAz miss)
{
   
    // miss [rad]
    const MTime t(-1);

    miss *= kRad2Deg;

    // Set status flag
    if (stat&kError)
        SetStatus(0);
    if (stat&kStopped)
        SetStatus(1);
    if (stat&kStandby)
        SetStatus(2);
    if (stat&kMonitoring)
        SetStatus(4);
    
    MString txt;

    TString str;
    str += txt.Print("%05.3f ", miss.Zd());
    str += txt.Print("%05.3f", miss.Az());
    
    return Send("STARG-REPORT", str, kTRUE);

}