// Copyright 1998-2016 Epic Games, Inc. All Rights Reserved.

#pragma comment(lib,"ws2_32.lib") //Winsock Library

#include "WSImuFunctionsPrivatePCH.h"
#include <sstream>
#include <vector>
#include <string>
#include <iostream>
#include "WSImuFunctionsBPLibrary.h"



#define SERVER "192.168.0.255"  //ip address of udp server
#define BUFLEN 512  //Max length of buffer
#define PORT 1234 //The port on which to listen for incoming data

static struct sockaddr_in si_other;
static int s, slen, countframe;
static char buf[512];
static char message[512];

UWSImuFunctionsBPLibrary::UWSImuFunctionsBPLibrary(const FObjectInitializer& ObjectInitializer)
: Super(ObjectInitializer)
{
    WSADATA wsa;
    //Initialise winsock
    printf("\nInitialising Winsock...");
    if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
    {
        printf("Failed. Error Code : %d", WSAGetLastError());
        return;
        exit(EXIT_FAILURE);
    }
    printf("Initialised.\n");

    //create socket
    if ((s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == SOCKET_ERROR)
    {
        printf("socket() failed with error code : %d", WSAGetLastError());
        exit(EXIT_FAILURE);
    }
    slen = sizeof(si_other);
    //setup address structure
    memset((char *)&si_other, 0, sizeof(si_other));
    si_other.sin_family = AF_INET;
    si_other.sin_port = htons(PORT);
    si_other.sin_addr.S_un.S_addr = inet_addr(SERVER);


}

float UWSFunctionsBPLibrary::GetAngle()
{
    float result = 0;

    try
    {

        //send the message
        if (sendto(s, "", strlen(message), 0, (struct sockaddr *) &si_other, slen) == SOCKET_ERROR)
        {
        printf("sendto() failed with error code : %d", WSAGetLastError());
        return false;
        }



        //receive a reply and print it
        //clear the buffer by filling null, it might have previously received data
        memset(buf, '\0', BUFLEN);
        //try to receive some data, this is a blocking call


        struct timeval timeout;
        timeout.tv_sec = 1;
        timeout.tv_usec = 0;
        setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout));

        int rs = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *) &si_other, &slen);
        if (rs == SOCKET_ERROR || rs == -1)
        {
        printf("recvfrom() failed with error code : %d", WSAGetLastError());
        result = 0;

        }

        std::stringstream ss(buf); // Insert the string into a stream

        result = stof(ss.str());
        return result;
        }

    catch (std::exception &e)
    {
        printf(e.what());
        return 0;
    }
    return result;
}