/* -*- c++ -*- */
/*
 * Copyright 2005 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 2, 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., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

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

#include <gr_tv_sync.h>
#include <gr_io_signature.h>
#include <assert.h>
#include <stdexcept>


//static const int BITS_PER_BYTE = 8;
//static const int OVERHEAD = 6 + 4;	    // 4 bytes sync, 1 byte seqno, 1 byte cmd + 4

gr_tv_sync_sptr
gr_make_tv_sync (double sampling_freq, unsigned int tv_format)
{
  return gr_tv_sync_sptr (new gr_tv_sync (sampling_freq, tv_format));
}

gr_tv_sync::gr_tv_sync (double sampling_freq, unsigned int tv_format)
  : gr_block ("tv_sync",
	      gr_make_io_signature (1, 1, sizeof (unsigned char)),
	      gr_make_io_signature (1, 1, sizeof (unsigned char))),
		d_sampling_freq(sampling_freq), 
		d_tv_format(tv_format),
    d_blacklevel(40),
    d_synclevel(0),
    d_locked(false),
    d_field_start(0),
		d_nsample_in_initial_field(0),
		d_nsample_in_field(0),
    d_vsyncstart_start_to_active_videostart(0)
{
  //d_vsyncstart_start_to_active_videostart=linelength*23+10.4*d_sampling_freq*0.000001;//PAL
	d_initial_fieldsize=d_sampling_freq/50;//PAL
	d_fieldsize=d_initial_fieldsize;
	d_linelength=(d_sampling_freq/25)/625;
	d_vsync_nlines=3;//PAL 2.5
  set_relative_rate (0.5);//1.0/d_ninput);
	d_min_nsamples=(d_vsync_nlines+22)*d_linelength;
	d_ninput=d_min_nsamples;//d_fieldsize*3;
	set_output_multiple(d_min_nsamples);
}

void
gr_tv_sync::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
	if(d_locked)
	{
		d_ninput=noutput_items;//d_linelength*10;
	} else
	{
		//d_ninput=d_initial_fieldsize;
		 d_ninput=std::max(noutput_items,d_min_nsamples*2);//+d_fieldsize*3; 
	}
	unsigned ninputs = ninput_items_required.size();
  for (unsigned int i = 0; i < ninputs; i++)
    ninput_items_required[i] = d_ninput;

  /*assert (noutput_items % d_output_block_size == 0);

  int	nblocks = noutput_items / d_output_block_size;
  int	input_required = nblocks * d_input_block_size;

  unsigned ninputs = ninput_items_required.size();
  for (unsigned int i = 0; i < ninputs; i++)
    ninput_items_required[i] = input_required;
	*/
}

int
gr_tv_sync::general_work (int noutput_items,
				gr_vector_int &ninput_items,
				gr_vector_const_void_star &input_items,
				gr_vector_void_star &output_items)
{
  const unsigned char *in = (const unsigned char *) input_items[0];
  unsigned char *out = (unsigned char *) output_items[0];
	int start=0;
	int next_vsync=0;
	int n_samples=std::max(noutput_items,d_min_nsamples);//std::max(noutput_items,(d_vsync_nlines+10)*d_linelength;//field_length_estimate*3
	next_vsync=d_fieldsize - d_nsample_in_field;//vsyncstart_start_to_field_start
	int field_type,active_videostart;
	int skip=0;
	int expected_fieldstart=d_fieldsize-d_nsample_in_initial_field;
	if(d_locked)
	{
		//TODO implement me
		printf("gr_tv_sync::general_work locked noutput_items=%i\n",noutput_items);
		
    if ((n_samples>expected_fieldstart-d_vsyncstart_start_to_active_videostart+d_linelength*3) &&(expected_fieldstart-d_vsyncstart_start_to_active_videostart-d_linelength*1 >0))//next_vsync)
		{			
			if (find_initial_fielddata (in, n_samples,d_field_start,d_fieldsize, field_type, active_videostart))
			{
				printf("gr_tv_sync::general_work initial_fielddata: d_field_start=%i ,d_fieldsize=%i, field_type=%i, active_videostart=%i\n",d_field_start,d_fieldsize, field_type, active_videostart);
				//d_linelength=(d_fieldsize*2)/625;//PAL
			}
			if(d_field_start>=0)
		  {
			  skip=expected_fieldstart-active_videostart;//d_field_start;
		  } else
			{
				skip=0;
				d_locked=false;
			}
			/*if(active_videostart>0)//if(d_field_start>=0)
			{
				start=next_vsync-d_field_start;//active_videostart;	
				printf("locked start=%i\n",start);
				d_locked=true;		
			} else
			{ 
				d_locked=false;
			}*/
	  }

	} else
	{
		//const int field_length_estimate=d_sampling_freq/50;//PAL
	  printf("gr_tv_sync::general_work NOT locked n_samples=%i ,noutput_items=%i\n",n_samples,noutput_items);
		if (find_initial_fielddata (in, n_samples,d_field_start,d_fieldsize, field_type, active_videostart))
		{
			printf("gr_tv_sync::general_work initial_fielddata: d_field_start=%i ,d_fieldsize=%i, field_type=%i, active_videostart=%i\n",d_field_start,d_fieldsize, field_type, active_videostart);
			//d_linelength=(d_fieldsize*2)/625;//PAL
		}
		if(d_field_start>0)
		{
			skip=expected_fieldstart-active_videostart;//d_field_start;
		}
	  if(active_videostart>=0)
		{
			printf(" d_initial_fieldsize=%i,d_nsample_in_initial_field=%i d_field_start=%i active_videostart=%i\n",d_initial_fieldsize,d_nsample_in_initial_field,d_field_start,active_videostart);
    	start=(d_initial_fieldsize -d_nsample_in_initial_field) + active_videostart;
			if(start>d_initial_fieldsize) start -=d_initial_fieldsize;
			start=-start;
			//d_start=start;
			//start=next_vsync-d_field_start;//active_videostart;	
			printf("unlocked start=%i\n",start);
      //d_expected_start=(d_fieldsize -noutputs) % d_initial_fieldsize;//start+d_fieldsize;			
    	d_locked=true;		
		}
	}

	/*if(start<0 && -start<noutput_items)
	{
		//memcpy (&out[-start], &in[0], noutput_items+start);
		//consume=0;//consume_each (0);
		int expected_fieldstart=d_fieldsize-d_nsample_in_initial_field;
		skip= expected_fieldstart - d_field_start;//return d_fieldsize-active_videostart;
	}*/
 

	int noutput_items_realized;
	if(0==skip)
	{
		noutput_items_realized=noutput_items;
		consume_each (noutput_items_realized);
		memcpy (&out[0], &in[0], noutput_items_realized);
	} else
	{
		printf("skip %i expected_fieldstart=%i active_videostart %i d_field_start=%i \n",skip,expected_fieldstart,active_videostart,d_field_start);
		if(skip>0)
		{
			consume_each (0);
			//int outstart=std::max(noutput_items,skip);
			//int instart=0;
			//int length=std::min(noutput_items-outstart,0);//std::min(noutput_items-skip,0);
			//memcpy (&out[outstart], &in[0], length);
			noutput_items_realized=skip;
			//memcpy (&out[skip], &in[0], noutput_items_realized);
			//return noutput_items_realized;
		} else //skip<0
		{
			noutput_items_realized=noutput_items+skip;
			consume_each (noutput_items);
			//memcpy (&out[0], &in[-skip], noutput_items_realized);
			//return noutput_items_realized;
		}
	}
		d_nsample_in_initial_field+=noutput_items_realized;
	while(d_nsample_in_initial_field>d_initial_fieldsize) //d_initial_fieldsize
		d_nsample_in_initial_field-=d_initial_fieldsize;
	//d_total_noutput_items=d_total_noutput_items % d_initial_fieldsize;
	d_nsample_in_field+=start+noutput_items_realized;
	while(d_nsample_in_field>d_fieldsize) //d_initial_fieldsize
		d_nsample_in_field-=d_fieldsize;
	
  return noutput_items_realized;
	
	if(start>=0)
  	memcpy (&out[0], &in[start], noutput_items);
	printf("start=%i noutput_items=%i \n",start,noutput_items);
	consume_each (start+noutput_items);
	
	/*
  int n = 0;
  int nblocks = 0;

  memset (out, 0x55, noutput_items);

  while (n < noutput_items){
    out[0] = (GRSF_SYNC >> 24) & 0xff;
    out[1] = (GRSF_SYNC >> 16) & 0xff;
    out[2] = (GRSF_SYNC >>  8) & 0xff;
    out[3] = (GRSF_SYNC >>  0) & 0xff;
    out[4] = d_seqno++;
    out[5] = 0;

    memcpy (&out[6], in, d_input_block_size);
    in += d_input_block_size;
    out += d_output_block_size;
    n += d_output_block_size;
    nblocks++;
  }

  assert (n == noutput_items);

  consume_each (nblocks * d_input_block_size);
  return n;
	*/
	return noutput_items;
}

/*find_syncs()
{
	#syncgenerator
initial_frameperiod=sampling_freq/25
initial_fieldperiod=initial_frameperiod/2
initial_lineperiod=initial_frameperiod/625 #=sampling_freq/15625
last_hsyncstart=startoffirsthorizontalsync
expected_hsyncstart=last_hsyncstart+pixelsinline
for i=0 to pixelsinfield
in=input[i]
delta=1
hmargin=10
while running:
  linenumber=0
  while linenumber<313:
    if(linenumber>22 && linenumber<311)
      hstartactivevideo=expected_hsyncend+hsyncend_to_hstartactivevideo
      push_line_to_output(&input[hstartactivevideo],linelength)
    for i=expected_hsyncstart-hmargin to expected_hsyncstart +hmargin
     if(input[i] > synclevel && i<expected_hsyncstart)
        linelengthx16-=delta
     if(input[i] < synclevel && i>expected_hsyncstart)
        linelengthx16+=delta
    for i=expected_hsyncend-hmargin to expected_hsyncend +hmargin
     if(input[i] < synclevel && i<expected_hsyncend)
        linelengthx16-=delta
     if(input[i] > synclevel && i>expected_hsyncstart)
        linelengthx16+=delta
    linelength=linelengthx16>>4
    expected_hsyncstart+=linelength
    expected_hsyncend  +=linelength
    linenumber++
  
  hsyncend_to_hstartactivevideo=(float)linelengthx16*f_hsyncend_to_hstartactivevideo_per_linelengthx16
  check_for_vsync()
	 }
*/
/*void
gr_tv_sync::find_initial_vsync (const unsigned char *input, int framesize,int &field_start,int &field_length)
{
	//not in lock
	const unsigned char d_synclevel=235;
	const bool inverted=true;
	const int max_nvsubsyncs=12;
	//insync=false
	int syncstart=0
	int syncend=0
	int ninsync=0
	bool insinc=false;
	int vsubsync=-1;
	int synclength=0;
	int nosynclength=0;
	for(int i=0;i<framesize,i++)
	{
		unsigned char in = input[counter];
		
		if(in>=synclevel)
			synclength++;
		  if(nosynclength>0)
				nosynclength--;
		else
			nosynclength++;
			if(synclength>0)
				synclength--;
		
		if(0 == synclength)
			last_before_sync=counter;
		if(0 == nosynclength)
			last_before_syncend=counter;
    if(!insync && synclength>vsubsync_min_length)
		{
			vsubsync++;
			if(vsubsync>max_nvsubsyncs) break;
		  vsubsync_start[vsubsync]=last_before_sync+1;
			insync=true
		}
		if(insync && nosynclength>max_sync_gap)
		{
			vsubsync_end[vsubsync]=last_before_syncend+1;
			insync=false;
		}
	}

int field_start=-1;
int field_length=-1;	
	for(int i=1;i<vsubsync;i++)
	{
		if(vsubsync_start[i]-vsubsync_start[i-1]>max_sync_length)
		{
			field_start=vsubsync_end[i-1]+last_vsubsync_end_to_fieldstart;
		  if(i>nsubsyncs_expected)
				field_length=vsubsync_start[i]-vsubsync_start[i-nsubsyncs_expected];
			else if(i+nsubsyncs_expected-1<vsubsync)
				field_length=vsubsync_start[i+nsubsyncs_expected-1]-vsubsync_start[i-1];
			break;
		}
	}
	return;
}
*/

bool
gr_tv_sync::find_initial_fielddata (const unsigned char *in, int nsamples,int &field_start,int &field_length, int &field_type, int &active_videostart)
{
	field_start=-1;
	const unsigned char max_synclevel=(d_blacklevel*1)/4;
	const unsigned char min_nosynclevel=(d_blacklevel*3)/4;
	//printf("max_synclevel=%i min_nosynclevel=%i\n",max_synclevel,min_nosynclevel);
	//															405 Lines					525 Lines					625 Lines 						819 Lines					819 Lines
	//															(old UK)					(NTSC)						(System I/PAL)				System E (France)	System F (Belgium)
	//Line sync pulse width 				9 ±1µs						4.7 ±1µs					4.7 ±0.2µs						2.5 ±0.1µs				2.5 ±0.1µs
	//(between 50% points)
	//Field sync broad pulse width 	40 ±2µs 					27.1µs 						27.3 ±0.2µs 					20±1µs 						21µs
	//(between 50% points)
  //Field sync duration 					4 lines						3 lines						2.5 lines [5]					0.5 line					3.5 lines
	//															(8 broad pulses) 	(6 broad pulses) 	(5 broad pulses) 			(1 broad pulse) 	(7 broad pulses) 	
	const float hsync_usec=4.7;//PAL/NTSC
	const float vsync_usec=155.3;//PAL (Check this) 
	const float vsubsync_usec=27.3;//PAL
	const float total_line_period_usec=64.0;//PAL=64 usec, NTSC=63.556 usec
	const float max_jitter_usec=2.0;//arbitrary chosen, needs to result in > 1 sample
	                            //2.0 usec means minimal sampling freq=0.5 Mhz 
	//const int max_jitter=(int)(max_jitter_usec*d_sampling_freq*0.000001)+1;
	const int max_nvsyncs=3;//get at the most two whole fields == 1 frame
	const int vsyncstart_start_to_field_start=0;
	const int vsynclength=vsync_usec*(d_sampling_freq*0.000001);
	const int vsubsynclength=vsubsync_usec*(d_sampling_freq*0.000001);
	const int linelength=total_line_period_usec*(d_sampling_freq*0.000001);
	const int hsynclength=hsync_usec*(d_sampling_freq*0.000001);
	const int max_even_vsyncstart_to_first_hsyncstart=linelength*(5.0+0.25);//PAL even field: first hsync is 5 lines after vsyncstart, we give it 0.25 line slack
	const int min_vsyncstart_to_first_hsyncstart=linelength*(5.0-0.25);//PAL even field: first hsync is 5 lines after vsyncstart
	const int max_vsyncstart_to_first_hsync_end=linelength*(5.5+0.25)+hsynclength;//PAL odd field: first hsync is 5.5 lines after vsyncstart
	                                                                             //we give it 0.25 line slack
	const int min_hsynclength=hsync_usec*0.5*d_sampling_freq*0.000001;//half of the hsync has to get through to get horizontal sync
	const int vsyncstart_start_to_active_videostart=linelength*23.5+10.4*d_sampling_freq*0.000001;//PAL
	d_vsyncstart_start_to_active_videostart=vsyncstart_start_to_active_videostart;
	const int norm_linesinfield=312;//PAL 312.5
	const int max_lines_jitter=10;//arbitrary chosen
	int synclength=0,nosynclength=0;
	const int max_novsync_length=vsubsynclength;
	const int max_vsynclength=vsynclength*4;//+vsubsynclength/4;
	const int min_vsync_length=vsynclength/2+10;//-vsubsynclength/4;//((hsync_usec+vsync_usec)/4)*(d_sampling_freq*0.000001);//FIXME /2 //average of hsynclength and vsynclength

	unsigned int nvsyncs=0;
	bool vsyncstart_found=false;
	bool vsync_found=false;
	bool all_field_data_found=false;
	int last_before_sync=-1,last_before_nosync=-1;
	int vsyncstart[max_nvsyncs],vsyncend[max_nvsyncs];
	int hsyncstart=-1;
	//field_length=-1;
	active_videostart=-1;
	printf("min_vsync_length %i\n",min_vsync_length);
	for(int counter=0;counter<nsamples;counter++)
	{
		synclength+=(in[counter]<=max_synclevel)?+1:-1;
		//if(synclength>600) printf("synclength=%i\n",synclength);
		if(synclength<0)synclength=0;
		if(0==synclength)
			last_before_sync=counter;
		nosynclength+=(in[counter]>min_nosynclevel)?+1:-1;
    if(nosynclength<0)nosynclength=0;
		if(0==nosynclength)
			last_before_nosync=counter;
		if(synclength>(unsigned int)min_vsync_length)
		{
			vsyncstart_found=true;
			vsyncstart[nvsyncs]=last_before_sync+1; 
      printf("vsyncstart[%i]=%i\n",nvsyncs,vsyncstart[nvsyncs]);		
			//nvsyncs++;
			//counter+=vsynclength-vsubsynclength/4;//linelength*312/2;
			synclength=0;
			nosynclength=0;
		}
		if(nosynclength>(unsigned int)max_novsync_length)
		{
			int current_vsyncend=last_before_nosync+1;
			if (vsyncstart_found && (current_vsyncend-vsyncstart[nvsyncs])< max_vsynclength)
			{
				vsync_found=true;
				vsyncend[nvsyncs]=current_vsyncend; 
      	printf("vsyncstart[%i]=%i vsyncend=%i vsynclength=%i expected_vsynclength=%i\n",nvsyncs,vsyncstart[nvsyncs],vsyncend[nvsyncs],vsyncend[nvsyncs]-vsyncstart[nvsyncs],vsynclength);		
				nvsyncs++;
				counter+=linelength*(norm_linesinfield-3-max_lines_jitter);//PAL vsynclines=2.5 
				nosynclength=0;
				synclength=0;
				if(max_nvsyncs==nvsyncs)
        	break;
			}
		}
		
	}
	if (nvsyncs>0) printf("gr_tv_sync::find_initial_fielddata nvsyncs found=%i synclength=%i\n",nvsyncs,vsyncend[0]-vsyncstart[0]);
	if(vsync_found)
	{
		field_start=vsyncstart[0]+vsyncstart_start_to_field_start;
		active_videostart=vsyncstart[0]+vsyncstart_start_to_active_videostart;
	}
	
	if(nvsyncs>1) //Need at least two vertical sync pulses to determine fieldlength 
	{
		//determine (average) field_length
		field_length=(vsyncstart[nvsyncs-1]-vsyncstart[0])/(nvsyncs-1);
		//for(int i=1;i<nvsyncs;i++)
		//{
		//	field_length+=vsyncstart[i]-vsyncstart[i-1];
		//}
		//field_length=field_length/(nvsyncs-1);
		
		//now findout if this is an even or an odd field;
		//Two ways of doing this
		//1: check if there is a whitelevel pulse
		//2: check if vertical sync starts at beginning of line or halfway a line
		//method 2 is implemented here:
		const int first_hsync_min_start=vsyncstart[0]+min_vsyncstart_to_first_hsyncstart;//-max_last_hsyncstart_to_next_vsyncstart;
		const int first_hsync_max_end=vsyncstart[0]+max_vsyncstart_to_first_hsync_end;//last_hsyncend_to_next_vsyncstart;
		synclength=0;
		bool hsyncstart_found=false;
		int last_zero=0;
		for(int counter=first_hsync_min_start;first_hsync_max_end;counter++)
		{
			synclength+=(in[counter]<=max_synclevel)?+1:-1;
			if(synclength<0) synclength=0;
			if(0==synclength)
				last_zero=counter;
			if(synclength>min_hsynclength)
			{
				hsyncstart_found=true;
				hsyncstart=last_zero+1;
        break;
			}
		}
		if(hsyncstart_found)
		{
			all_field_data_found=true;
			if((hsyncstart - vsyncstart[0])>max_even_vsyncstart_to_first_hsyncstart)
			{
				field_type=FIELD_TYPE_ODD;//odd
				active_videostart=vsyncstart[0]+vsyncstart_start_to_active_videostart;
			} else
			{
				field_type=FIELD_TYPE_EVEN;//even
				active_videostart=vsyncstart[0]+vsyncstart_start_to_active_videostart;//for PAL add linelength/2 here to have a very crude deinterlacer				
			}
		}
	}
  return 	all_field_data_found;//vsyncstartfound;
}
/*		if(!inlock)
		{
			//ninsync+=(in >=synclevel)&&(!insync):1:0;
			//ninsync-=(in <synclevel)&&(insync):-1:0;
		  if(in >=synclevel)
			{
				//ninsync+=(insync)?1:-1;
				if(!insync)
				{
			  	ninsync++;
					if(ninsync>10)
					{
			 			insync=true
			 			lastsyncstart=syncstart
			 			syncstart=counter-10
			 			syncstartfound=true
			 			ninsync=0
					}	
				}
			}	else
			{
			  if(insync)
				{
					ninsync--;
					if(ninsync<-10)
					{
						insync=false
					 	lastsyncend=syncend
			 			syncend=counter-10
			 			syncendfound=true
			 			ninsync=0
					}
				}
			}

				//if syncstartfound:
				//syncs.pushback(syncstart)
			}
		if syncendfound:
		   synclen=syncend-syncstart
		   if synclen>maxhorizonsynclen
			  vertsubsync=true
			  vertsubsyncsstarts.pushback(syncstart)
			  vertsubsyncends.pushback(syncend)
			  if(len(vertsubsyncstarts)>10):
				 if syncend(len(vertsubsyncends)) - syncstart(len(vertsubsyncstarts)-5)< maxvertsynclength
				   vertsync=true
				   vertsyncstart=syncstart(len(vertsubsyncstarts)-5)
				   vertsyncend=syncend(len(vertsubsyncends))
				   framestart=vertsyncend+vertsyncendtoframestart
				   vertsyncstarts.pushback(vertsyncstart)
				   vertssyncends.pushback(vertsyncend)
		
				 for i = len(vertsyncstarts)-5 to len(vertsyncstarts)
		
		   else
			  horizsync=true
			  horizsyncstarts.pushback(syncstart)
			  horizsyncends.pushback(syncend)
		
		else:
*/