#include <SDL.h>

#include "khepcc.h"
#include "sensors.h"
#include "events.h"

Uint32 callback_sensors(Uint32 interval, void *param)
{
  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 1s. */
  return 1000;
}


void update_sensors_data()
{
  printf("Read sensors\n");
  khep_cmd_ret_t res;

  res = khep_cmd_ambient_sensors(hand);
  if ( res.result == 0 )
    memcpy(amb_sens, res.data.sensor, sizeof(amb_sens));

  res = khep_cmd_proximity_sensors(hand);
  if ( res.result == 0 )
    memcpy(prox_sens, res.data.sensor, sizeof(prox_sens));

  res = khep_cmd_read_speed(hand);
  if ( res.result == 0 )
    memcpy(motor_speed, res.data.motor, sizeof(motor_speed));
}