/**
 * @file Listener.cpp
 * @author Petr Egert (egertpet@fel.cvut.cz)
 * @brief Contains Listener class implementation
 */

#include "Listener.h"

#include <math.h>
#include <iostream>
#include <sstream>

using namespace std;
using namespace VRUT;

/**
 * @brief Initializes the server to listen on the specified port
 * @param port Port on which to listen
 * @return True on success, false otherwise
 */
bool Listener::Init(int port)
{
    //windows stuff
    WSADATA w;
    if(WSAStartup(0x0202, &w))
    {
        cout << "Failed to init WinSock" << endl;
        return false;
    }

    if(w.wVersion != 0x0202)
    {
        cout << "Wrong WinSock version" << endl;
        WSACleanup();
        return false;
    }
    //end windows stuff

    server = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

    if (server == (unsigned int) -1)
    {
        cout << "Failed to create socket" << endl;
        WSACleanup();
        return false;
    }

    u_long iMode=1;
    ioctlsocket(server, FIONBIO, &iMode);

    sockaddr_in sin;
    memset(&sin, 0, sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_port = htons(port);
    sin.sin_addr.s_addr = INADDR_ANY;

    if(bind(server, (sockaddr*) &sin, sizeof(sin)) == -1)
    {
        cout << "Failed to bind to socket" << endl;
        closesocket(server);
        WSACleanup();
        return false;
    }

    if(listen(server, 1) == -1)
    {
        cout << "Failed to listen on socket" << endl;
        closesocket(server);
        WSACleanup();
        return false;
    }

    client = -1;
    parseBufferPos = 0;

    return true;
}

/**
 * @brief Performs class deinitialization
 */
void Listener::Release()
{
    closesocket(server);

    //windows stuff
    WSACleanup();
    //end windows stuff
}

/**
 * @brief Updates the statistics and controls information from the data received from a client (if any)
 *
 * Non-blocking mode is used, so this function returs whether or not any new data is received
 */
void Listener::Update()
{
    sockaddr_in inAddr;
    int length;

    if(client == (unsigned int) -1)
    {
        client = accept(server, (sockaddr*) &inAddr, &length);
        if(client != (unsigned int) -1)
        {
            cout << "Client connected from " << inet_ntoa(inAddr.sin_addr) << endl;
        }
    }
    else
    {
        unsigned int len = recv(client, recvBuffer, BUFFSIZE, 0);
        if(len != (unsigned int) -1)
        {
            if(len == 0)
            {
                cout << "Client disconnected" << endl;
                closesocket(client);
                client = -1;
                parseBufferPos = 0;
            }
            else
            {
                memcpy(&parseBuffer[parseBufferPos], recvBuffer, len);
                parseBufferPos += len;
                parseBuffer[parseBufferPos] = 0;

                char *endTag = strstr(parseBuffer, "TURV");
                while(endTag)
                {
                    endTag[0] = 0;
                    ParseData();
                    parseBufferPos = 0;
                    memcpy(parseBuffer, endTag + 4, BUFFSIZE - (endTag - parseBuffer) + 1);
                    endTag = strstr(parseBuffer, "TURV");
                };
            }
        }
        else
        {
            if(WSAGetLastError() != WSAEWOULDBLOCK)
            {
                cout << "Connection lost" << endl;
                closesocket(client);
                parseBufferPos = 0;
                client = -1;
            }
        }
    }
}

/**
 * @brief Parses the data received from a client and fills up the statistics and controls structures
 *
 * Communication protocol is described in the "protocol.txt" file
 * @return True on success, false otherwise
 */
bool Listener::ParseData()
{
    if(strncmp(parseBuffer, "VRUT", 4))
    {
        cout << "Invalid packet" << endl;
        return false;
    }

    istringstream in(&parseBuffer[4]);
    in.setf(ios::skipws);
    WheelContactInfo wheelInfo;

    while(in)
    {
        unsigned int target;
        in >> target;

        switch(target)
        {
            //statistics
            case 0:     //Vector3 velocity;
                in >> statistics.velocity.x;
                in >> statistics.velocity.y;
                in >> statistics.velocity.z;
                break;

            case 1:     //Vector3 angularVelocity;
                in >> statistics.angularVelocity.x;
                in >> statistics.angularVelocity.y;
                in >> statistics.angularVelocity.z;
                break;

            case 2:     //float enginePower;
                in >> statistics.enginePower;
                break;

            case 3:     //float engineRPM;
                in >> statistics.engineRPM;
                break;

            case 4:     //float wheelRPM;
                in >> statistics.wheelRPM;
                break;

            case 5:     //Vector3 position;
                in >> statistics.position.x;
                in >> statistics.position.y;
                in >> statistics.position.z;
                break;

            case 6:     //Vector3 orientation;
                in >> statistics.orientation.x;
                in >> statistics.orientation.y;
                in >> statistics.orientation.z;
                break;

            case 7:     //vector<WheelContactInfo> wheelContactInfo.begin();
                statistics.wheelContactInfo.clear();

                break;

            case 8:     //Vector3 WheelContactInfo.wheelPos;
                in >> wheelInfo.wheelPos.x;
                in >> wheelInfo.wheelPos.y;
                in >> wheelInfo.wheelPos.z;
                break;

            case 9:     //int WheelContactInfo.info;
                in >> wheelInfo.info;
                break;

            case 10:    //vector<WheelContactInfo> wheelContactInfo.push_back();
                statistics.wheelContactInfo.push_back(wheelInfo);
                break;

            case 11:    //float distance;
                in >> statistics.distance;
                break;

            case 12:    //float range;
                in >> statistics.range;;
                break;
            case 13:    //float charge;

                in >> statistics.charge;
                break;


            //controls
            case 100:    //float gas;
                in >> controls.gas;
                break;

            case 101:    //float wheel;
                in >> controls.wheel;
                break;

            case 102:    //float brake;
                in >> controls.brake;
                break;

            case 103:    //float coupling;
                in >> controls.coupling;
                break;

            case 104:    //float gear;
                in >> controls.gear;
                break;

        }
    }

    return true;
}

/**
 * @brief Sends a console command to the currently connected VRUT client (if any)
 * @param command Command to be executed
 */
void Listener::SendCommand(const std::string &command)
{
    cout << "Sending command: " << command << endl;
    if(client != (unsigned int) -1)
    {
        string buff = string("VRUT200 ") + command + string("TURV");
        if((unsigned int) send(client, buff.c_str(), buff.length(), 0) != buff.length())
        {
            cout << "Error sending command" << endl;
        }
    }
}

/**
 * @brief Sends a message to be logged on console of the currently connected VRUT client (if any)
 * @param message Message to be logged
 */
void Listener::SendLog(const std::string &message)
{
    cout << "Sending message: " << message << endl;
    if(client != (unsigned int) -1)
    {
        string buff = string("VRUT201 ") + message + string("TURV");
        if((unsigned int) send(client, buff.c_str(), buff.length(), 0) != buff.length())
        {
            cout << "Error sending message" << endl;
        }
    }
}
