#include "diabolo.h"
#include "bolo.h"
#include "synchro.h"
#include "fichier.h"
#include "trace.h"
#include "menu.h"
#define max_couleur 10

static int	ds_bolo_coul[2*nb_max_bolo];

	
static int 		choix_DS=0;
static	int		npt_DS=0;					//  nombre de points (periode) pour le calcul de la DS
static	double		table_DS[2*max_points_DS];	//  table de detection synchrone, dupliquer en double longueur
static  double		norme_DS;
static  int		raz_parasite=2;

void	montracen_ds(int fen,int n,double x,double *y);

void	exec_trace_DS_suite(int u);
void 	trace_DS(double *y,int enregistre);
void	calcul_table_DS(double periode);

#define degre(x)	((int)(x*180./3.14159))

//  calcul ya  pendant la periode   ,  yb  a cheval sur 2 periodes  , yc= somme des deux


void exec_trace_DS(int fen,int item,double valeur,...) 
{
static int u=-1;
static flag_err=0;
int i;
switch(item)
    {
    case ouverture	:	gg->trace_ouvert=gg->trace_ouvert | 4 ;
 	    			graph->ymin=-20;
 		   		graph->ymax=20;
	    			graph->ypas=5;
	    			graph->xmax=500;
	    			graph->xpas=50;
    				graph->taille_graduations=7;
//   				graph->flag_tracen=0;
//   				for(j=0;j<nb_max_bolo;j++)	if( (j<10) && gg->bolo_type[j])	graph->flag_tracen+=(1<<j);
//				graph->flag_tracen+=(1<<nb_max_bolo);	//  voie pour affichage du scan 

    case ds_choix_DS	:		    
    case ds_phase	:
    case ds_phase+1000	:
    case ds_temps_mort	:	
    case ds_temps_mort+1000 :
                 			
                 			
    case ds_bolo_couleur :
    case ds_bolo_couleur+1 :
    case ds_bolo_couleur+2 :
    case ds_bolo_couleur+3 :
    case ds_bolo_couleur+4 :
    case ds_bolo_couleur+5 :
    case ds_bolo_couleur+6 :
    case ds_bolo_couleur+7 :
    case ds_bolo_couleur+8 :
    case ds_bolo_couleur+9 :
    
    				for(i=0;i<max_couleur;i+=2)	
    					{
    					ds_bolo_coul[i]=litD(fen,ds_bolo_couleur+i,0L)-1; // 0=rien : 1=bolo0 // 2=bolo1
    					if( (ds_bolo_coul[i]<1) ou (ds_bolo_coul[i]>nb_max_bolo) )   ds_bolo_coul[i]=0;
    					if (ds_bolo_coul[i]<=24) ds_bolo_coul[i+1]=ds_bolo_coul[i]+24;
    					printf(" i=%d bolo=%d    ",i,ds_bolo_coul[i]);
    					}
    				printf("\n");				    
 
    				calcul_table_DS(synchro_periode);
				break;

    case ds_raz_parasite :	if (valeur)	raz_parasite=1;		//  soustraction
    				else		raz_parasite=2;		//  plus de soustraction
				printf("remise a zero signal parasite \n");
    				break;
							
	case ds_raz	:	gg->temps_origine=gg->periode_echantillonage*(double)gg->temps_cntl;
    				efface(fen);	
    				break;


	case ds_debut	:	if( (u<8*npt_DS/10) ou (u>15*npt_DS/10)	)	
					{
					if(!flag_err) {printf("erreur periode \n");flag_err=20;}
					flag_err--;
					}
				else	flag_err=0;
				while(u<npt_DS)	{exec_trace_DS_suite(u);u++;}	// pour finir la periode
				u=0;


	case ds_suite	:	if( (u>=0) && (u<npt_DS) ) exec_trace_DS_suite(u);
				u++;
				break;
	
	case ds_enregistre :	if(!fenetre(fenetre_enregistre_DS)) nouveauT(fenetre_enregistre_DS,0,"valeurs_DS");
				trace_DS(0,1);
				break;
	

	case fermeture	:	gg->trace_ouvert=gg->trace_ouvert & (255-4);	// enleve le 4 
    				break;

	default		:	break;
	}

}


//   le trace de la detection synchrone passe par un appel a chaque point  avec u=0,1,2,.... , npt_DS-1
//  
//   la commande de trace sera faite par  exec_trace_DS_suite pour certaines valeures de u
//  pour une detection avec trois positions qui se suivent, le parametre supplementaire pernum
//  compte de 0 a 2. Il est remis a zero pour un appel de la fonction avec u=-1 (qui ne fait rien d'autre)

//  la detection est accumulée simultanement dans deux registres y1 et y2 qui sont plotte simultanement.
//  on a ainsi deux points par periode 

 
void	exec_trace_DS_suite(int u)	// appellé quoiqu'il arrive avec u=0,1,2,.... , npt_DS-1
{
static double y[2*nb_max_bolo],y1[2*nb_max_bolo],y2[2*nb_max_bolo],bb[2*nb_max_bolo];
int j;
static double  phase_mesure[nb_max_bolo],confiance[nb_max_bolo],phase_memoire;
//printf("suite avec u=%d \n");
switch(choix_DS)
	{
	case choix_DS_sinus_1f : 
		
		for(j=0;j<nb_max_bolo;j++)
			{
			y1[j]+= xbol(j)*table_DS[u];			//  le sinus
			y2[j]+= xbol(j)*table_DS[u+npt_DS/4];		// dephase de pi/2 -> le cosinus
			bb[j]+=	xbol(j)* (((double)(u%2))-0.5);		// estimation du bruit
			}

		if(u==npt_DS-1)
			{
			for(j=0;j<nb_max_bolo;j++)
				{
				phase_mesure[j]=atan2(y1[j],y2[j]);
				confiance[j]=bb[j]*bb[j];
				if(confiance[j]) 	confiance[j]=log((y1[j]*y1[j]+y2[j]*y2[j])/(bb[j]*bb[j]));
				}
			j=5;	// bolo pur mesurer la phase
			phase_memoire=0.95*phase_memoire+0.05*phase_mesure[j];
			ecritD(fenetre_trace_DS,ds_phase,"%d°",degre(phase_memoire));
			printf(" phase=%d°(%2.1f)  \n",degre(phase_mesure[j]),confiance[j]);
//			printf(": phase memoire=%g  \n",phase_memoire);
//			printf(" trace y1= %g , %g , %g , %g , %g \n",y1[0],y1[1],y1[2],y1[3],y1[4]); 
			for(j=0;j<nb_max_bolo;j++)
				{
				y1[j+nb_max_bolo]=y2[j];
				}

/*				
			for(j=0;j<nb_max_bolo;j++)
				{
				y1[j]=sin(phase_memoire)*y1[j]+cos(phase_memoire)*y2[j];
				}
			printf(" trace y1= %g , %g , %g , %g , %g \n",y1[0],y1[1],y1[2],y1[3],y1[4]); 
*/
			trace_DS(y1,0);
			for(j=0;j<nb_max_bolo;j++)
				{
				y1[j]=0;y2[j]=0;bb[j]=0;
				}
			
			}
		
		break;		
				

	case choix_DS_hacheur	:
		
		if( (u==npt_DS/8) ou (u==5*npt_DS/8) )		trace_DS(y1,0);
			if( (u==3*npt_DS/8) ou (u==7*npt_DS/8) )	trace_DS(y2,0);

		for(j=0;j<nb_max_bolo;j++)
			{
			y1[j]+= xbol(j)*table_DS[u];
			y2[j]+= xbol(j)*table_DS[u];
			}
		break;
	

	case choix_DS_secondaire	:	if(u==npt_DS/4) 	trace_DS(y1,0);
				if(u==3*npt_DS/4) 	trace_DS(y2,0);

				for(j=0;j<nb_max_bolo;j++)
					{
					y1[j]+= xbol(j)*table_DS[u];
					y2[j]+= xbol(j)*table_DS[u];
					}
				break;
	
	case choix_DS_prisme	:	if(u==npt_DS/6)		{y[2]=y1[2];y1[2]=0;trace_DS(y,0);}	// phase 1  (bolo 3) 

				if(u==4*npt_DS/6)	{y[2]=y2[2];y2[2]=0;trace_DS(y,0);}	// phase 1  (bolo 3) 

				if(u==0) 			{y[1]=y1[1];y1[1]=0;}				// phase  2  (bolo 2)
					
				if(u==3*npt_DS/6) 	{y[1]=y2[1];y2[1]=0;}				// phase  2  (bolo 2)
					
				if(u==2*npt_DS/6) 	{y[3]=y1[3];y1[3]=0;				// phase  3  (bolo 4)
									y[0]=y1[0];y1[0]=0;	// bolo1
									y[4]=y1[4];y1[4]=0;	// temperature
									}

				if(u==5*npt_DS/6) 	{y[3]=y2[3];y2[3]=0;				// phase  3  (bolo 4)
									y[0]=y2[0];y2[0]=0;	// bolo1
									y[4]=y2[4];y2[4]=0;	// temperature
									}
				
				y1[0]+= xbol(0)*table_DS[u];
				y2[0]+= xbol(0)*table_DS[u];
				
				y1[1]+= xbol(1)*table_DS[u+4*npt_DS/6];
				y2[1]+= xbol(1)*table_DS[u+4*npt_DS/6];
				
				y1[2]+= xbol(2)*table_DS[u+2*npt_DS/6];
				y2[2]+= xbol(2)*table_DS[u+2*npt_DS/6];
				
				y1[3]+= xbol(3)*table_DS[u];
				y2[3]+= xbol(3)*table_DS[u];
				
				y1[4]+= xbol(4)*table_DS[u];
				y2[4]+= xbol(4)*table_DS[u];
				
				break;
	


	default :	break;
	}
}	
							

// divise par la norme, trace y puis fait un RAZ
void trace_DS(double *y,int enregistre)		//   y [2*nb_max_bolo]
{
double x;
static double yraz[2*nb_max_bolo];
static double yt[2*nb_max_bolo];
int j,k,moy;
static double yfiltre[2*nb_max_bolo][10];
static int uf=0;
static int scan_ds=0;
x=gg->periode_echantillonage*(double)gg->temps_cntl-gg->temps_origine;

if(enregistre)
	{
	printf("traceDS avec enregistre=1\n");
	ecritT(fenetre_enregistre_DS,fin_f," %9.3f ",x );
	for(j=0;j<nb_max_bolo;j++) if(parametr.bolo[j].bolo_code_util)
				{
				ecritT(fenetre_enregistre_DS,fin_f,"  %9.3f",yt[j]);
				}
	ecritT(fenetre_enregistre_DS,fin_f,"\n");
	return;
	}


for(j=0;j<2*nb_max_bolo;j++)	yt[j]=y[j];
for(j=0;j<2*nb_max_bolo;j++)	y[j]=0;

//printf(" au debut = %g , %g , %g , %g , %g \n",yt[0],yt[1],yt[2],yt[3],yt[4]); 




for(j=0;j<2*nb_max_bolo;j++)
			{
			yt[j]/=norme_DS;
			if(raz_parasite==1)	yraz[j]=yt[j];
			if(raz_parasite==2)	yraz[j]=0;
			yt[j]-=yraz[j];
			}

if (gg->trace_ouvert&16)  trace_annexe(yt);	// données  razparasite  mais  non filtrées

raz_parasite=0;
selectgra(fenetre_trace_DS);
if(scan_ds != ( (gg->don.sync[sync_subscan]!=0) et (gg->don.sync[sync_scan]!=0) ) )
	{
	if(scan_ds)	rectangle(fenetre_trace_DS,x,graph->ymin,x,graph->ymax,rouge);		// trace le segment x1,y1 - x2,y2		
	  else	rectangle(fenetre_trace_DS,x,graph->ymin,x,graph->ymax,bleu);		// trace le segment x1,y1 - x2,y2		

	scan_ds = ( (gg->don.sync[sync_subscan]!=0) et (gg->don.sync[sync_scan]!=0) ) ;
	}


moy=litD(fenetre_trace_DS,ds_moyenne,0L);
if(moy>10) {moy=10;ecritD(fenetre_trace_DS,ds_moyenne,"10");}
if(moy)
	{
	uf++;
	if(uf>=moy) uf=0;
	for(j=0;j<2*nb_max_bolo;j++) 					 
		{
		for(k=0;k<moy;k++)	yfiltre[j][k]+=yt[j];
		yt[j]=yfiltre[j][uf]/moy;
		yfiltre[j][uf]=0;
		}
	}
//printf(" montracen yt= %g , %g , %g , %g , %g \n",yt[0],yt[1],yt[2],yt[3],yt[4]); 
//printf(" avant trace : =%g  \n",yt[nb_max_bolo]);
montracen_ds(fenetre_trace_DS,2*nb_max_bolo,x,yt);
if(!(graph->scroll_courbe) && litD(fenetre_trace_DS,ds_retour,0L) &&
		 ( (x<graph->xmin) ou (x>graph->xmax) )) 
			{
			graph->xmax-=graph->xmin;graph->xmin=0;
			gg->temps_origine=gg->periode_echantillonage*(double)gg->temps_cntl;
			x=0;	
			}

//ecritD(fenetre_trace_DS,ds_bol1,"%gµV",yt[0]);
//ecritD(fenetre_trace_DS,ds_bol2,"%gµV",yt[1]);

}




//  fabrique une table double longueur pour ne pas poser de problèmes 
//  la table de detection synchrone est deja dephase comme il faut

void	calcul_table_DS(double periode)
{
int i,v,tm,phase;
npt_DS=(int)(periode+0.2);	// synchro_periode est un double  et npt_DS est un entier
if (npt_DS>max_points_DS)	npt_DS=max_points_DS;
choix_DS=litD(fenetre_trace_DS,ds_choix_DS,0L);
tm=litD(fenetre_trace_DS,ds_temps_mort,0L);
phase=litD(fenetre_trace_DS,ds_phase,0L);
norme_DS=0;


switch(choix_DS)
	{
	case choix_DS_sinus_1f : npt_DS=2* (int) (npt_DS/2);			//  plus  moins  plus  moins  (hacheur)
				for(i=0	;i<npt_DS;i++)	
					{
					table_DS[i]=sin(2 * 3.14159 * (double)i / (double)npt_DS ) ;
					norme_DS+=0.5;
					}
				break;
				

	case choix_DS_hacheur :	npt_DS=8* (int) (npt_DS/8);			//  plus  moins  plus  moins  (hacheur)
				printf(" DS1 hacheur pour %d points : ",npt_DS);
				i=phase;
				while(i<0) i+=npt_DS;
				for(v=0	;v<npt_DS/4	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		{table_DS[i]=-1;	norme_DS++;}
					i++;
					}
				for(v=0	;v<npt_DS/4	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		table_DS[i]=1;
					i++;
					}
				for(v=0	;v<npt_DS/4	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		table_DS[i]=-1; //	norme_DS++; la norme ne contient qu'une demi période
					i++;
					}
				for(v=0	;v<npt_DS/4	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		table_DS[i]=1;
					i++;
					}
				break;


	case choix_DS_secondaire :	npt_DS=4* (int) (npt_DS/4);								//  plus  moins   (diodes/secondaire)
				printf(" DS2 (diodes) pour %d points : ",npt_DS);
				i=phase;
				while(i<0) i+=npt_DS;
				for(v=0	;v<npt_DS/2	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		{table_DS[i]=-1;	norme_DS++;}
					i++;
					}
				for(v=0	;v<npt_DS/2	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		table_DS[i]=1;
					i++;
					}
				break;



	case choix_DS_prisme :	npt_DS=6* (int) (npt_DS/6);						//   moins  plus  moins  (prisme)
				printf(" DS3 (prisme) pour %d points : ",npt_DS);
				i=phase;
				while(i<0) i+=npt_DS;
				for(v=0	;v<npt_DS/3	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		{table_DS[i]=-1;}
					i++;
					}
				for(v=0	;v<npt_DS/3	;v++)	
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		{table_DS[i]=-1;}
					i++;
					}
				for(v=0	;v<npt_DS/3	;v++)			//  plus pour la phase de référence   
					{
					if(i>=npt_DS) i=0;
					if(v<tm) 	table_DS[i]=0;
					else 		{table_DS[i]=2;	norme_DS+=2;}
					i++;
					}
				break;
				
	default	:	break;
	}

for(i=0;i<npt_DS;i++)	table_DS[i+npt_DS]=table_DS[i];	// complete la table (double longueur)
ecritD(fenetre_trace_DS,ds_periode,"periode: %d pts",npt_DS);

/*
printf("\n normeDS=%d  : ", norme_DS);
for(i=0;i<npt_DS;i++)
	{
	printf(" %d ",table_DS[i]);
	}
printf("\n");
alerte(2,"bonjour");
*/
}



void	montracen_ds(int fen,int n,double x,double *y)
{
int i,cc;
double yy[max_couleur];   // 0=rien : 1 2=bolo0 // 3 4 =bolo
for(i=0;i<max_couleur;i++)
	{
	cc=ds_bolo_coul[i];
	if(cc && (cc<=2*nb_max_bolo) ) 	yy[i]=y[cc-1];
		else		yy[i]=0;
	}
tracen(fen,max_couleur,x,yy);
//	printf("0=%g  1=%g  2=%g  \n",yy[0],yy[1],yy[2]);
}