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

    file: input.cpp
*/

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

#include "main.h"
#include "input.h"
#include "robot.h"
#include "display.h"
#include "network.h"
#include "timer.h"
#include "target.h"
#include "joystick.h"

bool keys[256] = {0}; // Key table

//extern bool running;



bool debug = false;


bool update_camera = false;

double last_camera_update = 0;


bool in_motion = false;
double motion_time = 0;



/* Reset robot values when focus is lost */
void input_lose_focus( void )
{
    robot_clear_values();
}
/* End of input lose focus */




/* handle mouse movement */
void process_mouse( LPARAM lParam )
{
    if (!update_camera) return;

//    unsigned int pos_x = lParam & 0xffff; // decode position
//    unsigned int pos_y = ; 
     
    double current = get_time();

    // Do not update more than 100 times a second
    if (current - last_camera_update > 0.01)
    {
        last_camera_update = current;

        robot_update_pan_tilt(lParam & 0xffff, lParam >> 16);
    }

}
/* end of process_mouse */


/* Set the current robot motion */
void set_motion(double x_move, double y_move)
{
    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;

    // Set motion values
    robot_data.l_speed = (unsigned char )(fabs(l_motor) * 255); 
    robot_data.r_speed = (unsigned char )(fabs(r_motor) * 255);

    robot_data.l_dir = l_motor < 0;
    robot_data.r_dir = r_motor < 0;      
   
    robot_update();
    
}
/* End of set_motion */



/* Update robot movement based on current states */
void input_update(void)
{
    if (keys['W'] || joystick.up)
    {
        if (keys['A']|| joystick.left)  set_motion(-0.7, 0.3);  else
        if (keys['D']|| joystick.right) set_motion(0.7, 0.3);  else
            set_motion(0, 0.3);  
    } else
    if (keys['S'] || joystick.down)
    {
        if (keys['A']|| joystick.left)  set_motion(0.7, -0.3); else 
        if (keys['D']|| joystick.right) set_motion(-0.7, -0.3); else
            set_motion(0, -0.3); 
    } else    
    if (keys['A'] || joystick.left)  set_motion(-1, 0); else 
    if (keys['D'] || joystick.right) set_motion( 1, 0); else
    {
        set_motion(0, 0);
    } 

/*    if (keys['W'])
    {
        if (keys['A']) set_motion(-0.7, 0.5);  else
        if (keys['D']) set_motion(0.7, 0.5);  else
            set_motion(0, 0.1);  
    } else
    if (keys['S'])
    {
        if (keys['A']) set_motion(0.7, -0.5); else 
        if (keys['D']) set_motion(-0.7, -0.5); else
            set_motion(0, -0.1); 
    } else    
    if (keys['A']) set_motion(-1, 0); else 
    if (keys['D']) set_motion( 1, 0); else
    {
        set_motion(0, 0);
    } */
}
/* End of update input */



/* Update speeds for accel curves */
void check_input( void )
{
//     robot_update();
      
}


/* Called when a key is released */
void key_up( WPARAM wParam )
{
    keys[wParam % 256] = false;

    input_update();;    
}
/* End of process key up */




/* Called when a key is pressed */
void key_down(WPARAM wParam,  LPARAM lParam)
{
    if (lParam & REPEAT_CODE) return;        // Ignore repeat keys

    keys[wParam % 0xff] = true;

    bool update = false; // true if robot updated needed
    bool motion_update = false; // true if robot updated needed

    if (keys['K']) { robot_data.l_light = !robot_data.l_light; update = true; }
    if (keys['L']) { robot_data.r_light = !robot_data.r_light; update = true; }
   
    if (wParam == VK_ESCAPE) system_shutdown();     // Escape key

    if (wParam == 'F') display_fps = !display_fps;
    if (wParam == 'X') debug = !debug;
    if (wParam == 'C') update_camera = !update_camera;

    if (wParam == 'T') targeting = !targeting;
    if (wParam == 'R') d_frame = !d_frame;

    input_update();

}
/* End of process key down */


