#include <camera_sdl.h>
#include <math.h>

void convert_to_yuv_for_SDL(dc1394video_frame_t *in, SDL_Overlay *sdloverlay, unsigned int overlay_byte_order);
dc1394video_frame_t apply_frame_treatment(dc1394video_frame_t* frame, unsigned char* bit_buffer, unsigned char* bayer_buffer);


sdl_screen_t*  InitSDLScreen(dc1394camera_t* camera){
    
    
    if (SDL_Init (SDL_INIT_VIDEO) < 0)
    {
        fprintf (stderr, "Couldn't initialize SDL: %s\n", SDL_GetError ());
        return NULL;
    }
    sdl_screen_t* ss = (sdl_screen_t*)malloc(sizeof(sdl_screen_t));
   // atexit (SDL_Quit);
    
    /*Discover video settings using camera params*/
    dc1394video_mode_t video_mode;
    dc1394_video_get_mode(camera,&video_mode);
    uint32_t w,h;
    dc1394_get_image_size_from_video_mode(camera,video_mode,&w,&h); 
    /*Convert from 16 to 8 bits if needed bits*/
    
    dc1394color_coding_t  color_coding;
    dc1394_get_color_coding_from_video_mode(camera,video_mode,&color_coding);
    
    uint32_t bit_depth;
    dc1394_get_color_coding_data_depth(color_coding,&bit_depth);
    fprintf(stderr,"Camera is configured to %d bits\n",bit_depth);
    
    ss->scratch_buffer_bit = NULL;
    ss->scratch_buffer_bayer =(unsigned char*)malloc(sizeof(unsigned char)*h*w*3); 
    ss->bit_depth = bit_depth;
    if(bit_depth == 16){
      fprintf(stderr,"16 bit mode, scratch buffer is needed...");
      ss->scratch_buffer_bit = (unsigned char*)malloc(sizeof(unsigned char)*h*w);
    }
    /*Now, SDL Operations*/
    ss->screen = SDL_SetVideoMode (w,h, 0,0);
    ss->h = h;
    ss->w = w;
    if(ss->screen != NULL){
      ss->bmp = SDL_CreateYUVOverlay(w,h ,SDL_YUY2_OVERLAY, ss->screen);
      if( ss->bmp == NULL){
	fprintf (stderr, "Couldn't create overlay !: %s\n", SDL_GetError ());
	return NULL;
      }
    }else{
      fprintf (stderr, "Couldn't initialize SDL Window: %s\n", SDL_GetError ());
      free(ss);
      return NULL;
    }
    return ss;
}

void releaseSDLScreen(sdl_screen_t* ss){
  if(ss->scratch_buffer_bit != NULL){
    free(ss->scratch_buffer_bit);
  }
  if(ss->scratch_buffer_bayer != NULL){
    free(ss->scratch_buffer_bayer);
  }
  SDL_Quit();
}


void displayFrame(sdl_screen_t* ss, dc1394video_frame_t *frame){
	SDL_Rect rect;
	rect.x = 0;
	rect.y = 0;
	rect.w = ss->w;
	rect.h = ss->h;
	SDL_LockYUVOverlay(ss->bmp);
	//First, convert to RGB 8 bits and treat every frame/*
	dc1394video_frame_t frame_rgb = apply_frame_treatment(frame,ss->scratch_buffer_bit,ss->scratch_buffer_bayer);
	/*Then to YUV*/
	
	convert_to_yuv_for_SDL(&frame_rgb,ss->bmp,DC1394_BYTE_ORDER_YUYV);
//	convert_to_yuv_for_SDL(frame,ss->bmp,DC1394_BYTE_ORDER_YUYV);
	SDL_UnlockYUVOverlay(ss->bmp);
	SDL_DisplayYUVOverlay(ss->bmp, &rect);
}



dc1394video_frame_t apply_frame_treatment(dc1394video_frame_t* frame, unsigned char* bit_buffer, unsigned char* bayer_buffer){
  dc1394video_frame_t rgb_frame = *frame;
  uint32_t w,h;
  w =  frame->size[0];
  h =  frame->size[1];
  rgb_frame.color_coding = DC1394_COLOR_CODING_RGB8;
  rgb_frame.image = bayer_buffer;
  rgb_frame.total_bytes = 3*w*h*(sizeof(unsigned char));
  rgb_frame.image_bytes = 3*w*h*(sizeof(unsigned char));
  rgb_frame.padding_bytes = 0;
  
  /*Bits first.. aways !*/
  uint32_t bit_depth;
  dc1394_get_color_coding_bit_size(frame->color_coding, &bit_depth);
  
  dc1394video_frame_t frame_in = *frame;
  
  if(bit_depth == 16){
   // fprintf(stderr,"BIT convertion..");
    int i;
    if(frame->color_coding == DC1394_COLOR_CODING_RAW16){
      frame_in.color_coding = DC1394_COLOR_CODING_RAW8;
    }
    if(frame->color_coding == DC1394_COLOR_CODING_MONO16){
      frame_in.color_coding = DC1394_COLOR_CODING_MONO8;
    }
    frame_in.image_bytes = frame->image_bytes/2;
    frame_in.total_bytes = frame->image_bytes;
    frame_in.padding_bytes = 0;
    frame_in.image = bit_buffer;
    Uint8* buffer8 = (Uint8*) frame_in.image;
    Uint16* buffer16 = (Uint16*) frame->image;
    for(i = 0; i < h*w; i++){
      Uint32 val8,val16;
      val16 = buffer16[i];
      val8 = val16 & 255;
      buffer8[i] = val8;
    }
  }
  //fprintf(stderr,"END\n");
  /*Color convertion*/
  
//  fprintf(stderr,"Converting colors...");
  if( (frame->color_coding != DC1394_COLOR_CODING_RAW8) && (frame->color_coding!=  DC1394_COLOR_CODING_RAW16)){
    dc1394_convert_frames(&frame_in, &rgb_frame);
  }else{
    dc1394_bayer_decoding_8bit(frame_in.image,rgb_frame.image, w, h,frame_in.color_filter,DC1394_BAYER_METHOD_NEAREST);
  }
 // fprintf(stderr,"END\n");
  /*Finally, treat oversposed channels*/
  
  int limit = 255*0.99;
  int i;
  for(i = 0; i < (h*w); i++){
    int Y;
    //Y = (rgb_frame.image[i*3]*66) + (rgb_frame.image[i*3 + 1]*129) + (rgb_frame.image[i*3 + 2]*25);
    Y = rgb_frame.image[i*3];
    if( Y < rgb_frame.image[i*3+1]){
      Y = rgb_frame.image[i*3+1]; 
    }
    if( Y < rgb_frame.image[i*3+2] ){
      Y = rgb_frame.image[i*3+2];
    }
   // Y = (Y+128) >> 8;
   // Y += 16;
    if( Y > limit){
      rgb_frame.image[i*3] = 0;
      rgb_frame.image[i*3 + 1] = 255;
      rgb_frame.image[i*3 + 2] = 0;
    }
  }
  
  return rgb_frame;
}



void convert_to_yuv_for_SDL(dc1394video_frame_t *in, SDL_Overlay *sdloverlay, unsigned int overlay_byte_order)
{
  dc1394video_frame_t out;
  unsigned int padding=in->padding_bytes;
  in->padding_bytes=0;             // don't print padding in YUV video buffer
  in->total_bytes=in->image_bytes; // don't print padding in YUV video buffer
  
  
  
  //fprintf(stderr,"%d ",in->color_coding);
  out.color_coding=DC1394_COLOR_CODING_YUV422;
  out.yuv_byte_order=overlay_byte_order;
  out.image=sdloverlay->pixels[0];
  // very large value to avoid re-allocation. In reality, this buffer is allocated by SDL functions so
  // we don't want to touch it here.
  out.allocated_image_bytes=1e12;

  /*If needed we debayer the frame*/
  
  
  dc1394_convert_frames(in, &out);
 
   // revert to true values:
  in->padding_bytes=padding;
  in->total_bytes=in->image_bytes+padding;
}