TargetServer.cpp

#include "TargetServer.h"
#include <errnoLib.h>
#include <hostLib.h>
#include <inetLib.h>
#include <sockLib.h>
#include "Task.h"
#include "Timer.h"
#include "WPIErrors.h"
#include <vxWorks.h>
#include <taskLibCommon.h>
#include <sstream>
#include <algorithm>
#include <iostream>
#include <string>
#include "Synchronized.h"

using std::cout;
using std::endl;
using std::istringstream;
using std::string;

static const int BUFLEN = 1024;
static const int SERVER_PORT_NUM = 1130;

#define ACK "ACK"

TargetServer::TargetServer()
: m_serverTask("RATargetServer", (FUNCPTR)s_ServerTask)
	, m_newTargetSem (NULL)
	, m_stopServer (false)
{
	StartServerTask();
}

TargetServer::~TargetServer()
{
	Stop();
	ClearError();
}


int TargetServer::StartServerTask()
{
	cout << "Starting target server..." << endl;
	if (StatusIsFatal())
	{
		return -1;
	}
	int id = 0;
	m_stopServer = false;
	// Check for prior copy of running task
	int oldId = taskNameToId((char*)m_serverTask.GetName());
	if(oldId != ERROR)
	{
		// TODO: Report error. You are in a bad state.
		taskDelete(oldId);
	}

	// spawn target server task
	// this is done to ensure that the task is spawned with the
	// floating point context save parameter.
	bool started = m_serverTask.Start((int)this);

	id = m_serverTask.GetID();

	if (!started)
	{
		wpi_setWPIError(TaskError);
		return id;
	}
	taskDelay(1);
	cout << "Done: id " << id << endl;
	return id;
}


void TargetServer::Start()
{
	StartServerTask();
}

void TargetServer::Stop()
{
	m_stopServer = true;
}


int TargetServer::s_ServerTask(TargetServer *thisPtr)
{
	return thisPtr->ServerTask();
}

/**
 * @brief Initialize the socket and serve images to the PC.
 * This is the task that serves images to the PC in a loop. This runs
 * as a separate task.
 */
int TargetServer::ServerTask()
{
#if 1
    struct sockaddr_in  serverAddr;    /* server's socket address */ 
    struct sockaddr_in  clientAddr;    /* client's socket address */ 
    char                clientRequest[128]; /* request/Message from client */ 
    int                 sockAddrSize;  /* size of socket address structure */ 
    int                 sFd;           /* socket file descriptor */ 
    char                inetAddr[INET_ADDR_LEN]; 
                                       /* buffer for client's inet addr */ 
 
    /* set up the local address */ 
 
    sockAddrSize = sizeof (struct sockaddr_in); 
    bzero ((char *) &serverAddr, sockAddrSize); 
    serverAddr.sin_len = (u_char) sockAddrSize; 
    serverAddr.sin_family = AF_INET; 
    serverAddr.sin_port = htons (SERVER_PORT_NUM); 
    serverAddr.sin_addr.s_addr = htonl (INADDR_ANY); 
 
    /* create a UDP-based socket */ 
    printf("Socket call...");
    if ((sFd = socket (AF_INET, SOCK_DGRAM, 0)) == ERROR) 
        { 
        perror ("socket"); 
        return (ERROR); 
        } 
    printf("Success!\n");
    /* bind socket to local address */ 
    printf("Binding...");
    if (bind (sFd, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR) 
        { 
        perror ("bind"); 
        close (sFd); 
        return (ERROR); 
        } 
    int smallRcvBufferSize = 256; // two d-grams
    // hopefully this works
    setsockopt(sFd, SOL_SOCKET, SO_RCVBUF, reinterpret_cast<char*>(& smallRcvBufferSize), sizeof(smallRcvBufferSize));
    printf("Done!\n");
    /* read data from a socket and satisfy requests */
    
    /* read data from a socket and satisfy requests */ 
     
        FOREVER 
            { 
    	//printf("Waiting for a packet...\n");
            if (recvfrom (sFd, (char *) &clientRequest, sizeof (clientRequest), 0, 
                (struct sockaddr *) &clientAddr, &sockAddrSize) == ERROR) 
            { 
                perror ("recvfrom"); 
                close (sFd); 
                return (ERROR); 
            } 
            //printf("Recvfrom returned.\n");
     
            /* if client requested that message be displayed, print it */ 
     
            //if (clientRequest.display) 
            //    { 
                /* convert inet address to dot notation */
            
            istringstream packet(clientRequest);
            float x,y,dist;
            
            
            //{
            	//Synchronized sync(m_newTargetSem); 
            	m_targets.clear();
            	while (packet >> x >> y >> dist) {
            		Target t(x,y,dist);
            		m_targets.push_back(t);
            	}
            //}
            //Wait(.1);
             
             /*
            
                inet_ntoa_b (clientAddr.sin_addr, inetAddr); 
                printf ("MSG FROM CLIENT (Internet Address %s, port %d):\n%s\n", 
                    inetAddr, ntohs (clientAddr.sin_port), clientRequest);
                
                for (int i = 0; i < sizeof(clientRequest); ++i) {
                            	clientRequest[i] = ' ';
                }
                
                */
            	
                //} 
            } 
#else
	/* Setup to PC sockets */
	struct sockaddr_in serverAddr;
	int sockAddrSize = sizeof(serverAddr);
	int pcSock = ERROR;
	bzero ((char *) &serverAddr, sockAddrSize);
	serverAddr.sin_len = (u_char) sockAddrSize;
	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons (1735);
	serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);

	int success;
	while (true)
	{
		taskSafe();
		// Create the socket.
		if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
		{
			wpi_setErrnoErrorWithContext("Failed to create the TargetServer socket");
			continue;
		}
		// Set the TCP socket so that it can be reused if it is in the wait state.
		int reuseAddr = 1;
		setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
		// Bind socket to local address.
		if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
		{
			wpi_setErrnoErrorWithContext("Failed to bind the TargetServer port");
			close (pcSock);
			continue;
		}
		// Create queue for client connection requests.
		if (listen (pcSock, 1) == ERROR)
		{
			wpi_setErrnoErrorWithContext("Failed to listen on the TargetServer port");
			close (pcSock);
			continue;
		}

		struct sockaddr_in clientAddr;
		int clientAddrSize;
		int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
		if (newPCSock  == ERROR)
		{
			close(pcSock);
			continue;
		}
		//TODO: check camera error
		char *bp;
		char buffer[BUFLEN];
		bp = buffer;
		int bytes_to_read = BUFLEN-1;
		int n;
		
		
		while(!m_stopServer)
		{
			bool quick_escape = false; // ESCAPE!!
			
			// Keep reading, either until we don't have any more
			// stuff to read or we'll fill up the buffer
			while ((n = read(newPCSock, bp, bytes_to_read)) > 0) {
				// Advance "next available char" buffer by n bytes
				bp += n;
				// Subtact n bytes from bytes remaining in buffer
				bytes_to_read -= n;	
				
				// Check to see if we processed a newline. That's all
				// we care about for now.
				for (char * b = bp; b >= buffer && !quick_escape; --b) {
					if (*b == '\n') {
						quick_escape = true;
						break;
					}
				}
				
				// Did we see a newline? Then stop reading, it's a waste
				// of time.
				if (quick_escape) break;
			}
			// Some kind of error occured during read
			if (bytes_to_read < 0) {
				// Whoops, buffer full
				// Ignore the latest targets and restart
				cout << "Some kinda problem occured, I'm breaking out" << endl;
				
				// Break out of the server loop, reinitialize network link
				break;
			}
			
			// Tie off the end of the string with a NUL character
			*bp = 0;
			// Reset buffer pointer back at the beginning of buffer,
			// in case we loop through again
			bp = buffer;
			
			// Start reading numbers from the string we read
			istringstream line(buffer);
			
			*bp = 0;
			float x, y, dist;
			
			//semTake(m_newTargetSem, 1000);
			m_targets.clear();
			
			while (line >> x >> y >> dist) {
				// Grab three numbers at a time, these
				// constitute a single target
				
				// Create the target
				Target t(x,y,dist);
				
				// Add it to our list
				m_targets.push_back(t);
				
				// DEBUG: report that we received it
				cout << "TARGET FOUND: " << x << " " << y << " " << dist << endl;
			}
			
			int ack_len = strlen(ACK);
			success = send(newPCSock, ACK, ack_len, 0);
			
			if (success == ERROR) {
				cout << "ACK failed, setting up connection again" << endl;
				break;
			}
			
			
			//semGive(m_newTargetSem);
			// buffer now contains cool data
			//cout << buffer << endl;
			//Wait(.1);
			//continue;
		}
		// Clean up
		close (newPCSock);
		newPCSock = ERROR;
		close (pcSock);
		pcSock = ERROR;
		taskUnsafe();
		Wait(0.1);
	}
	return (OK);
#endif
}

std::vector<Target> TargetServer::GetTargets()
{
	std::vector<Target> temp_targets;
	/*
	if (semTake(m_newTargetSem, 1000) == ERROR) {
		return temp_targets;
	}*/
	
	//std::copy(m_targets.begin(), m_targets.end(), temp_targets.begin());
	/*{
		Synchronized sync(m_newTargetSem); */
		temp_targets = m_targets;
	//}
		
	//semGive(m_newTargetSem);
	return temp_targets;
}