#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 "HeadersFAD.h"
#include "HeadersBIAS.h"

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

using namespace std;

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

#include "DimDescriptionService.h"

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

class StateMachineFeedback : 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,
        kStateRunning,
        kStateControlling,
    };

    PixelMap fMap;

    DimServiceInfoList fNetwork;

    pair<Time, int> fStatusDim;
    pair<Time, int> fStatusFAD;
    pair<Time, int> fStatusBias;

    DimStampedInfo fDim;
    DimStampedInfo fFAD;
    DimStampedInfo fBias;
    DimStampedInfo *fBiasData;

    DimDescribedService fDimReference;
    DimDescribedService fDimDeviation;

    vector<vector<float>> fData;

    uint64_t fCursor;

    Time fBiasLast;
    Time fStartTime;

    valarray<double> fPV[3];  // Process variable (intgerated/averaged amplitudes)
    valarray<double> fSP;     // Set point        (target amplitudes)

    double fT;
    double fKp;               // Proportional constant
    double fKi;               // Integral     constant
    double fKd;               // Derivative   constant

    bool fOutputEnabled;

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

    void ResetData()
    {
        fData.clear();
        fData.resize(500);
        fCursor = 0;
        fStartTime = Time();

        fSP.resize(416);
        fSP = valarray<double>(0., 416);

        vector<float> vec(2*BIAS::kNumChannels);
        fDimDeviation.Update(vec);

        fPV[0].resize(0);
        fPV[1].resize(0);
        fPV[2].resize(0);

        if (fKp==0 && fKi==0 && fKd==0)
            Warn("Control loop parameters are all set to zero.");
    }

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

        if (curr==&fBias)
        {
            fStatusBias = GetNewState(fBias);
            return;
        }

        if (curr==&fFAD)
        {
            fStatusFAD = GetNewState(fFAD);
            return;
        }

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

        if (curr==fBiasData)
        {
            if (curr->getSize()==0)
                return;
            if (curr->getSize()!=1440*sizeof(float))
                return;

            // -------- Check age of last stored event --------

            // Must be called in this order
            const int tsec = curr->getTimestamp();
            const int tms  = curr->getTimestampMillisecs();

            const Time tm(tsec, tms*1000);

            if (Time()-fBiasLast>boost::posix_time::seconds(30))
            {
                Warn("Last received event data older than 30s... resetting average calculation.");
                ResetData();
            }
            fBiasLast = tm;

            // -------- Store new event --------

            fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
                                               reinterpret_cast<float*>(curr->getData())+1440);


            fCursor++;

            if (fCursor<fData.size())
                return;

            // -------- Calculate statistics --------

            valarray<double> med(1440);

            for (int ch=0; ch<1440; ch++)
            {
                vector<float> arr(fData.size());
                for (size_t i=0; i<fData.size(); i++)
                    arr[i] = fData[i][ch];

                sort(arr.begin(), arr.end());

                med[ch] = arr[arr.size()/2];
            }

            /*
            vector<float> med(1440);
            vector<float> rms(1440);
            for (size_t i=0; i<fData.size(); i++)
            {
                if (fData[i].size()==0)
                    return;

                for (int j=0; j<1440; j++)
                {
                    med[j] += fData[i][j];
                    rms[j] += fData[i][j]*fData[i][j];
                }
            }
            */

            vector<double> avg(BIAS::kNumChannels);
            vector<int>    num(BIAS::kNumChannels);
            for (int i=0; i<1440; i++)
            {
                const PixelMapEntry &ch = fMap.hw(i);

                // FIXME: Add a consistency check if the median makes sense...
                // FIXME: Add a consistency check to remove pixels with bright stars (median?)

                avg[ch.hv()] += med[i];
                num[ch.hv()]++;
            }

            for (int i=0; i<BIAS::kNumChannels; i++)
            {
                if (num[i])
                    avg[i] /= num[i];

            }

            // -------- Calculate correction --------

            // http://bestune.50megs.com/typeABC.htm

            // CO: Controller output
            // PV: Process variable
            // SP: Set point
            // T:  Sampling period (loop update period)
            // e = SP - PV
            //
            // Kp : No units
            // Ki : per seconds
            // Kd : seconds

            // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]

            if (fCursor%fData.size()==0)
            {
                // FIXME: Take out broken / dead boards.

                const Time tm0 = Time();

                /*const*/ double T21 = (tm0-fStartTime).total_microseconds()/1000000.;
                const double T10 = fT;
                fT = T21;

                fStartTime = tm0;

                ostringstream out;
                out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
                Info(out);

                if (fPV[0].size()==0)
                {
                    fPV[0].resize(avg.size());
                    fPV[0] = valarray<double>(avg.data(), avg.size());
                }
                else
                    if (fPV[1].size()==0)
                    {
                        fPV[1].resize(avg.size());
                        fPV[1] = valarray<double>(avg.data(), avg.size());
                    }
                    else
                        if (fPV[2].size()==0)
                        {
                            fPV[2].resize(avg.size());
                            fPV[2] = valarray<double>(avg.data(), avg.size());
                        }
                        else
                        {
                            fPV[0] = fPV[1];
                            fPV[1] = fPV[2];

                            fPV[2].resize(avg.size());
                            fPV[2] = valarray<double>(avg.data(), avg.size());

                            if (T10<=0 || T21<=0)
                                return;

                            cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;

                            // fKi[j] = response[j]*gain;
                            // Kp = 0;
                            // Kd = 0;

                            //                    -110 / -110 (-23 DAC / -0.51V)
                            // Reference voltage: -238 / -203
                            //                    -360 / -343 ( 23 DAC /  0.51V)

                            // 0.005 A/V
                            // 220 Amplitude / 1V

                            // Gain = 1V / 200 = 0.005

                            // => Kp = 0.01 * gain     = 0.00005
                            // => Ki = 0.8  * gain/20s = 0.00025
                            // => Kd = 0.1  * gain/20s = 0.00003

                            fKp = 0;
                            fKd = 0;
                            fKi = 0.00003*20;
                            T21 = 1;

                            //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
                            //valarray<double> correction =
                            //    - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
                            const valarray<double> correction =
                                - (fKp+fKd/T21)*(fPV[2] - fPV[1])
                                +  fKi*T21*(fSP-fPV[2])
                                +  fKd/T10*(fPV[1]-fPV[0]);

                            vector<float> vec(2*BIAS::kNumChannels);
                            for (int i=0; i<BIAS::kNumChannels; i++)
                                vec[i] = fPV[2][i]-fSP[i];

                            for (int i=0; i<BIAS::kNumChannels; i++)
                                vec[i+416] = correction[i];

                            fDimDeviation.Update(vec);

                            if (fOutputEnabled)
                            {
                                Info("Sending correction to feedback.");

                                dic_cmnd_service("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
                                                 (void*)(vec.data()+416), 416*sizeof(float));

                                /*
                                if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
                                                      (const void*)(vec.data()+416), 416*sizeof(float)))
                                {
                                    Error("Sending correction to bias control failed... switching off.");
                                    fOutputEnabled=false;
                                }
                                else
                                   Info("Success!");
                                */
                            }
                        }

            }

            /*
            vector<float> correction(416);
            vector<float> response(416);
            vector<float> dev(416);

            double gain = 0;

            for (int j=0; j<416; j++)
            {
                //avg[j] /= fData.size();
                //rms[j] /= sqrt((rms[j]*fData.size() - avg[j]*avg[j]))/fData.size();

                dev[j] = avg[j] - target[j];

                // Determine correction from response maxtrix and change voltages
                correction[j] = -dev[j]*response[j]*gain;

                if (correction[j]==0)
                    continue;

                // Limit voltage steps // Limit changes to 100 mV
//                if (fabs(correction[j]) > 0.1)
//                    correction[j] = 0.1*fabs(correction[j])/correction[j];

                // Add voltage change command if not too noisy
//                if (fabs(Average[i]) > 2*Sigma[i])
//                    Cmd << fIDTable[i] << " " << std::showpos << Correction/Multiplicity[i] << " ";

            }
            */
            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 << ": ";
        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(fStatusFAD,  "FAD_CONTROL");
        PrintState(fStatusBias, "BIAS_CONTROL");

        return GetCurrentState();
    }

    int SetConstant(const EventImp &evt, int constant)
    {
        if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
            return kSM_FatalError;

        switch (constant)
        {
        case 0: fKi = evt.GetDouble(); break;
        case 1: fKp = evt.GetDouble(); break;
        case 2: fKd = evt.GetDouble(); break;
        default:
            Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
            return kSM_FatalError;
        }

        return GetCurrentState();
    }

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

        fOutputEnabled = evt.GetBool();

        return GetCurrentState();
    }

    int StartFeedback()
    {
        fBiasData = new DimStampedInfo("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this);

        ResetData();

        return GetCurrentState();
    }

    int StopFeedback()
    {
        delete fBiasData;
        fBiasData = 0;

        return GetCurrentState();
    }

    int StoreReference()
    {
        if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
        {
            Warn("No values in memory. Take enough events first!");
            return GetCurrentState();
        }

        // FIXME: Check age

        if (!fPV[1].size() && !fPV[2].size())
            fSP = fPV[0];

        if (!fPV[2].size())
            fSP = fPV[1];
        else
            fSP = fPV[2];

        vector<float> vec(BIAS::kNumChannels);
        for (int i=0; i<BIAS::kNumChannels; i++)
            vec[i] = fSP[i];
        fDimReference.Update(vec);

        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;

        if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnecting)
            return kStateDisconnected;

        if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnected)
            return kStateConnecting;

        return fBiasData ? kStateRunning : kStateConnected;
    }

public:
    StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
        fStatusDim(make_pair(Time(), -2)),
        fStatusFAD(make_pair(Time(), -2)),
        fStatusBias(make_pair(Time(), -2)),
        fDim("DIS_DNS/VERSION_NUMBER",  (void*)NULL, 0, this),
        fFAD("FAD_CONTROL/STATE",       (void*)NULL, 0, this),
        fBias("BIAS_CONTROL/STATE",     (void*)NULL, 0, this),
        fDimReference("FEEDBACK/REFERENCE", "F:416",        ""),
        fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",  ""),
        fBiasData(NULL), fKp(0), fKi(0), fKd(0), fOutputEnabled(false)
    {
        // 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",
                     ".");

        AddStateName(kStateDisconnected, "Disconnected",
                     ".");

        AddStateName(kStateConnecting, "Connecting",
                     ".");

        AddStateName(kStateConnected, "Connected",
                     ".");

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

        AddEvent("START")//, kStateIdle)
            (bind(&StateMachineFeedback::StartFeedback, this))
            ("");

        AddEvent("STOP")//, kStateIdle)
            (bind(&StateMachineFeedback::StopFeedback, this))
            ("");

        AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
            (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
            ("");

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

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

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

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

        // 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(&StateMachineFeedback::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;
        }

        return -1;
    }
};

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

#include "Main.h"

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

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("BIAS control 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 ftmctrl controls the FSC (FACT Slow Control) board.\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: fscctrl [-c type] [OPTIONS]\n"
        "  or:  fscctrl [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;
}

const PixelMapEntry PixelMap::empty = { 0, 0, 0, 0, 0, 0 };