#ifndef THREE_PI_FUNCTIONS_H
#define THREE_PI_FUNCTIONS_H

#include <QThread>
#include <QTime>
#include <QString>
#include <conio.h>
#include "convert.h"
#include "serverconnection.h"

typedef char int8_t;
typedef unsigned char uint8_t;
typedef short int16_t;
typedef unsigned short uint16_t;
typedef int int32_t;
typedef unsigned int uint32_t;

// Motors

void setLeftMotor(int16_t speed)
{
    sendMessageToServer("setLeftMotor", IntToStr(speed/2) );
}

void setRightMotor(int16_t speed)
{
    sendMessageToServer("setRightMotor", IntToStr(speed/2) );
}

void setMotorPower(int16_t left, int16_t right)
{
    setLeftMotor(left);
    setRightMotor(right);
}

void setSoftAccel(bool enabled)
{
    //todo
    Q_UNUSED(enabled);
}

// Timer

void delayMicroseconds(uint16_t microseconds)
{
    QThread::usleep(microseconds);
}

void delay(uint16_t ms)
{
    QThread::msleep(ms);
}

QTime globalTimer;

uint32_t getTicksCount()
{
    return globalTimer.elapsed();
}

void resetTicks()
{
    globalTimer.restart();
}

// Sensors

int16_t getSensorValue(uint8_t index)
{
    //todo: rozmisteni senzoru? link na wiki nefunguje, poloha senzoru se definuje v config.ini
    return StrToInt(sendMessageToServer("getSensorValue", IntToStr(index)));
}

void calibrate_sensors()
{
    //todo: nothing or little waiting or rotation?
}

void cal_round()
{
    //todo: ???
}

void store_sensor_cal(uint16_t address)
{
    //todo: ???
    Q_UNUSED(address);
}

void load_sensor_cal(uint16_t address)
{
    //todo: ???
    Q_UNUSED(address);
}

uint16_t getBatteryVoltage()
{
    return 5000;
}

// RS232

class rs232_t
{
    bool enableTCP;
    hostent * host;				//for TCP send to Lorris
    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 (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" << std::endl;
            WSACleanup();
            return false;
        }

        std::cout << "Connected to TCP (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()
    {
        return getchar();
    }
    bool peek(char & ch)
    {
        if(kbhit())
        {
            ch = getchar();
            return true;
        }
        else
        {
            return false;
        }
    }
    void wait()
    {
    }
    //todo: standardize IO behavior
    //todo: support other functions
};
rs232_t rs232;

// Buttons

const uint8_t BUTTON_A = 1;
const uint8_t BUTTON_B = 2;
const uint8_t BUTTON_C = 4;

uint8_t getPressedButtons()
{
    uint8_t pressed = 0;
    if(GetAsyncKeyState(VK_F1))
        pressed |= BUTTON_A;
    if(GetAsyncKeyState(VK_F2))
        pressed |= BUTTON_B;
    if(GetAsyncKeyState(VK_F3))
        pressed |= BUTTON_C;
    return pressed;
}

bool isPressed(uint8_t buttons)
{
    return getPressedButtons() == buttons;
}

uint8_t waitForPress(uint8_t buttons)
{
    uint8_t pressed;
    while( (pressed = getPressedButtons()) != buttons )
        delay(100);
    return pressed;
}

uint8_t waitForRelease(uint8_t buttons)
{
    uint8_t pressed;
    while( (pressed = getPressedButtons()) == buttons )
        delay(100);
    return pressed;
}

void waitForButton(uint8_t buttons)
{
    waitForPress(buttons);
    waitForRelease(buttons);
}

// Buzzer

class Buzzer
{
    bool isActive;

public:
    Buzzer()
    {
        isActive = true;
    }

    void start()
    {
        isActive = true;
    }

    void stop()
    {
        isActive = false;
    }

    void set(uint16_t time_on = 0, uint16_t time_off = 0, bool run = true)
    {
        //todo
        Q_UNUSED(time_on);
        Q_UNUSED(time_off);
        Q_UNUSED(run);
    }

    bool isStarted()
    {
        return isActive;
    }
};

//todo: support display
//todo: support EEPROM

#endif // THREE_PI_FUNCTIONS_H
