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

#include "gm.h"
#include "vubars.h"
#include "heuristics.h"

// This is our current idea of how we want to influence the weather
// 128 means: do nothing at all
// 
// We want to default to softening the weather, so our value always
// flows back towards 100
static double vu_value_main       = 100.0;
static double vu_value_softening  = 0.0;
static double vu_value_activating = 0.0;

static int wii_last[WII_COUNT * 4];
static int wii_bars[WII_COUNT * 4];

int vubars_peak( int wii_id, int led, int is_on ) {
  if( is_on ) {
    wii_last[wii_id*4+led] = 0;
  } else {
    if( ++wii_last[wii_id*4+led] == 1 )
      wii_bars[wii_id*4+led] = VU_PEAK;
  }
}

int vubars_getinfo( int wii_id, int led ) {
  return wii_bars[wii_id*4+led];
}

int vubars_getweather( ) {
  return (int)vu_value_main;
}

static int GESTURE_A_dd  = 0;
static int GESTURE_A_dx  = 0;
static int GESTURE_A_dy  = 0;

static int GESTURE_A_off = 0;
static int GESTURE_A_ttl = 0;

static int GESTURE_B_dd  = 0;
static int GESTURE_B_dx  = 0;
static int GESTURE_B_dy  = 0;

static int GESTURE_B_off = 0;
static int GESTURE_B_ttl = 0;

static void match_gesture( int off, int dx, int dy, int dd ) {
  if( ( GESTURE_A_dd == dd ) && ( GESTURE_A_off == off + dd ) ) {
    printf( "...continued A\n" );

    GESTURE_A_off = off;
    GESTURE_A_dx  = dx;
    GESTURE_A_dy  = dy;
    GESTURE_A_ttl = VU_PEAK;

  } else if ( ( GESTURE_B_dd == dd ) && ( GESTURE_B_off == off + dd ) ) {
    printf( "...continued B\n" );

    GESTURE_B_off = off;
    GESTURE_B_dx  = dx;
    GESTURE_B_dy  = dy;
    GESTURE_B_ttl = VU_PEAK;

  } else {
    printf( "\n" );

    if( GESTURE_B_ttl >= GESTURE_A_ttl ) {
      GESTURE_A_off = off;
      GESTURE_A_dd  = dd;
      GESTURE_A_dx  = dx;
      GESTURE_A_dy  = dy;
      GESTURE_A_ttl = VU_PEAK;
    } else {
      GESTURE_B_off = off;
      GESTURE_B_dd  = dd;
      GESTURE_B_dx  = dx;
      GESTURE_B_dy  = dy;
      GESTURE_B_ttl = VU_PEAK;
    }
  }
}

void vubars_reduce() {
  int i;
  double diff;

  // First do some heuristics voodoo
  int offa, offb, dxa, dxb, dya, dyb, dda, ddb;

  heuristics_getGestures( 16, wii_bars, &offa, &dxa, &dya, &dda, &offb, &dxb, &dyb, &ddb );

  if( ( offa != -1 ) && ( dxa > 2 ) ) {
    printf( "1mary gesture detected: offset: % 2d direction: %s len: % 2d speed: %lf",
            offa, dda < 0 ? "left" : "rite", dxa, (double)dya / (double)dxa );
    match_gesture( offa, dxa, dya, dda );
  }
  

  if( ( offb != -1 ) && ( dxb > 2 ) ) {
    printf( "2ndry gesture detected: offset: % 2d direction: %s len: % 2d speed: %lf\n",
            offb, ddb < 0 ? "left" : "rite", dxb, (double)dyb / (double)dxb );
    match_gesture( offb, dxb, dyb, ddb );
  }

  // Have two gestures living at one
  if( GESTURE_A_ttl && GESTURE_B_ttl ) {
    if( GESTURE_A_dd == GESTURE_B_dd ) {
//      printf( "Two gestures pointing into the same direction.\n" );
    } else {
      // The more interesting case: hands move into opposite directions
      if( GESTURE_A_dd == -1 ) {
         if ( GESTURE_A_off < GESTURE_B_off )
           printf( "Slower! (Case 1)\n" );
         else
           printf( "Faster! (Case 2)\n" );
      } else {
         if ( GESTURE_A_off < GESTURE_B_off )
           printf( "Faster! (Case 3)\n" );
         else
           printf( "Slower! (Case 4)\n" );
      }
    }
  }

  // Now try to modify the weather by using values from our vu-bars
  diff = (double)heuristics_getEnergy( 16, wii_bars );
  vu_value_main += diff / 40000.0;

  diff = vu_value_main - 100.0;
  if( diff<0 ) diff = -diff;
  diff = sqrt( diff );

  if( vu_value_main > 100.0 ) diff = -diff;

  vu_value_main += diff;

  for( i=0; i<WII_COUNT*4; ++i )
    if( wii_bars[i] )
      wii_bars[i]-=VU_STEP;

  if( GESTURE_A_ttl ) GESTURE_A_ttl -= VU_STEP;
  if( GESTURE_B_ttl ) GESTURE_B_ttl -= VU_STEP;

}