/*
    Robot Interface
    (C) 2006 Jason Hunt
    nulluser@gmail.com
    
    File: robot.cpp
*/


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

#include "robot.h"
#include "serial.h"
#include "display.h"
#include "utility.h"
#include "time.h"
#include "math.h"

bool remote_mode = false;


robot_class robot;  // Core instance of robot class


/* Robot constructor, set inital values */
robot_class::robot_class(void)
{
    // Setup Critical Sections
    InitializeCriticalSection(&control_data_lock);
    InitializeCriticalSection(&telemetry_data_lock);
    InitializeCriticalSection(&update_speed_lock);
    InitializeCriticalSection(&auto_mode_lock);
    
    
    update_speed = 0;
    updates = 0;

    auto_mode = false;

    reset_control();
}
/* End of robot_class constructor */

/* Stops motors and centers servos */
void robot_class::reset_control(void)
{
    control_type c;

    // Setup inital robot conditions

    c.left_motor_speed = 0;
    c.left_motor_target = 0;
    c.left_motor_dir = 0;

    c.right_motor_speed = 0;
    c.right_motor_target = 0;
    c.right_motor_dir = 0;

    c.servo_pos[0] = 0x80;
    c.servo_pos[1] = 0x80;

    set_control(c);
   
}
/* End of reset_control */


/* Robot destructor */
robot_class::~robot_class(void)
{
}
/* End of robot_class */


/* Start all robot sub systems */
void robot_class::start(void)
{
    start_serial();         // Start serial system
}
/* End of robot_class::start */


/* Stop all robot sub systems */
void robot_class::stop(void)
{
    stop_serial();          // Stop serial system
}
/* End of robot_class::stop */


/* Set robot auto mode */
void robot_class::set_auto_mode(bool mode)
{
    EnterCriticalSection(&auto_mode_lock);
    auto_mode = mode;    
    LeaveCriticalSection(&auto_mode_lock);
    
    if (!mode)
        reset_control();
    
}
/* End of set_auto_mode */


/* Set robot auto mode */
bool robot_class::get_auto_mode(void)
{
    bool mode;

    EnterCriticalSection(&auto_mode_lock);
    mode = auto_mode;
    LeaveCriticalSection(&auto_mode_lock);
    
    return(mode);
}
/* End of set_auto_mode */



/* Copy control data into the parameter */
// This consolidates all access to the control data to 
// Make it thread safe
//
void robot_class::get_control(control_type *control_data_copy)
{
    EnterCriticalSection(&control_data_lock);

    // Copy control data to copy
    memcpy(control_data_copy, &control_data, sizeof(control_data));

    LeaveCriticalSection(&control_data_lock);
}
/* End of get_control */


/* Put the control data into a buffer to be sent to the controller board */
void robot_class::get_control(unsigned char * buffer)
{
    EnterCriticalSection(&control_data_lock);

    // Clear buffer 
    memset(buffer, 0, SERIAL_SEND_SIZE);

    unsigned int i = 0;                                     // Index
    
    append_byte(buffer, i, 0xff);                           // Start byte
  
    unsigned char temp = 0;                                 // Temp control byte
    
    // Set direction bits      
    if (control_data.left_motor_dir) temp |= 0x01;          // Bit for left reverse
    if (control_data.right_motor_dir) temp |= 0x02;         // Bit for right reverse
      
    append_byte(buffer, i, temp);                           // Add the control byte
  
    // Speeds
    append_byte(buffer, i, (byte)control_data.left_motor_speed);  // Add left speed
    append_byte(buffer, i, (byte)control_data.right_motor_speed); // Add right speed

    // Servo positions
    append_byte(buffer, i, control_data.servo_pos[0]);      // Add pan
    append_byte(buffer, i, control_data.servo_pos[1]);      // Add tilt
                       
    buffer[SERIAL_SEND_SIZE-1] = 0xff;                      // End byte




   /* FILE *f = fopen("send.txt", "at");
    fprintf(f, "Send to robot %d %d %4f  %4f\n", control_data.left_motor_dir,
                                  control_data.right_motor_dir,
                                  control_data.left_motor_speed,
                                  control_data.right_motor_speed);

    fclose(f);*/





    LeaveCriticalSection(&control_data_lock);
}
/* End of get control */


/*  Copy passed control data to actual data */
void robot_class::set_control(control_type &new_control_data)
{
    EnterCriticalSection(&control_data_lock);

    // Copy new control data to actual control data
    memcpy(&control_data, &new_control_data, sizeof(control_type));

    LeaveCriticalSection(&control_data_lock);
}
/* End of set control */


/* Extract data from buffer into control data */
void robot_class::set_control(unsigned char *buffer)
{
    EnterCriticalSection(&control_data_lock);

    // Decode direction bits
    control_data.left_motor_dir = buffer[1] & 0x01;
    control_data.right_motor_dir = buffer[1] & 0x02;

    // Speeds
    control_data.left_motor_target = buffer[2];
    control_data.right_motor_target = buffer[3];

    if (control_data.left_motor_target == 0)
        control_data.left_motor_speed = 0;

    if (control_data.right_motor_target == 0)
        control_data.right_motor_speed = 0;



    // Servo Pos
    control_data.servo_pos[0] = buffer[4];
    control_data.servo_pos[1] = buffer[5];


   /*  FILE *f = fopen("send.txt", "at");
    fprintf(f, "sent from client %d %d %4f  %4f\n", control_data.left_motor_dir,
                                  control_data.right_motor_dir,
                                  control_data.left_motor_target,
                                  control_data.right_motor_target);

    fclose(f);*/

    
    LeaveCriticalSection(&control_data_lock);
}
/* End of set_control(buffer) */


/* Copy telemetry data into a struct */
void robot_class::get_telemetry(telemetry_type * telemetry_data_copy)   // telemetry into struct
{
    EnterCriticalSection(&telemetry_data_lock);

    // Copy telemetry data
    memcpy(telemetry_data_copy, &telemetry_data, sizeof(telemetry_type));

    LeaveCriticalSection(&telemetry_data_lock);    
}
/* End of get_telemetry */



/* Put telemetry data into buffer */
void robot_class::get_telemetry(unsigned char * buffer)   // tTlemetry into buffer
{
    EnterCriticalSection(&telemetry_data_lock);

    unsigned int i = 0;                                     // Index
    
    append_byte(buffer, i, 0xff);                           // Start byter
     
    // Add low words of analog
    append_byte(buffer, i, telemetry_data.analog_data[0] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[1] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[2] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[3] & 0xff);     

    append_byte(buffer, i, telemetry_data.analog_data[4] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[5] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[6] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[7] & 0xff);     

    unsigned char temp = 0;                                 // Temp control byte
  
    // High words of analog
    temp |= (telemetry_data.analog_data[0] >> 8) & 0x03;
    temp |= ((telemetry_data.analog_data[1] >> 8) & 0x03) << 2;
    temp |= ((telemetry_data.analog_data[2] >> 8) & 0x03) << 4;
    temp |= ((telemetry_data.analog_data[3] >> 8) & 0x03) << 6;
                        
    append_byte(buffer, i, temp);       

    temp = 0;                           
  
    temp |= (telemetry_data.analog_data[4] >> 8) & 0x03;
    temp |= ((telemetry_data.analog_data[5] >> 8) & 0x03) << 2;
    temp |= ((telemetry_data.analog_data[6] >> 8) & 0x03) << 4;
    temp |= ((telemetry_data.analog_data[7] >> 8) & 0x03) << 6;

    append_byte(buffer, i, temp); 
           
    append_byte(buffer, i, telemetry_data.encoder_counts[0] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[0] >> 8);     

    append_byte(buffer, i, telemetry_data.encoder_counts[1] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[1] >> 8);     

    append_byte(buffer, i, telemetry_data.encoder_counts[2] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[2] >> 8);     

    append_byte(buffer, i, telemetry_data.encoder_counts[3] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[3] >> 8);     
                        
    buffer[SERIAL_RECV_SIZE-1] = 0xff;                      // End byte
       
  /*  char b[300];
    sprintf(b, "data: %x %x %x %x %x %x %x %x", 
        buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7]);
    add_line(b);
    */   
       
       
       
    LeaveCriticalSection(&telemetry_data_lock); 
}
/* End of get_telemetry (buffer) */



#define num_long_ave 50

double long_aves[num_long_ave];
double long_ave = 0;
double ave_offset = 0;


double vel = 0;
double pos = 0;

void reset_vals( void )
{
  ave_offset = long_ave;
 vel = 0;
pos = 0;   
     
}



/* Set telemeter data from buffer */
void robot_class::set_telemetry(unsigned char * buffer)   // telemetry into buffer
{
    EnterCriticalSection(&telemetry_data_lock);

    // Decode the compacted analog data
    telemetry_data.analog_data[0] = (buffer[1] + (buffer[9] & 0x03) *256 )  ;
    telemetry_data.analog_data[1] = (buffer[2] + ((buffer[9]>>2)& 0x03) *256) ;
    telemetry_data.analog_data[2] = (buffer[3] + ((buffer[9]>>4)& 0x03) *256)  ;

    telemetry_data.analog_data[3] = (buffer[4] + ((buffer[9]>>6)& 0x03 )*256) ;
    telemetry_data.analog_data[4] = (buffer[5] + ((buffer[10])& 0x03) *256)  ;  
    telemetry_data.analog_data[5] = (buffer[6] + ((buffer[10]>>2)& 0x03) *256)  ;  

    telemetry_data.analog_data[6] = (buffer[7] + ((buffer[10]>>4)& 0x03) *256)  ;  
    telemetry_data.analog_data[7] = (buffer[8] + ((buffer[10]>>6)& 0x03) *256)  ;  
    
//     telemetry_data.encoders[0] = buffer[8];     
     
     telemetry_data.encoder_counts[0] = buffer[11];
     telemetry_data.encoder_counts[0] += (unsigned int)(buffer[12]) << 8;

     telemetry_data.encoder_counts[1] = buffer[13];
     telemetry_data.encoder_counts[1] += (unsigned int)(buffer[14]) << 8;

     telemetry_data.encoder_counts[2] = buffer[15];
     telemetry_data.encoder_counts[2] += (unsigned int)(buffer[16]) << 8;
     
     telemetry_data.encoder_counts[3] = buffer[17];
     telemetry_data.encoder_counts[3] += (unsigned int)(buffer[18]) << 8;
     
      char b[100];    
     
     
     
     static double last_time = get_time();
     static int last_count = 0;
     
     if (telemetry_data.encoder_counts[0] != last_count)
     {
         last_count =  telemetry_data.encoder_counts[0];
         last_time = get_time();
     }
     
//     sprintf(b, "last_time: %f  cur_time: %f diff: %f", last_time, get_time(), get_time() - last_time);
//     add_line(b);
         
    bool  moving = false; 
         
    if (get_time() - last_time < 0.10 || control_data.left_motor_target > 0
                                      || control_data.right_motor_target > 0)
    {
        moving = true;                 
    }      
                                          
   
     
       
     for (int i = 1; i < num_long_ave; i++)
          long_aves[i-1] = long_aves[i];
     
     long_aves[num_long_ave-1] = (double)(telemetry_data.analog_data[6]);
     
     double sum = 0;     

     for (int i = 0; i < num_long_ave; i++)
         sum += long_aves[i];
         
     long_ave =  sum / (double)(num_long_ave);     
     
     
     
     
     
     
     

     
     #define num_ave 3
     
     static double last_nums[num_ave];
     
     
     for (int i = 1; i < num_ave; i++)
          last_nums[i-1] = last_nums[i];
     
     last_nums[num_ave-1] = (double)(telemetry_data.analog_data[6]) - ave_offset;
     
     sum = 0;     

     for (int i = 0; i < num_ave; i++)
         sum += last_nums[i];

     double accl = sum / (double)(num_ave);
     
//     double accl = (double)telemetry_data.analog_data[6] - 480;
     


    static double last_accl_time = get_time();

     

     
     if (moving)//fabs(accl) > 5)
     {
      double cur_time = get_time();
      
      double time_diff = cur_time - last_accl_time;
      
      last_accl_time = cur_time;
      
      vel += accl * time_diff;
                    
     } else
     vel = 0;
     
      static double last_pos_time = get_time();

  
     
     
   //  if (fabs(vel) > 1)
     {
      double cur_time = get_time();
      
      double time_diff = cur_time - last_pos_time;
      
      last_pos_time = cur_time;
      
      pos += vel * time_diff * 10;
     }   



     
  //   FILE *f = fopen("accl.csv", "at");
   //  fprintf(f, "%d\n", telemetry_data.analog_data[6]);
  //   fclose(f);
     
     
     sprintf(b, "long ave: %f  Accel: %6.2f  vel: %6.2f  pos: %6.2f", long_ave, accl, vel, pos);
   //  add_line(b);
     
         /*
     sprintf(b, "Encoder counts: %5d %5d %5d %5d",
             telemetry_data.encoder_counts[0],
            telemetry_data.encoder_counts[1],
            telemetry_data.encoder_counts[2],
            telemetry_data.encoder_counts[3]);
     add_line(b);*/
     

     
     
    
    LeaveCriticalSection(&telemetry_data_lock);     
}
/* End of set_telemetry (buffer) */


/* Apply new motion data to robot */
void robot_class::set_motion(double x_move, double y_move)
{
//    char b[400];
//    sprintf(b, "x_motion: %f  y_motion: %f", x_move, y_move);
//    add_line(b);

    double l_motor = 2*y_move + x_move;
    double r_motor = 2*y_move - x_move;
    
    if (l_motor > 1) l_motor = 1;
    if (l_motor < -1) l_motor = -1;

    if (r_motor > 1) r_motor = 1;
    if (r_motor < -1) r_motor = -1;

//    sprintf(b, "l_motor: %f  r_motor: %f", l_motor, r_motor);
//    add_line(b);

    control_type c;

    robot.get_control(&c);

    // Set motion values
    c.left_motor_target = fabs(l_motor) * 255; 
    c.right_motor_target = fabs(r_motor) * 255;

    c.left_motor_dir = l_motor < 0;
    c.right_motor_dir = r_motor < 0;  
    
    
    char b[300];
    
/*    sprintf(b, "l:  %f  r:  %f     ld: %d  rd:  %d", 
    
      c.left_motor_target,
    c.right_motor_target,

    c.left_motor_dir, 
    c.right_motor_dir);  
    
    
    
    debug(b);*/
    
    
    
    robot.set_control(c);
}
/* End of set_motion */


/* Return the robot update speed */
double robot_class::get_update_speed(void)
{
    EnterCriticalSection(&update_speed_lock);

    // Copy update speed
    double speed = update_speed;

    LeaveCriticalSection(&update_speed_lock);

    return(speed);
}
/* End of get_update_speed */


/* This is called when an update is done */
void robot_class::update_complete(bool fail)
{
    // Check for update fail
    if (fail)
    {
        updates = 0;    
        update_speed = 0;
        return;
    }

    // Init on first run
    static double last_time = get_time();
    
    // Currect
    double cur_time = get_time();
    
    // Update sucess
    updates++;
    
    // Update speed 10 times per second
    if (cur_time - last_time > 0.1)
    {
        // Calculate update speed
        update_speed = updates / (cur_time - last_time);
        updates = 0;

        // Set new time offset
        last_time = cur_time;
    }
}
/* End of update_complete */








