#include <carmen/carmen.h>
#include <assert.h>
#include "img1.xpm"
#include "img2.xpm"
#include "img3.xpm"
#include "img4.xpm"
#include "../camera_hw_interface.h"

static carmen_camera_image_t saved_image1;
static carmen_camera_image_t saved_image2;
static carmen_camera_image_t saved_image3;
static carmen_camera_image_t saved_image4;

static int image_state = 0;

int lookup_colour(char *xpm_ptr, char *colour_table[], int num_colours)
{
  int i;

  for (i = 0; i < num_colours; i++) {
    if (strncmp(xpm_ptr, colour_table[i], 2) == 0) 
      return strtol(colour_table[i]+6, NULL, 16);
  }
  return 0;
}


carmen_camera_image_t *carmen_camera_start(int argc __attribute__ ((unused)), 
					   char **argv __attribute__ ((unused)))
{
  carmen_camera_image_t *image;
  char *xpm_ptr;
  char *image_ptr;
  int x, y;
  long int colour;
  int num_colours;

  // get image 4
  sscanf(img4[0], "%d %d %d", &(saved_image4.width), &(saved_image4.height),
	 &num_colours);
  saved_image4.bytes_per_pixel = 3;
  saved_image4.image_size = saved_image4.width*saved_image4.height*
    saved_image4.bytes_per_pixel;
  saved_image4.is_new = 0;
  saved_image4.timestamp = 0;
  saved_image4.image = (char *)calloc(saved_image4.image_size, sizeof(char));
  carmen_test_alloc(saved_image4.image);
  image_ptr = saved_image4.image;
  for (y = 0; y < saved_image4.height; y++) {
    xpm_ptr = img4[num_colours+y+1];
    for (x = 0; x < saved_image4.width; x++) {    
      colour = lookup_colour(xpm_ptr, img4+1, num_colours);    
      image_ptr[0] = (colour >> 16) & 0xFF;
      image_ptr[1] = (colour >> 8) & 0xFF;
      image_ptr[2] = colour & 0xFF;
      xpm_ptr += 2;
      image_ptr += 3;
    }
  }

  // get image 3
  sscanf(img3[0], "%d %d %d", &(saved_image3.width), &(saved_image3.height),
	 &num_colours);
  saved_image3.bytes_per_pixel = 3;
  saved_image3.image_size = saved_image3.width*saved_image3.height*
    saved_image3.bytes_per_pixel;
  saved_image3.is_new = 0;
  saved_image3.timestamp = 0;
  saved_image3.image = (char *)calloc(saved_image3.image_size, sizeof(char));
  carmen_test_alloc(saved_image3.image);
  image_ptr = saved_image3.image;
  for (y = 0; y < saved_image3.height; y++) {
    xpm_ptr = img3[num_colours+y+1];
    for (x = 0; x < saved_image3.width; x++) {    
      colour = lookup_colour(xpm_ptr, img3+1, num_colours);    
      image_ptr[0] = (colour >> 16) & 0xFF;
      image_ptr[1] = (colour >> 8) & 0xFF;
      image_ptr[2] = colour & 0xFF;
      xpm_ptr += 2;
      image_ptr += 3;
    }
  }

  // get image 2
  sscanf(img2[0], "%d %d %d", &(saved_image2.width), &(saved_image2.height),
	 &num_colours);
  saved_image2.bytes_per_pixel = 3;
  saved_image2.image_size = saved_image2.width*saved_image2.height*
    saved_image2.bytes_per_pixel;
  saved_image2.is_new = 0;
  saved_image2.timestamp = 0;
  saved_image2.image = (char *)calloc(saved_image2.image_size, sizeof(char));
  carmen_test_alloc(saved_image2.image);
  image_ptr = saved_image2.image;
  for (y = 0; y < saved_image2.height; y++) {
    xpm_ptr = img2[num_colours+y+1];
    for (x = 0; x < saved_image2.width; x++) {    
      colour = lookup_colour(xpm_ptr, img2+1, num_colours);    
      image_ptr[0] = (colour >> 16) & 0xFF;
      image_ptr[1] = (colour >> 8) & 0xFF;
      image_ptr[2] = colour & 0xFF;
      xpm_ptr += 2;
      image_ptr += 3;
    }
  }

  // get image 1 and return it
  sscanf(img1[0], "%d %d %d", &(saved_image1.width), &(saved_image1.height),
	 &num_colours);

  saved_image1.bytes_per_pixel = 3;

  saved_image1.image_size = saved_image1.width*saved_image1.height*
    saved_image1.bytes_per_pixel;
  saved_image1.is_new = 0;
  saved_image1.timestamp = 0;

  saved_image1.image = (char *)calloc(saved_image1.image_size, sizeof(char));
  carmen_test_alloc(saved_image1.image);

  image_ptr = saved_image1.image;
  for (y = 0; y < saved_image1.height; y++) {
    xpm_ptr = img1[num_colours+y+1];
    for (x = 0; x < saved_image1.width; x++) {    
      colour = lookup_colour(xpm_ptr, img1+1, num_colours);    
      image_ptr[0] = (colour >> 16) & 0xFF;
      image_ptr[1] = (colour >> 8) & 0xFF;
      image_ptr[2] = colour & 0xFF;
      
      xpm_ptr += 2;
      image_ptr += 3;
    }
  }
  
  image = (carmen_camera_image_t *)calloc(1, sizeof(carmen_camera_image_t));
  memcpy(image, &saved_image1, sizeof(carmen_camera_image_t));
  image->image = (char *)calloc(image->image_size, sizeof(char));
  carmen_test_alloc(image->image);
  memcpy(image->image, saved_image1.image, sizeof(char)*image->image_size);

  return image;
}

void carmen_camera_shutdown(void)
{
}

void carmen_camera_grab_image(carmen_camera_image_t *image)
{
  if(image_state == 0) {
    memcpy(image->image, saved_image1.image, sizeof(char)*image->image_size);
    image_state = 1;
  } else if(image_state == 1) {
    memcpy(image->image, saved_image2.image, sizeof(char)*image->image_size);
    image_state = 2;
  } else if(image_state == 2) {
    memcpy(image->image, saved_image3.image, sizeof(char)*image->image_size);
    image_state = 3;
  } else if(image_state == 3) {
    memcpy(image->image, saved_image4.image, sizeof(char)*image->image_size);
    image_state = 0;
  }
  image->timestamp = carmen_get_time();
  image->is_new = 1;
}