#include "diabolo.h" 
#include "senseur_stellaire.h" 
#include "compress.h" 
#include "recons_sst.h" 
#include <math.h>

#define M_PI 3.1415926535

//--------------------  fonction  exec  de  la  fenetre : senseur_stellaire  -------------------------------

static int diodes[48];

#define DIODE_UNUSED_1 3
#define DIODE_UNUSED_2 7


int diodesbuffer[dbufsz][48];int off[48];
float zfoundstars[20];
float mfoundstars[20];
double tfoundstars[20];
int nfoundstars;
int seuilSST=5;

static void init_sst_buffers(void);
static void remove_diode_offset(void);
static void find_stars(double secondes);

static void trace_lin(int fen, double secondes);
static void trace_radar(int fen, double secondes);
static void init_radar(void);
static void setup_gra(int fen);
static void clean_radar(float phase,int fen);

static double radarPer = 30.; // periode du radar, en secondes.
static double radarTUp = 0; // le temps de reference pour pointage "NORD"
static int    radarSens = 1;
static int  sstHas2Bars = false;

//static double tm = 0;

void exec_senseur_stellaire(int fen,int item,double valeur,...) 
{
if(item>1000) item-=1000;		// pour appeler le case pour tous les cara d'un edit texte
switch(item)
    {
    case ouverture               :
    				selectgra(fen);
    				setup_gra(fen);
 				init_sst_buffers();
 			    radarPer = litD(fenetre_senseur_stellaire,sst_periode,0);
 			    radarTUp = litD(fenetre_senseur_stellaire,sst_phase,0);

                 break;
    case fermeture               :
                 break;
    case sst_txt :
                 break;
    case sst_twobars :
                 sstHas2Bars = litD(fenetre_senseur_stellaire,sst_twobars,0);
                 break;
    case sst_radar :
                 setup_gra(fen);
                 break;
    case sst_seuil : {
            seuilSST = litD(fenetre_senseur_stellaire,sst_seuil,0);
            if (seuilSST <=0) {
              seuilSST = 1;
            }
            }
            break;
    case sst_periode :
    case sst_sens_horaire:
    case sst_phase : {
            double sns = litD(fenetre_senseur_stellaire,sst_sens_horaire,0);
            double phs = litD(fenetre_senseur_stellaire,sst_phase,0);
            radarPer = litD(fenetre_senseur_stellaire,sst_periode,0);
            radarTUp = phs/360. * radarPer;
    		selectgra(fen);
    		if (radarSens != (sns ? -1 : 1)) efface(fen);
            radarSens = sns ? -1 : 1;
            break;
            }
    case sst_efface :
                 setup_gra(fen);
                 efface(fen);
                 break;
    case tache_de_fond: 
            selectgra(fen);
            if(litD(fenetre_senseur_stellaire,sst_radar,0)) {
               trace_radar(fen, valeur);
            } else {
               trace_lin(fen, valeur);
            }
                 break;
    default  :   break;
    }
}

void setup_gra(fen) {
 selectgra(fen);
  if(litD(fenetre_senseur_stellaire,sst_radar,0)) {
    graph->ymin=-60;
    graph->ymax=60;
    graph->ypas=0;
    graph->xmin=-60;
    graph->xmax=60; 
    graph->xpas=0;
    graph->grille=0;
    graph->graduations=0;
    graph->taille_graduations=0;
    graph->sans_image=1;      	
    graph->avec_icones=0;      	
    strcpy(graph->xtitre, "");    	
    graph->ytitre[0]=0;   
    init_radar();
  } else {
    graph->ymin=0;
    graph->ymax=48;
    graph->ypas=5;
    graph->xmin=0;
    graph->xmax=240; 
    graph->xpas=30;
    graph->grille=0;
    graph->graduations=1;
    graph->taille_graduations=10;
    graph->sans_image=0;      	
    graph->avec_icones=0;      	
    strcpy(graph->xtitre, "T");    	
    graph->ytitre[0]=0;   
  }
  efface(fen);
}

#define seuil1 500
#define seuil2 250
#define seuil3 120
#define seuil4 60
#define seuil5 30
#define seuil6 10

#define frc(x) ((x) - (int)(x))

void trace_lin(int fen, double secondes) {
  int i;
  //tm += gg->periode_echantillonage;
  //if (tm>graph->xmax) tm = 0;
  double tm = secondes - (int)(secondes/graph->xmax)*graph->xmax;
  if(!litD(fenetre_senseur_stellaire,sst_etoiles,0)) {
   for (i=0; i<48; i++) {
   if(fabs(off[i])>1000.) continue;
     if (-diodes[i] +off[i]> seuil1) 
       symbole(fen, tm, i,  5, rondplein, 0, rouge);
     else if (-diodes[i]+off[i] > seuil2) 
       symbole(fen, tm, i,  4, rondplein, 0, rouge);
     else if (-diodes[i]+off[i] > seuil3) 
       symbole(fen, tm, i, 3, rondplein, 0, rouge);
     else if (-diodes[i]+off[i] > seuil4) 
       symbole(fen, tm, i, 2, rondplein, 0, rouge);
     else if (-diodes[i]+off[i] > seuil5) 
       symbole(fen, tm, i, 2, point, 0, rouge);
     else if (-diodes[i]+off[i] > seuil6) 
       symbole(fen, tm, i, 2, point, 0, noir);
    }
  } else {
   for (i=0; i<nfoundstars; i++) {
     float z = zfoundstars[i];
     if (mfoundstars[i] > seuil1) {
       symbole(fen, tm, z,  5, rondplein, 0, rouge);
     } else if (mfoundstars[i] > seuil2) {
       symbole(fen, tm, z,  4, rondplein, 0, rouge);
     } else if (mfoundstars[i] > seuil3) {
       symbole(fen, tm, z,  3, rondplein, 0, rouge);
     } else if (mfoundstars[i] > seuil4) {
       symbole(fen, tm, z,  2, rondplein, 0, rouge);
     } else if (mfoundstars[i] > seuil5) {
       symbole(fen, tm, z,  2, point, 0, rouge);
     } else
       symbole(fen, tm, z,  2, point, 0, noir);
   }
  }
}

#define NMAXRADARSTAR 1000

struct radarStar {
  float phase;
  float rayon;
  float flux;
};

static struct radarStar* radarstars = 0;
static double lastPhase = 0;

void init_radar() {
  int i;
  if (!radarstars) {
    radarstars = (struct radarStar*) malloc(NMAXRADARSTAR*sizeof(struct radarStar));
  }
  for (i=0; i<NMAXRADARSTAR; i++) {
    radarstars[i].phase = -1;
    radarstars[i].rayon = -1;
  }
  
}


// Nettoyage radar pour trace jusqu'a phase

static void clean_radar(float phase, int fen) {
  int i;
  double x,y,r,th;
  for (i=0; i<NMAXRADARSTAR; i++) {
  //  if (radarstars[i].phase >= 0 && radarstars[i].phase < 10 &&
  //      ((phase > lastPhase && radarstars[i].phase > lastPhase &&
  //        radarstars[i].phase <= phase) ||
  //      ((phase < lastPhase && (radarstars[i].phase > lastPhase ||
  //        radarstars[i].phase <= phase))))) {
    if (radarstars[i].phase >= 0 && radarstars[i].phase < phase-1) {
      r = (60-48) + radarstars[i].rayon;
      th = (frc(radarstars[i].phase)*radarSens+.25) * 2 * M_PI ;  
      x = r * cos(th);
      y = r * sin(th);
      if (radarstars[i].flux > seuil1) {
        symbole(fen, x, y,  5, rondplein, 0, blanc);
      } else if (radarstars[i].flux > seuil2) {
        symbole(fen, x, y,  4, rondplein, 0, blanc);
      } else if (radarstars[i].flux > seuil3) {
        symbole(fen, x, y,  3, rondplein, 0, blanc);
      } else if (radarstars[i].flux > seuil4) {
        symbole(fen, x, y,  2, rondplein, 0, blanc);
      } else  {
        symbole(fen, x, y,  2, point, 0, blanc);
      } 
      radarstars[i].phase = -1;
    }
    //if (radarstars[i].phase >= 10) {
    //  radarstars[i].phase -= 10;
    //} 
  }
}

static long lastRadarTrace = 0;

void trace_radar(int fen, double secondes) {
  // On a des nouvelles etoiles...
  double phase;
  int i,j;
  long tk;
    
  phase = (secondes - radarTUp)/radarPer;
  //phase = phase - (int)(phase);
  
  for (i=0; i<nfoundstars; i++) {
    for (j=0; j<NMAXRADARSTAR; j++) {
      if (radarstars[j].phase < 0) {
        radarstars[j].phase = phase;//  +10; // >=10 : nouvelle...
        radarstars[j].rayon = zfoundstars[i];
        radarstars[j].flux  = mfoundstars[i];
        break;
      }
    }
  }
  
  // Le trace...
  // mais on ne trace pas tout le temps...
  tk = TickCount();
  if (tk - lastRadarTrace < 10) return;
  lastRadarTrace = tk;
  
  if (litD(fenetre_senseur_stellaire,sst_autolock,0)) {
    float per,phs,sns;
    per = DonnePeriod();
    phs = DonnePhase();
    sns = DonneSens();
    if (per>0) {
      ecritD(fenetre_senseur_stellaire, sst_periode, "%8.3f",per);
      radarPer = per;
    }
    if (phs>-9000) {
      ecritD(fenetre_senseur_stellaire, sst_phase, "%8.1f",phs);
      radarTUp = phs/360. * radarPer;
    }
    if (sns > -9000) {
      radarSens = -sns;
      ecritC(fenetre_senseur_stellaire, sst_sens_horaire, sns > 0);
    }
  }
  
  //cercle(fen,-(60-48),-(60-48),(60-48),(60-48),jaune);	// trace le cercle x1,y1 - x2,y2		

 modtrace(fen,1,blanc);
  segment(fen, (60-48)*cos((lastPhase*radarSens+.25)*M_PI*2),  
               (60-48)*sin((lastPhase*radarSens+.25)*M_PI*2),
                60*cos((lastPhase*radarSens+.25)*M_PI*2),       
                60*sin((lastPhase*radarSens+.25)*M_PI*2));
                
  clean_radar(phase, fen);
  
  modtrace(fen,1,jaune);
  segment(fen, (60-48)*cos((frc(phase)*radarSens+.25)*M_PI*2),  
               (60-48)*sin((frc(phase)*radarSens+.25)*M_PI*2),
                60*cos((frc(phase)*radarSens+.25)*M_PI*2), 
                60*sin((frc(phase)*radarSens+.25)*M_PI*2));
              
  for (i=0; i<NMAXRADARSTAR; i++) {
    if (radarstars[i].phase >= 0) {
      float x,y,r,th;
      r = (60-48) + radarstars[i].rayon;
      th = (frc(radarstars[i].phase)*radarSens+.25) * 2 * M_PI ;  
      x = r * cos(th);
      y = r * sin(th);
      if (radarstars[i].flux > seuil1) {
        symbole(fen, x, y,  5, rondplein, 0, rouge);
      } else if (radarstars[i].flux > seuil2) {
        symbole(fen, x, y,  4, rondplein, 0, rouge);
      } else if (radarstars[i].flux > seuil3) {
        symbole(fen, x, y,  3, rondplein, 0, rouge);
      } else if (radarstars[i].flux > seuil4) {
        symbole(fen, x, y,  2, rondplein, 0, rouge);
      } else if (radarstars[i].flux > seuil5) {
        symbole(fen, x, y,  2, point, 0, rouge);
      } else
        symbole(fen, x, y,  2, point, 0, noir);
    }
  }
  lastPhase = frc(phase);

}



//#define	bit_sst(i,j,k)  (((blk->sst[i][i*3+k/4])>>(j+8*k%4))&1)   


//   i est le numero du point dans le block de 72
//	j	est le numero de la diode  de 0 a 47
//  	j%8	est la place du bit dans le mot de 8 bit 
//	j/8	(de 0 a 5 ) est la serie de mesure
//	k est le numero du bit dans le mot de 12 bits
//		les k se suivent dans les données

//   k est le numero du point dans le block de 72
//	i	est le numero de la diode  de 0 a 47
//  	j	est le paquet de 4 bits de 0 a 2 


// on prend des paquets de 4 bits
// chaque diode est formee de 3 paquets
// dans l'ordre :  les paquets de 8 diodes  (8 paquets)
//  recommencer 3 fois	(24 paquets)
// enfin, faire 6 fois cette operation  (144 paquets)

// soit q  la place du paquet  i,j,k
#define place_paquet(i,j)	 (i/8) * 24  + j*8 + (i%8) 


void decode_sst(block_type_sst*	blk, int i, int* diodes); // diodes = tableau a 48 entrees

void decode_sst(block_type_sst*	blk, int i, int* diodes) {
  int j; // 0-5 : numero du bloc de 8 diodes
  int k; // 0-2 : indice du bloc de 4 bits (une diode = 12 bits = 3 blocs de 4 bits)
  int l; // 0-7 : indice de la diode dans son bloc (8 diodes * 4 bits = 1 mot de 32 bits)
  
  // numero de la diode (0-47) = j*8+l;
  // indice dans le bloc sst du mot de 32 bits (0-17) = j*3+k;
  // indice dans mot de 32 bits du premier bit utile = 4*l;
  
  for (j=0; j<48; j++) diodes[j] = 0;
  
  for (j=0; j<6; j++)
    for (k=0; k<3; k++)
      for (l=0; l<8; l++) {
        long word = blk->sst[i][j*3+k];
        word = (word >> (4*l)) & 0xF;
        //printf("diode %d mot %d valeur %d\n", j*8+l, k, word);
        diodes[j*8+l] = (diodes[j*8+l] << 4) + word;
      }
      
  //for (j=0; j<48; j++) if (diodes[j]>2047) diodes[j] -= 4096;
  for (j=0; j<48; j++)  diodes[j] -= 2048;
}



//#define place_paquet(i,j)	 ((i/8) * 24  + j*8 + (i%8) )

void	traite_block_sst_comprime	(block_type_sst_comprime*	blk){
  block_type_sst blk2;
  unsigned long	sst_vrai[nb_per_block*2];
  int j,jc,i,k;
  unsigned long	a,b0,b1,b2;

  
  for (j=0; j<18; j++)
    for (i=0; i<nb_per_block*2; i++)
      blk2.sst[i][j] = 0;

  jc=0;
  for(j=0;j<48;j++) {
    if ((j!=0) && (j!=4)) 
    {
      decompress_4_1((long*)blk->sst[jc],(long*)sst_vrai,nb_per_block*2);
      for(k=0;k<nb_per_block*2;k++) {
         b2 = sst_vrai[k] & 0xf;
         b1 = (sst_vrai[k] >> 4) & 0xf;
         b0 = (sst_vrai[k] >> 8) & 0xf;
	 a=place_paquet(j,0);
	 blk2.sst[k][a/8] |= (b0 << (a%8)*4);
	 a=place_paquet(j,1);
	 blk2.sst[k][a/8] |= (b1 << (a%8)*4);
	 a=place_paquet(j,2);
	 blk2.sst[k][a/8] |= (b2 << (a%8)*4);
      }
      jc++;
    }
  }
  valide_block((block_type_modele*)&blk2,block_sst,numero_block(blk));	
  traite_block_sst(&blk2);
}


void	traite_block_sst(block_type_sst*	blk)
{
int i,k;
//double	y[15];
//double	x;

int temps_cntl;
double secondes;
//int a,b,b0,b1,b2;
//char tab[5000];

for (i=0; i<nb_per_block*2; i++) {
  k=0;
  decode_sst(blk, i, diodes);
  temps_cntl=numero_block(blk)*nb_per_block*2+i;
  secondes = temps_cntl*gg->periode_echantillonage;
  
  // 1. Suppression d'offset sur la rangee de diodes, et remise en ordre
  remove_diode_offset();
  // 2. Suppression des doubles impulsions et detection des etoiles
  find_stars(secondes);
  exec_recons_sst();
  
  if(fenetre(fenetre_senseur_stellaire)) 
    exec_senseur_stellaire(fenetre_senseur_stellaire,tache_de_fond,secondes);
}
}

void init_sst_buffers(void) {
  int i,j;
  for (i=0; i<dbufsz; i++)
    for (j=0; j<48; j++) 
      diodesbuffer[i][j] = 0;
  nfoundstars = 0;
}

// sans objet a cause des filtres de l'electronique ?
// se contente de permuter les diodes

// diodpermut[i] = channel de la diode i
static int diodpermut[46]=
 { 8,24,40, 9,25,41,10,26,42,11,
  27,43,16,32, 1,17,33, 2,18,34,
   3,19,35,12,28,44,13,29,45,14,
  30,46,15,31,47,20,36, 5,21,37,
   6,22,38, 7,23,39};
 // voies 0 et 4 non connectees, voie 1 en panne.

void remove_diode_offset(void) {
  int dd[46];
  int i;
  for (i=0; i<46; i++) {
    dd[i] = diodes[i];
  }
  diodes[46] = diodes[47] = 0;
  for (i=0; i<46; i++) {
    diodes[i] = dd[diodpermut[i]];
  }
  return;
/*
  int i,j;
  float m,sig;
  // suppression des positions non utilisees. 3 et 7 ?
  for (i=DIODE_UNUSED_1; i<46; i++)
    diodes[i] = diodes[ i<DIODE_UNUSED_2-1 ? i+1 : i+2 ];
  
  // calcul d'un fond sur la rangee. Moyenne clippee.
  m = 0; sig = 1.e10;
  for (i=0; i<2; i++) {
    float s=0; float s2=0; int n=0;
    for (j=0; j<46; j++) {
      if (fabs(diodes[j]-m)<3*sig+1) {
        s += diodes[j]; s2 += diodes[j]*diodes[j]; n++;
      }
    }
    if (n>0) {
      m = s/n; sig = sqrt(s2/n - m*m);
    } else {
      m = 0; break;
    }
  }
  for (j=0; j<46; j++) 
    diodes[j] -= m;
    
  diodes[46] = diodes[47] = 0;
  */
}


void find_stars(double secondes) {
  int i,j,feelasleep,sousoff;
  float corrtemps,aufsete;
  // Une etoile est validee seulement si impulsion dans meme canal
  // ou dans canal juste au dessus dans les 4 echantillons qui precedent
  // (en excluant le precedent).
  // On demande aussi que le signal soit en train de remonter...
  // en pratique on pourrait restreindre la contrainte avec une estimation
  // de la vitesse de rotation. echantillon -2 ou -3...
  nfoundstars = 0;
  for (i=0; i<46; i++) {
  // la diode 14 est morte (canal 1)...
  if (i==14) continue;
  aufsete=0.;feelasleep=0;
  for (sousoff=0;sousoff<5;sousoff++)
  {aufsete+=diodesbuffer[sousoff][i];feelasleep++;}
  aufsete/=feelasleep;
  off[i]=aufsete;
    if ((diodes[i] -aufsete< -seuilSST ||
    diodesbuffer[dbufsz-1][i]-aufsete< -seuilSST ||
    diodesbuffer[dbufsz-2][i]-aufsete< -seuilSST)
    && fabs(off[i])<1000.) {
     if (sstHas2Bars) {
      for (j=dbufsz-2; j>=0; j--) {
        if (diodesbuffer[j][i] < -seuilSST) {
          //printf("Found star same %d\n",dbufsz-j);
          zfoundstars[nfoundstars] = i;
          mfoundstars[nfoundstars] = diodes[i]-aufsete;
          tfoundstars[nfoundstars] = secondes;
          nfoundstars++;
          if (nfoundstars >= MAXFOUNDSTARS) return;
          break;
        }
        if (i < 45 && diodesbuffer[j][i+1] < -seuilSST) {
          //printf("Found star decal %d\n",dbufsz-j);
          zfoundstars[nfoundstars] = i+.5;
          mfoundstars[nfoundstars] = diodes[i]-aufsete;
          tfoundstars[nfoundstars] = secondes;
          nfoundstars++;
          if (nfoundstars >= MAXFOUNDSTARS) return;
          break;
        }
      }
     } else {
       if ((diodes[i] > diodesbuffer[dbufsz-1][i]) 
          && (diodesbuffer[dbufsz-1][i] < 
          .5*(diodesbuffer[dbufsz-2][i]+diodesbuffer[dbufsz-3][i]))) {
          zfoundstars[nfoundstars] = i;
          mfoundstars[nfoundstars] = -(diodesbuffer[dbufsz-1][i]+diodesbuffer[dbufsz-2][i])/2.+aufsete;
          corrtemps=(diodesbuffer[dbufsz-2][i]-diodes[i])*gg->periode_echantillonage;
          corrtemps=-corrtemps/
          ((diodesbuffer[dbufsz-2][i]+diodesbuffer[dbufsz-1][i]+diodes[i])-3.*aufsete);
//	  corrtemps=0;
          tfoundstars[nfoundstars] = secondes+corrtemps;
          nfoundstars++;
          if (nfoundstars >= MAXFOUNDSTARS) return;
       }
     }
    }
  }
  
  // remplissage buffer echantillons precedents
  for (j=0; j<dbufsz-1; j++)
    for (i=0; i<46; i++) 
      diodesbuffer[j][i] = diodesbuffer[j+1][i];
  for (i=0; i<46; i++) 
    diodesbuffer[dbufsz-1][i] = diodes[i];
}


// Comparaison avec le GSC.
// Preparer une carte pour une position et une heure.