#include <boost/bind.hpp>
#include <boost/regex.hpp>

#include "FACT.h"
#include "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "Connection.h"
#include "LocalControl.h"
#include "Configuration.h"
#include "Timers.h"
#include "Console.h"
#include "Converter.h"

#include "tools.h"

#define HAS_SQL

#ifdef HAS_SQL
#include <mysql++/mysql++.h>
#endif

namespace ba = boost::asio;
namespace bs = boost::system;
namespace dummy = ba::placeholders;

using namespace std;

// ------------------------------------------------------------------------

namespace Drive
{
    struct DimPointing
    {
    } __attribute__((__packed__));

    struct DimTracking
    {
    } __attribute__((__packed__));

    struct DimStarguider
    {
        double fMissZd;
        double fMissAz;

        double fNominalZd;
        double fNominalAz;

        double fCenterX;
        double fCenterY;

        double fBrightness;

        uint16_t fNumCorrelated;
        uint16_t fNumLeds;
        uint16_t fNumRings;
        uint16_t fNumStars;

    } __attribute__((__packed__));

    struct DimTPoint
    {
        double fRa;
        double fDec;

        double fNominalZd;
        double fNominalAz;

        double fPointingZd;
        double fPointingAz;

        double fFeedbackZd;
        double fFeedbackAz;

        uint16_t fNumLeds;
        uint16_t fNumRings;
 
        double fCenterX;
        double fCenterY;
        double fCenterMag;

        double fStarX;
        double fStarY;
        double fStarMag;

        double fRealMag;

    } __attribute__((__packed__));
};



// ------------------------------------------------------------------------

class ConnectionDrive : public Connection
{
    int  fState;

    bool fIsVerbose;

    // --verbose
    // --hex-out
    // --dynamic-out
    // --load-file
    // --leds
    // --trigger-interval
    // --physcis-coincidence
    // --calib-coincidence
    // --physcis-window
    // --physcis-window
    // --trigger-delay
    // --time-marker-delay
    // --dead-time
    // --clock-conditioner-r0
    // --clock-conditioner-r1
    // --clock-conditioner-r8
    // --clock-conditioner-r9
    // --clock-conditioner-r11
    // --clock-conditioner-r13
    // --clock-conditioner-r14
    // --clock-conditioner-r15
    // ...

    virtual void UpdatePointing(const Time &, const array<double, 2> &)
    {
    }

    virtual void UpdateTracking(const Time &, const array<double, 7> &)
    {
    }

    virtual void UpdateStatus(const Time &, const array<uint8_t, 3> &)
    {
    }

    virtual void UpdateStarguider(const Time &, const Drive::DimStarguider &)
    {
    }

    virtual void UpdateTPoint(const Time &, const Drive::DimTPoint &, const string &)
    {
    }

public:
    virtual void UpdateSource()
    {
    }
    virtual void UpdateSource(const array<double, 6> &, const string& = "")
    {
    }

protected:
    map<uint16_t, int> fCounter;

    ba::streambuf fBuffer;

public:
    static Time ReadTime(istream &in)
    {
        uint16_t y, m, d, hh, mm, ss, ms;
        in >> y >> m >> d >> hh >> mm >> ss >> ms;

        return Time(y, m, d, hh, mm, ss, ms*1000);
    }

    static double ReadAngle(istream &in)
    {
        char     sgn;
        uint16_t d, m;
        float    s;

        in >> sgn >> d >> m >> s;

        const double ret = ((60.0 * (60.0 * (double)d + (double)m) + s))/3600.;
        return sgn=='-' ? -ret : ret;
    }

protected:
    void HandleReceivedReport(const boost::system::error_code& err, size_t bytes_received)
    {
        // Do not schedule a new read if the connection failed.
        if (bytes_received==0 || err)
        {
            if (err==ba::error::eof)
                Warn("Connection closed by remote host (FTM).");

            // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
            // 125: Operation canceled
            if (err && err!=ba::error::eof &&                     // Connection closed by remote host
                err!=ba::error::basic_errors::not_connected &&    // Connection closed by remote host
                err!=ba::error::basic_errors::operation_aborted)  // Connection closed by us
            {
                ostringstream str;
                str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
                Error(str);
            }
            PostClose(err!=ba::error::basic_errors::operation_aborted);
            return;
        }

        istream is(&fBuffer);

        string line;
        getline(is, line);

        if (fIsVerbose)
            Out() << line << endl;

        StartReadReport();

        if (line.substr(0, 13)=="DRIVE-STATUS ")
        {
            Message(line.substr(70));
            return;
        }

        if (line.substr(0, 13)=="STARG-REPORT ")
        {
            istringstream stream(line.substr(16));

            // 0: Error
            // 1: Standby
            // 2: Monitoring
            uint16_t status1;
            stream >> status1;
            /*const Time t1 = */ReadTime(stream);

            uint16_t status2;
            stream >> status2;
            /*const Time t2 = */ReadTime(stream);

            double misszd, missaz;
            stream >> misszd >> missaz;

            const double zd = ReadAngle(stream);
            const double az = ReadAngle(stream);

            double cx, cy;
            stream >> cx >> cy;

            int ncor;
            stream >> ncor;

            double bright, mjd;
            stream >> bright >> mjd;

            int nled, nring, nstars;
            stream >> nled >> nring >> nstars;

            if (stream.fail())
                return;

            Drive::DimStarguider data;

            data.fMissZd = misszd;
            data.fMissAz = missaz;
            data.fNominalZd = zd;
            data.fNominalAz = az;
            data.fCenterX = cx;
            data.fCenterY = cy;
            data.fNumCorrelated = ncor;
            data.fBrightness = bright;
            data.fNumLeds = nled;
            data.fNumRings = nring;
            data.fNumStars = nstars;

            UpdateStarguider(Time(mjd), data);

            return;

        }

        if (line.substr(0, 14)=="TPOINT-REPORT ")
        {
            istringstream stream(line.substr(17));

            uint16_t status1;
            stream >> status1;
            const Time t1 = ReadTime(stream);

            uint16_t status2;
            stream >> status2;
            /*const Time t2 =*/ ReadTime(stream);

            char type;
            stream >> type;
            if (type != 'T')
                return;

            double az1, alt1, az2, alt2, ra, dec, dzd, daz;
            stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;

            // c: center, s:start
            double mjd, cmag, smag, cx, cy, sx, sy;
            stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;

            int nled, nring, nstar, ncor;
            stream >> nled >> nring >> nstar >> ncor;

            double bright, mag;
            stream >> bright >> mag;

            string name;
            stream >> name;

            if (stream.fail())
                return;

            Drive::DimTPoint tpoint;

            tpoint.fRa         = ra;
            tpoint.fDec        = dec;

            tpoint.fNominalZd  = 90-alt1-dzd;
            tpoint.fNominalAz  = 90-az1 +daz;

            tpoint.fPointingZd = 90-alt1;
            tpoint.fPointingAz = az1;
 
            tpoint.fFeedbackZd = 90-alt2;
            tpoint.fFeedbackAz = az2;

            tpoint.fNumLeds    = nled;
            tpoint.fNumRings   = nring;

            tpoint.fCenterX    = cx;
            tpoint.fCenterY    = cy;
            tpoint.fCenterMag  = cmag;

            tpoint.fStarX      = sx;
            tpoint.fStarY      = sy;
            tpoint.fStarMag    = smag;

            tpoint.fRealMag    = mag;

            UpdateTPoint(t1, tpoint, name);

            return;
        }

        if (line.substr(0, 13)=="DRIVE-REPORT ")
        {
            // DRIVE-REPORT M1
            // 01 2011 05 14 11 31 19 038
            // 02 1858 11 17 00 00 00 000
            // + 000 00 000 + 000 00 000
            // + 000 00 000
            // 55695.480081
            // + 000 00 000 + 000 00 000
            // + 000 00 000 + 000 00 000
            // 0000.000 0000.000
            // 0 2

            // status
            // year month day hour minute seconds millisec
            // year month day hour minute seconds millisec
            // ra(+ h m s) dec(+ d m s) ha(+ h m s)
            // mjd
            // zd(+ d m s) az(+ d m s)
            // zd(+ d m s) az(+ d m s)
            // zd_err az_err
            // armed(0=unlocked, 1=locked)
            // stgmd(0=none, 1=starguider, 2=starguider off)
            istringstream stream(line.substr(16));

            uint16_t status1;
            stream >> status1;
            const Time t1 = ReadTime(stream);

            uint16_t status2;
            stream >> status2;
            /*const Time t2 =*/ ReadTime(stream);

            const double ra  = ReadAngle(stream);
            const double dec = ReadAngle(stream);
            const double ha  = ReadAngle(stream);

            double mjd;
            stream >> mjd;

            const double zd1 = ReadAngle(stream);
            const double az1 = ReadAngle(stream);
            const double zd2 = ReadAngle(stream);
            const double az2 = ReadAngle(stream);

            double zd_err, az_err;
            stream >> zd_err;
            stream >> az_err;

            uint16_t armed, stgmd;
            stream >> armed;
            stream >> stgmd;

            uint32_t pdo3;
            stream >> hex >> pdo3;

            if (stream.fail())
                return;

            // Status 0: Error
            // Status 1: Stopped
            // Status 3: Stopping || Moving
            // Status 4: Tracking
            if (status1==0)
                status1 = 0x100-3;

            const bool ready = (pdo3&0xef00ef)==0xef00ef;
            if (!ready)
                fState = 0;
            else   
                fState = status1==1 ? armed+1 : status1;

            //  1: Not ready
            //  2: Not armed
            //  3: Armed
            //  4: Moving
            //  5: Tracking
            // 99: Error



            // pdo3:
            //   1 Ab
            //   2 1
            //   4 Emergency
            //   8 OverVolt
            //  10 Move (Drehen-soll)
            //  20 Af
            //  40 1
            //  80 Power on Az
            // ------------------
            // 100 NOT UPS Alarm
            // 200 UPS on Battery
            // 400 UPS charging

            // Power cut: 2ef02ef
            // charging:  4ef04ef

            const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
            UpdateStatus(t1, state);

            const array<double, 2> point = {{ zd2, az2 }};
            UpdatePointing(t1, point);

            const array<double, 7> track =
            {{
                ra, dec, ha,
                zd1, az1,
                zd_err/3600, az_err/3600
            }};
            if (mjd>0)
                UpdateTracking(Time(mjd), track);

            // ---- DIM ----> t1 as event time
            //                status1
            //                mjd
            //                ra/dec/ha
            //                zd/az (nominal)
            //                zd/az (current)
            //                err(zd/az)
            //                [armed] [stgmd]

            // Maybe:
            // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
            //
            // if (mjd>0)
            // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
            //                       ra/dec, ha(not well defined),
            //                       [Nominal + Error == Current]

            // MJD is the time which corresponds to the nominal position
            // t1  is the time which corresponds to the current position/HA

            return;
        }
    }

    void StartReadReport()
    {
        boost::asio::async_read_until(*this, fBuffer, '\n',
                                      boost::bind(&ConnectionDrive::HandleReceivedReport, this,
                                                  dummy::error, dummy::bytes_transferred));
    }

    boost::asio::deadline_timer fKeepAlive;

    void KeepAlive()
    {
        PostMessage(string("KEEP_ALIVE"));

        fKeepAlive.expires_from_now(boost::posix_time::seconds(10));
        fKeepAlive.async_wait(boost::bind(&ConnectionDrive::HandleKeepAlive,
                                          this, dummy::error));
    }

    void HandleKeepAlive(const bs::error_code &error)
    {
        // 125: Operation canceled (bs::error_code(125, bs::system_category))
        if (error && error!=ba::error::basic_errors::operation_aborted)
        {
            ostringstream str;
            str << "Write timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
            Error(str);

            PostClose(false);
            return;
        }

        if (!is_open())
        {
            // For example: Here we could schedule a new accept if we
            // would not want to allow two connections at the same time.
            return;
        }

        // Check whether the deadline has passed. We compare the deadline
        // against the current time since a new asynchronous operation
        // may have moved the deadline before this actor had a chance
        // to run.
        if (fKeepAlive.expires_at() > ba::deadline_timer::traits_type::now())
            return;

        KeepAlive();
    }


private:
    // This is called when a connection was established
    void ConnectionEstablished()
    {
        StartReadReport();
        KeepAlive();
    }

    /*
    void HandleReadTimeout(const bs::error_code &error)
    {
        if (error && error!=ba::error::basic_errors::operation_aborted)
        {
            stringstream str;
            str << "Read timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
            Error(str);

            PostClose();
            return;

        }

        if (!is_open())
        {
            // For example: Here we could schedule a new accept if we
            // would not want to allow two connections at the same time.
            return;
        }

        // Check whether the deadline has passed. We compare the deadline
        // against the current time since a new asynchronous operation
        // may have moved the deadline before this actor had a chance
        // to run.
        if (fInTimeout.expires_at() > ba::deadline_timer::traits_type::now())
            return;

        Error("Timeout reading data from "+URL());

        PostClose();
    }*/


public:

    static const uint16_t kMaxAddr;

public:
    ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
        fState(-1), fIsVerbose(true), fKeepAlive(ioservice)
    {
        SetLogStream(&imp);
    }

    void SetVerbose(bool b)
    {
        fIsVerbose = b;
    }

    int GetState() const
    {
        if (!IsConnected())
            return 1;
        if (IsConnected() && fState<0)
            return 2;
        return fState+3;
    }
};

const uint16_t ConnectionDrive::kMaxAddr = 0xfff;

// ------------------------------------------------------------------------

#include "DimDescriptionService.h"

class ConnectionDimDrive : public ConnectionDrive
{
private:
    DimDescribedService fDimPointing;
    DimDescribedService fDimTracking;
    DimDescribedService fDimSource;
    DimDescribedService fDimTPoint;
    DimDescribedService fDimStatus;

    void UpdatePointing(const Time &t, const array<double, 2> &arr)
    {
        fDimPointing.setData(arr);
        fDimPointing.Update(t);
    }

    void UpdateTracking(const Time &t,const array<double, 7> &arr)
    {
        fDimTracking.setData(arr);
        fDimTracking.Update(t);
    }

    void UpdateStatus(const Time &t, const array<uint8_t, 3> &arr)
    {
        fDimStatus.setData(arr);
        fDimStatus.Update(t);
    }

    void UpdateTPoint(const Time &t, const Drive::DimTPoint &data,
                      const string &name)
    {
        vector<char> dim(sizeof(data)+name.length()+1);
        memcpy(dim.data(), &data, sizeof(data));
        memcpy(dim.data()+sizeof(data), name.c_str(), name.length()+1);

        fDimTPoint.setData(dim);
        fDimTPoint.Update(t);
    }

public:
    ConnectionDimDrive(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionDrive(ioservice, imp),
        fDimPointing("DRIVE_CONTROL/POINTING_POSITION", "D:1;D:1",
                     "|Zd[deg]:Zenith distance (encoder readout)"
                     "|Az[deg]:Azimuth angle (encoder readout)"),
        fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1",
                     "|Ra[h]:Command right ascension"
                     "|Dec[deg]:Command declination"
                     "|Ha[h]:Corresponding hour angle"
                     "|Zd[deg]:Nominal zenith distance"
                     "|Az[deg]:Nominal azimuth angle"
                     "|dZd[deg]:Control deviation Zd"
                     "|dAz[deg]:Control deviation Az"),
        fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;C:31",
                     "|Ra_src[h]:Source right ascension"
                     "|Dec_src[deg]:Source declination"
                     "|Ra_cmd[h]:Command right ascension"
                     "|Dec_cmd[deg]:Command declination"
                     "|Offset[deg]:Wobble offset"
                     "|Angle[deg]:Wobble angle"
                     "|Name[string]:Source name if available"),
        fDimTPoint("DRIVE_CONTROL/TPOINT", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;S:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;C",
                   "|Ra[h]:Command right ascension"
                   "|Dec[deg]:Command declination"
                   "|Zd_nom[deg]:Nominal zenith distance"
                   "|Az_nom[deg]:Nominal azimuth angle"
                   "|Zd_cur[deg]:Current zenith distance (calculated from image)"
                   "|Az_cur[deg]:Current azimuth angle (calculated from image)"
                   "|Zd_enc[deg]:Feedback zenith axis (from encoder)"
                   "|Az_enc[deg]:Feedback azimuth angle (from encoder)"
                   "|N_leds[cnt]:Number of detected LEDs"
                   "|N_rings[cnt]:Number of rings used to calculate the camera center"
                   "|Xc[pix]:X position of center in CCD camera frame"
                   "|Yc[pix]:Y position of center in CCD camera frame"
                   "|Ic[au]:Average intensity (LED intensity weighted with their frequency of occurance in the calculation)"
                   "|Xs[pix]:X position of start in CCD camera frame"
                   "|Ys[pix]:Y position of star in CCD camera frame"
                   "|Ms[mag]:Artifical magnitude of star (calculated form image))"
                   "|Mc[mag]:Catalog magnitude of star"),
        fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", "")

    {
    }

    void UpdateSource()
    {
        const vector<char> empty(6*sizeof(double)+31, 0);
        fDimSource.setQuality(0);
        fDimSource.Update(empty);
    }

    void UpdateSource(const array<double, 6> &arr, const string &name="")
    {
        vector<char> dat(6*sizeof(double)+31, 0);
        memcpy(dat.data(), arr.data(), 6*sizeof(double));
        strncpy(dat.data()+6*sizeof(double), name.c_str(), 30);

        fDimSource.setQuality(1);
        fDimSource.Update(dat);
    }

    // A B [C] [D] E [F] G H [I] J K [L] M N O P Q R [S] T U V W [X] Y Z
};

// ------------------------------------------------------------------------

template <class T, class S>
class StateMachineDrive : public T, public ba::io_service, public ba::io_service::work
{
    int Wrap(boost::function<void()> f)
    {
        f();
        return T::GetCurrentState();
    }

    boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
    {
        return bind(&StateMachineDrive::Wrap, this, func);
    }

private:
    S fDrive;

    enum states_t
    {
        kStateDisconnected = 1,
        kStateConnected,
        kStateNotReady,
        kStateReady,
        kStateArmed,
        kStateMoving,
        kStateTracking,
    };

    typedef map<string,pair<double,double>> sources;
    sources fSources;

    // Status 0: Error
    // Status 1: Unlocked
    // Status 2: Locked
    // Status 3: Stopping || Moving
    // Status 4: Tracking

    bool CheckEventSize(size_t has, const char *name, size_t size)
    {
        if (has==size)
            return true;

        ostringstream msg;
        msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
        T::Fatal(msg);
        return false;
    }

    enum Coordinates
    {
        kPoint,
        kTrackSlow,
        kTrackFast
    };

    string AngleToStr(double angle)
    {
        /* Handle sign */
        const char sgn = angle<0?'-':'+';

        /* Round interval and express in smallest units required */
        double a = round(3600. * fabs(angle)); // deg to seconds

        /* Separate into fields */
        const double ad = trunc(a/3600.);
        a -= ad * 3600.;
        const double am = trunc(a/60.);
        a -= am * 60.;
        const double as = trunc(a);

        /* Return results */
        ostringstream str;
        str << sgn << " " << uint16_t(ad) << " " << uint16_t(am) << " " << as;
        return str.str();
    }

    int SendCommand(const string &str, bool upd=true)
    {
        fDrive.PostMessage(str);
        T::Message("Sending: "+str);

        if (upd)
            fDrive.UpdateSource();

        return T::GetCurrentState();
    }

    int SendCoordinates(const EventImp &evt, const Coordinates type)
    {
        if (!CheckEventSize(evt.GetSize(), "SendCoordinates", 16))
            return T::kSM_FatalError;

        const double *dat = evt.Ptr<double>();

        string command;

        switch (type)
        {
        case kPoint:      command += "ZDAZ ";  break;
        case kTrackSlow:  command += "RADEC "; break;
        case kTrackFast:  command += "GRB ";   break;
        }

        if (type!=kPoint)
        {
            const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
            fDrive.UpdateSource(dim);
        }

        command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
        return SendCommand(command, type==kPoint);
    }

    int StartWobble(const double &srcra,  const double &srcdec,
                    const double &woboff, const double &wobang,
                    const string name="")
    {
        const double ra  = srcra *M_PI/12;
        const double dec = srcdec*M_PI/180;
        const double off = woboff*M_PI/180;
        const double dir = wobang*M_PI/180;

        const double cosdir = cos(dir);
        const double sindir = sin(dir);
        const double cosoff = cos(off);
        const double sinoff = sin(off);
        const double cosdec = cos(dec);
        const double sindec = sin(dec);

        if (off==0)
        {
            const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
            fDrive.UpdateSource(dim, name);

            string command = "RADEC ";
            command += AngleToStr(srcra) + ' ' + AngleToStr(srcdec);
            return SendCommand(command, false);
        }

        const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
        if (sintheta >= 1)
        {
            T::Error("cos(Zd) > 1");
            return T::GetCurrentState();
        }

        const double costheta = sqrt(1 - sintheta*sintheta);

        const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
        const double sindeltara = sindir*sinoff/costheta;

        const double ndec = asin(sintheta)*180/M_PI;
        const double nra  = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;

        const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
        fDrive.UpdateSource(dim, name);

        string command = "RADEC ";
        command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
        return SendCommand(command, false);
    }

    int Wobble(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
            return T::kSM_FatalError;

        const double *dat = evt.Ptr<double>();

        return StartWobble(dat[0], dat[1], dat[2], dat[3]);
    }

    int Track(const EventImp &evt)
    {
        if (evt.GetSize()<=16)
        {
            ostringstream msg;
            msg << "Track - Received event has " << evt.GetSize() << " bytes, but expected at least 17.";
            T::Fatal(msg);
            return false;
        }

        const double *dat  = evt.Ptr<double>();
        const char   *ptr  = evt.Ptr<char>(16);
        const char   *last = ptr+evt.GetSize()-16;

        if (find(ptr, last, '\0')==last)
        {
            T::Fatal("Track - The name transmitted by dim is not null-terminated.");
            return false;
        }

        const string name(ptr);

        const sources::const_iterator it = fSources.find(name);
        if (it==fSources.end())
        {
            T::Error("Source '"+name+"' not found in list.");
            return false;
        }

        const double &ra  = it->second.first;
        const double &dec = it->second.second;

        return StartWobble(ra, dec, dat[0], dat[1], name);
    }

    int SetLedBrightness(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetLedBrightness", 8))
            return T::kSM_FatalError;

        const uint32_t *led = evt.Ptr<uint32_t>();

        ostringstream cmd;
        cmd << "LEDS " << led[0] << " " << led[1];
        return SendCommand(cmd.str(), false);
    }


    int SetVerbosity(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
            return T::kSM_FatalError;

        fDrive.SetVerbose(evt.GetBool());

        return T::GetCurrentState();
    }

    int Print()
    {
        for (sources::const_iterator it=fSources.begin();
             it!=fSources.end(); it++)
        {
            const string &name = it->first;
            const double &ra   = it->second.first;
            const double &dec  = it->second.second;

            T::Out() << name << "," << ra << "," << dec << endl;
        }
        return T::GetCurrentState();
    }

    int Disconnect()
    {
        // Close all connections
        fDrive.PostClose(false);

        /*
         // Now wait until all connection have been closed and
         // all pending handlers have been processed
         poll();
         */

        return T::GetCurrentState();
    }

    int Reconnect(const EventImp &evt)
    {
        // Close all connections to supress the warning in SetEndpoint
        fDrive.PostClose(false);

        // Now wait until all connection have been closed and
        // all pending handlers have been processed
        poll();

        if (evt.GetBool())
            fDrive.SetEndpoint(evt.GetString());

        // Now we can reopen the connection
        fDrive.PostClose(true);

        return T::GetCurrentState();
    }

    int Execute()
    {
        // Dispatch (execute) at most one handler from the queue. In contrary
        // to run_one(), it doesn't wait until a handler is available
        // which can be dispatched, so poll_one() might return with 0
        // handlers dispatched. The handlers are always dispatched/executed
        // synchronously, i.e. within the call to poll_one()
        poll_one();

        return fDrive.GetState();
    }


public:
    StateMachineDrive(ostream &out=cout) :
        T(out, "DRIVE_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
        fDrive(*this, *this)
    {
        // ba::io_service::work is a kind of keep_alive for the loop.
        // It prevents the io_service to go to stopped state, which
        // would prevent any consecutive calls to run()
        // or poll() to do nothing. reset() could also revoke to the
        // previous state but this might introduce some overhead of
        // deletion and creation of threads and more.

        // State names
        AddStateName(kStateDisconnected, "Disconnected",
                     "No connection to cosy");

        AddStateName(kStateConnected, "Connected",
                     "Cosy connected, drive stopped");

        AddStateName(kStateNotReady, "NotReady",
                     "Drive system not ready for movement");

        AddStateName(kStateReady, "Ready",
                     "Drive system ready for movement");

        AddStateName(kStateArmed, "Armed",
                     "Cosy armed, drive stopped");

        AddStateName(kStateMoving, "Moving",
                     "Telescope moving");

        AddStateName(kStateTracking, "Tracking",
                     "Telescope tracking");

        // kStateIdle
        // kStateArmed
        // kStateMoving
        // kStateTracking

        // Init
        // -----------
        // "ARM lock"
        // "STGMD off"

        /*
         [ ] WAIT   -> WM_WAIT
         [x] STOP!  -> WM_STOP
         [x] RADEC  ra(+ d m s.f)  dec(+ d m s.f)
         [x] GRB    ra(+ d m s.f)  dec(+ d m s.f)
         [x] ZDAZ   zd(+ d m s.f)  az (+ d m s.f)
         [ ] CELEST id offset angle
         [ ] MOON   wobble offset
         [ ] PREPS  string
         [ ] TPOIN  star mag
         [ ] ARM    lock/unlock
         [ ] STGMD  on/off
         */

        // Drive Commands
        T::AddEvent("MOVE_TO", "D:2", kStateArmed)  // ->ZDAZ
            (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kPoint))
            ("Move the telescope to the given local coordinates"
             "|Zd[deg]:Zenith distance"
             "|Az[deg]:Azimuth");

        T::AddEvent("TRACK", "D:2", kStateArmed)   // ->RADEC/GRB
            (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
            ("Move the telescope to the given sky coordinates and start tracking them"
             "|Ra[h]:Right ascension"
             "|Dec[deg]:Declination");

        T::AddEvent("WOBBLE", "D:4", kStateArmed)   // ->RADEC/GRB
            (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
            ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
             "|Ra[h]:Right ascension"
             "|Dec[deg]:Declination"
             "|Offset[deg]:Wobble offset"
             "|Angle[deg]:Wobble angle");

        T::AddEvent("TRACK_SOURCE", "D:2;W", kStateArmed)   // ->RADEC/GRB
            (bind(&StateMachineDrive::Track, this, placeholders::_1))
            ("Move the telescope to the given wobble position around the given source and start tracking"
             "|Offset[deg]:Wobble offset"
             "|Angle[deg]:Wobble angle"
             "|Name[string]:Source name");

        T::AddEvent("MOON", kStateArmed)
            (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
            ("Start tracking the moon");
        T::AddEvent("VENUS", kStateArmed)
            (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
            ("Start tracking Venus");
        T::AddEvent("MARS", kStateArmed)
            (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
            ("Start tracking Mars");
        T::AddEvent("JUPITER", kStateArmed)
            (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
            ("Start tracking Jupiter");
        T::AddEvent("SATURN", kStateArmed)
            (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
            ("Start tracking Saturn");

        T::AddEvent("TAKE_TPOINT")
            (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
            ("Take a TPoint");

        T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
            (bind(&StateMachineDrive::SetLedBrightness, this, placeholders::_1))
            ("Set the LED brightness of the top and bottom leds"
             "|top[au]:Allowed range 0-32767 for top LEDs"
             "|bot[au]:Allowed range 0-32767 for bottom LEDs");

        T::AddEvent("LEDS_OFF")
            (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
            ("Switch off TPoint LEDs");

        T::AddEvent("STOP")
            (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
            ("Stop any kind of movement.");

//        T::AddEvent("ARM", kStateConnected)
//            (bind(&StateMachineDrive::SendCommand, this, "ARM lock"))
//            ("");


        // Verbosity commands
        T::AddEvent("SET_VERBOSE", "B")
            (bind(&StateMachineDrive::SetVerbosity, this, placeholders::_1))
            ("set verbosity state"
             "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");

        // Conenction commands
        AddEvent("DISCONNECT", kStateConnected, kStateArmed)
            (bind(&StateMachineDrive::Disconnect, this))
            ("disconnect from ethernet");

        AddEvent("RECONNECT", "O", kStateDisconnected, kStateConnected, kStateArmed)
            (bind(&StateMachineDrive::Reconnect, this, placeholders::_1))
            ("(Re)connect ethernet connection to FTM, a new address can be given"
             "|[host][string]:new ethernet address in the form <host:port>");


        T::AddEvent("PRINT")
            (bind(&StateMachineDrive::Print, this))
            ("Print source list.");

        fDrive.StartConnect();
    }

    void SetEndpoint(const string &url)
    {
        fDrive.SetEndpoint(url);
    }

    void ReadDatabase(const string &database)
    {
        //static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))?");
        static const boost::regex expr("(([[:word:].-]+)(:(.+))?@)?([[:word:].-]+)(:([[:digit:]]+))?(/([[:word:].-]+))");
        // 2: user
        // 4: pass
        // 5: server
        // 7: port
        // 9: db

        boost::smatch what;
        if (!boost::regex_match(database, what, expr, boost::match_extra))
            throw runtime_error("Couldn't parse '"+database+"'.");

        if (what.size()!=10)
            throw runtime_error("Error parsing '"+database+"'.");

        const string user   = what[2];
        const string passwd = what[4];
        const string server = what[5];
        const string db     = what[9];
        const int port      = atoi(string(what[7]).c_str());

        ostringstream out;
        out << "Connecting to '";
        if (!user.empty())
            out << user << "@";
        out << server;
        if (port)
            out << ":" << port;
        if (!db.empty())
            out << "/" << db;

        T::Message(out);

        mysqlpp::Connection conn(db.c_str(), server.c_str(), user.c_str(), passwd.c_str(), port);
        /* throws exceptions
         if (!conn.connected())
         {
             cout << "MySQL connection error: " << conn.error() << endl;
             throw;
         }*/

        const mysqlpp::StoreQueryResult res =
            conn.query("SELECT fSourceName, fRightAscension, fDeclination FROM source").store();
        /* throws exceptions
        if (!res)
        {
            cout << "MySQL query failed: " << query.error() << endl;
            throw;
        }*/

        for (vector<mysqlpp::Row>::const_iterator v=res.begin(); v<res.end(); v++)
        {
            const string name = (*v)[0].c_str();
            const double ra   = (*v)[1];
            const double dec  = (*v)[2];

            // FIXME: Check double names
            fSources[name] = make_pair(ra, dec);

            ostringstream msg;
            msg << " " << name << setprecision(8) << ":   Ra=" << ra << "h Dec=" << dec << "deg";
            T::Message(msg);
        }
    }

    int EvalOptions(Configuration &conf)
    {
        fDrive.SetVerbose(!conf.Get<bool>("quiet"));

        const vector<string> &vec = conf.Vec<string>("source");

        for (vector<string>::const_iterator it=vec.begin(); it!=vec.end(); it++)
        {
            istringstream stream(*it);

            string name;
            double ra=0;
            double dec=0;

            int i=0;

            string buffer;
            while (getline(stream, buffer, ','))
            {
                istringstream is(buffer);

                switch (i++)
                {
                case 0: name = buffer; break;
                case 1: ra   = ConnectionDrive::ReadAngle(is); break;
                case 2: dec  = ConnectionDrive::ReadAngle(is); break;
                }

                if (is.fail())
                    break;
            }

            if (i==3)
            {
                // FIXME: Check double names
                fSources[name] = make_pair(ra, dec);
            }
        }

        if (conf.Has("source-database"))
            ReadDatabase(conf.Get<string>("source-database"));

        // The possibility to connect should be last, so that
        // everything else is already initialized.
        SetEndpoint(conf.Get<string>("addr"));

        return -1;
    }
};

// ------------------------------------------------------------------------

#include "Main.h"


template<class T, class S, class R>
int RunShell(Configuration &conf)
{
    return Main::execute<T, StateMachineDrive<S, R>>(conf);
}

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("Drive control options");
    control.add_options()
        ("no-dim,d",  po_switch(),    "Disable dim services")
        ("addr,a",  var<string>("localhost:7404"),  "Network address of Cosy")
        ("quiet,q", po_bool(true),  "Disable printing contents of all received messages (except dynamic data) in clear text.")
        ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
        ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
        ;

    conf.AddOptions(control);
}

/*
 Extract usage clause(s) [if any] for SYNOPSIS.
 Translators: "Usage" and "or" here are patterns (regular expressions) which
 are used to match the usage synopsis in program output.  An example from cp
 (GNU coreutils) which contains both strings:
  Usage: cp [OPTION]... [-T] SOURCE DEST
    or:  cp [OPTION]... SOURCE... DIRECTORY
    or:  cp [OPTION]... -t DIRECTORY SOURCE...
 */
void PrintUsage()
{
    cout <<
        "The drivectrl is an interface to cosy.\n"
        "\n"
        "The default is that the program is started without user intercation. "
        "All actions are supposed to arrive as DimCommands. Using the -c "
        "option, a local shell can be initialized. With h or help a short "
        "help message about the usuage can be brought to the screen.\n"
        "\n"
        "Usage: drivectrl [-c type] [OPTIONS]\n"
        "  or:  drivectrl [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    Main::PrintHelp<StateMachineDrive<StateMachine,ConnectionDrive>>();

    /* Additional help text which is printed after the configuration
     options goes here */

    /*
     cout << "bla bla bla" << endl << endl;
     cout << endl;
     cout << "Environment:" << endl;
     cout << "environment" << endl;
     cout << endl;
     cout << "Examples:" << endl;
     cout << "test exam" << endl;
     cout << endl;
     cout << "Files:" << endl;
     cout << "files" << endl;
     cout << endl;
     */
}

int main(int argc, const char* argv[])
{
    Configuration conf(argv[0]);
    conf.SetPrintUsage(PrintUsage);
    Main::SetupConfiguration(conf);
    SetupConfiguration(conf);

    if (!conf.DoParse(argc, argv, PrintHelp))
        return -1;

    //try
    {
        // No console access at all
        if (!conf.Has("console"))
        {
            if (conf.Get<bool>("no-dim"))
                return RunShell<LocalStream, StateMachine, ConnectionDrive>(conf);
            else
                return RunShell<LocalStream, StateMachineDim, ConnectionDimDrive>(conf);
        }
        // Cosole access w/ and w/o Dim
        if (conf.Get<bool>("no-dim"))
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachine, ConnectionDrive>(conf);
            else
                return RunShell<LocalConsole, StateMachine, ConnectionDrive>(conf);
        }
        else
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachineDim, ConnectionDimDrive>(conf);
            else
                return RunShell<LocalConsole, StateMachineDim, ConnectionDimDrive>(conf);
        }
    }
    /*catch (std::exception& e)
    {
        cerr << "Exception: " << e.what() << endl;
        return -1;
    }*/

    return 0;
}
