/*
    Robot servo controller

    Sends contrller data to the server

    (C) 2004 Jason Hunt

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    Contact: huntjas2@msu.edu

    Compile with -lwsock32

    FILE: camera.cpp
    
    Network control for dealing with the camera feed.
*/


#include <windows.h>
#include <stdio.h>

#pragma hdrstop

#include "display.h"
#include "debug.h"
#include "camera.h"
#include "decompress.h"
#include "target.h"


SOCKET server_camera_socket;

bool camera_connected = false;          // true if date connected
bool attempting_camera_connect = false;
bool getting_camera_data = false;

unsigned int frames = 0;

static char compressed_image[MAX_COMPRESSED_SIZE];
static char buffer[REC_BUFFER_SIZE];

    
/* disconnect from server */
void camera_disconnect(void)
{
    closesocket(server_camera_socket);
    camera_connected = 0;
    add_line("[Camera] Disconnect.");
}
/* end of disconnect */


/* send keepalive data */
void request_image( void )
{
    char data_packet[1]; // the data packet
    data_packet[0] = 0x40;

    if (send(server_camera_socket, data_packet, 1, 0) == SOCKET_ERROR)
    {
        add_line("[Camera] Unable to send data");
        camera_connected = 0;
    }
}
/* end of send_control_state */



/* get data from the image server */
void read_camera_data(void)
{
    static unsigned int c_index, c_remain, c_size;

    // get the data
    unsigned int n_bytes = recv(server_camera_socket, buffer, REC_BUFFER_SIZE, 0);

    unsigned int bi = 0; // buffer index
    
    if (n_bytes == 0) return;


    while(bi < n_bytes)
    {
        // debug("bi: ", bi);
        if (getting_camera_data)
        {
            
            // final chunk
            if (n_bytes-bi >= c_remain)
            {
                if (c_index + c_remain > MAX_COMPRESSED_SIZE)
                {
                   add_line("[Camera] Buffer too small");            
                   getting_camera_data = false;
                   request_image();
                   return;
                }

                memcpy(compressed_image + c_index, buffer+bi, c_remain);

                bi += c_remain;
            
                c_remain = 0;

                // decompress image into display buffer
                decompress_image(compressed_image, c_size, camera_buffer, image_x, image_y);
                 
                // process image if targeting is enabled
                if (targeting)  process_frame( );                 
                 
                frames++;   // increment frame counter

                // force display
                InvalidateRect (main_win, NULL, FALSE);
                
                getting_camera_data = 0; 
                    
                request_image(); // get next image
            }
            else  // normal receive
            {    
                if (c_index + n_bytes > MAX_COMPRESSED_SIZE)
                {
                   add_line("[Camera] Buffer too small");            
                   return;
                }

                // add what we recived
                memcpy(compressed_image + c_index, buffer+bi, n_bytes-bi);

                c_index += n_bytes-bi;
                c_remain -= n_bytes-bi;
                                
                bi += n_bytes-bi;
            }                
        }
        else
        // check camera commands
        if (buffer[bi] == 0x40)
        {
            if (bi + 9 <= n_bytes)
            {
                // decode size data
                unsigned int image_x_size = (unsigned char)  buffer[bi+1] << 8;
                image_x_size += (unsigned char)  buffer[bi+2];
            
                unsigned int image_y_size = (unsigned char)  buffer[bi+3] << 8;
                image_y_size += (unsigned char) buffer[bi+4];

                // resize buffer if needed
                if (image_x_size != image_x || image_y_size != image_y)
                {
                    image_x = image_x_size;
                    image_y = image_y_size;
                    setup_buffer(); // resize buff for new image size
                }

                // decode compressed size
                c_size =  (unsigned char) buffer[bi + 5] << 24;
                c_size += (unsigned char) buffer[bi + 6] << 16;
                c_size += (unsigned char) buffer[bi + 7] << 8;
                c_size += (unsigned char) buffer[bi + 8];
            
                c_index = 0;
                c_remain = c_size;
                getting_camera_data = true;
                bi += 9; // skip header
            }
        }
        else // unknown, ignore
            bi++;
    
    }    
    
}
/* get of get camera data */



/* deal with network messages */
void camera_socket_message(WPARAM wParam,LPARAM lParam)
{
    switch (WSAGETSELECTEVENT(lParam))
    {
        case FD_ACCEPT:
            break;

        case FD_CONNECT:
            if(WSAGETSELECTERROR(lParam))
            {
                add_line("[Camera] Time out.");
                attempting_camera_connect = false;
                closesocket(server_camera_socket);
            }
            else
            {
                add_line("[Camera] Connected.");
                camera_connected = true;
                attempting_camera_connect = false;
                request_image(); // start the request chain
            }
            break;

        case FD_CLOSE:
            add_line("[Camera] Server disconnect.");
            camera_connected = false;
            closesocket(server_camera_socket);
            break;

        // data back from server
        case FD_READ:
            read_camera_data();
            break;

        case FD_WRITE:
            break;
    }
}
/* end of network_message */



/* attept to connect to te server */
void connect_to_camera_server( void )
{
    char out[256];
    sprintf(out, "[Camera] Connecting to %s:%d", host_name, CAMERA_PORT);
    add_line(out);

    // create the socket
    server_camera_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

    if (server_camera_socket == INVALID_SOCKET)
    {
        add_line("[Camera] Socket error.");
        return;
    }

    LPHOSTENT hostent;
    hostent = gethostbyname(host_name);

    if (!hostent)
    {
        add_line("[Camera] Host name error.");
        return;
    }

    SOCKADDR_IN server;

    memset(&server, 0, sizeof(server));

    server.sin_addr = *((LPIN_ADDR)*hostent->h_addr_list);
    server.sin_port = htons(CAMERA_PORT);
    server.sin_family = AF_INET;

    WSAAsyncSelect(server_camera_socket, main_win, WM_CAMEVENT, FD_WRITE | FD_CONNECT | FD_READ | FD_CLOSE);

    camera_connected = false;
    attempting_camera_connect = true;

    connect(server_camera_socket, (LPSOCKADDR)&server, sizeof(struct sockaddr));
}
/* end of connect_to_server */



