#define _GNU_SOURCE

#include <camera_image.h>
#include <stdio.h>
#include <stdlib.h>
#include <camera_errors.h>


/*
NOTE: all the save functions automaticaly detects the image depth
*/

dc1394error_t save_frame_simple(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame);
/*Saves a PPM/PGM file from CCD data encoded as MONO or RGB*/

dc1394error_t save_frame_yuv(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame);
/*Saves three files each one containing Y U and V from CCD data encoded as YUV444,YUV422 or YUV411
For YUV444, the images have all (WxH) pixels. In YUV422 Y is (WxH) and U,V are (W/2xH).
In YUV411 Y is (WxH) when U,V are (W/4xH)*/

dc1394error_t save_frame_raw(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame);
/* Apply a Debayering method (bilinear filter) in the CCD RAW data generating a RGB prefixwith same bot depth.
*/

dc1394error_t save_frame_simple(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame){
    
   dc1394color_coding_t color_coding = frame->color_coding;
  
    uint32_t width,height;
    width = frame->size[0];
    height = frame->size[1];
       
    dc1394error_t err ;
 
    uint32_t  bits;
    err = dc1394_get_color_coding_data_depth(color_coding,&bits);
    
    dc1394bool_t is_colored;
    
    err = dc1394_is_color(color_coding,&is_colored);
    DC1394_ERR_RTN(err,"Could not determine if image is coloured \n");
    int NC = 1;
    int magic_num = 5;
    if( is_colored){ 
      magic_num = 6;
      NC = 3; 
    }
    
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not get data bit depth \n");
    
    uint8_t *rgb_buff8 = NULL;
    uint16_t *rgb_buff16 = NULL;
    
    if(bits == 8){
      rgb_buff8 = (uint8_t*)frame->image;
      
    }
    
    if(bits == 16){
      rgb_buff16 = (uint16_t*)frame->image;
    }
    if((bits != 16) && (bits != 8)){
      fprintf(stderr,"Data is coded to use %d bits ! Cannot decode !\n",bits);
      return DC1394_FAILURE; 
    }
    /*fprintf(stderr,"INFO\n");
    fprintf(stderr,"BUFFER has %lld bytes( %d bytes per pixel) (padding %lld)\n",frame->total_bytes,frame->total_bytes/numPixels,frame->padding_bytes);
    fprintf(stderr,"WE think that it has %lld bytes (%d bytes per pixel) \n",(bits/8)*numPixels,bits/8);
   /*/
  
  //  DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not convert frame to RGB16\n");
  
    char*filename = NULL;
    char*sufix = "pgm";
    if(NC == 3){ sufix = "ppm";}
    
    asprintf(&filename,"%s.%s",prefix,sufix);
    
    FILE* imagefile=fopen(filename, "wb");

    if( imagefile == NULL) {
        fprintf(stderr,"Can't create file %s\n",filename);
        cleanup_and_exit(camera);
    }

   
    if(bits == 8){
      fprintf(imagefile,"P%d\n%u %u\n255\n",magic_num, width, height);
      int wroten = fwrite(rgb_buff8, sizeof(uint8_t),frame->image_bytes, imagefile);
      fprintf(stderr,"Wrote %d bytes \n",wroten);
     // free(rgb_buff8);
    }

    if(bits == 16){
      fprintf(imagefile,"P%d\n%u %u\n65535\n",magic_num, width, height);
      fwrite(rgb_buff16, sizeof(uint16_t),frame->image_bytes, imagefile);
     // free(rgb_buff16);
    }
    
    
    fclose(imagefile);
    printf("wrote: %s \n",prefix);
    
    return DC1394_SUCCESS;
}

dc1394error_t save_frame_yuv(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame){
    
    dc1394color_coding_t color_coding = frame->color_coding;
  
    uint32_t width,height;
    width = frame->size[0];
    height = frame->size[1];
    uint64_t numPixels = height*width;

    dc1394error_t err ;
 
    uint32_t  bits;
    err = dc1394_get_color_coding_data_depth(color_coding,&bits);
    
    dc1394bool_t is_colored;
    
    err = dc1394_is_color(color_coding,&is_colored);
    DC1394_ERR_RTN(err,"Could not determine if image is coloured \n");
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not get data bit depth \n");
    
    char*  y_prefix = NULL;
    char*  u_prefix = NULL;
    char*  v_prefix = NULL;
    
    asprintf(&y_prefix,"%s_Y.pgm",prefix);
    asprintf(&u_prefix,"%s_U.pgm",prefix);
    asprintf(&v_prefix,"%s_V.pgm",prefix);
    FILE* y_file = fopen(y_prefix,"wb");
    FILE* u_file = fopen(u_prefix,"wb");
    FILE* v_file = fopen(v_prefix,"wb");
    
    
    
    if( y_file == NULL) {
        fprintf(stderr,"Can't create %s\n", y_prefix);
        cleanup_and_exit(camera);
    }
    if( u_file == NULL) {
        fprintf(stderr,"Can't create %s\n", u_prefix);
        cleanup_and_exit(camera);
    }
    if( v_file == NULL) {
        fprintf(stderr,"Can't create %s\n", v_prefix);
        cleanup_and_exit(camera);
    }

    
    
    fprintf(y_file,"P5\n%u %u\n255\n", width, height);
    
    
    if(color_coding == DC1394_COLOR_CODING_YUV444){
      
      fprintf(u_file,"P5\n%u %u\n255\n", width, height);
      fprintf(v_file,"P5\n%u %u\n255\n", width, height);  
      
      uint32_t i;
      uint32_t count = 0;
      for(i =0; i < numPixels; i++){
	uint8_t Yvalue = frame->image[count];
	uint8_t Uvalue = frame->image[count+1];
	uint8_t Vvalue = frame->image[count+2];
	fwrite(&Yvalue,sizeof(uint8_t),1,y_file);
	fwrite(&Uvalue,sizeof(uint8_t),1,u_file);
	fwrite(&Vvalue,sizeof(uint8_t),1,v_file);
	count= count+ 3;
      }
      
    }
    
    if(color_coding == DC1394_COLOR_CODING_YUV422){
      
      fprintf(u_file,"P5\n%u %u\n255\n", width/2, height);
      fprintf(v_file,"P5\n%u %u\n255\n", width/2, height); 
      
      uint32_t yuv_byte_order = frame->yuv_byte_order;        /* the order of the fields for 422 formats: YUYV or UYVY */
      int Y1,Y2,U,V;
      Y1 = 0;
      U = 1;
      Y2 = 2;
      V = 3;
      if(yuv_byte_order == DC1394_BYTE_ORDER_UYVY){
	U = 0;
	Y1 = 1;
	V = 2;
	Y2 = 3;
      }
      
      uint32_t i;
      
      for(i =0; i < numPixels/2; i++){
	uint8_t values[4];
	values[0] = frame->image[(i*4)];
	values[1] = frame->image[(i*4)+1];
	values[2] = frame->image[(i*4)+2];
	values[3] = frame->image[(i*4)+3];
	uint8_t y1v,y2v;
	uint8_t uv,vv;
	y1v = values[Y1];
	y2v = values[Y2];
	uv = values[U];
	vv = values[V];
	
	fwrite(&y1v,sizeof(uint8_t),1,y_file);
	fwrite(&y2v,sizeof(uint8_t),1,y_file);
	fwrite(&uv,sizeof(uint8_t),1,u_file);
	fwrite(&vv,sizeof(uint8_t),1,v_file);
	
      }
    }
    
    if(color_coding == DC1394_COLOR_CODING_YUV411){
      
      fprintf(u_file,"P5\n%u %u\n255\n", width/4, height);
      fprintf(v_file,"P5\n%u %u\n255\n", width/4, height); 
      
      uint32_t i;
      int seq = 0;
      
      for(i =0; i < numPixels/2; i++){
	uint8_t values[3];
	values[0] = frame->image[(i*3)];
	values[1] = frame->image[(i*3)+1];
	values[2] = frame->image[(i*3)+2];
	
	uint32_t uvv = values[0];
	uint32_t y1v = values[1];
	uint32_t y2v = values[2];
	
	fwrite(&y1v,sizeof(uint8_t),1,y_file);
	fwrite(&y2v,sizeof(uint8_t),1,y_file);
	
	if((seq%2) == 0){
	  fwrite(&uvv,sizeof(uint8_t),1,u_file);
	}else{
	  fwrite(&uvv,sizeof(uint8_t),1,v_file);
	}
	
	
	
	seq= (seq+1)%2;
      }
    }
    
    
    fclose(y_file);
    fclose(u_file);
    fclose(v_file);
    
    printf("wrote: %s \n",prefix);
    
    return DC1394_SUCCESS;
}


dc1394error_t save_frame_raw(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame){
    
    dc1394color_coding_t color_coding = frame->color_coding;
  
    uint32_t width,height;
    width = frame->size[0];
    height = frame->size[1];
    uint64_t numPixels = height*width;

    
    
    dc1394error_t err ;
 
    dc1394color_filter_t tile = frame->color_filter;
    //dc1394error_t err = dc1394_format7_get_color_filter(camera,video_mode, &tile);
    //DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could get color encoding \n");
   
    
    dc1394bayer_method_t method = DC1394_BAYER_METHOD_BILINEAR;
    
    
    uint32_t  bits;
    err = dc1394_get_color_coding_data_depth(color_coding,&bits);
    
    dc1394bool_t is_colored;
    
    err = dc1394_is_color(color_coding,&is_colored);
    DC1394_ERR_RTN(err,"Could not determine if image is coloured \n");
    int NC = 3;
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not get data bit depth \n");
    
    uint8_t *rgb_buff8 = NULL;
    uint16_t *rgb_buff16 = NULL;
    
    if(bits == 8){
      rgb_buff8 = (uint8_t*)malloc(sizeof(uint8_t)*(NC*numPixels));
      err = dc1394_bayer_decoding_8bit((uint8_t*)frame->image,rgb_buff8,
                           width, height, tile,
                             method);
      //rgb_buff8 = frame->image;
      DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not decode framebuffer - 8 bits \n");
      
    }
    
    if(bits == 16){
      rgb_buff16 = (uint16_t*)malloc(sizeof(uint16_t)*(NC*numPixels));
      err = dc1394_bayer_decoding_16bit((uint16_t*)frame->image,rgb_buff16,
                           width, height, tile,
                             method, bits);
    //  rgb_buff16 = frame->image;
      DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not decode framebuffer - 16 bits \n");
      
    }
    if((bits != 16) && (bits != 8)){
      fprintf(stderr,"RAW data is coded to use %d bits ! Cannot decode !\n",bits);
      return DC1394_FAILURE; 
    }
		
   //  puts("AQUI");
  
  //  DC1394_ERR_CLN_RTN(err,cleanup_and_exit(camera),"Could not convert frame to RGB16\n");
  
    char*filename = NULL;
    asprintf(&filename,"%s.ppm",prefix);
    
    FILE* imagefile=fopen(filename, "wb");
    
    if( imagefile == NULL) {
        fprintf(stderr,"Can't create %s\n", filename);
        cleanup_and_exit(camera);
    }
    
    char*filenameraw = NULL;
    asprintf(&filenameraw,"%s.raw",prefix);
    
    FILE* imagefileraw=fopen(filenameraw, "wb");
    
    if( imagefile == NULL) {
        fprintf(stderr,"Can't create %s\n", filenameraw);
        cleanup_and_exit(camera);
    }


    if(bits == 8){
      fprintf(imagefile,"P6\n%u %u\n255\n", width, height);
      fwrite(rgb_buff8, sizeof(uint8_t),numPixels*NC, imagefile);
      free(rgb_buff8);
    }

    if(bits == 16){
      fprintf(imagefile,"P6\n%u %u\n65535\n", width, height);
      fwrite(rgb_buff16, sizeof(uint16_t),numPixels*NC, imagefile);
      free(rgb_buff16);
    }
    
    fwrite(frame->image, sizeof(unsigned char),frame->total_bytes, imagefileraw);
    
    fclose(imagefile);
    fclose(imagefileraw);
    printf("wrote: %s with (%lld - %d) bytes in RAW\n",prefix,frame->total_bytes, frame->image_bytes);
    
    return DC1394_SUCCESS;
}

dc1394error_t save_frame(char* prefix,dc1394camera_t* camera,dc1394video_frame_t* frame){
  dc1394color_coding_t color_coding = frame->color_coding;
  //printf("Deciding color coding\n");
  if((color_coding == DC1394_COLOR_CODING_MONO8) || (color_coding == DC1394_COLOR_CODING_MONO16) || (color_coding == DC1394_COLOR_CODING_MONO16S)){
   // fprintf(stderr,"MONO\n");
    return save_frame_simple(prefix,camera,frame);
    
  }
  else if((color_coding == DC1394_COLOR_CODING_YUV411) || (color_coding == DC1394_COLOR_CODING_YUV422) || (color_coding == DC1394_COLOR_CODING_YUV444)){
   // fprintf(stderr,"YUV\n");
    return save_frame_yuv(prefix,camera,frame);
    
  }
  else if((color_coding == DC1394_COLOR_CODING_RGB8) || (color_coding == DC1394_COLOR_CODING_RGB16) || (color_coding == DC1394_COLOR_CODING_RGB16S)){
    //return save_frame_rgb(prefix,camera,frame);
   // fprintf(stderr,"RGB\n");
    return save_frame_simple(prefix,camera,frame);
  }
  else{
    //fprintf(stderr,"RAW\n");
    return save_frame_raw(prefix,camera,frame);
  }
  fprintf(stderr,"Unsuported frame format !");
  return DC1394_FAILURE ;
  
}