//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.

#include <survive.h>
#include <stdio.h>
#include <stdlib.h>

#include <string.h>
#include <sys/time.h>

#include "survive_config.h"
#include "survive_default_devices.h"

#include "redist/os_generic.h"

struct SurvivePlaybackData {
  SurviveContext * ctx;
  const char* playback_dir; 
  FILE* playback_file;
  int lineno;

  FLT time_factor;
  double next_time_us; 
};
typedef struct SurvivePlaybackData SurvivePlaybackData;


double timestamp_in_us() {
  static double start_time_us = 0;
  if(start_time_us == 0.)
    start_time_us = OGGetAbsoluteTime();
  return OGGetAbsoluteTime() - start_time_us;
}

static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) {  
  char dev[10];
  int timecode = 0;
  FLT accelgyro[6];
  int mask;
  int id;

  int rr = sscanf(line,"I %s %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev,
		  &mask,
		  &timecode,
		  &accelgyro[0], &accelgyro[1], &accelgyro[2],
		  &accelgyro[3], &accelgyro[4], &accelgyro[5], &id );

  if( rr != 10 )
    {
      fprintf( stderr, "Warning:  On line %d, only %d values read: '%s'\n", driver->lineno, rr, line );
      return -1;
    }

  SurviveObject * so = survive_get_so_by_name( driver->ctx, dev);
  if(!so) {
    fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno);	  
    return -1;
  }
	
  driver->ctx->imuproc( so, mask, accelgyro, timecode, id);
  return 0;
}


static int parse_and_run_lightcode(const char* line, SurvivePlaybackData* driver) {  
  char lhn[10];
  char axn[10];
  char dev[10];
  uint32_t timecode = 0;
  int sensor = 0;
  int acode = 0;
  int timeinsweep = 0;
  uint32_t pulselength = 0;
  uint32_t lh = 0;

  int rr = sscanf(line,"%8s %8s %8s %u %d %d %d %u %u\n",
		  lhn, axn, dev,
		  &timecode, &sensor, &acode,
		  &timeinsweep, &pulselength, &lh );

	if( rr != 9 )
	  {
	    fprintf( stderr, "Warning:  On line %d, only %d values read: '%s'\n", driver->lineno, rr, line );
	    return -1;
	  }

	SurviveObject * so = survive_get_so_by_name( driver->ctx, dev);
	if(!so) {
	  fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno);	  
	  return -1;
	}
	
	driver->ctx->lightproc( so, sensor, acode, timeinsweep, timecode, pulselength, lh);
	return 0;
}

static int playback_poll( struct SurviveContext * ctx, void * _driver ) {
  SurvivePlaybackData* driver = _driver;
  FILE* f = driver->playback_file;

  if(f && !feof(f) && !ferror(f) )
    {
      int i;
      driver->lineno++;
      char  * line;

      if(driver->next_time_us == 0) {
	char * buffer;
	size_t n = 0;      
	ssize_t r = getdelim( &line, &n, ' ', f );
	if( r <= 0 ) return 0;

	if(sscanf(line, "%lf", &driver->next_time_us) != 1) {
	  free(line);
	  return 0;
	}
	free(line);
	line = 0;
      }

      if(driver->next_time_us * driver->time_factor > timestamp_in_us())
	return 0;
      driver->next_time_us = 0;
      
      char * buffer;
      size_t n = 0;      
      ssize_t r = getline( &line, &n, f );
      if( r <= 0 ) return 0;
      
      if((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || line[1] != ' ' )
	return 0;

      switch(line[0]) {
      case 'L':
      case 'R':
	parse_and_run_lightcode(line, driver);
	break;
      case 'I':
	parse_and_run_imu(line, driver);
	break;
      }

      free( line );
    } else {
    if(f) {
      fclose(driver->playback_file);
    }
    driver->playback_file = 0;
    return -1;
  }
  
  return 0;
}

static int playback_close( struct SurviveContext * ctx, void * _driver ) {
    SurvivePlaybackData* driver = _driver;
    if(driver->playback_file)
      fclose(driver->playback_file);
    driver->playback_file = 0; 
  return 0;
}


static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so)
{
	SurviveContext * ctx = sv->ctx;
	char * ct0conf = 0;
	
	char fname[100];
	sprintf( fname, "%s/%s_config.json", sv->playback_dir, so->codename );
	FILE * f = fopen( fname, "r" );

	if(f == 0 || feof(f) || ferror(f) )
	  return 1;
	    
	fseek(f, 0, SEEK_END);
	int len = ftell(f);
	fseek(f, 0, SEEK_SET);  //same as rewind(f);

	ct0conf = malloc(len+1);       
	int read = fread( ct0conf, len, 1, f);
	fclose( f );
	ct0conf[len] = 0;

	printf( "Loading config: %d\n", len );
	return survive_load_htc_config_format(ct0conf, len, so);
}



int DriverRegPlayback( SurviveContext * ctx )
{
  const char* playback_dir = config_read_str(ctx->global_config_values,
					      "PlaybackDir", "");

  if(strlen(playback_dir) == 0) {
    return 0;
  }

  SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) );
  sp->ctx = ctx;
  sp->playback_dir = playback_dir;
  sp->time_factor = config_read_float(ctx->global_config_values, "PlaybackFactor", 1.); 
    
  printf("%s\n", playback_dir);

  char playback_file[100];
  sprintf( playback_file, "%s/events", playback_dir );	
  sp->playback_file = fopen( playback_file, "r"); 
  if(sp->playback_file == 0) {
  fprintf(stderr, "Could not open playback events file %s", playback_file);
  return -1;
}
  SurviveObject * hmd = survive_create_hmd(ctx, "Playback", sp);
  SurviveObject * wm0 = survive_create_wm0(ctx, "Playback", sp, 0);
  SurviveObject * wm1 = survive_create_wm1(ctx, "Playback", sp, 0);
  SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp);
  SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp);
  
  if( !LoadConfig( sp, hmd )) { survive_add_object( ctx, hmd ); }
  if( !LoadConfig( sp, wm0 )) { survive_add_object( ctx, wm0 ); }
  if( !LoadConfig( sp, wm1 )) { survive_add_object( ctx, wm1 ); }
  if( !LoadConfig( sp, tr0 )) { survive_add_object( ctx, tr0 ); }
  if( !LoadConfig( sp, ww0 )) { survive_add_object( ctx, ww0 ); }

  
  survive_add_driver(ctx, sp, playback_poll, playback_close, 0); 
  return 0;
 fail_gracefully:
  return -1;
}

REGISTER_LINKTIME( DriverRegPlayback );