#include <functional>

#include "Dim.h"
#include "Event.h"
#include "Shell.h"
#include "StateMachineDim.h"
#include "ConnectionUSB.h"
#include "Configuration.h"
#include "Console.h"
#include "Converter.h"

#include "tools.h"

#include "LocalControl.h"

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

using namespace std::placeholders;
using namespace std;

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

class ConnectionBias : public ConnectionUSB
{
    vector<char> fBuffer;

    bool fIsVerbose;

    enum
    {
        kNumBoards           = 13,
        kNumChannelsPerBoard = 32,
        kNumChannels = kNumBoards*kNumChannelsPerBoard
    };

    enum Command_t
    {
        kCmdReset = 0,
        kCmdRead  = 1,
        kCmdWrite = 3
    };


     // Resistance in Ohm for voltage correction
#define RESISTOR float(1000)

protected:

    vector<uint16_t> fVolt;        // Voltage in DAC units
    vector<uint16_t> fRefVolt;

    vector<uint16_t> fCurrent;     // Current in ADC units
    vector<uint16_t> fRefCurrent;

    vector<bool>     fOC;
    vector<bool>     fPresent;

    bool fResetHit;

    virtual void UpdateA()
    {
        if (!fIsVerbose)
            return;

        for (int c=0; c<kNumChannels; c++)
        {
            Out() << setw(2) << c << ":";
            for (int b=0; b<kNumBoards; b++)
                Out() << " " << setprecision(2) << fixed << setw(5) << fCurrent[b+c*kNumBoards];
            Out() << endl;
        }
    }

private:
    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int /*type*/)
    {
        // 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 (BIAS).");

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

        if (bytes_received%3)
        {
            Error("Number of received bytes not a multiple of 3, can't read data.");
            PostClose(true);
            return;
        }

        if (fIsVerbose)
        {
            Out() << endl << kBold << "Data received:" << endl;
            Out() << Converter::GetHex<uint8_t>(fBuffer, 32) << endl;
        }

        int wrapcnt = -1;

        // === Check/update all wrap counter ===
        for (unsigned int i=0; i<fBuffer.size(); i += 3)
        {
            const int old = wrapcnt;
            wrapcnt = (fBuffer[i]>>4)&7;

            if (wrapcnt==-1 || (old+1)%8 == wrapcnt)
                continue;

            Error("WrapCnt wrong");
            // Error receiving proper answer!
            return;
        }

        // Success with received answer

        if (fBuffer.size()!=kNumChannels*3)
            return;

        /*
         data[0] = (cmd<<5) | (board<<1) | (((channel&16)>>4) & 1);
         data[1] = (channel<<4) | (cmdval>>8);
         data[2] = val&0xff;
         */

        // ################## Read all channels status ##################

        // Evaluate data returned from crate
        for (int i=0; i<kNumChannels; i++)
        {
            fOC[i]      =  fBuffer[i*3]&0x80;
            fCurrent[i] =  fBuffer[i*3+1] | ((fBuffer[i*3]&0xf)<<8);
            fPresent[i] =  fBuffer[i*3+2]&0x70 ? false : true;

            // FIXME FIXME FIXME
            fResetHit   = fBuffer[i*3+2] & 0x80;

            if (i==2*kNumChannelsPerBoard+19)
                fOC[i] = false;
        }

        UpdateA();
    }

    void HandleTransmittedData(size_t n)
    {
        fBuffer.resize(n);
        AsyncRead(ba::buffer(fBuffer));
        AsyncWait(fInTimeout, 50, &ConnectionUSB::HandleReadTimeout);
    }

    // This is called when a connection was established
    void ConnectionEstablished()
    {
    }

    void HandleReadTimeout(const bs::error_code &error)
    {
        if (error==ba::error::basic_errors::operation_aborted)
            return;

        if (error)
        {
            ostringstream 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();
    }


    void SystemReset()
    {
        PostMessage(GetCmd(0, kCmdReset));
    }

    vector<char> GetCmd(uint16_t id, Command_t cmd, uint16_t dac=0)
    {
        const unsigned int board   = id/kNumChannelsPerBoard;
        const unsigned int channel = id%kNumChannelsPerBoard;

        return GetCmd(board, channel, cmd, dac);
    }

    vector<char> GetCmd(uint16_t board, uint16_t channel, Command_t cmd, uint16_t dac=0)
    {
        vector<char> data(3);

        /*
        if (board>kNumBoards)
            return;
        if (channel>kNumChannelsPerBoard)
            return;
        if (dac>0xfff)
            return;
        */

        data[0] = (cmd<<5) | (board<<1) | (((channel&16)>>4) & 1);
        data[1] = (channel<<4) | (dac>>8);
        data[2] =  dac&0xff;

        return data;
    }

    void GlobalSetDac(uint16_t dac)
    {
        PostMessage(GetCmd(0, kCmdWrite, dac));
        /*
        // On success
        if (fBuffer.size() == 3)
        {
            for (int i=0; i<MAX_NUM_BOARDS; i++)
                for (int j=0; j<NUM_CHANNELS; j++)
                {
                    DAC[i][j]     = SetPoint;
                    Volt[i][j]    = Voltage;
                    RefVolt[i][j] = Voltage;
                }
        }
        */
    }

    void SetChannels(const map<uint16_t, uint16_t> &vals)
    {
        if (vals.empty())
            return;

        vector<char> data;

        // Build and execute commands
        for (map<uint16_t, uint16_t>::const_iterator it=vals.begin();
             it!=vals.end(); it++)
        {
            //const uint16_t dac = it->second/90.0*0xfff;

            // If DAC value unchanged, do not send command
            if (fVolt[it->first] == it->second)
                continue;

            const vector<char> cmd = GetCmd(it->first, kCmdWrite, it->second);
            data.insert(data.end(), cmd.begin(), cmd.end());
        }

        PostMessage(data);

        /*
        // On success
        if (Data.size() == Buf.size())
        {
            for (map<unsigned int, double>::const_iterator it = V.begin(); it != V.end(); ++it) {
                DAC[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = (unsigned int) (it->second/90.0*0x0fff);
                Volt[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = it->second;
                RefVolt[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = it->second;
        }
        */
    }

    /*
    // ***** Synchronize board *****
    bool Crate::Synch()
    {
        //############################################################
        int Trial = 0;
        vector<unsigned char> Data;

        while(++Trial <= 3) {
            Data = Communicate(string(1, 0));
            if (Data.size() == 3) return true;
        }
        return false;
        //############################################################
    }
    */

public:
    ConnectionBias(ba::io_service& ioservice, MessageImp &imp) : ConnectionUSB(ioservice, imp()),
        fIsVerbose(true),
        fVolt(kNumChannels),
        fRefVolt(kNumChannels),
        fCurrent(kNumChannels),
        fRefCurrent(kNumChannels),
        fOC(kNumChannels),
        fPresent(kNumChannels)
    {
        SetLogStream(&imp);
    }

    bool GlobalSet(double voltage)
    {
        if (voltage<0 || voltage>90)
            return false;

        GlobalSetDac(voltage/90.0*0xfff);

        return true;
    }

    void ReadAllChannelsStatus()
    {
        vector<char> data;
        data.reserve(kNumChannels*3);

        // Prepare command to read all channels
        for (int i=0; i<kNumChannels; i++)
            {
                const vector<char> cmd = GetCmd(i, kCmdRead);
                data.insert(data.end(), cmd.begin(), cmd.end());
            }

        PostMessage(data);
    }

    // ***** Correct voltages according to current *****
    void AdaptVoltages()
    {
        map<uint16_t, uint16_t> values;

        for (int i=0; i<kNumChannels; i++)
        {
            if (fRefVolt[i]==0)
                continue;

            // Calculate difference and convert ADC units to Amps
            const double diffcur = (fRefCurrent[i]-fCurrent[i])*1.22;

            // Calculate voltage difference
            const double diffvolt = diffcur*RESISTOR/1e6;

            // Calculate new vlaue by onverting voltage difference to DAC units
            const int32_t dac = fRefVolt[i] + diffvolt/90.0*0xfff;

            if (dac<0 || dac>0xfff)
                continue;

            values[i] = fRefVolt[i] + dac;
        }

        SetChannels(values);

        /*
        static int LastUpdate = 0;
        if (time(NULL)-LastUpdate > 5)
        {
            LastUpdate = time(NULL);
            UpdateDIM();
        }*/
    }

    void SetReferenceCurrent()
    {
        fRefCurrent = fCurrent;
    }

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

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

#include "DimDescriptionService.h"

class ConnectionDimBias : public ConnectionBias
{
private:

    DimDescribedService fDimCurrent;
    DimDescribedService fDimVoltage;

    void UpdateA()
    {
        fDimCurrent.Update(fCurrent);
        ConnectionBias::UpdateA();
    }

    /*
    void UpdateV()
    {
        fDimCurrent.Update(fRefVolt);
    }*/

public:
    ConnectionDimBias(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionBias(ioservice, imp),
        fDimCurrent("BIAS_CONTROL/CURRENT", "F:416", ""),
        fDimVoltage("BIAS_CONTROL/VOLTAGE", "F:416", "")
    {
    }

    // 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 StateMachineBias : public T, public ba::io_service, public ba::io_service::work
{
    int Wrap(boost::function<void()> f)
    {
        f();
        return T::GetCurrentState();
    }

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

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

private:
    S fBias;

    enum states_t
    {
        kStateDisconnected = 1,
        kStateConnected    = 2,
    };

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

        if (!fBias.GlobalSet(evt.GetFloat()))
            T::Error("Supplied voltage out of range (0-90)");

        return T::GetCurrentState();
    }


    int Disconnect()
    {
        // Close all connections
        fBias.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
        fBias.PostClose(false);

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

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

        // Now we can reopen the connection
        fBias.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 fBias.IsConnected() ? kStateConnected : kStateDisconnected;
    }

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

        fBias.SetVerbose(evt.GetBool());

        return T::GetCurrentState();
    }

public:
    StateMachineBias(ostream &out=cout) :
        T(out, "BIAS_CONTROL"), ba::io_service::work(static_cast<ba::io_service&>(*this)),
        fBias(*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",
                     "Bias-power supply not connected via USB.");

        AddStateName(kStateConnected, "Connected",
                     "USB connection to bias-power supply established.");

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

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

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


        AddEvent("REQUEST_STATUS", kStateConnected)
            (Wrap(bind(&ConnectionBias::ReadAllChannelsStatus, &fBias)))
            ("");

        AddEvent("SET_GLOBAL", "F:1", kStateConnected)
            (bind(&StateMachineBias::SetGlobal, this, _1))
            ("");

        AddEvent("RESET", kStateConnected)
            (Wrap(bind(&ConnectionBias::SystemReset, &fBias)))
            ("");

        AddEvent("SET_REFERENCE_CURRENT", kStateConnected)
            (Wrap(bind(&ConnectionBias::SetReferenceCurrent, &fBias)))
            ("");

        AddEvent("ADAPT_VOLTAGES", kStateConnected)
            (Wrap(bind(&ConnectionBias::AdaptVoltages, &fBias)))
            ("");
    }

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

    int EvalOptions(Configuration &conf)
    {
        SetEndpoint(conf.Get<string>("addr"));

        fBias.SetVerbose(!conf.Get<bool>("quiet"));

        fBias.Connect();

        return -1;
    }
};

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

#include "Main.h"

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

void SetupConfiguration(Configuration &conf)
{
    po::options_description control("BIAS control options");
    control.add_options()
        ("no-dim,d",      po_bool(),  "Disable dim services")
        ("addr,a",        var<string>("ttysS0"),  "Device address of USB port to bias-power supply")
        ("quiet,q",       po_bool(),  "Disable printing contents of all received messages (except dynamic data) in clear text.")
        ;

    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 biasctrl controls the bias-power supply boards.\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: biasctrl [-c type] [OPTIONS]\n"
        "  or:  biasctrl [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, ConnectionBias>(conf);
            else
                return RunShell<LocalStream, StateMachineDim, ConnectionDimBias>(conf);
        }
        // Cosole access w/ and w/o Dim
        if (conf.Get<bool>("no-dim"))
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachine, ConnectionBias>(conf);
            else
                return RunShell<LocalConsole, StateMachine, ConnectionBias>(conf);
        }
        else
        {
            if (conf.Get<int>("console")==0)
                return RunShell<LocalShell, StateMachineDim, ConnectionDimBias>(conf);
            else
                return RunShell<LocalConsole, StateMachineDim, ConnectionDimBias>(conf);
        }
    }
    /*catch (std::exception& e)
    {
        cerr << "Exception: " << e.what() << endl;
        return -1;
    }*/

    return 0;
}