/*
    Robot Interface Remote Client
    (C) 2006 Jason Hunt
    nulluser@gmail.com
    
    file: network.h
*/

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

#include "robot.h"
#include "types.h"
#include "main.h"
#include "image.h"
#include "display.h"
#include "network.h"


robot_data_type robot_data;                 // Actual robot data

/* Called to set inital values */
void robot_start( void )
{
    robot_clear_values();
}
/* End of robot start */


/* Send all data to robot */
void robot_update( void )
{
    unsigned char  buff[30];
    
    buff[0] = 0x30; // command to update robot data

    buff[1] = 0xff;

    buff[2] = 0;

    if (robot_data.l_dir) buff[2] |= 0x01;
    if (robot_data.r_dir) buff[2] |= 0x02;

    buff[3] = robot_data.l_speed;
    buff[4] = robot_data.r_speed;

    buff[5] = robot_data.pan;
    buff[6] = robot_data.tilt;

    buff[11] = 0xff;

    network_send(buff, 12);
}
/* End of robot_data */


/* End of robot clear values */
void robot_clear_values( void )
{
    robot_data.max_l_speed = 0;
    robot_data.max_r_speed = 0;    

    robot_data.l_speed = 0;
    robot_data.r_speed = 0;    

    robot_data.l_dir = 0;
    robot_data.r_dir = 0;    

    robot_data.l_light = 0;
    robot_data.r_light = 0;    

    robot_data.pan = 0x80;
    robot_data.tilt = 0x80;    

    for (int i = 0; i < 8; i++)
        robot_data.analog[i] = 0;
    
}
/* End of robot clear values */


void robot_reset( void )
{
    robot_clear_values();

    robot_update();     
}


void request_sensor_data( void )
{
    network_send(ROBOT_SENSOR_DATA);
}


/* Deal with sensor data from server */
// Size is the amount of data the server has
// Return the number of bytes used
//
int robot_sensor_data( unsigned char * data, unsigned int size )
{
    if (size < SERIAL_RECV_SIZE) return(0);
    
    robot_data.analog[0] = (((unsigned int)(data[9]) & 0x03) << 8) + data[1];                              
    robot_data.analog[1] = (((unsigned int)(data[9]>>2) & 0x03) << 8) + data[2];                              
    robot_data.analog[2] = (((unsigned int)(data[9]>>4) & 0x03) << 8) + data[3];                              
    robot_data.analog[3] = (((unsigned int)(data[9]>>6) & 0x03) << 8) + data[4];                              

    robot_data.analog[4] = (((unsigned int)(data[10]) & 0x03) << 8) + data[5];                              
    robot_data.analog[5] = (((unsigned int)(data[10]>>2) & 0x03) << 8) + data[6];                              
    robot_data.analog[6] = (((unsigned int)(data[10]>>4) & 0x03) << 8) + data[7];                              
    robot_data.analog[7] = (((unsigned int)(data[10]>>6) & 0x03) << 8) + data[8];                              
    
    return(SERIAL_RECV_SIZE); // used two bytes
}
/* End of robot_sensor_data */


/* Update the pan and tilt servo values */
void robot_update_pan_tilt(unsigned int x, unsigned int y)
{
       // range check
  //  if (!targeting) return;

    if (y > display_y - 1) return;


    if (x < 0) x = 0;
    if (x > image_x_size - 1) x = image_x_size - 1;

    if (y < 0) y = 0;
    if (y > image_y_size - 1) y = image_y_size - 1;
        
    const int tilt_upper_lim = 0xff;
    const int tilt_lower_lim = 0x1f;
    
    const int pan_upper_lim = 0xff;
    const int pan_lower_lim = 0x00;  
     
    // scale into servo pos
    robot_data.pan = 255 - pan_lower_lim - (unsigned char)(((pan_upper_lim - pan_lower_lim)/ double(image_x_size)) * x);
    robot_data.tilt = 255 - tilt_lower_lim - (unsigned char)(((tilt_upper_lim - tilt_lower_lim)/ double(image_y_size)) * y);

  //  robot_update();
}
/* End of robot_update_pan_tilt */