/* -*- 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_s_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_s::sptr
    sink_s::make(double framerate, int width, int height,
		 unsigned int format, int dst_width, int dst_height)
    {
      return gnuradio::get_initial_sptr
	(new sink_s_impl(framerate, width, height,format, dst_width, dst_height));
    }

    sink_s_impl::sink_s_impl(double framerate, int width, int height,
			     unsigned int format, int dst_width, int dst_height)
      : sync_block("video_sdl_sink_s",
		      io_signature::make(1, 3, sizeof(short)),
		      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_s: Couldn't initialize SDL:" << SDL_GetError()
		  << " \n SDL_Init(SDL_INIT_VIDEO) failed\n";
	throw std::runtime_error("video_sdl::sink_s");
      };

      /* 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_s");
      }

      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_s");
      }

      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_s_impl::~sink_s_impl()
    {
      SDL_Quit();
    }

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

    void
    sink_s_impl::copy_line_line_interleaved(unsigned char *dst_pixels_u,
					    unsigned char *dst_pixels_v,
					    const short * src_pixels,
					    int src_width)
    {
      for(int i = 0; i < src_width; i++) {
	dst_pixels_u[i] = (unsigned char)src_pixels[i];
	dst_pixels_v[i] = (unsigned char)src_pixels[i+src_width];
      }

      for(int i = src_width; i < src_width*2; i++) {
	dst_pixels_v[i] = (unsigned char)src_pixels[i];
      }
    }

    void
    sink_s_impl::copy_line_single_plane(unsigned char *dst_pixels,
					const short * src_pixels,
					int src_width)
    {
      for(int i = 0; i < src_width; i++) {
	dst_pixels[i] = (unsigned char)src_pixels[i];
      }
    }

    void
    sink_s_impl::copy_line_single_plane_dec2(unsigned char *dst_pixels,
					     const short * 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_s_impl::copy_plane_to_surface(int plane,int noutput_items,
				       const short * 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 seperate 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_s_impl::work(int noutput_items,
		      gr_vector_const_void_star &input_items,
		      gr_vector_void_star &output_items)
    {
      short *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 = (short *)input_items[0];
	src_pixels_1 = (short *)input_items[1];
	src_pixels_2 = (short *)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 = (short *)input_items[0];
	  src_pixels_1 = (short *)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 = (short *)input_items[0];
	  src_pixels_1 = (short *)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 = (short *)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_s: 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_s");
      }

      SDL_UnlockYUVOverlay(d_image);
      return noutput_items_produced;
    }

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