#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<uint8_t> fBuffer;

    bool fIsVerbose;

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

    enum Command_t
    {
        kCmdReset      = 0,
        kCmdRead       = 1,
        kCmdGlobalSet  = 2,
        kCmdChannelSet = 3,
        kCmdPrint      = 4
    };

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

protected:

    vector<uint16_t> fVolt;        // Voltage in DAC units (12bit = 90V)
    vector<uint16_t> fRefVolt;

    vector<int16_t>  fCurrent;     // Current in ADC units (12bit = 5mA)
    vector<int16_t>  fRefCurrent;

    vector<bool>     fPresent;

    int fWrapCounter;


    virtual void UpdateA()
    {
    }

    virtual void UpdateV()
    {
    }

private:
    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int type)
    {
        if (type==kCmdPrint && bytes_received==0 && !err)
        {
            Print();
            return;
        }

        // 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 (size=" << bytes_received << "):" << endl;
            //Out() << Converter::GetHex<uint8_t>(fBuffer, 32) << endl;
            // FIXME: Check why more is printed than expected
        }

        const uint16_t command = type&0xf;
        const uint16_t id      = type>>4;
        const uint16_t status  = (fBuffer[0]>>7)&1;
        const uint16_t wrap    = (fBuffer[0]>>4)&7;
        const uint16_t ddd     = ((uint16_t(fBuffer[0])&0xf)<<8) | fBuffer[1];
        const uint16_t error   = (fBuffer[2]>>4)&0xf;
        const uint16_t board   =  fBuffer[2]&0xf;

        if (fWrapCounter>=0 && (fWrapCounter+1)%8 != wrap)
        {
            Error("Corrupted answer (wrap counter not as it ought to be.");
            return;
        }
        fWrapCounter = wrap;

        if (error==0x8) // No device
        {
            ostringstream out;
            out << "HV down requested!";
            Fatal(out);

            GlobalSetDac(0);

            // Resynchronize input and output
            // SystemReset and status request
            PostClose(true);

            return;
        }

        if (command==kCmdReset)
        {
            if (status==0 && ddd==0 && error==0 && board==0)
            {
                Message("Reset successfully executed.");
                return;
            }

            Warn("Answer to 'reset' command contains unexpected data.");
            return;
        }

        if (command==kCmdGlobalSet)
        {
            if (status==0 && ddd==0 && error==0 && board==0)
            {
                Message("GlobalSet successfully executed.");
                return;
            }

            Warn("Answer to 'global set' command contains unexpected data.");
            return;
        }

        if (command==kCmdRead || command==kCmdChannelSet)
        {
            if (error==0x7 || error==0xf)
            {
                fPresent[board] = false;
                fCurrent[id]    = 0x8000;
                return;
            }

            if (board!=id/kNumChannelsPerBoard)
            {
                ostringstream out;
                out << "Talked to board " << id/kNumChannelsPerBoard << " but board " <<  board << " answered.";
                Error(out);
                return;
            }

            fCurrent[id]    = status ? -ddd : ddd;
            fPresent[board] = true;

            UpdateA();

            return;
        }
    }

    // This is called when a connection was established
    void ConnectionEstablished()
    {
        fWrapCounter = -1;
        fBuffer.resize(3);

        SystemReset();
        ReadAllChannels();
    }

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

    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 SetChannels(const map<uint16_t, uint16_t> &vals)
    {
        if (vals.empty())
            return;

        // Build and execute commands
        for (map<uint16_t, uint16_t>::const_iterator it=vals.begin();
             it!=vals.end(); it++)
        {
            // If DAC value unchanged, do not send command
            if (fVolt[it->first] == it->second)
                continue;

            const vector<char> cmd = GetCmd(it->first, kCmdChannelSet, it->second);
            PostMessage(cmd);
            AsyncRead(ba::buffer(fBuffer), kCmdChannelSet|(it->first<<4));
        }
    }*/

    /*
    // ***** 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),
        fPresent(kNumBoards)
    {
        SetLogStream(&imp);
    }

    void SystemReset()
    {
        Message("Sending system reset.");
        PostMessage(GetCmd(0, kCmdReset));
        AsyncRead(ba::buffer(fBuffer), kCmdReset);
    }

    void ReadChannel(int ch)
    {
        PostMessage(GetCmd(ch, kCmdRead));
        AsyncRead(ba::buffer(fBuffer), kCmdRead|(ch<<4));
    }


    void ReadAllChannels()
    {
        Message("Requesting full system status.");

        // Prepare command to read all channels
        for (int i=0; i<kNumChannels; i++)
            ReadChannel(i);

        //vector<char> buf;
        //AsyncRead(ba::buffer(buf), kCmdPrint);
    }

    bool GlobalSetDac(uint16_t dac)
    {
        if (dac>0xfff)
            return false;

        PostMessage(GetCmd(0, kCmdGlobalSet, dac));
        AsyncRead(ba::buffer(fBuffer), kCmdGlobalSet);

        ReadAllChannels();

        fVolt.assign(kNumChannels, dac);
        UpdateV();

        return true;
    }

    bool GlobalSet(double voltage)
    {
        return GlobalSetDac(voltage*4096/90);
    }

    bool ChannelSetDac(uint16_t ch, uint16_t dac)
    {
        if (dac>0xfff)
            return false;

        if (ch>=kNumChannels)
            return false;

        if (fVolt[ch]==dac)
            return true;

        PostMessage(GetCmd(ch, kCmdChannelSet, dac));
        AsyncRead(ba::buffer(fBuffer), kCmdChannelSet|(ch<<4));

        ReadChannel(ch);

        fVolt[ch] = dac;
        UpdateV();

        return true;
    }

    bool ChannelSet(uint16_t ch, double voltage)
    {
        return ChannelSetDac(ch, voltage*4096/90);
    }

    void SetVoltage(int ch, int32_t dac)
    {
        if (dac<0)
            dac = 0;

        if (dac>0xfff)
            dac = 0xfff;

        ChannelSetDac(ch, dac);
    }

    // ***** Correct voltages according to current *****
    void AdaptVoltages()
    {
        for (int i=0; i<kNumChannels; i++)
        {
            if (fRefVolt[i]==0 || fCurrent[i]<0 || fRefCurrent[i]<0)
                continue;

            // Calculate difference and convert ADC units to Amp
            // const double diffcur = (fRefCurrent[i]-fCurrent[i])*5000/4096
            //const int32_t diffcur = int32_t(fRefCurrent[i]-fCurrent[i])*5000;

            // Calculate voltage difference
            // #define RESISTOR 1000 // Ohm
            //const double diffvolt = diffcur*RESISTOR/1e6;

            // Calculate new vlaue by onverting voltage difference to DAC units
            //const int32_t dac = fRefVolt[i] + diffvolt*4096/90.0;
            SetVoltage(i, fRefVolt[i] + (fRefCurrent[i]-fCurrent[i])/18);
        }
    }

    void SetReferenceCurrent()
    {
        fRefCurrent = fCurrent;
    }

    bool SetReferenceVoltage(const vector<float> &volt)
    {
        if (volt.size()!=kNumChannels)
        {
            ostringstream out;
            out << "SetReferenceVoltage - Given vector has " << volt.size() << " elements - expected " << kNumChannels << endl;
            Error(out);
            return false;
        }

        for (size_t i=0; i<volt.size(); i++)
            fRefVolt[i] = volt[i]*4096/90;

        return true;
    }

    void ApplyReferenceVoltage()
    {
        for (size_t i=0; i<fRefVolt.size(); i++)
            SetVoltage(i, fRefVolt[i]);
    }

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

    void PrintLine(int b, int ch)
    {
        Out() << setw(2) << b << "|";

        for (int c=ch; c<ch+8; c++)
        {
            const int id = c+kNumChannelsPerBoard*b;
            Out() << " " << setw(7) << abs(fCurrent[id])*5000/4096.;
            if (fCurrent[id]<0)
                Out() << "!";
            else
                Out() << " ";
        }
        Out() << endl;

    }
    void Print()
    {
        Out() << dec << setprecision(2) << fixed;
        for (int b=0; b<kNumBoards; b++)
        {
            if (!fPresent[b])
            {
                Out() << setw(2) << b << "-" << endl;
                continue;
            }

            PrintLine(b,  0);
            PrintLine(b,  8);
            PrintLine(b, 16);
            PrintLine(b, 24);
        }
    }
};

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

#include "DimDescriptionService.h"

class ConnectionDimBias : public ConnectionBias
{
private:

    DimDescribedService fDimCurrent;
    DimDescribedService fDimVoltage;

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

    void UpdateV()
    {
        fDimVoltage.Update(fVolt);
    }

public:
    ConnectionDimBias(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionBias(ioservice, imp),
        fDimCurrent("BIAS_CONTROL/CURRENT", "S:416", ""),
        fDimVoltage("BIAS_CONTROL/VOLTAGE", "S: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 SetGlobalDac(const EventImp &evt)
    {
        if (!CheckEventSize(evt.GetSize(), "SetGlobalDac", 2))
            return false;

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

        return T::GetCurrentState();
    }

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

        if (!fBias.ChannelSet(evt.GetUShort(), evt.Get<float>(2)))
            T::Error("Value out of range");

        return T::GetCurrentState();
    }

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

        if (!fBias.ChannelSetDac(evt.Get<uint16_t>(), evt.Get<uint16_t>(2)))
            T::Error("Value out of range");

        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)
            (Wrapper(bind(&ConnectionBias::ReadAllChannels, &fBias)))
            ("");

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

        AddEvent("SET_GLOBAL_DAC", "S:1", kStateConnected)
            (bind(&StateMachineBias::SetGlobalDac, this, _1))
            ("");

        AddEvent("SET_CHANNEL_VOLTAGE", "S:1;F:1", kStateConnected)
            (bind(&StateMachineBias::SetChannel, this, _1))
            ("");

        AddEvent("SET_CHANNEL_DAC", "S:1;S:1", kStateConnected)
            (bind(&StateMachineBias::SetChannelDac, this, _1))
            ("");

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

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

        AddEvent("APPLY_REFERENCE_VOLTAGE", kStateConnected)
            (Wrapper(bind(&ConnectionBias::ApplyReferenceVoltage, &fBias)))
            ("");

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

        AddEvent("PRINT", kStateConnected)
            (Wrapper(bind(&ConnectionBias::Print, &fBias)))
            ("");
    }

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

        fBias.SetEndpoint(conf.Get<string>("dev"));
        T::Message("Setting device to "+fBias.URL());

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

        ifstream fin("FACTmapV5.txt");

        int l = 0;

        vector<float> vec(ConnectionBias::kNumChannels);

        string buf;
        while (getline(fin, buf, '\n'))
        {
            if (l>1439)
                break;

            buf = Tools::Trim(buf);
            if (buf[0]=='#')
                continue;

            stringstream str(buf);

            int   idummy, board, channel;
            float fdummy, volt;

            str >> idummy >> idummy >> idummy >> idummy >> idummy;
            str >> volt;
            str >> board;
            str >> channel;
            str >> fdummy >> fdummy >> fdummy;

            if (channel+32*board>=ConnectionBias::kNumChannels)
            {
                T::Error("Invalid board/channel read from FACTmapV5.txt.");
                return 1;
            }

            vec[channel+32*board] = volt;
            l++;
        }

        if (l!=1440)
        {
            T::Error("Reading reference voltages from FACTmapV5.txt failed.");
            return 2;
        }

        if (!fBias.SetReferenceVoltage(vec))
        {
            T::Error("Setting reference voltages failed.");
            return 3;
        }

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

        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")
        ("dev",           var<string>("FTE00FOH"),  "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;
}