#include <SDL.h>

#include <stdlib.h>
#include <time.h>

#include <access.h>
#include <raw.h>
#include <oneway_cmds.h>
#include <twoway_cmds.h>

#include "khepcc.h"
#include "render.h"
#include "events.h"
#include "display.h"
#include "sensors.h"
#include "commands.h"

/* Globals */
unsigned int amb_sens[KHEP_SENS_COUNT];
unsigned int prox_sens[KHEP_SENS_COUNT];
int motor_speed[KHEP_MOT_COUNT];

khep_state_t *st;
khep_hand_t hand;


int main(int argc, char *argv[])
{
    SDL_Surface *screen;

    /* Init display */
    if ( SDL_Init(SDL_INIT_TIMER | SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0 ) 
      {
        fprintf(stderr, "Impossible d'initialiser SDL: %s\n", SDL_GetError());
        exit(1);
      }

    atexit(onExit);

    screen = SDL_SetVideoMode(400, 400, 32, SDL_SWSURFACE);

    /* Feed random seed */
    srandom(time(NULL));

    if ( screen == NULL ) 
      {
	fprintf(stderr, "Impossible d'init le mode video : %s\n", SDL_GetError());
        exit(1);
      }

    SDL_WM_SetCaption("Khepera CC", "ICON");

    /* Create a timer which will redraw the screen every 100 ms. */
    SDL_AddTimer(100, callback_draw, NULL);

    /* Create a timer which will update sensors data every 100 ms. */
    //SDL_AddTimer(1000, callback_sensors, NULL);

    /* Create a timer which will redraw the screen every 100 ms. */
    SDL_AddTimer(500, callback_joystick, NULL);

    
    /* Open the joystick */
    joystick = SDL_JoystickOpen(0);


    khep_state_t state;
    memset(&state, 0, sizeof(khep_state_t));
    st = &state;

    /* Open a khepera device */
    khep_cmd_ret_code_t ret;

    hand = khep_open("/dev/rfcomm0", KHEP_B115200);

    if ( hand < 0 )
      {
	printf("Unable to open a Khepera, exiting\n");
	exit(1);
      }

    printf("%u\n", hand);

    for(;;)
      {
	
	SDL_Event event;

	if ( st->need_update )
	  {
	    ret = khep_cmd_setspeed(hand, st->mot_l, st->mot_r);

	    st->need_update = 0;
	  }
	
	while ( SDL_PollEvent(&event) ) 
	  {
	    switch ( event.type )
	      {
	      case SDL_KEYDOWN:
	      case SDL_KEYUP:
		treat_keys(st, event);
		break;

	      case SDL_QUIT:
		exit(0);
		break;
		
	      case SDL_USEREVENT:
		switch ( event.user.code )
		  {
		  case REDRAW_SCREEN:
		    draw_screen(st, screen);
		    break;
		  case UPDATE_SENSORS:
		    update_sensors_data();
		    break;
		  case UPDATE_JOYSTICK:
		    printf("updating joystick : %d, %d\n", (int)event.user.data1, (int)event.user.data2);
		    khepcc_set_speed(st, (int)event.user.data2);
		    khepcc_turn(st, (int)event.user.data1);
		    break;
		  }
		break;
	      }
	  }
      }
    
}