#include "ifqcam.h"
#include "error.h"
#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
 
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/videodev.h>
 
#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <X11/Xos.h>
#include <X11/Xatom.h>
 
#define SETWIDTH 320
#define SETHEIGHT 240
#define SETDEPTH 3
#define SETFOV 0.1
#define SETZDIST 50
#define WINDOWWIDTH 980
#define WINDOWHEIGHT 480
#define MAXVIEWS 15
 
 
// Globals that all files can see
int imgwidth = SETWIDTH;
int imgheight = SETHEIGHT;
int imgdepth = SETDEPTH;
 
 
extern Display *display;
extern Window win;
 
typedef unsigned char * charp;
 
pthread_mutex_t a_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 
struct thread_data
{
  char deviceName[40];
  int device;
  char name[200];
  int maxFrames;
  int xOffset;
  int yOffset;
  char experiment;
  char nopng;
  int cam;
  char isbottom;
  pthread_mutex_t the_mutex;
};
 
void * cam_read( void * theInfo )
{
  int width, height, depth;
  int i, result;
  unsigned char * image;
  struct thread_data *data;
  data = (struct thread_data *)theInfo;
 
  char prefix[200];
  int rc;
 
  fflush(stdout);
  printf( "%s\n", data->deviceName );
/*   rc = pthread_mutex_lock( &a_mutex ); */
/*   if( !rc ) */
/*     { */
/*       printf( "Mutex locked in cam_open\n" ); */
/*     }   */
/*   else */
/*     { */
/*       printf( "Cant lock mutex in cam_open\n" ); */
/*     } */
  //data->device = open( data->deviceName, O_RDWR);
  data->device = open_camera( data->deviceName, a_mutex );  
/*   if( !rc ) */
/*     { */
/*       printf( "Mutex unlocked in cam_open\n" ); */
/*     } */
  fflush(stdout);
  printf( "%s %d\n", "hi", data->device );
 
 
  for( i = 0; i < data->maxFrames; i++ )
    {
      printf( "%s %i\n", "READ FRAME:", i );
      // read the cam image
      result = qcam_read( data->device, &width, &height, &depth,
			  &image, a_mutex );
      printf( "%d\n", result );
      if( !result )	
	{
	  printf( "%s %i\n", "BLEAH:", i );
	  get_actual_image( image, width, height, depth,
			    data->xOffset, data->yOffset, data->isbottom, 1 );
	}
 
      if( !data->nopng )
	{
	  if( !data->experiment )
	    {
	      sprintf( prefix, "../var/%s_%d/%s_%d %03d.png", data->name, data->cam, data->name, data->cam, i );
	    }
	  else if ( data->experiment )
	    {
	      sprintf( prefix, "../var/experiments/image_sequences/%s/%s_%d/%s_%d %03d.png", data->name, 
		       data->name, data->cam, data->name, 
		       data->cam, i );
	    }
	  // Ends up looknig like "dirname_cam/frame.png"
	  png_write_qcam( prefix, width, height, i, image, data->isbottom );
	  //fprintf( stdout, "%s\n", prefix );
	}
      free( image );
    }
 
  close_camera( data->device );
  pthread_exit(0);
}
 
 
 
int main (int argc, char ** argv) 
{
  charp image[MAXVIEWS];
  int width[MAXVIEWS], height[MAXVIEWS], depth[MAXVIEWS];
  int xOffSet[MAXVIEWS], yOffSet[MAXVIEWS];
  int error;
  int i, j;
  int gzpx[MAXVIEWS], gzpy[MAXVIEWS], gzpz[MAXVIEWS];
  double pitch[MAXVIEWS], roll[MAXVIEWS], yaw[MAXVIEWS];
  int isbottom[MAXVIEWS];
  int devices[MAXVIEWS]; // Store the camera device(s)
  FILE *stereo; // stereo.dat file pointer
  FILE *directory[MAXVIEWS]; // File pointer for the flatview info in each dir
  int numDirs = 0; // Holds the number of directorys we will be looking at, must be less than MAXVIEWS.
  int maxFrames, currentFrame;
  int viewNum; // Stores the current view being looked at.
  char name[125] = {};// Stores the name of the capture we are working on
  char filename[125];
  FILE *fp;
  char prefix[125];
 
  int nopng = 0; // if 1 no png will be writen.
  int up_down = 0;
  int ud_cam = 0;
  int go = 0;
  int experiment = 0; // if 1 this is capturing an experiment and will create the corect directories accordingly.
 
  pthread_t threads[MAXVIEWS] = {};
  struct thread_data *info[MAXVIEWS] = {};
 
 
  // Get the program flags
  for( i = 0; i < argc; i++ )
    {
      if( !strcmp( argv[i], "-npng" ))
	nopng = 1;
      if( !strcmp( argv[i], "-name" ))
	strcpy( name, argv[++i] );
      if( !strcmp( argv[i], "-exp" ))
	experiment = 1;
      if( !strcmp( argv[i], "-cams" ))
	numDirs = argv[++i];
      if( !strcmp( argv[i], "-u" ))
	up_down = 1;
      if( !strcmp( argv[i], "-go" ))
	go = 1;
    }
 
  if( !go )
    {
      // First get all the info from the user about what we will be caputring
      if( !strcmp( name, "" ) && !nopng)
	{
	  fprintf( stdout, "What will this test be called? " );
	  fscanf( stdin, "%s", name );
	}
 
      if( numDirs == 0 )
	{
	  fprintf( stdout, "How many cameras will we be capturing from? " );
	  fscanf( stdin, "%d", &numDirs );
	}
 
      if( up_down )
	{
	  fprintf( stdout, "What camera is upside down? " );
	  fscanf( stdin, "%d", &ud_cam );
	}
    }
  for( i = 0; i < numDirs; i++ )
    {
      if( !nopng && !go )
	{
	  // Get the x, y, z, yaw, pitch, roll for each camera
	  fprintf( stdout, "cam %d X pos: ", i+1 );
	  fscanf( stdin, "%d", &gzpx[i] );
	  fprintf( stdout, "cam %d Y pos: ", i+1 );
	  fscanf( stdin, "%d", &gzpy[i] );
	  fprintf( stdout, "cam %d Z pos: ", i+1 );
	  fscanf( stdin, "%d", &gzpz[i] );
 
	  fprintf( stdout, "cam %d Yaw: ", i+1 );
	  fscanf( stdin, "%lf", &yaw[i] );
	  fprintf( stdout, "cam %d Pitch: ", i+1 );
	  fscanf( stdin, "%lf", &pitch[i] );
	  fprintf( stdout, "cam %d Roll: ", i+1 );
	  fscanf( stdin, "%lf", &roll[i] );
 
	  fprintf( stdout, "If the camera is upside down enter 1, otherwise enter 0. " );
	  fscanf( stdin, "%d", &isbottom[i] ); 
 
	  // Make the directories that the pics will be stored in
	  {
	    if( !experiment )
	      {
		sprintf( filename, "mkdir ../var/%s_%d", name, i ); 
		//printf( "%s\n", filename );
	      }
	    else if ( experiment )
	      {
		sprintf( filename, "mkdir ../var/experiments/image_sequences/%s/%s_%d", name, name, i );
	      }
	    system( filename );
	  }
	}
      else
	{
	  gzpx[i]      = 0;
	  gzpy[i]      = 0;
	  gzpz[i]      = 0;
	  yaw[i]       = 0;
	  pitch[i]     = 0;
	  roll[i]      = 0;
	  if( up_down == 1 && (ud_cam - 1) == i )
	    isbottom[i]  = 1;
	  else
	    isbottom[i] = 0;
	}
    }
 
  if( !nopng )
    {
      fprintf( stdout, "How many frames will we capture? " );
      fscanf( stdin, "%d", &maxFrames );
    }
  else
    {
      maxFrames = 5000;
    }
 
 
  // init the display
  init_display(argv[0],WINDOWWIDTH,WINDOWHEIGHT);
 
 
  // Setup the x and y offset for the images 
  for( i = 0; i < numDirs; i++ )
    {
      if( i < 2 )
	{
	  xOffSet[i] = ( i % 3 ) * 320;
	  yOffSet[i] = ( i / 2 ) * 240;
	}
      if( i >= 2 )
	{
	  xOffSet[i] = (( i + 1 ) % 3 ) *  320;
	  yOffSet[i] = (( i + 1 ) / 2 ) + 240;
	}
    }
 
  // Write the .dat file for each directory
  if( !nopng )
    {
      for( i = 0; i < numDirs; i++ )
	{
	  if( !experiment )
	    {
	      sprintf( filename, "../var/%s_%d.dat", name, i );
	      sprintf( prefix, "%s_%d/%s_%d", name, i, name, i );
	    }
	  else if ( experiment )
	    {
	      sprintf( filename, "../var/experiments/image_sequences/%s/%s_%d.dat", name, name, i );
	      sprintf( prefix, "experiments/image_sequences/%s/%s_%d/%s_%d", name, name, i, name, i );
	    }
	  fp = fopen( filename, "w" );
	  fprintf( fp, "%s\n%d\n%d\n%d\n%d %d %d\n%lf %lf %lf\n", prefix, 0,
		   currentFrame, 3, gzpx[i], gzpy[i], gzpz[i],
		   yaw[i], pitch[i], roll[i] );
	  fclose( fp );
	}
    }
 
 
  // Make all the threads
  for( i = 0; i < numDirs; i++ )
    {
      info[i] = ( struct thread_data* )malloc( sizeof( struct thread_data ));
      sprintf( info[i]->deviceName, "/dev/video%d", i );
      strcpy( info[i]->name, name );
      info[i]->maxFrames   = maxFrames;
      info[i]->xOffset     = xOffSet[i];
      info[i]->yOffset     = yOffSet[i];
      info[i]->nopng       = nopng;
      info[i]->experiment  = experiment;
      info[i]->cam         = i;
      info[i]->isbottom    = isbottom[i];
      //info[i]->the_mutex   = a_mutex;
 
      pthread_create( &threads[i], 0,
		      cam_read,
		      (void *)info[i]);
    }
 
/*   for( i = 0; i < numDirs; i++ ) */
/*     { */
/*       pthread_join(info[i], 0); */
/*     } */
 
 
}

int open_camera(const char *devicename, pthread_mutex_t mutex)
{
  int rc;
  rc = pthread_mutex_lock( &a_mutex );
  if( !rc )
    {
      printf( "Mutex locked in cam_open\n" );
    }  
  else
    {
      printf( "Cant lock mutex in cam_open\n" );
    }
  int device = open(devicename, O_RDWR);
  pthread_mutex_unlock( &a_mutex );
  if( !rc )
    {
      printf( "Mutex unlocked in cam_open\n" );
    }  
  if(device <= 0)
    {
      printf("Device %s couldn't be opened\n", devicename);
      return 0;
    }
  //return 1;
  return device;
}