#include <functional>
#include <boost/bind.hpp>
#include <cstdlib>

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

#include "tools.h"

#include "LocalControl.h"
#include "HeadersLid.h"

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

using namespace std::placeholders;
using namespace std;

using namespace Lid;

#include "DimDescriptionService.h"

class ConnectionLid : public ConnectionUSB
{
public:
    vector<char> fRecievedBytes;
    vector<char> fCollectedBytes;
    State::states_t fStateOfArduino;
    State::states_t fLastStateOfArduino;

    DimDescribedService fDim;
    message_t fLastMessage;
    boost::asio::deadline_timer fReadTimer;

    void PrintMessage(const message_t msg)
    {
        Out() << "LastMessage:" << endl;
        Out() << "    OuterMotor:" << endl;
        Out() << "        Speed:    " << msg.outer_motor_speed << endl;
        Out() << "        Current:  " << msg.outer_motor_current.mean << endl;
        Out() << "        Position: " << msg.outer_motor_position.mean << endl;
        Out() << "    InnerMotor:" << endl;
        Out() << "        Speed:    " << msg.inner_motor_speed << endl;
        Out() << "        Current:  " << msg.inner_motor_current.mean << endl;
        Out() << "        Position: " << msg.inner_motor_position.mean << endl;
        Out() << "    LastCommand: " << msg.current_cmd << endl;
        Out() << "    State: " << (int)msg.system_state << endl;
    }

    void CollectBytes(size_t bytes_received)
    {
        // append fRecievedBytes to fCollectedBytes
        std::copy(
            fRecievedBytes.begin(),
            fRecievedBytes.begin() + bytes_received,
            std::back_inserter(fCollectedBytes)
        );
    }

    void TruncateUntilStartFlagFound()
    {
        // search for the 1st occurence of "start:" in the whole fCollectedBytes
        const string search_string("start:");
        string s(fCollectedBytes.begin(), fCollectedBytes.end());
        int pos=s.find(search_string);
	if (pos != 0){
            Out() << "TruncateUntilStartFlagFound: pos=" << pos << endl; 
	}
        // cut off the first 'pos' bytes from fCollectedBytes
        vector<decltype(fCollectedBytes)::value_type>(
            fCollectedBytes.begin() + pos,
            fCollectedBytes.end()
        ).swap(fCollectedBytes);
    }

    complete_message_t
    PopCompleteMessage(){

        complete_message_t tmp;
        memcpy(&tmp, fCollectedBytes.data(), sizeof(complete_message_t));
        // cut off the bytes we just copied into tmp
        std::vector<decltype(fCollectedBytes)::value_type>(
            fCollectedBytes.begin() + sizeof(complete_message_t),
            fCollectedBytes.end()
        ).swap(fCollectedBytes);
        return tmp;
    }

    void UpdateDimService(const message_t& msg){
        dim_service_t update;

        update.inner_current_mean = (float)(msg.inner_motor_current.mean) * 3.4;
        update.outer_current_mean = (float)(msg.outer_motor_current.mean) * 3.4;
        update.inner_position_mean = (float)(msg.inner_motor_position.mean);
        update.outer_position_mean = (float)(msg.outer_motor_position.mean);
        update.inner_speed = (int32_t)msg.inner_motor_speed;
        update.outer_speed = (int32_t)msg.outer_motor_speed;

        // update only if some interesting happened, i.e.
        //  - if the fStateOfArduino is not one of the steady states: kOpen or kclosed
        //  - if the fStateOfArduino just changed.
        if ( (fLastStateOfArduino != fStateOfArduino) || !((fStateOfArduino == State::kOpen) || (fStateOfArduino == State::kClosed)) )
        {
            fDim.Update(update);
        }
        fLastStateOfArduino = fStateOfArduino;
    }

    void HandleReceivedData(const boost::system::error_code&, size_t bytes_received)
    {
        CollectBytes(bytes_received);
        while (fCollectedBytes.size() > 2*sizeof(complete_message_t))
        {
            TruncateUntilStartFlagFound();
            complete_message_t complete_message = PopCompleteMessage();
            message_t msg = complete_message.msg;

            if (Tools::Fletcher16(&msg, 1) == complete_message.checksum)
            {
                fLastMessage = msg;
                fStateOfArduino = static_cast<State::states_t>(msg.system_state + 1);
                UpdateDimService(msg);
            }
        }
	ConnectionEstablished();
    }

    void DoAsyncRead(){
        async_read(
            *this,
            boost::asio::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)),
            boost::bind(
                &ConnectionLid::HandleReceivedData,
                this,
                dummy::error,
                dummy::bytes_transferred
            )
        );
    }

    void ConnectionEstablished()
    {
        fReadTimer.expires_from_now(boost::posix_time::milliseconds(50));
	fReadTimer.async_wait(boost::bind(&ConnectionLid::DoAsyncRead, this));
    }

    ConnectionLid(ba::io_service& ioservice, MessageImp &imp) :
        ConnectionUSB(ioservice, imp()),
        fRecievedBytes(2 * sizeof(complete_message_t)),
        fCollectedBytes(0),
        fStateOfArduino(State::kOpen),
        fLastStateOfArduino(State::kOpen),
        fDim("LID_CONTROL/MOTORS", "F:1;F:1;F:1;F:1;I:1;I:1",
            "|inner_current_mean[mA]: "
            "|outer_current_mean[mA]: "
            "|inner_position_mean[LSB]: "
            "|outer_position_mean[LSB]: "
            "|inner_speed[LSB]: "
            "|outer_speed[LSB]: "),
        fLastMessage(),
	fReadTimer(ioservice)
    {
        SetLogStream(&imp);
    }

    State::states_t GetStatus()
    {
        return fStateOfArduino;
    }
};

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

template <class T, class S>
class StateMachineLid : public StateMachineAsio<T>
{
    int Wrap(boost::function<void()> f)
    {
        f();
        return T::GetCurrentState();
    }

    function<int(const EventImp &)> Wrapper(function<void()> func)
    {
        return bind(&StateMachineLid::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 fLid;

    int Execute()
    {
        return fLid.GetStatus();
    }

    int Open()
    {
        std::array<char, 1> cmd = {{'o'}};
        fLid.PostMessage(cmd);
        return T::GetCurrentState();
    }

    int Close()
    {
        std::array<char, 1> cmd = {{'c'}};
        fLid.PostMessage(cmd);
        return T::GetCurrentState();
    }

    int Print()
    {
        fLid.PrintMessage(fLid.fLastMessage);
        return T::GetCurrentState();
    }

public:
    StateMachineLid(ostream &out=cout) :
        StateMachineAsio<T>(out, "LID_CONTROL"), fLid(*this, *this)
    {
        T::AddStateName(State::kOpen, "Open", "Lid is open");
        T::AddStateName(State::kClosed, "Closed", "Lid is closed");
        T::AddStateName(State::kUpperOpening, "UpperOpening", "upper(right) lid is opening");
        T::AddStateName(State::kHalfOpen, "HalfOpen", "upper(right) lid open, lower(left) lid closed");
        T::AddStateName(State::kLowerOpening, "LowerOpening", "lower(left) lid is opening");
        T::AddStateName(State::kLowerClosing, "LowerClosing", "lower(left) lid is closing");
        T::AddStateName(State::kUpperClosing, "UpperClosing", "upper(right) lid is closing");
        T::AddStateName(State::kUnknown, "Unknown", "Position unknown - arduino just restarted");

        T::AddEvent("OPEN")(bind(&StateMachineLid::Open, this))("Open Lid");
        T::AddEvent("CLOSE")(bind(&StateMachineLid::Close, this))("Close Lid");
        T::AddEvent("PRINT")(bind(&StateMachineLid::Print, this))("Print the last message");
    }

    ~StateMachineLid()
    {
    }

    int EvalOptions(Configuration &conf)
    {
        if (conf.Has("dev"))
        {
            fLid.SetEndpoint(conf.Get<string>("dev"));
            T::Message("Setting device to "+fLid.URL());
            fLid.Connect();
        }
        return -1;
    }
};

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

#include "Main.h"

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

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

    conf.AddOptions(control);
}

void PrintUsage()
{
    cout <<
        "This lidctrl_usb program controls the Lid.\n"
        "Usage: lidctrl_usb [-c type] [OPTIONS]\n"
        "  or:  biaslidctrl_usbctrl [OPTIONS]\n";
    cout << endl;
}

void PrintHelp()
{
    Main::PrintHelp<StateMachineLid<StateMachine,ConnectionLid>>();
}

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

    {
        // No console access at all
        if (!conf.Has("console"))
        {
            return RunShell<LocalStream, StateMachineDim, ConnectionLid>(conf);
        }
        if (conf.Get<int>("console")==0)
            return RunShell<LocalShell, StateMachineDim, ConnectionLid>(conf);
        else
            return RunShell<LocalConsole, StateMachineDim, ConnectionLid>(conf);
    }
    return 0;
}