#ifndef YUNIMINFUNCTIONS_H
#define YUNIMINFUNCTIONS_H

#include <iostream>
#include <sstream>
#include <string>
#include <time.h>
#include <conio.h>
#include "ServerConnection.h"
#include "convert.h"

int g_req_speed_left = 0;
int g_req_speed_right = 0;
bool g_led_status = false;

void setLeftMotor(int leftMotor)
{
    sendMessageToServer("setLeftMotor", IntToStr(leftMotor) );
    g_req_speed_left = leftMotor;
}

void setRightMotor(int rightMotor)
{
    sendMessageToServer("setRightMotor", IntToStr(rightMotor));
    g_req_speed_right = rightMotor;
}

void stopLeftMotor()
{
    setLeftMotor(0);
}

void stopRightMotor()
{
    setRightMotor(0);
}

void setMotorPower(int leftMotor, int rightMotor)
{
    setLeftMotor(leftMotor);
    setRightMotor(rightMotor);
}

int getSensorValue(int sensor)
{
    return StrToInt( sendMessageToServer("getSensorValue",IntToStr(sensor)) );
}

void wait(unsigned int microseconds)
{
    //CLOCKS_PER_SEC == 1000 => neresim presnejsi cekani
    Sleep(microseconds/1000);
}

class stopwatch
{
private:
    int startTime;
    int stopTime;
    bool run;
public:
    stopwatch()
    {
        startTime = clock();
        run = true;
    }
    void start()
    {
        if(!run)
            startTime = clock();
        run = true;
    }
    void stop()
    {
        if(run)
        {
            stopTime = clock() - startTime;
            run = false;
        }
    }
    void clear()
    {
        startTime = clock();
    }
    int getTime()
    {
        if(run)
            return (clock()-startTime)*(1000000/CLOCKS_PER_SEC);
        else
            return stopTime*(1000000/CLOCKS_PER_SEC);
    }
};

void waitForStartButton()
{
    while( sendMessageToServer("startButton","get") == "0" )
    {
        wait(100000);
    }
}

bool isStartButtonPressed()
{
    if( sendMessageToServer("startButton","get") == "0" )
        return false;
    else
        return true;
}

void setLeftServo(int value)
{
    sendMessageToServer("leftServoValue",IntToStr(value));
}

void setRightServo(int value)
{
    sendMessageToServer("rightServoValue",IntToStr(value));
}

void setLed()
{
    sendMessageToServer("led","1");
    g_led_status = true;
}

void clearLed()
{
    sendMessageToServer("led","0");
    g_led_status = false;
}

void toggleLed()
{
    if(g_led_status)
        clearLed();
    else
        setLed();
}

class left_encoder
{
private:
    int value;
    bool run;
public:
    left_encoder()
    {
        if(connectionIsInitialize())
            value = StrToInt( sendMessageToServer("leftEncoderValue","get") );
        else
            value = 0;
        run = true;
    }
    void start()
    {
        if(!run)
            value = StrToInt( sendMessageToServer("leftEncoderValue","get") );
        run = true;
    }
    void stop()
    {
        if(run)
        {
            value = StrToInt( sendMessageToServer("leftEncoderValue","get") ) - value;
            run = false;
        }
    }
    void clear()
    {
        value = StrToInt( sendMessageToServer("leftEncoderValue","get") );
    }
    int get()
    {
        if(run)
            return StrToInt( sendMessageToServer("leftEncoderValue","get") ) - value;
        else
            return value;
    }
};

class right_encoder
{
private:
    int value;
    bool run;
public:
    right_encoder()
    {
        if(connectionIsInitialize())
            value = StrToInt( sendMessageToServer("rightEncoderValue","get") );
        else
            value = 0;
        run = true;
    }
    void start()
    {
        if(!run)
            value = StrToInt( sendMessageToServer("rightEncoderValue","get") );
        run = true;
    }
    void stop()
    {
        if(run)
        {
            value = StrToInt( sendMessageToServer("rightEncoderValue","get") ) - value;
            run = false;
        }
    }
    void clear()
    {
        value = StrToInt( sendMessageToServer("rightEncoderValue","get") );
    }
    int get()
    {
        if(run)
            return StrToInt( sendMessageToServer("rightEncoderValue","get") ) - value;
        else
            return value;
    }
};

class rs232_t
{
    bool enableTCP;
    hostent * host;				//for TCP send to the Socket (e.g. for aplication Lorris Toolbox)
    sockaddr_in serverSocket;
    SOCKET conSocket;

    void sendStringToTCP(std::string text)
    {
        if (::send(conSocket, text.data(), text.size(), 0) == -1)
        {
            std::cout << "Problem with sending data to server (e.g. Lorris)." << std::endl;
        }
    }

public:
    rs232_t()
    {
        enableTCP = false;
    }

    ~rs232_t()
    {
        closesocket(mySocket);
        std::cout << "Disconnected from server..." << std::endl;
    }

    bool setTCPConnection(std::string ip, int port)
    {
        enableTCP = true;

        if (WSAStartup(wVersionRequested, &data) != 0)
        {
            std::cout << "Cannot initialize sockets" << std::endl;
            return false;
        }
        if ((host = gethostbyname(ip.c_str())) == NULL)
        {
            std::cout << "Wrong IP address" << std::endl;
            WSACleanup();
            return false;
        }
        if ((conSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == 0)
        {
            std::cout << "Cannot create socket" << std::endl;
            WSACleanup();
            return false;
        }
        serverSocket.sin_family = AF_INET;
        serverSocket.sin_port = htons(port);
        memcpy(&(serverSocket.sin_addr), host->h_addr, host->h_length);
        if (connect(conSocket, (sockaddr *)&serverSocket, sizeof(serverSocket)) == -1)
        {
            std::cout << "Cannot create connection - server (e.g Lorris) isn't available" << std::endl;
            WSACleanup();
            return false;
        }

        std::cout << "Connected to TCP (e.g Lorris)..." << std::endl;
        return true;
    }

    void send(std::string const & text)
    {
        //sendMessageToServer("rs232",text);
        std::cout << text;
        if(enableTCP)
        {
            sendStringToTCP(text);
        }
    }
    void sendCharacter(char pismeno)
    {
        //sendMessageToServer("rs232",CharToString(pismeno));
        std::cout << pismeno;

        std::string toSend;
        toSend.push_back(pismeno);
        sendStringToTCP(toSend);
    }
    void sendNumber(int number,int lengh = 0)
    {
        std::string buffer = "";
        int mezer = lengh - IntToStr(number).size();
        for(int i=0;i<mezer;++i)
        {
            buffer += ' ';
        }
        buffer += IntToStr(number);
        //sendMessageToServer("rs232",buffer);
        std::cout << buffer;

        sendStringToTCP(buffer);
    }
    char get()
    {
        /*std::string pomoc = "";
        while( (pomoc = sendMessageToServer("rs232get","")) == "" )
        {
            doNothing();
        }
        return pomoc[0];*/
        return _getch();
    }
    bool peek(char & ch)
    {
        /*std::string pomoc = sendMessageToServer("rs232get","");
        if( pomoc != "" )
        {
            ch = pomoc[0];
            return true;
        }
        else
        {
            return false;
        }*/
        if(_kbhit())
        {
            ch = _getch();
            return true;
        }
        else
        {
            return false;
        }
    }
    void wait()
    {
    }
};
rs232_t rs232;

//implementation of imaginar funkcions

void setRColor(int red)
{
    sendMessageToServer("Rcolor",IntToStr(red));
}

void setGColor(int green)
{
    sendMessageToServer("Gcolor",IntToStr(green));
}

void setBColor(int blue)
{
    sendMessageToServer("Bcolor",IntToStr(blue));
}

void setRGBColor(int R, int G, int B)
{
    setRColor(R);
    setGColor(G);
    setBColor(B);
}

int getNumberOfRobots()
{
    return StrToInt( sendMessageToServer("numberOfRobots","get") );
}

float getDistanceFromRobot(int idRobota)
{
    return (float)StrToFloat( sendMessageToServer("distanceFromRobot", IntToStr(idRobota) ) );
}

float getAngleFromRobot(int idRobota)
{
    return (float)StrToFloat( sendMessageToServer("angleFromRobot", IntToStr(idRobota) ) );
}

int getMyId()
{
    return StrToInt( sendMessageToServer("getMyId","") );
}

bool amIAlive()
{
    if( sendMessageToServer("amIAlive","") == "1" )
        return true;
    else
        return false;
}

bool setPosition(float X, float Y)
{
    sendMessageToServer("positionX",FloatToStr(X));
    sendMessageToServer("positionY",FloatToStr(Y));
    return true;
}

bool setRotation(float rot)
{
    sendMessageToServer("rotationZ",FloatToStr(rot));
    return true;
}

#endif // YUNIMINFUNCTIONS_H

