#include <valarray>

#include "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "Connection.h"
#include "Configuration.h"
#include "Console.h"
#include "Converter.h"
#include "DimServiceInfoList.h"
#include "PixelMap.h"

#include "tools.h"
#include "DimData.h"

#include "LocalControl.h"

#include "HeadersFAD.h"
#include "HeadersBIAS.h"

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

using namespace std;

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

#include "DimDescriptionService.h"

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

class StateMachineSmartFACT : public StateMachineDim, public DimInfoHandler
{
private:
    enum states_t
    {
        kStateDimNetworkNA = 1,
        kStateRunning,
    };

    PixelMap fPixelMap;

    DimServiceInfoList fNetwork;

    pair<Time, int> fStatusDim;
    pair<Time, int> fStatusDriveControl;
    pair<Time, int> fStatusMagicWeather;
    pair<Time, int> fStatusFeedback;
    pair<Time, int> fStatusBiasControl;
    pair<Time, int> fStatusFadControl;

    DimStampedInfo fDim;

    DimStampedInfo fDimDriveControl;
    DimStampedInfo fDimDriveControlPointing;
    DimStampedInfo fDimDriveControlTracking;
    DimStampedInfo fDimDriveControlSource;

    DimStampedInfo fDimMagicWeather;
    DimStampedInfo fDimMagicWeatherData;

    DimStampedInfo fDimFeedbackCalibration;

    DimStampedInfo fDimBiasControl;
    DimStampedInfo fDimBiasControlVoltage;
    DimStampedInfo fDimBiasControlCurrent;

    DimStampedInfo fDimFadControl;
    DimStampedInfo *fDimFadControlEventData;

    Time fLastUpdate;

    enum weather_t { kTemp = 0, kDew, kHum, kPress, kWind, kGusts, kDir };
    float fMagicWeatherData[7];

    float  fBiasControlVoltageMed;
    float  fBiasControlCurrentMed;
    float  fBiasControlCurrentMax;

    float  fDriveControlPointingZd;
    string fDriveControlPointingAz;
    float  fDriveControlTrackingDev;
    string fDriveControlSourceName;

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

    pair<Time, int> GetNewState(DimStampedInfo &info) const
    {
        const bool disconnected = info.getSize()==0;

        // Make sure getTimestamp is called _before_ getTimestampMillisecs
        const int tsec = info.getTimestamp();
        const int tms  = info.getTimestampMillisecs();

        return make_pair(Time(tsec, tms*1000),
                         disconnected ? -2 : info.getQuality());
    }

    bool UpdateState(DimInfo *curr, DimStampedInfo &service, pair<Time,int> &rc)
    {
        if (curr!=&service)
            return false;

        rc = GetNewState(service);
        return true;
    }

    bool HandleService(DimInfo *curr, const DimInfo &service, void (StateMachineSmartFACT::*handle)(const DimData &))
    {
        if (curr!=&service)
            return false;

        (this->*handle)(DimData(curr));
        return true;
    }


    bool CheckDataSize(const DimData &d, const char *name, size_t size)
    {
        if (d.data.size()==size)
            return true;

        ostringstream msg;
        msg << name << " - Received service has " << d.data.size() << " bytes, but expected " << size << ".";
        Warn(msg);
        return false;
    }


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

    void HandleMagicWeatherData(const DimData &d)
    {
        if (!CheckDataSize(d, "MagicWeather:Data", 7*4+2))
            return;

        // FIXME: Check size (7*4+2)

        //const uint16_t status = d.get<uint16_t>();
        memcpy(fMagicWeatherData, d.ptr<float>(2), 7*sizeof(float));

        ostringstream out;
        out << uint64_t(d.time.UnixTime()*1000) << '\n';

        for (int i=0; i<7; i++)
            out << "#ffffff\t" << fMagicWeatherData[i] << '\n';

        ofstream fout("www/magicweather.txt");
        fout << out.str();
    }

    void HandleDrivePointing(const DimData &d)
    {
        if (!CheckDataSize(d, "DriveControl:Pointing", 7*4+2))
            return;

        fDriveControlPointingZd = d.get<double>();

        const double az = d.get<double>(8);

        static const char *dir[] =
        {
            "N", "NNE", "NE", "ENE",
            "E", "ESE", "SE", "SSE",
            "S", "SSW", "SW", "WSW",
            "W", "WNW", "NW", "NNW"
        };

        const uint16_t i = uint16_t(floor(fmod(az+11.25, 360)/22));
        fDriveControlPointingAz = dir[i];

        ostringstream out;
        out << uint64_t(d.time.UnixTime()*1000) << '\n';

        out << setprecision(5);
        out << fDriveControlPointingZd << '\n';
        out << az << '\n';

        ofstream fout("www/drive-pointing.txt");
        fout << out.str();
    }

    void HandleDriveTracking(const DimData &d)
    {
        if (!CheckDataSize(d, "DriveControl:Tracking", 7*4+2))
            return;

        const double zd  = d.get<double>(4*8) * M_PI / 180;
        const double dzd = d.get<double>(6*8) * M_PI / 180;
        const double daz = d.get<double>(7*8) * M_PI / 180;

        // Correct:
        // const double d  = cos(del) - sin(zd+dzd)*sin(zd)*(1.-cos(daz));

        // Simplified:
        const double dev = cos(dzd) - sin(zd)*sin(zd)*(1.-cos(daz));
        fDriveControlTrackingDev = acos(dev) * 180 / M_PI;
    }

    void HandleDriveSource(const DimData &d)
    {
        if (!CheckDataSize(d, "DriveControl:Source", 7*4+2))
            return;

        const double *ptr = d.ptr<double>();

        const double ra   = ptr[0];  // Ra[h]
        const double dec  = ptr[1];  // Dec[deg]
        const double woff = ptr[4];  // Wobble offset [deg]
        const double wang = ptr[5];  // Wobble angle  [deg]

        fDriveControlSourceName = d.ptr<char>(6*8);

        ostringstream out;
        out << uint64_t(d.time.UnixTime()*1000) << '\n';

        out << fDriveControlSourceName << '\n';
        out << setprecision(5);
        out << "#ffffff\t" << ra  << '\n';
        out << "#ffffff\t" << dec << '\n';
        out << setprecision(3);
        out << "#ffffff\t" << woff << '\n';
        out << "#ffffff\t" << wang << '\n';
            
        ofstream fout("www/drive.txt");
        fout << out.str();
    }

    void HandleFeedbackCalibration(const DimData &d)
    {
        if (!CheckDataSize(d, "BiasControl:Voltage", 1664))
            return;

        vector<float> v(d.ptr<float>(), d.ptr<float>()+320);
        sort(v.begin(), v.end());

        fBiasControlVoltageMed = (v[159]+v[160])/2;

        const char *ptr = d.ptr<char>();

        ofstream fout("www/biascontrol-voltage.bin");
        fout.write(ptr, 320*sizeof(float));
    }

    void HandleBiasControlVoltage(const DimData &d)
    {
        if (!CheckDataSize(d, "BiasControl:Voltage", 1664))
            return;

        vector<float> v(d.ptr<float>(), d.ptr<float>()+320);
        sort(v.begin(), v.end());

        fBiasControlVoltageMed = (v[159]+v[160])/2;

        const char *ptr = d.ptr<char>();

        ofstream fout("www/biascontrol-voltage.bin");
        fout.write(ptr, 320*sizeof(float));
    }

    void HandleBiasControlCurrent(const DimData &d)
    {
        if (!CheckDataSize(d, "BiasControl:Current", 832))
            return;

        vector<uint16_t> v(d.ptr<uint16_t>(), d.ptr<uint16_t>()+320);
        sort(v.begin(), v.end());

        // Exclude the three crazy channels
        fBiasControlCurrentMed = (v[159]+v[160])/2 * 5000./4096;
        fBiasControlCurrentMax = v[316]            * 5000./4096;

        const char *ptr = d.ptr<char>();

        ofstream fout("www/biascontrol-current.bin");
        fout.write(ptr, 320*sizeof(uint16_t));
    }

    uint8_t fEventCounter;

    void HandleFadControlEventData(const DimData &d)
    {
        if (!CheckDataSize(d, "FadControl:EventData", 23040))
            return;

        if (fEventCounter++%30)
            return;

        //const float *avg = d.ptr<float>();
        //const float *rms = d.ptr<float>(1440*sizeof(float));
        const float *max = d.ptr<float>(1440*sizeof(float)*2);
        //const float *pos = d.ptr<float>(1440*sizeof(float)*3);

        vector<float> dat(160, 0);
        for (int i=0; i<1440; i++)
        {
            const int idx = fPixelMap.index(i).hw()/9;
            if (max[i]>dat[idx])
                dat[idx]=max[i];
            //dat[idx] += max[i];
        }

        vector<uint8_t> val(160, 0);
        for (int i=0; i<160; i++)
        {
            float range = nearbyint(64*dat[i]/2000)+64; // [-2V; 2V]
            if (range>127)
                range=127;
            if (range<0)
                range=0;
            val[i] = (uint8_t)range;
        }

        const char *ptr = reinterpret_cast<char*>(val.data());

        ofstream fout("www/fadcontrol-eventdata.bin");
        fout.write(ptr, 160*sizeof(int8_t));
    }

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

    void infoHandler()
    {
        DimInfo *curr = getInfo(); // get current DimInfo address
        if (!curr)
            return;

        if (HandleService(curr, fDimMagicWeatherData,     &StateMachineSmartFACT::HandleMagicWeatherData))
            return;
        if (HandleService(curr, fDimFeedbackCalibration,  &StateMachineSmartFACT::HandleFeedbackCalibration))
            return;
        if (HandleService(curr, fDimBiasControlVoltage,   &StateMachineSmartFACT::HandleBiasControlVoltage))
            return;
        if (HandleService(curr, fDimBiasControlCurrent,   &StateMachineSmartFACT::HandleBiasControlCurrent))
            return;
        if (HandleService(curr, *fDimFadControlEventData, &StateMachineSmartFACT::HandleFadControlEventData))
            return;

        if (UpdateState(curr, fDimMagicWeather, fStatusMagicWeather))
            return;
        if (UpdateState(curr, fDimFeedback, fStatusFeedback))
            return;
        if (UpdateState(curr, fDimBiasControl, fStatusBiasControl))
            return;
        if (UpdateState(curr, fDimFadControl, fStatusFadControl))
            return;

        if (curr==&fDim)
        {
            fStatusDim = GetNewState(fDim);
            fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
            return;
        }
    }

    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 << ".";
        Fatal(msg);
        return false;
    }

    void PrintState(const pair<Time,int> &state, const char *server)
    {
        const State rc = fNetwork.GetState(server, state.second);

        Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
        Out() << kBold << server << ": ";
        if (rc.index==-2)
        {
            Out() << kReset << "Offline" << endl;
            return;
        }
        Out() << rc.name << "[" << rc.index << "]";
        Out() << kReset << " - " << kBlue << rc.comment << endl;
    }

    int Print()
    {
        Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
        Out() << kBold << "DIM_DNS: ";
        if (fStatusDim.second==0)
            Out() << "Offline" << endl;
        else
            Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;

        PrintState(fStatusMagicWeather, "MAGIC_WEATHER");
        PrintState(fStatusDriveControl, "DRIVE_CONTROL");
        PrintState(fStatusFeedback,     "FEEDBACK");
        PrintState(fStatusBiasControl,  "BIAS_CONTROL");
        PrintState(fStatusFadControl,   "FAD_CONTROL");

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

        if (fStatusDim.second==0)
            return kStateDimNetworkNA;

        Time now;
        if (now-fLastUpdate<boost::posix_time::seconds(1))
            return kStateRunning;

        fLastUpdate=now;

        ostringstream out;
        out << uint64_t(nearbyint(now.UnixTime()*1000)) << '\n';
        out << setprecision(3);

        // -------------- System status --------------
        out << "n/a\n";

        // ------------------ Drive -----------------
        if (fStatusDriveControl.second>=5)   // Armed, Moving, Tracking
        {
            const State rc = fNetwork.GetState("DRIVE_CONTROL", fStatusDriveControl.second);
            out << "#ffffff\t";
            out << rc.name << '\t';
            out << fDriveControlPointingZd  << '\t';
            out << fDriveControlPointingAz  << '\t';
            out << fDriveControlTrackingDev << '\t';
            out << fDriveControlSourceName  << '\n';
        }
        else
            out << "#ffffff\t\t\t\t\t\n";

        // --------------- MagicWeather -------------
        if (fStatusMagicWeather.second==3)
        {
            const float diff = fMagicWeatherData[kTemp]-fMagicWeatherData[kDew];
            string col1 = "#fff8f0";
            if (diff>0.3)
                col1="#fffff0";
            if (diff>0.7)
                col1="#f0fff0";

            const float wind = fMagicWeatherData[kGusts];
            string col2 = "#f0fff0";
            if (wind>35)
                col2="#fffff0";
            if (wind>50)
                col2="#fff8f0";

            out << col1 << "\t";
            out << fMagicWeatherData[kTemp]  << '\t';
            out << fMagicWeatherData[kDew]   << '\n';
            out << col2 << "\t";
            out << fMagicWeatherData[kGusts] << '\n';
        }
        else
            out << "#ffffff\t\t\n\n";

        // --------------- BiasControl -------------
        if (fStatusBiasControl.second==5 || // Ramping
            fStatusBiasControl.second==7 || // On
            fStatusBiasControl.second==9)   // Off
        {
            string col = fBiasControlVoltageMed>3?"#fff8f0":"#ffffff";
            if (fBiasControlCurrentMax>280)
                col = "#fffff0";
            if (fBiasControlCurrentMax>350)
                col = "#fff8f0";

            out << col << "\t";
            out << fBiasControlCurrentMed << '\t';
            out << fBiasControlCurrentMax << '\t';
            out << fBiasControlVoltageMed << '\n';
        }
        else
            out << "#ffffff\t\t\t\n";


        // ------------------------------------------
        ofstream fout("www/fact.txt");
        fout << out.str();

        return kStateRunning;
    }

public:
    StateMachineSmartFACT(ostream &out=cout) : StateMachineDim(out, "SMART_FACT"),
        fStatusDim         (make_pair(Time(), -2)),
        fStatusDriveControl(make_pair(Time(), -2)),
        fStatusMagicWeather(make_pair(Time(), -2)),
        fStatusFeedback    (make_pair(Time(), -2)),
        fStatusBiasControl (make_pair(Time(), -2)),
        fStatusFadControl  (make_pair(Time(), -2)),
        //---
        fDim                    ("DIS_DNS/VERSION_NUMBER",          (void*)NULL, 0, this),
        //---
        fDimDriveControl        ("DRIVE_CONTROL/STATE",             (void*)NULL, 0, this),
        fDimDriveControlPointing("DRIVE_CONTROL/POINTING_POSITION", (void*)NULL, 0, this),
        fDimDriveControlTracking("DRIVE_CONTROL/TRACKING_POSITION", (void*)NULL, 0, this),
        fDimDriveControlSource  ("DRIVE_CONTROL/SOURCE_POSITION",   (void*)NULL, 0, this),
        //---
        fDimMagicWeather        ("MAGIC_WEATHER/STATE",             (void*)NULL, 0, this),
        fDimMagicWeatherData    ("MAGIC_WEATHER/DATA",              (void*)NULL, 0, this),
        //---
        fDimFeedback            ("FEEDBACK/STATE",                  (void*)NULL, 0, this),
        fDimFeedbackCalibration ("FEEDBACK/CALIBRATION",            (void*)NULL, 0, this),
        //---
        fDimBiasControl         ("BIAS_CONTROL/STATE",              (void*)NULL, 0, this),
        fDimBiasControlVoltage  ("BIAS_CONTROL/VOLTAGE",            (void*)NULL, 0, this),
        fDimBiasControlCurrent  ("BIAS_CONTROL/CURRENT",            (void*)NULL, 0, this),
        //---
        fDimFadControl          ("FAD_CONTROL/STATE",               (void*)NULL, 0, this),
        fDimFadControlEventData(0),
        //---
        fEventCounter(0)
    {
        // State names
        AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
                     "The Dim DNS is not reachable.");

        AddStateName(kStateRunning, "Running", "");

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

        AddEvent("PRINT")
            (bind(&StateMachineSmartFACT::Print, this))
            ("");
    }
    ~StateMachineSmartFACT()
    {
        delete fDimFadControlEventData;
    }
    int EvalOptions(Configuration &conf)
    {
        if (!fPixelMap.Read(conf.Get<string>("pixel-map-file")))
        {
            Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
            return 1;
        }

        // Pixel map is needed to deal with this service
        fDimFadControlEventData=new DimStampedInfo("FAD_CONTROL/EVENT_DATA", (void*)NULL, 0, this);

        return -1;
    }
};

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

#include "Main.h"

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

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("Smart FACT");
    control.add_options()
        ("pixel-map-file", var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
        ;

    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 <<
        "SmartFACT is a tool writing the files needed for the SmartFACT web interface.\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: smartfact [-c type] [OPTIONS]\n"
        "  or:  smartfact [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    Main::PrintHelp<StateMachineSmartFACT>();

    /* 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, ConnectionFSC>(conf);
//            else
                return RunShell<LocalStream>(conf);
        }
        // Cosole access w/ and w/o Dim
/*        if (conf.Get<bool>("no-dim"))
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
            else
                return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
        }
        else
*/        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell>(conf);
            else
                return RunShell<LocalConsole>(conf);
        }
    }
    /*catch (std::exception& e)
    {
        cerr << "Exception: " << e.what() << endl;
        return -1;
    }*/

    return 0;
}