/* -*- c++ -*- */
/*
 * Copyright 2006,2010,2012 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "sink_uc_impl.h"
#include <gnuradio/io_signature.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <iostream>
#include <stdexcept>
#include <string.h>


namespace gr {
  namespace video_sdl {

    sink_uc::sptr
    sink_uc::make(double framerate, int width, int height,
		  unsigned int format, int dst_width, int dst_height)
    {
      return gnuradio::get_initial_sptr
	(new sink_uc_impl(framerate, width, height, format, dst_width, dst_height));
    }

    sink_uc_impl::sink_uc_impl(double framerate, int width, int height,
			       unsigned int format, int dst_width, int dst_height)
      : sync_block("video_sdl_sink_uc",
		      io_signature::make(1, 3, sizeof(unsigned char)),
		      io_signature::make(0, 0, 0)),
	d_chunk_size(width*height), d_framerate(framerate),
	d_wanted_frametime_ms(0), d_width(width), d_height(height),
	d_dst_width(dst_width), d_dst_height(dst_height),
	d_format(format),
	d_current_line(0), d_screen(NULL), d_image(NULL),
	d_avg_delay(0.0), d_wanted_ticks(0)
    {
      if(framerate <= 0.0)
	d_wanted_frametime_ms = 0; //Go as fast as possible
      else
	d_wanted_frametime_ms = (int)(1000.0/framerate);

      if(dst_width < 0)
	d_dst_width = d_width;

      if(dst_height < 0)
	d_dst_height = d_height;

      if(0==format)
	d_format= IMGFMT_YV12;

      atexit(SDL_Quit); //check if this is the way to do this
      if(SDL_Init(SDL_INIT_VIDEO) < 0) {
	std::cerr << "video_sdl::sink_uc: Couldn't initialize SDL:"
		  << SDL_GetError() << " \n SDL_Init(SDL_INIT_VIDEO) failed\n";
	throw std::runtime_error ("video_sdl::sink_uc");
      }

      /* accept any depth */
      d_screen = SDL_SetVideoMode(dst_width, dst_height, 0,
				  SDL_SWSURFACE|SDL_RESIZABLE|SDL_ANYFORMAT);//SDL_DOUBLEBUF |SDL_SWSURFACE| SDL_HWSURFACE||SDL_FULLSCREEN

      if(d_screen == NULL) {
	std::cerr << "Unable to set SDL video mode: " << SDL_GetError()
		  <<"\n SDL_SetVideoMode() Failed \n";
	exit(1);
      }

      if(d_image) {
	SDL_FreeYUVOverlay(d_image);
      }

      /* Initialize and create the YUV Overlay used for video out */
      if(!(d_image = SDL_CreateYUVOverlay(d_width, d_height, SDL_YV12_OVERLAY, d_screen))) {
	std::cerr << "SDL: Couldn't create a YUV overlay: \n"<< SDL_GetError() <<"\n";
	throw std::runtime_error("video_sdl::sink_uc");
      }

      printf("SDL screen_mode %d bits-per-pixel\n",
	     d_screen->format->BitsPerPixel);
      printf("SDL overlay_mode %i \n",
	     d_image->format);

      d_chunk_size = std::min(1,16384/width); //width*16;
      d_chunk_size = d_chunk_size*width;
      //d_chunk_size = (int)(width);
      set_output_multiple(d_chunk_size);

      /* Set the default playback area */
      d_dst_rect.x = 0;
      d_dst_rect.y = 0;
      d_dst_rect.w = d_dst_width;
      d_dst_rect.h = d_dst_height;
      //clear the surface to grey

      if(SDL_LockYUVOverlay(d_image)) {
	std::cerr << "SDL: Couldn't lock YUV overlay: \n" << SDL_GetError() << "\n";
	throw std::runtime_error ("video_sdl::sink_uc");
      }

      memset(d_image->pixels[0], 128, d_image->pitches[0]*d_height);
      memset(d_image->pixels[1], 128, d_image->pitches[1]*d_height/2);
      memset(d_image->pixels[2], 128, d_image->pitches[2]*d_height/2);
      SDL_UnlockYUVOverlay(d_image);
    }

    sink_uc_impl::~sink_uc_impl()
    {
      SDL_Quit();
    }

    void
    sink_uc_impl::copy_line_pixel_interleaved(unsigned char *dst_pixels_u,
					      unsigned char *dst_pixels_v,
					      const unsigned char *src_pixels,
					      int src_width)
    {
      for(int i=0;i<src_width;i++) {
	dst_pixels_u[i]=src_pixels[i*2];
	dst_pixels_v[i]=src_pixels[i*2+1];
      }
    }

    void
    sink_uc_impl::copy_line_line_interleaved(unsigned char *dst_pixels_u,
					     unsigned char *dst_pixels_v,
					     const unsigned char *src_pixels,
					     int src_width)
    {
      memcpy(dst_pixels_u, src_pixels, src_width);
      memcpy(dst_pixels_v, src_pixels+src_width, src_width);
    }

    void
    sink_uc_impl::copy_line_single_plane(unsigned char *dst_pixels,
					 const unsigned char *src_pixels,
					 int src_width)
    {
      memcpy(dst_pixels, src_pixels, src_width);
    }

    void
    sink_uc_impl::copy_line_single_plane_dec2(unsigned char *dst_pixels,
					      const unsigned char *src_pixels,
					      int src_width)
    {
      for(int i = 0,j = 0; i < src_width; i += 2, j++) {
	dst_pixels[j] = (unsigned char)src_pixels[i];
      }
    }

    int
    sink_uc_impl::copy_plane_to_surface(int plane,int noutput_items,
					const unsigned char * src_pixels)
    {
      const int first_dst_plane = (12==plane || 1122 == plane) ? 1 : plane;
      const int second_dst_plane = (12==plane || 1122 == plane) ? 2 : plane;
      int current_line = (0 == plane) ? d_current_line : d_current_line/2;

      unsigned char *dst_pixels = (unsigned char*)d_image->pixels[first_dst_plane];
      dst_pixels =& dst_pixels[current_line*d_image->pitches[first_dst_plane]];

      unsigned char * dst_pixels_2 = (unsigned char *)d_image->pixels[second_dst_plane];
      dst_pixels_2 =& dst_pixels_2[current_line*d_image->pitches[second_dst_plane]];

      int src_width = (0 == plane || 12 == plane || 1122 == plane) ? d_width : d_width/2;
      int noutput_items_produced = 0;
      int max_height = (0 == plane)? d_height-1 : d_height/2-1;

      for(int i = 0; i < noutput_items; i += src_width) {
        //output one line at a time
        if(12 == plane) {
          copy_line_pixel_interleaved(dst_pixels, dst_pixels_2, src_pixels, src_width);
          dst_pixels_2 += d_image->pitches[second_dst_plane];
        }
        else if(1122 == plane) {
	  copy_line_line_interleaved(dst_pixels, dst_pixels_2, src_pixels, src_width);
	  dst_pixels_2 += d_image->pitches[second_dst_plane];
	  src_pixels += src_width;
	}
        else if(0 == plane)
          copy_line_single_plane(dst_pixels, src_pixels, src_width);
        else /* 1==plane || 2==plane*/
          copy_line_single_plane_dec2(dst_pixels, src_pixels, src_width); //decimate by two horizontally

        src_pixels += src_width;
        dst_pixels += d_image->pitches[first_dst_plane];
        noutput_items_produced+=src_width;
        current_line++;

        if(current_line > max_height) {
          //Start new frame
          //TODO, do this all in a separate thread
          current_line = 0;
          dst_pixels = d_image->pixels[first_dst_plane];
          dst_pixels_2 = d_image->pixels[second_dst_plane];
          if(0 == plane) {
            SDL_DisplayYUVOverlay(d_image, &d_dst_rect);
            //SDL_Flip(d_screen);
            unsigned int ticks = SDL_GetTicks();//milliseconds
            d_wanted_ticks += d_wanted_frametime_ms;
            float avg_alpha = 0.1;
            int time_diff = d_wanted_ticks-ticks;
            d_avg_delay = time_diff*avg_alpha + d_avg_delay*(1.0-avg_alpha);
          }
        }
      }

      if(0==plane)
	d_current_line=current_line;

      return noutput_items_produced;
    }

    int
    sink_uc_impl::work(int noutput_items,
		       gr_vector_const_void_star &input_items,
		       gr_vector_void_star &output_items)
    {
      unsigned char *src_pixels_0,*src_pixels_1,*src_pixels_2;
      int noutput_items_produced = 0;
      int plane;
      int delay = (int)d_avg_delay;

      if(0 == d_wanted_ticks)
	d_wanted_ticks=SDL_GetTicks();

      if(delay > 0)
	SDL_Delay((unsigned int)delay);//compensate if running too fast

      if(SDL_LockYUVOverlay(d_image)) {
	return 0;
      }

      switch(input_items.size ()) {
      case 3:		// first channel=Y, second channel is  U , third channel is V
	src_pixels_0 = (unsigned char *) input_items[0];
	src_pixels_1 = (unsigned char *) input_items[1];
	src_pixels_2 = (unsigned char *) input_items[2];
	for(int i = 0; i < noutput_items; i += d_chunk_size) {
	  copy_plane_to_surface (1,d_chunk_size, src_pixels_1);
	  copy_plane_to_surface (2,d_chunk_size, src_pixels_2);
	  noutput_items_produced+=copy_plane_to_surface(0,d_chunk_size, src_pixels_0);
	  src_pixels_0 += d_chunk_size;
	  src_pixels_1 += d_chunk_size;
	  src_pixels_2 += d_chunk_size;
	}
	break;
      case 2:
	if(1) {//if(pixel_interleaved_uv)
	  // first channel=Y, second channel is alternating pixels U and V
	  src_pixels_0 = (unsigned char *) input_items[0];
	  src_pixels_1 = (unsigned char *) input_items[1];
	  for(int i = 0; i < noutput_items; i += d_chunk_size) {
	    copy_plane_to_surface(12, d_chunk_size/2, src_pixels_1);
	    noutput_items_produced += copy_plane_to_surface(0, d_chunk_size, src_pixels_0);
	    src_pixels_0 += d_chunk_size;
	    src_pixels_1 += d_chunk_size;
	  }
	}
	else {
	  // first channel=Y, second channel is alternating lines U and V
	  src_pixels_0 = (unsigned char*)input_items[0];
	  src_pixels_1 = (unsigned char*)input_items[1];
	  for(int i = 0; i < noutput_items; i += d_chunk_size) {
	    copy_plane_to_surface (1222,d_chunk_size/2, src_pixels_1);
	    noutput_items_produced += copy_plane_to_surface(0, d_chunk_size, src_pixels_0);
	    src_pixels_0 += d_chunk_size;
	    src_pixels_1 += d_chunk_size;
	  }
	}
	break;
      case 1:		// grey (Y) input
	/* Y component */
	plane=0;
	src_pixels_0 = (unsigned char*)input_items[plane];
	for(int i = 0; i < noutput_items; i += d_chunk_size) {
	  noutput_items_produced += copy_plane_to_surface(plane, d_chunk_size, src_pixels_0);
	  src_pixels_0 += d_chunk_size;
	}
	break;
      default: //0 or more then 3 channels
	std::cerr << "video_sdl::sink_uc: Wrong number of channels: ";
	std::cerr <<"1, 2 or 3 channels are supported.\n  Requested number of channels is "
		  << input_items.size () <<"\n";
	throw std::runtime_error("video_sdl::sink_uc");
      }

      SDL_UnlockYUVOverlay(d_image);
      return noutput_items_produced;
    }

  } /* namespace video_sdl */
} /* namespace gr */
