#include <SDL.h>

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


SDL_Joystick *joystick;

/* Clear all stuff on exit */
void onExit()
{
        free(displayData);
	khep_close(hand);
	SDL_JoystickClose(joystick);

        SDL_FreeSurface(displaySurface);
        SDL_Quit();
}

static int last_x=0;
static int last_y=0;

Uint32 callback_joystick(Uint32 interval, void *param)
{
  SDL_Event event;
  event.type = SDL_USEREVENT;
  event.user.code = UPDATE_JOYSTICK;
  //SDL_PushEvent(&event);

  int x = -(SDL_JoystickGetAxis(joystick, 0) - 512) / 100;
  int y = -(SDL_JoystickGetAxis(joystick, 1) - 512) / 100;

  int update=0;

  // default
  event.user.data1 = (int*)last_x;
  event.user.data2 = (int*)last_y;

  if ( last_x != x )
    {
      event.user.data1 = (int*)x;
      last_x = x;
      update = 1;
    }

  if ( last_y != y )
    {
      event.user.data2 = (int*)y;
      last_y = y;
      update = 1;
    }

  if ( update )
    SDL_PushEvent(&event);

  // Check if a button is pressed
  if ( SDL_JoystickGetButton(joystick, 0) )
    {
      SDL_Event event;
      event.type = SDL_USEREVENT;
      event.user.code = UPDATE_SENSORS;
      SDL_PushEvent(&event);
    }

  /* We want the callback function to be called again in 200 ms. */
  return 200;
}

void treat_keys(khep_state_t *st, SDL_Event event)
{
  SDL_Event quit;

  switch ( event.type )
    {
    case SDL_KEYDOWN:
      switch ( event.key.keysym.sym )
	{
	  /* Reads */
	case SDLK_o:
	  printf("Read sensors\n");
	  khep_cmd_ret_t res = khep_cmd_read_speed(hand);
	  printf("Speed = %d, %d\n", 
		 res.data.motor[KHEP_MOT_L],
		 res.data.motor[KHEP_MOT_R]);
		 
	  break;

	  /* Moves */
	case SDLK_UP:
	  printf("Forward\n");
	  st->mot_l += khep_speed;
	  st->mot_r += khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_DOWN:
	  printf("Backward\n");
	  st->mot_l -= khep_speed;
	  st->mot_r -= khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_RIGHT:
	  printf("Right\n");
	  st->mot_l += khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_LEFT:
	  printf("Left\n");
	  st->mot_r += khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_ESCAPE:
	  quit.type = SDL_QUIT;
	  SDL_PushEvent(&quit);
	default:
	  break;
	}
      break;

      /* Release key */
    case SDL_KEYUP:
      switch ( event.key.keysym.sym )
	{
	case SDLK_UP:
	  printf("Stop Forward\n");
	  st->mot_r -= khep_speed;
	  st->mot_l -= khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_DOWN:
	  printf("Stop Backward\n");
	  st->mot_r += khep_speed;
	  st->mot_l += khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_RIGHT:
	  printf("Stop Right\n");
	  st->mot_l -= khep_speed;
	  st->need_update = 1;
	  break;
	case SDLK_LEFT:
	  printf("Stop Left\n");
	  st->mot_r -= khep_speed;
	  st->need_update = 1;
	  break;
	default:
	  break;
	}

    default:
      break;
    }
}