/***************************************************************************
 *   Copyright (C) 2004 by Johan Maes                                      *
 *   on4qz@telenet.be                                                      *
 *                                                                         *
 *   This program 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 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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 this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "syncproc.h"
#include "sstvparam.h"
#include "configdialog.h"
#include "qsstvglobal.h"
#include "dspfunctions.h"
#include <debugplot.h>
#include <stdarg.h>
#include "utils.h"

#define MINIMUMSYNCENTRIES 2


syncProc::syncProc()
{
	
}


syncProc::~syncProc()
{
}

void syncProc::init()
{
	syncIndex=0;
	oldS=FALSE;
	clearSyncArray();
}

/*! \fn syncProcs::recordSync(float &f,bool s)
		\brief collect sync information in sync array
		
		This functions sets up an array with sync information. The function returns True if MINIMUMSYNCENTRIES are made.
		This function does not validate the sync info. It also calculates the average frequency during sync.
		\param f value of the sample
		\param s TRUE duringd sync detection
		\param endSync TRUE if trailing edge of sync pulse
		\sa  evaluateSync()
		\return TRUE if a start or end of sync (edge) is detected.
		///@todo check of we need nosFreq
*/

bool syncProc::recordSync(float f,bool s,int tPos, bool &endSync)
{
	if (s)
		{
			syncArray[SYNCENTRIES].sync++;
			if((syncArray[SYNCENTRIES].sync>=30) && (syncArray[SYNCENTRIES].sync<60))
				{ 
					syncArray[SYNCENTRIES].freq+=f;
				}
			
		}
	else
		{
	 		syncArray[SYNCENTRIES].noSync++;
			syncArray[SYNCENTRIES].nosFreq+=f;
		}
	if(s!=oldS) 
		{ // we have a state change
			oldS=s;
			if(s) 
				{
					
					// we have a new sync pulse
					//calculate frequency average in this sync period
					if(syncArray[SYNCENTRIES].sync>=60) 
						{
							syncArray[SYNCENTRIES].freq/=30.;
						}
					else if(syncArray[SYNCENTRIES].sync>30) 
						{
							syncArray[SYNCENTRIES].freq/=(30-syncArray[SYNCENTRIES].sync);
						}
					
					if(syncArray[SYNCENTRIES].noSync!=0) syncArray[SYNCENTRIES].nosFreq/=(float)syncArray[SYNCENTRIES].noSync;
					syncArray[SYNCENTRIES].status=0;
					// sync must at least be half of a normal syncwidth i.e. 0.0025 seconds
					if(syncArray[SYNCENTRIES].sync<(uint)(0.0025*rxClock)) syncArray[SYNCENTRIES].status+=SHORTSYNC;
					// we take the Robot BW (8s, 120 lines) mode - Syncpulse + tolerance, as the minimum nosync:
					//     (8-0.03)/120 -0.05-some delta =~0.058 seconds
					if(syncArray[SYNCENTRIES].noSync<(uint)(0.058*rxClock)) syncArray[SYNCENTRIES].status+=SHORTNOSYNC;  
					// we need at least MINIMUMSYNCENTRIES valid entries
					if (fabs(syncArray[SYNCENTRIES].freq-1200.)>DELTAFREQ) syncArray[SYNCENTRIES].status+=INVALIDFREQ;
					if(abs(syncArray[SYNCENTRIES].sync-(int)(0.3*rxClock)<250)) syncArray[SYNCENTRIES].status+=ISVISCODE;
					logfile.add("sync=%8d  nosync=%8d  freq=%4.3f  pos=%u, status=%d ,syncT=%2.3f,nosynT=%2.3f",
						syncArray[SYNCENTRIES].sync,syncArray[SYNCENTRIES].noSync,syncArray[SYNCENTRIES].freq,syncArray[SYNCENTRIES].pos,
						syncArray[SYNCENTRIES].status,syncArray[SYNCENTRIES].sync/rxClock,syncArray[SYNCENTRIES].noSync/rxClock); 
					if(syncArray[SYNCENTRIES].sync>=(uint)(0.0025*rxClock))
						{
							emit setOffset((int)syncArray[SYNCENTRIES].freq,syncArray[SYNCENTRIES].pos);
//							debug("syncdetect %d",syncArray[SYNCENTRIES].pos);
						}	
					
					shiftUpSyncArray(0);
					if(syncArray[SYNCENTRIES].status &(SHORTSYNC+INVALIDFREQ))
						{
							if (syncCounter!=0) syncCounter--;
						}
					else
						{
							if (syncCounter<10) syncCounter++;
						}
					if (syncIndex>=MINIMUMSYNCENTRIES);
					endSync=FALSE;
				}
			else
				{
					syncArray[SYNCENTRIES].pos=tPos; // record the position of the trailing edge of the sync
					endSync=TRUE;
				}
			return TRUE;
		}
	return FALSE;
}

/*!
	\fn	syncProc::inSync()
	\brief return actual sync status
	
	\return TRUE if called during sync
*/

bool syncProc::inSync()
{
	return oldS;
}

/*!
	\fn	syncProc::evaluateSync()
	\brief Evaluate the synchro pulses. 

	The reference position will be set to the trailing edge of the sync pulse
	We will only get here after the end of a non-sync period. The entry at
	index=[SYNCENTRIES] is not used (just being scrolled up).
	most recent is at SYNCENTRIES-1, oldest is at SYNCENTRIES -synIndex.
	<ul>
	<li>Phase 1: check if newest entry is within spec (i.e freq and minimum sync duration),
		if not combine it with the previous one.
	</ul>
	\param sens sensitivity
	\param pos  contains the sync trailing edge position in the ringbuffer when this function returns TRUE.
	\return average linelength (in samples) if a valid sync sequence is found, else 0
*/
unsigned int syncProc::evaluateSync(int sens, unsigned int &pos, bool &afterVIS)
{
	int i,j;
	int avgSync=0;
	int avgNoSync=0;
	avgFreq=0.;
	afterVIS=FALSE;
	logfile.add("syncIndex=%d",syncIndex); 
	for(i=(SYNCENTRIES-syncIndex);i<SYNCENTRIES;i++)
		{
			logfile.add("sync=%8d  nosync=%8d  freq=%4.3f  pos=%u, status=%d ,syncT=%2.3f,nosynT=%2.3f",
			syncArray[i].sync,syncArray[i].noSync,syncArray[i].freq,syncArray[i].pos, syncArray[i].status,
			syncArray[i].sync/rxClock,syncArray[i].noSync/rxClock); 
		}
	
	int st=syncArray[SYNCENTRIES-1].status;
	// what if we do not have a sync (to short or wrong frequency).
	// then clearly the previous nosync should be extended by this entry: i.e. sync and nosync together 
	 if (st&(INVALIDFREQ+SHORTSYNC)) 
	 	{
			syncArray[SYNCENTRIES-2].noSync+=(syncArray[SYNCENTRIES-1].sync+syncArray[SYNCENTRIES-1].noSync);
			shiftDownSyncArray(SYNCENTRIES-1);
			if(syncIndex<MINIMUMSYNCENTRIES) return 0;
		}
	//  now check for a very long sync
	// we can't do anything with it, so complete sync detect restart.
	if((syncArray[SYNCENTRIES-1].sync>(uint)(0.022*rxClock))&&(syncArray[SYNCENTRIES-1].sync<(uint)(0.250*rxClock)))
		{
			syncIndex=0;
		  logfile.add("SYNC to long eliminated %d",i);
			return 0;
		}
	// syncwidth of 0.3s is a viscode

	for(i=(SYNCENTRIES-syncIndex),j=0;i<SYNCENTRIES;i++)
		{
			// we already handled short syncs, and invalid frequencies during sync
			if(syncArray[i].sync>(0.022)*rxClock) continue; // do not count long sync (could be vis code)
			j++;
			avgSync+=syncArray[i].sync;
			avgNoSync+=syncArray[i].noSync;
			avgFreq+=syncArray[i].freq;
		}
	if (j<2) return 0; // in case we had all long syncs
	avgSync/=j; avgNoSync/=j; avgFreq/=j;
	logfile.add("averages: sync=%d, nosync=%d, avgFreq=%f",avgSync,avgNoSync, avgFreq);

//	emit setOffset((int)avgFreq);
	// Estimating the linetime
	unsigned int lt;
	unsigned int avglt=avgSync+avgNoSync;
	unsigned int avgltCorrected=0;
	for(i=(SYNCENTRIES-1),j=0;i>=(int)(SYNCENTRIES-syncIndex);i--)
		{
			if(syncArray[i].status==ISVISCODE) // do not count long sync (could be vis code)
				{
					syncIndex=SYNCENTRIES-i; // drop everything before this viscode
					lt=avglt; // force to use this entry
				}
			else
				{
					lt=syncArray[i].sync+syncArray[i].noSync;
				}
			if(abs(avglt-lt) < 100)
				{
					pos=syncArray[i].pos;
					afterVIS=(bool)(syncArray[i].status&ISVISCODE);
					j++;
					avgltCorrected+=lt;
				}
		}
	if(j>=(4-sens))
		{
//			debug("sensitivity=%d",sensitivity);
//			selectedMode=modeLookup(avgltCorrected/j,rxClock);
//			if(selectedMode==NOTVALID)
//				{
//			 		return FALSE;
//				}
			logfile.add("start at %d",pos);
			initTrackSync();
			return ((avgltCorrected+j/2)/j);
			
		}
	return 0;	
}


/*!
	 \fn syncProc::clearSyncArray()
	 \brief clear the syncArray and reset the syncIndex
*/
void syncProc::clearSyncArray()
{
	for (int i=0;i<=SYNCENTRIES;i++)
		{
			syncArray[i].sync=0;
			syncArray[i].noSync=0;
			syncArray[i].freq=0;
			syncArray[i].nosFreq=0;
		}
	syncIndex=0;
}

void syncProc::shiftUpSyncArray(int start)
{
		for(int i=start;i<(SYNCENTRIES);i++)
		{
			syncArray[i].noSync=syncArray[i+1].noSync;
			syncArray[i].sync=syncArray[i+1].sync;
			syncArray[i].freq=syncArray[i+1].freq;
			syncArray[i].pos=syncArray[i+1].pos;
			syncArray[i].nosFreq=syncArray[i+1].nosFreq;
			syncArray[i].status=syncArray[i+1].status;
		}
	syncArray[SYNCENTRIES].noSync=0;
	syncArray[SYNCENTRIES].sync=0;
	syncArray[SYNCENTRIES].freq=0;
	syncArray[SYNCENTRIES].nosFreq=0;
	syncIndex++;
	if(syncIndex>SYNCENTRIES) syncIndex=SYNCENTRIES;
}

void syncProc::shiftDownSyncArray(int start)
{
	for(int i=start;i>=1;i--)
		{
			syncArray[i].noSync=syncArray[i-1].noSync;
			syncArray[i].sync=syncArray[i-1].sync;
			syncArray[i].freq=syncArray[i-1].freq;
			syncArray[i].nosFreq=syncArray[i-1].nosFreq;
			syncArray[i].pos=syncArray[i-1].pos;
			syncArray[i].status=syncArray[i-1].status;
		}
	syncIndex--;
	logfile.add("shiftdown si=%d",syncIndex);
	if(syncIndex>SYNCENTRIES) syncIndex=0;
}
/*! \fn syncProc::evaluateVIS()
		\brief check if we found a valid VIS code
		
		Check if we have a valid VIS code.  
*/

unsigned int syncProc::evaluateVIS(unsigned int &pos)
{
	unsigned int posAct=dsp->getPos();
	float avgFreq;
	unsigned int visCode=0;
	int i,j;
	float f;
	bool s;
	uint posx;
	unsigned int calcPos;
	logfile.add("check vis code");
	if(syncArray[SYNCENTRIES-2].status==ISVISCODE)
		{
			// pos at SYNCENTRIES-2 is the position of the trailing edge of the viscode
			// the start of the viscode is given by pos - syncduration.
			// we have a VIS code
			// we need the startbit position and go 1 bit forward
			calcPos=syncArray[SYNCENTRIES-2].pos-syncArray[SYNCENTRIES-2].sync+(uint)(0.03*rxClock);
			logfile.add("calcPos=%u, pos =%u",calcPos,syncArray[SYNCENTRIES-2].pos);
			dsp->reposition(calcPos);
			
			for(i=0;i<8;i++)
				{
					avgFreq=0;
					visCode=visCode >>1;
					for(j=0;j<(int)(0.03*rxClock);j++)
						{
							dsp->getDemodulatedData(f,s,posx); // we are sure that there is enough data, so don't check for availability  
							avgFreq+=f;
						}
					avgFreq/=(0.03*rxClock);
					logfile.add("avgFreq=%f ,endpos=%d",avgFreq,posx);
					if(avgFreq<1200)
						{
							visCode|=0x80;
						}
				}
			logfile.add("visCode=%x",(unsigned int)visCode);	 
			if(lookupVIS(visCode)==NOTVALID)
				{
					return 0;
				}
			pos=syncArray[SYNCENTRIES-2].pos;
			dsp->reposition(posAct);
//			debugPtr->reposition(pos);
			initTrackSync();
			return visCode;
		}
	return 0;
}

void syncProc::initTrackSync()
{
	restartTrackSync();
	lastCorrected=15;
	
}

void syncProc::restartTrackSync()
{
	trackIndex=0;
	trackArray[trackIndex].pos=0;
	trackArray[trackIndex].sc=dsp->getPos();
	trackArray[trackIndex].lineCounter=0;
	trackArray[trackIndex].sync=80;   // dummy value
	trackArray[trackIndex].freq=1200;
	trackIndex++;
	oldS=FALSE;
	outOfSync=0;
	lineLen=((double)(lineTimeTable[9]))/10.;
	logfile.add("init track sync lineLen=%lf",lineLen);

}


void syncProc::setOffsetCompensation(int oc)
{
	offsetCompensation=oc;
//	offsetCompensation=0;
	logfile.add("offsetCompensation=%d",offsetCompensation);
	
}

void syncProc::trackSync(float &f,bool s,int sc,int lineCount)
{
	int pos;
	//if(lineCount<60) return ;
	if(oldS!=s)
		{
			oldS=s; // we have an edge
			if(s) // we detected the leading edge of the sync
				{
					trackArray[trackIndex].sync=1;
					trackArray[trackIndex].freq=0;
				}
			else // we detected the trailing edge of the sync
				{
					if(trackArray[trackIndex].sync>=60)
						{
							trackArray[trackIndex].freq/=30;
						}
					else if(trackArray[trackIndex].sync>30) 
						{
							trackArray[trackIndex].freq/=(30-trackArray[trackIndex].sync);
						} 
					
					
					if((fabs(trackArray[trackIndex].freq-1200.)<DELTAFREQ)
						 &&(fabs((float)trackArray[trackIndex].sync-activeSSTVParam.sync*rxClock)<25))
						{
							pos=sc-(unsigned int)round(lineTimeTable[lineCount])+offsetCompensation;
							if (pos<-(lineLen/2))
								{
									trackArray[trackIndex].lineCounter=lineCount;
									pos+=(int)round(lineLen);
								}
							else if (pos>(lineLen/2))
								{
										trackArray[trackIndex].lineCounter=lineCount+2;
										pos-=(int)round(lineLen);
								}
							else
								{
									trackArray[trackIndex].lineCounter=lineCount+1;
								}
//							if (pos>lineLen/2) pos=(int)round(((float)trackArray[i].pos-lineLen)); //left slant
							trackArray[trackIndex].pos=pos;
							trackArray[trackIndex].sc=sc;
							logfile.add("acc: ti=%d freq=%f lc=%d sc=%d sync=%d, pos=%d",
										trackIndex,trackArray[trackIndex].freq,trackArray[trackIndex].lineCounter,
									trackArray[trackIndex].sc, trackArray[trackIndex].sync,trackArray[trackIndex].pos);
							outOfSync=0;
							if(trackIndex>0)
								{
//								 emit setOffset((int)trackArray[trackIndex-1].freq,trackArray[trackIndex-1].dspIndex);
//								 debug("synctrack %d",trackArray[trackIndex-1].dspIndex);
								}
							trackIndex++;
						}
					else
						{
							outOfSync++;
							logfile.add("rej: ti=%d freq=%f lc=%d sc=%d sync=%d, outOfSync=%d, pos=%d",
							trackIndex,trackArray[trackIndex].freq,lineCount,sc, trackArray[trackIndex].sync,outOfSync,dsp->getPos());
							
						}
				}
		}
	else
		{
			if(s)
				{
			 		trackArray[trackIndex].sync++;
					if((trackArray[trackIndex].sync>=30) &&(trackArray[trackIndex].sync<60))
						{
							trackArray[trackIndex].freq+=f;
						}
				}
		}
}

bool syncProc::isOutOfSync()
{
	return outOfSync>100;
}

void syncProc::calcRegression(int numb,double &a,double &b,double &r)
{
	unsigned int i;
	double sumx=0,sumy=0,sumx2=0,sumy2=0,sumxy=0;
  double sxx,syy,sxy;
	for (i=(trackIndex-numb);i<trackIndex;i++)
		{
			
			if((float)trackArray[i].pos>lineLen/2) trackArray[i].pos=(int)round(((float)trackArray[i].pos-lineLen)); //left slant
			logfile.add("sync=%d freq=%lf pos=%d linecount=%d",
			trackArray[i].sync,trackArray[i].freq,trackArray[i].pos,trackArray[i].lineCounter);
      
   /* Conpute some things we need */
			sumx += trackArray[i].lineCounter;
			sumy += trackArray[i].pos;
			sumx2 += (trackArray[i].lineCounter * trackArray[i].lineCounter);
			sumy2 += (trackArray[i].pos * trackArray[i].pos);
			sumxy += (trackArray[i].lineCounter * trackArray[i].pos);
   }
	sxx = sumx2 - sumx * sumx / numb;
	syy = sumy2 - sumy * sumy / numb;
	sxy = sumxy - sumx * sumy / numb;
	
	/* Calculate the slope (b) and intercept (a) */
	b = sxy / sxx;
	a = sumy / numb - b*sumx / numb;
	if (fabs(syy) <= 0.00001)  r = 1;
	else r = sxy / sqrt(sxx * syy); 
	logfile.add("sumx=%lf,sumy=%lf,sumx2=%lf,sumy2=%lf,sumxy=%lf",sumx,sumy,sumx2,sumy2,sumxy);
	logfile.add("sxx=%lf,syy=%lf,sxy=%lf",sxx,syy,sxy);
	logfile.add ("a=%lf b=%lf r=%lf",a,b,r);
}

bool syncProc::correctValues(int numb,double &a,double &b,double &r)
{
	unsigned int i;
	double sumx=0,sumy=0,sumx2=0,sumy2=0,sumxy=0;
  double sxx,syy,sxy;
	int j=0;
	lastCorrected=trackIndex;
	for (i=(trackIndex-numb);i<trackIndex;i++)
		{
			if(abs(trackArray[i].pos-((int)round(a+b*trackArray[i].lineCounter)))>10) continue; 
   /* Conpute some things we need */
			sumx += trackArray[i].lineCounter;
			sumy += trackArray[i].pos;
			sumx2 += (trackArray[i].lineCounter * trackArray[i].lineCounter);
			sumy2 += (trackArray[i].pos * trackArray[i].pos);
			sumxy += (trackArray[i].lineCounter * trackArray[i].pos);
			j++;
			
   }
	if (j<5) return FALSE;
	
	sxx = sumx2 - sumx * sumx / j;
	syy = sumy2 - sumy * sumy / j;
	sxy = sumxy - sumx * sumy / j;
	
	/* Calculate the new slope (b) and new intercept (a) */
	b = sxy / sxx;
	a = sumy / numb - b*sumx / numb;
	if (fabs(syy) <= 0.00001)  r = 1;
	else r = sxy / sqrt(sxx * syy); 
	logfile.add ("a=%lf b=%lf r=%lf",a,b,r);
	return TRUE;
}


bool syncProc::evaluateTrackSync(float &clock,int &offset)
{
	#define NUMB 20
	unsigned int numb=NUMB+lastCorrected;
	double a,b,r;
	if(trackIndex<numb) return FALSE;
	if(trackArray[numb].sc>(INPUTBUFFERLEN-2000)) return FALSE; //protect against inputbuffer wrapping
	float tmpClock;
	calcRegression(numb,a,b,r);
	if(!correctValues(numb,a,b,r)) return FALSE;
	/* Compute the regression coefficient */
	tmpClock=clock*(1+b/lineLen);
	offset=(int)round(a); // -1 because we lose one sample while restarting
	logfile.add ("a=%lf b=%lf newClock=%f,r=%lf",a,b,tmpClock,r);
	if(((fabs(b)>0.07) || (fabs(a)>10)))
		{
			 clock=tmpClock;
			 return TRUE;
		}
	return FALSE;
}

bool syncProc::checkRepeaterTone(float f,bool Init)
{
	return detectTone(f,1750.,Init);
}



bool syncProc::detectTone(float f,float freq,bool Init)
{
	if(Init)
		{
			dState=dSTART;
			count=0;
		}
	switch (dState)
		{
			case dSTART:
				{
					if(fabs(f-freq)<=DELTAFREQREPTONE) count++;
					else count--;
					if (count<0) count=0;
					if (count>((int)(rxClock*0.8)))
						{
						 dState=dEND;
						 count=(int)(rxClock*0.1);
						}
				}
			break;
			case dEND:
				{
					if(fabs(f-freq)>DELTAFREQ) count--;
					if(count<=0) return TRUE; 
				}
		}	
	return FALSE;
}


syncProc  syncProcessor;