#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <time.h>

#include <cwiid.h>
#include "gm.h"
#include "display.h"

/* Our wii's bluetooth and wii handles */
static bdaddr_t g_bdaddr[4];
static cwiid_wiimote_t *g_wiimotes[4];
static int g_num_controls = 0;
static int g_samples_received = 0;
static uint32_t g_starttime;

static volatile int g_redraw_lock = 0;

static const int g_width = 400, g_height = 300;

cwiid_mesg_callback_t cwiid_callback;
cwiid_err_t err;
void err(cwiid_wiimote_t *wiimote, const char *s, va_list ap)
{
  if (wiimote) printf("%d:", cwiid_get_id(wiimote)); else printf("-1:");
  vprintf(s, ap);
  printf("\n");
}

int main( int argc, char **argv ) {
  int LEDs[4] = { CWIID_LED1_ON,  CWIID_LED1_ON | CWIID_LED2_ON,
                  CWIID_LED1_ON | CWIID_LED2_ON | CWIID_LED3_ON,
                  CWIID_LED1_ON | CWIID_LED2_ON | CWIID_LED3_ON | CWIID_LED4_ON };
  int i;
  struct timeval now;

  signal( SIGINT, exit );
  display_init( g_width, g_height);

  cwiid_set_err(err);

  /* If no bdaddrs given on command line, just connect to
     first visible wii */
  if( argc <= 1 ) {
    char bt_out[64];
    g_bdaddr[0] = *BDADDR_ANY;

    while( !(g_wiimotes[g_num_controls] = cwiid_open( &g_bdaddr[0], 0 )))
      fprintf( stderr, "Unable to connect to wiimote\n" );

    ba2str( &g_bdaddr[0], bt_out );
    fprintf( stderr, "Connected to wiimote %i (BT-Addr: %s)\n", i, bt_out );
    g_num_controls++;
  }

  /* ... else try to reach every single wii */
  for( i=0; i<argc-1; ++i ) {
    str2ba(argv[i+1], &g_bdaddr[i]);

    while( !(g_wiimotes[g_num_controls] = cwiid_open( &g_bdaddr[i], 0 )))
      fprintf( stderr, "Unable to connect to wiimote %i (BT-Addr: %s)\n", i, argv[i+1] );

    g_num_controls++;
    fprintf( stderr, "Connected to wiimote %i (BT-Addr: %s)\n", i, argv[i+1] );
  }

  for( i=0; i<g_num_controls; ++i ) {
    if (cwiid_set_mesg_callback(g_wiimotes[i], cwiid_callback)) {
      fprintf(stderr, "Unable to set message callback\n");
      return -1;
    }

    /* Set report mode to IR */
    cwiid_set_rpt_mode(g_wiimotes[i], CWIID_RPT_IR);

    /* Enable call back */
    cwiid_enable(g_wiimotes[i], CWIID_FLAG_MESG_IFC);

    /* Reflect number in leds */
    cwiid_set_led(g_wiimotes[i], LEDs[i]);
  }

  gettimeofday(&now, (struct timezone *)NULL);
  g_starttime = now.tv_sec * 1000 + now.tv_usec / 1000; /* in ms */

  /* Spin and let call back do all the work */
  while( 1 ) {
    if( ! (++i & 0xffff ) ) {
      int wii_id, led;
      while( g_redraw_lock );

      for( wii_id=0; wii_id<4; ++wii_id )
        for( led = 0; led<4; ++led )
          display_rectangle( wii_id, led * g_width / 16, 0, g_width / 16, vubars_getinfo(wii_id, led)/2 );

      display_redraw();
      usleep( 1000 );
      vubars_reduce();
      display_rectangle( 2, g_width/2-10,    0, 20, -vubars_getweather()/2 );
      display_rectangle( 2, g_width/2-15, -128, 30, 1 );
      display_rectangle( 2, g_width/2-15,  -64, 30, 1 );
    }
  }

  return 0;
}

void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
                    union cwiid_mesg mesg[], struct timespec *timestamp)
{
  int i, j, k, wii_id, ledmask;
  int valid_sources;
  struct timeval now;
  uint32_t runtime;
  wii_pt coords[4];

  gettimeofday(&now, (struct timezone *)NULL);
  runtime = now.tv_sec * 1000 + now.tv_usec / 1000 - g_starttime; /* in ms */

  for( i=0; i<g_num_controls; ++i ) {
    if( wiimote == g_wiimotes[i] )
      wii_id = i;
  }
  g_redraw_lock = 1;
  display_clear();
  g_samples_received++;
  for (i=0; i < mesg_count; i++) {
    switch (mesg[i].type) {

    case CWIID_MESG_IR:
//      printf("IR Report (%i): ", wii_id );
//      printf("(%ld Hz) ", ( g_samples_received * 1000 ) / runtime );

      valid_sources = 0;
      for (j = 0; j < CWIID_IR_SRC_COUNT; j++) {
        if (mesg[i].ir_mesg.src[j].valid) {
          coords[valid_sources].x = (double)mesg[i].ir_mesg.src[j].pos[CWIID_X];
          coords[valid_sources].y = (double)mesg[i].ir_mesg.src[j].pos[CWIID_Y];

          display_circle( wii_id, coords[valid_sources].x / 5.0, coords[valid_sources].y / 5.0, 6 );
          valid_sources++;
//          printf("(%d,%d) ", mesg[i].ir_mesg.src[j].pos[CWIID_X],
//                             mesg[i].ir_mesg.src[j].pos[CWIID_Y]);
        }
      }

      if (!valid_sources) {
//        printf("no sources detected");
      }

      if( valid_sources == 4 ) {
        wii_calibrate( wii_id, coords );
      }

      ledmask = 0;
      for( j=0; j<valid_sources; ++j ) {
        int led = wii_point_to_led( wii_id, coords+j );
        ledmask |= 1<<led;
      }
      for( j=0; j<4; ++j ) {
        vubars_peak( wii_id, j, ( ledmask >> j ) & 1 );
      }

      break;
    default:
      break;
    }
  }
  g_redraw_lock = 0;
}