#include <ctime>
#include "mat.h"
#include "StringTool.h"
#include <iostream>
#include <iterator>
#include <algorithm>

#include "SvrHandling.h"
#include "SvrData.h"
#include "commands.h"
#include "Trajectroy.h"
#include "LinearTrajectory.h"
#include <boost/thread/thread.hpp>

void printUsage(SocketObject& client,  std::string& arg)
{
    (void) arg;
    for (unsigned int i = 0 ; i < commandCount; ++i)
    {
        if(commands[i].printHelp != NULL)
            commands[i].printHelp();
        else
            client.Send(commands[i].aberration+"\t-\t"+commands[i].longVersion);
    }
}


SvrHandling::SvrHandling()
{
    // add each command which is possible
    commands = new ClientCommand[20];

    int i = 0;
    commands[i].aberration     = "GPJ";
    commands[i].longVersion    = "GetPositionJoints";
    commands[i].processCommand = &getPositionJoints;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "GPHRW";
    commands[i].longVersion    = "GetPositionHomRowWise";
    commands[i].processCommand = &getPositionHomRowWise;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "GFT";
    commands[i].longVersion    = "GetForceTorqueTcp";
    commands[i].processCommand = &getForceTorqueTcp;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "MPTPJ";
    commands[i].longVersion    = "MovePTPJoints";
    commands[i].processCommand = &movePTPJoints;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "MHRWS";
    commands[i].longVersion    = "MoveHomRowWiseStatus";
    commands[i].processCommand = &moveHomRowWiseStatus;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SS";
    commands[i].longVersion    = "SetSpeed";
    commands[i].processCommand = &setSpeed;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SA";
    commands[i].longVersion    = "SetAccel";
    commands[i].processCommand = &setAccel;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "STPF";
    commands[i].longVersion    = "StartPotFieldMode";
    commands[i].processCommand = &startPotFieldMode;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SPPF";
    commands[i].longVersion    = "StopPotFieldMode";
    commands[i].processCommand = &stopPotFieldMode;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SP";
    commands[i].longVersion    = "SetPos";
    commands[i].processCommand = &setPos;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SJ";
    commands[i].longVersion    = "SetJoints";
    commands[i].processCommand = &setJoints;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "Q";
    commands[i].longVersion    = "Quit";
    commands[i].processCommand = &quit;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "?";
    commands[i].longVersion    = "Help";
    commands[i].processCommand = &printUsage;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "ISKUKA";
    commands[i].longVersion    = "IsKukaLWR";
    commands[i].processCommand = NULL;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "MC";
    commands[i].longVersion    = "MoveCartesian";
    commands[i].processCommand = &moveCartesian;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "GT";
    commands[i].longVersion    = "GetTrajectoryType";
    commands[i].processCommand = &getTrajectoryType;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "ST";
    commands[i].longVersion    = "SetTrajectoryType";
    commands[i].processCommand = &setTrajectoryType;
    commands[i].printHelp      = NULL;
    i+=1;

    commandCount = i;
}

SvrHandling::~SvrHandling()
{
}

void SvrHandling::run()
{
    this->run(SVR_DEFAULT_PORT);
}

void SvrHandling::run(int port)
{
    std::cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n";
    while (c_state !=done)
    {
        SocketObject *socket = new SocketObject;
        if (socket->Bind(port) <0)
        {
            exit (1);
        }
        socket->Listen();
        while(1)
        {
            SocketObject *client = new SocketObject;
            if (socket->Accept(*client))
            {
                std::cout << timestamp() + "Client accepted!\n";
                if (handshakeAccepted(*client))
                {
                    clientCommandLoop(*client);
                }
                client->Disconnect();
                std::cout << timestamp() + "Client disconnected.\n";
            }
            else
            {
                std::cout << timestamp() + "Client bin error.\n";
            }
            delete client;
        }
        delete socket;
    }
}

bool SvrHandling::handshakeAccepted(SocketObject& client)
{
    this->c_state = handshake;
    std::string message = SVR_HELLO_MSG;
    client.Send(message);
    client.Recv(message);
    if (message.find(SVR_HANDSHAKE,0) != std::string::npos)
    {
        c_state = accepted;
        message = SVR_ACCEPTED;
    }
    else
    {
        c_state = waiting;
        message = SVR_FAILED;
        std::cout << timestamp() + "Handshake failed. " << "Invalid recv msg \'" << message <<"\'";
    }
    client.Send(message);
    return(c_state == accepted);
}

void SvrHandling::clientCommandLoop(SocketObject& client)
{
    std::string message, cmd, arg;
    while (c_state == accepted) 
    {
        if (client.Recv(message) > 0) 
        {
            std::cout<<message;
            StringTool::String2CmdArg((const std::string) message, cmd, arg);

            // ignore blank lines
            if (cmd == "") 
                continue;

            // check command table for equivalence
            bool match = false;
            for (unsigned int i = 0 ; i < commandCount ; ++ i)
            {
                if (cmd == commands[i].longVersion || cmd == commands[i].aberration)
                {
                    if (commands[i].processCommand != NULL)
                    {
                        commands[i].processCommand(client,arg);
                    }else
                    {
                        client.Send(SVR_UNIMPLEMENTED_COMMAND);
                    }
                    match = true;
                    match = true;
                    break;
                }
            }
            // command not found
            if (match == false)
            {
                client.Send(SVR_UNKNOWN_COMMAND);
                printUsage(client, arg);
            }

        }else 
        {
            std::cout << timestamp() + "Connection to client lost..\n";
            c_state = waiting;
        }
    }
}

std::string SvrHandling::timestamp()
{
    time_t rawtime;
    struct tm * timeinfo;
    char buffer [80];

    time ( &rawtime );
    timeinfo = localtime ( &rawtime );

    strftime (buffer,80,"%Y-%m-%d-%H-%M-%S",timeinfo);
    std::string temp = ": ";
    std::string output = buffer + temp;
    return output;
}