#include "MDriveCom.h"

#include <iostream>

#include "coord.h"
#include "Slalib.h"

using namespace std;

bool MDriveCom::InterpreteCmd(TString cmd, TString str)
{
    if (cmd==(TString)"STOP!")
    {
        cout << "Stop! " << str << endl;
        return true;
    }

    if (cmd==(TString)"RADEC")
    {
        cout << "RaDec: " << str << endl;
        return true;
    }
    if (cmd==(TString)"ZDAZ")
    {
        cout << "ZdAz: " << str << endl;
        return true;
    }
    if (cmd==(TString)"PREPS")
    {
        cout << "Preposs: " << str << endl;
        return true;
    }
    cout << "Unknown Command: " << cmd << str << endl;
    return false;
}

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

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

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

bool MDriveCom::SendReport(UInt_t stat, RaDec rd, ZdAz so, ZdAz is, ZdAz er)
{
    // so [rad]
    // is [deg]
    // er [rad]

    so *= kRad2Deg;
    er *= kRad2Deg;

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

    Timer t;
    t.Now();

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

    return Send(str);
}