#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 "LocalControl.h"

#include "HeadersFTM.h"

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

using namespace std;

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

#include "DimDescriptionService.h"

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

class StateMachineRateScan : public StateMachineDim, public DimInfoHandler
{
    /*
    int Wrap(boost::function<void()> f)
    {
        f();
        return T::GetCurrentState();
    }

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

private:
    enum states_t
    {
        kStateDimNetworkNA = 1,
        kStateDisconnected,
        kStateConnecting,
        kStateConnected,
        kStateInProgress,
    };

//    PixelMap fMap;

    DimServiceInfoList fNetwork;

    pair<Time, int> fStatusDim;
    pair<Time, int> fStatusFTM;

    DimStampedInfo fDim;
    DimStampedInfo fFTM;
    DimStampedInfo fRates;

    int fCounter;
    int fSeconds;

    int fSecondsMax;
    int fThresholdMin;
    int fThresholdMax;
    int fThresholdStep;

    uint64_t fTriggers;
    uint64_t fTriggersBoard[40];
    uint64_t fTriggersPatch[160];

    uint64_t fOnTimeStart;

    float fResolution;

    enum reference_t
    {
        kCamera,
        kBoard,
        kPatch
    };

    reference_t fReference;
    uint16_t    fReferenceIdx;

    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 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 infoHandler()
    {
        DimInfo *curr = getInfo(); // get current DimInfo address
        if (!curr)
            return;

        if (curr==&fFTM)
        {
            fStatusFTM = GetNewState(fFTM);
            return;
        }

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

        if (curr==&fRates)
        {
            if (curr->getSize()!=sizeof(FTM::DimTriggerRates))
                return;

            if (fCounter<0/* || fStatusFTM.second!=FTM::kTakingData*/)
                return;

            const FTM::DimTriggerRates &sdata = *static_cast<FTM::DimTriggerRates*>(curr->getData());

            if (++fSeconds<0)
                return;

            if (fSeconds==0)
            {
                fTriggers = 0;

                memset(fTriggersBoard, 0,  40*sizeof(uint64_t));
                memset(fTriggersPatch, 0, 160*sizeof(uint64_t));

                fOnTimeStart = sdata.fOnTimeCounter;
                return;
            }

            fTriggers += sdata.fTriggerRate;
            for (int i=0; i<40; i++)
                fTriggersBoard[i] += sdata.fBoardRate[i];
            for (int i=0; i<40; i++)
                fTriggersPatch[i] += sdata.fPatchRate[i];

            double reference = fTriggers;
            if (fReference==kBoard)
                reference = fTriggersBoard[fReferenceIdx];
            if (fReference==kPatch)
                reference = fTriggersPatch[fReferenceIdx];

            if (sqrt(reference)>fResolution*reference && fSeconds<fSecondsMax)
            {
                ostringstream out;
                out << "Triggers so far: " << fTriggers;
                if (reference>0)
                    out << " (" << sqrt(reference)/reference << ")";
                Info(out);
                return;
            }

            ostringstream sout1, sout2, sout3;

            sout1 << fThresholdMin+fCounter*fThresholdStep << " ";
            sout1 << float(fTriggers)/fSeconds << " ";
            for (int i=0; i<40; i++)
                sout2 << float(fTriggersBoard[i])/fSeconds << " ";
            for (int i=0; i<160; i++)
                sout2 << float(fTriggersPatch[i])/fSeconds << " ";
            sout3 << fSeconds << " ";
            sout3 << float(sdata.fOnTimeCounter-fOnTimeStart)/fSeconds/1000000;

            Info(sout1.str()+sout3.str());


            ofstream fout("ratescan.txt", ios::app);
            fout << sout1.str() << sout2.str() << sout3.str() << endl;


            fCounter++;

            if (fSeconds>=fSecondsMax || fThresholdMin+fCounter*fThresholdStep>fThresholdMax)
            {
                fCounter = -1;
                cout << "The END" << endl;
                //DimClient::sendCommandNB("FTM_CONTROL/STOP_RUN", NULL, 0);
                return;
            }

            fSeconds = -2;  // FIXME: In principle one missed report is enough

            const int32_t data[2] = { -1, fThresholdMin+fCounter*fThresholdStep };
            DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)data, 8);
        }
    }

    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 << ": ";
        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(fStatusFTM,  "FTM_CONTROL");

        return GetCurrentState();
    }

    int StartRateScan()
    {
        ofstream fout("ratescan.txt", ios::app);
        fout << "# ----- " << Time() << " -----" << endl;
        fout << "# Reference: ";
        switch (fReference)
        {
        case kCamera: fout << "Camera";
        case kBoard:  fout << "Board #" << fReferenceIdx;
        case kPatch:  fout << "Patch #" << fReferenceIdx;
        }
        fout << '\n';
        fout << "# -----" << endl;

        Dim::SendCommand("FAD_CONTROL/SET_FILE_FORMAT", uint16_t(0));

        const int32_t data[2] = { -1, fThresholdMin };

        Message("Starting Trigger (FTM)");
        Dim::SendCommand("FTM_CONTROL/SET_PRESCALING", int32_t(20));
        Dim::SendCommand("FTM_CONTROL/SET_THRESHOLD", data);
        //Dim::SendCommand("FTM_CONTROL/STOP_RUN");

        fCounter = 0;
        fSeconds = -2;

        return GetCurrentState();
    }

    int StopRateScan()
    {
        fCounter = -1;

        //if (fStatusFTM.second==FTM::kTakingData)
	{
            //Message("Stopping FTM");
	    //Dim::SendCommand("FTM_CONTROL/STOP_RUN");
        }

        return GetCurrentState();
    }

    int SetReferenceCamera()
    {
        fReference = kCamera;

        return GetCurrentState();
    }

    int SetReferenceBoard(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetReferenceBoard", 4))
            return kSM_FatalError;

        if (evt.GetUInt()>39)
        {
            Error("SetReferenceBoard - Board index out of range [0;39]");
            return GetCurrentState();
        }

        fReference    = kBoard;
        fReferenceIdx = evt.GetUInt();

        return GetCurrentState();
    }

    int SetReferencePatch(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetReferencePatch", 4))
            return kSM_FatalError;

        if (evt.GetUInt()>159)
        {
            Error("SetReferencePatch - Patch index out of range [0;159]");
            return GetCurrentState();
        }

        fReference    = kPatch;
        fReferenceIdx = evt.GetUInt();

        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;

        // All subsystems are not connected
        if (fStatusFTM.second<FTM::kConnected)
            return kStateDisconnected;

        // At least one subsystem is not connected
        //        if (fStatusFTM.second>=FTM::kConnected)
        return fCounter<0 ? kStateConnected : kStateInProgress;
    }

public:
    StateMachineRateScan(ostream &out=cout) : StateMachineDim(out, "RATE_SCAN"),
        fStatusDim(make_pair(Time(), -2)),
        fStatusFTM(make_pair(Time(), -2)),
        fDim("DIS_DNS/VERSION_NUMBER",  (void*)NULL, 0, this),
        fFTM("FTM_CONTROL/STATE",       (void*)NULL, 0, this),
        fRates("FTM_CONTROL/TRIGGER_RATES", (void*)NULL, 0, this),
        fCounter(-1)
    {
        // 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(kStateDimNetworkNA, "DimNetworkNotAvailable",
                     "The Dim DNS is not reachable.");

        AddStateName(kStateDisconnected, "Disconnected",
                     "The Dim DNS is reachable, but the required subsystems are not available.");

        AddStateName(kStateConnected, "Connected",
                     "All needed subsystems are connected to their hardware, no action is performed.");

        AddStateName(kStateInProgress, "InProgress",
                     "Rate scan in progress.");

        AddEvent("START", kStateConnected)
            (bind(&StateMachineRateScan::StartRateScan, this))
            ("");

        AddEvent("START", kStateConnected)
            (bind(&StateMachineRateScan::StartRateScan, this))
            ("");

        AddEvent("STOP", kStateInProgress)
            (bind(&StateMachineRateScan::StopRateScan, this))
            ("");

        AddEvent("SET_REFERENCE_CAMERA", kStateDimNetworkNA, kStateDisconnected, kStateConnected)
            (bind(&StateMachineRateScan::SetReferenceCamera, this))
            ("");
        AddEvent("SET_REFERENCE_BOARD", "I:1", kStateDimNetworkNA, kStateDisconnected, kStateConnected)
            (bind(&StateMachineRateScan::SetReferenceBoard, this, placeholders::_1))
            ("");
        AddEvent("SET_REFERENCE_PATCH", "I:1", kStateDimNetworkNA, kStateDisconnected, kStateConnected)
            (bind(&StateMachineRateScan::SetReferenceBoard, this, placeholders::_1))
            ("");

/*
        AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
            (bind(&StateMachineRateScan::EnableOutput, this, placeholders::_1))
            ("Enable sending of correction values caluclated by the control loop to the biasctrl");

        AddEvent("STORE_REFERENCE")//, kStateIdle)
            (bind(&StateMachineRateScan::StoreReference, this))
            ("Store the last (averaged) value as new reference (for debug purpose only)");

        AddEvent("SET_REFERENCE", "F:1")//, kStateIdle)
            (bind(&StateMachineRateScan::SetReference, this, placeholders::_1))
            ("Set a new global reference value (for debug purpose only)");

        AddEvent("SET_Ki", "D:1")//, kStateIdle)
            (bind(&StateMachineRateScan::SetConstant, this, placeholders::_1, 0))
            ("Set integral constant Ki");

        AddEvent("SET_Kp", "D:1")//, kStateIdle)
            (bind(&StateMachineRateScan::SetConstant, this, placeholders::_1, 1))
            ("Set proportional constant Kp");

        AddEvent("SET_Kd", "D:1")//, kStateIdle)
            (bind(&StateMachineRateScan::SetConstant, this, placeholders::_1, 2))
            ("Set derivative constant Kd");

        AddEvent("SET_T", "D:1")//, kStateIdle)
            (bind(&StateMachineRateScan::SetConstant, this, placeholders::_1, 3))
            ("Set time-constant. (-1 to use the cycle time, i.e. the time for the last average cycle, instead)");

        // 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(&StateMachineRateScan::Print, this))
            ("");
    }

    int EvalOptions(Configuration &conf)
    {
        /*
        if (!fMap.Read(conf.Get<string>("pixel-map-file")))
        {
            Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
            return 1;
        }*/

        fThresholdMin  = 0;
        fThresholdMax  = 100;
        fThresholdStep = 1;

        fSecondsMax    = 150;
        fResolution    = 0.05;

        return -1;
    }
};

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

#include "Main.h"

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

void SetupConfiguration(Configuration &conf)
{
    /*
    po::options_description control("Rate scan options");
    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 <<
        /*
        "The feedback control the BIAS voltages based on the calibration signal.\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: ratescan [-c type] [OPTIONS]\n"
        "  or:  ratescan [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    /* 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;
}
