#include "defines.h"
#include "StringTool.h"
#include <string>
#include "friremote.h"

#ifndef SVR_HANDLING
#define SVR_HANDLING

// Serve status
#define SVR_NAME			"lwrsvr 4.11d"
#define SVR_DEFAULT_PORT	8000
#define SVR_HANDSHAKE		"Hello Robot"
#define SVR_HELLO_MSG		std::string("welcome to ") + SVR_NAME + ("!")
#define SVR_ACCEPTED		std::string("accepted")
#define SVR_FAILED			std::string("Failed.")
#define SVR_TRUE_RSP		"true"
#define SVR_FALSE_RSP		"false"

//Begin SVR_COMMANDS
#define CMD_GetPositionJoints			"GPJ"
#define CMD_GetPositionHomRowWise		"GPHRW"
#define CMD_GetForceTorqueTcp			"GFT"
#define CMD_MovePTPJoints				"MPTPJ"
#define CMD_MoveHomRowWiseStatus		"MHRWS"
#define CMD_SetSpeed					"SS"
#define CMD_SetAccel					"SA"
#define CMD_StartPotFieldMode			"STPF"
#define CMD_StopPotFieldMode			"SPPF"
#define CMD_SetPos						"SP"
#define CMD_SetJoints					"SJ"
#define CMD_QUIT						"Quit"
#define CMD_QUIT1						"quit"
#define CMD_QUIT2						"exit"
#define CMD_ISKUKA						"IsKukaLWR"
#define CMD_MoveCartesian				"MC"
//End SVR_COMMANDS

typedef enum {waiting, handshake, accepted, done} svr_state;

class SvrHandling : private StringTool
{
private:
    svr_state c_state;
    //Handshake
    bool handshakeAccepted(SocketObject& client);
    //Handle client
    void handle(SocketObject& client);
    //Handling request for current Joint Values
    void GetPositionJoints(SocketObject& client);
    //Get Position as POSE Matrix
    void GetPositionHomRowWise(SocketObject& client);
    //Get Force/torque values from TCP
    void GetForceTorqueTcp(SocketObject& client);
    //Move to given Joint combination
    void MovePTPJoints(SocketObject& client, std::string& args);
    //Move to given POSE position
    void MoveHomRowWiseStatus(SocketObject& client, std::string& args);
    //Set Velocity
    void SetSpeed(SocketObject& client, std::string& args);
    //Set Acceleration
    void SetAccel(SocketObject& client, std::string& args);
    //Starting Potential Field Movement Mode
    void StartPotFieldMode(SocketObject& client, std::string& args);
    //Stopping Potential Field Movement Mode
    void StopPotFieldMode(SocketObject& client, std::string& args);
    //Setting Target Position HomRowWise for Potential Move
    void SetPos(SocketObject& client, std::string& args);
    //Setting Target Position as Joints for Potential Move
    void SetJoints(SocketObject& client, std::string& args);
    //Cartesian Movement
    //Move to given POSE position
    void MoveCartesian(SocketObject& client, std::string& args);
    //Quit
    void quit(SocketObject& client);
    //DEBUGGING
    void debug(SocketObject& client);
public:
    //Constructor
    SvrHandling();
    //Destructor
    virtual ~SvrHandling();
    ///Run server with default port
    void run();
    ///Run server special port
    void run(int port);
    //Get current timestamp
    std::string timestamp();
};
#endif