// ssthandler.cc
// Eric Aubourg         CEA/DAPNIA/SPP   juillet 1999


// Prediction mouvement d'etoiles entre un tour et le suivant...
// si TS -> TS + dT, H -> H + dT,    dT=dH
//
// dz   =  - cos phi sin az dH    (check sign)
// daz  = (sin phi - cos az cotg z cos phi) dH    (check sign)
// 
// free parameters = period + phase

#include <math.h>
#include "ssthandler.h"
#include "pisteetoile.h"

// diodpermut[i] = channel de la diode i
int SSTHandler::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.

SSTHandler::SSTHandler()
:sstchass("SSTChassLogFile")
{
  diodeHistLength = nb_per_block*2+10;
  diodeT = new int[diodeHistLength*nb_photo_diodes];
  starHistLength = 300;

  lastBlkNum = -1;
  
  //Has2Bars(false);
  prcTodo=0;
  
  pPiste = NULL;
  LastBlockSSTNb = -2;
  noStarDet = 0;
  
  findStarConstruct();
  
}

SSTHandler::SSTHandler(SSTHandler const& x) 
:sstchass("SSTChassLogFile")
{
  diodeHistLength = x.diodeHistLength;
  diodeT = new int[diodeHistLength*nb_photo_diodes];
  memcpy(diodeT, x.diodeT, diodeHistLength*nb_photo_diodes);
  starHistLength = x.starHistLength;
    
  prcTodo    = x.prcTodo;
 // has2bars   = x.has2bars;
  lastBlkNum = x.lastBlkNum;
  
  pPiste = NULL;
  LastBlockSSTNb = x.LastBlockSSTNb;
  noStarDet = x.noStarDet;

  findStarConstruct();
  for(int i=0; i<NbPhotDiodBarette; i++) {
  	*PisteBar[i]=*(x.PisteBar[i]);
  }
}

SSTHandler& SSTHandler::operator = (SSTHandler const& x) {
  delete[] diodeT; 
  diodeHistLength = x.diodeHistLength;
  diodeT = new int[diodeHistLength*nb_photo_diodes];
  memcpy(diodeT, x.diodeT, diodeHistLength*nb_photo_diodes);
    
  prcTodo    = x.prcTodo;
 // has2bars   = x.has2bars;
 // elecOffset = x.elecOffset;
  lastBlkNum = x.lastBlkNum;
  for(int i=0; i<NbPhotDiodBarette; i++) *PisteBar[i]=*(x.PisteBar[i]);
  return *this;
}

SSTHandler::~SSTHandler(){
  delete[] diodeT;
  findStarDestruct();
}

void SSTHandler::NeedProcess(int prcMask)
{
  prcTodo |= prcMask;
  if (prcTodo & findAxis)   prcTodo |= findPeriod;
  if (prcTodo & findPeriod) prcTodo |= findStars;
  if (prcTodo & findStars)  prcTodo |= permDiode;
}

bool SSTHandler::has2bars = false;
int  SSTHandler::elecOffset = 1;

void SSTHandler::Has2Bars(bool has, int eo)
{
  has2bars = has;
  elecOffset = eo;
}

void SSTHandler::DecodeTMBlock(block_type_sst* blk, int i, int* diod)
{
  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++) diod[j] = 0;
  
  for (j=0; j<6; j++)
    for (k=0; k<3; k++)
      for (l=0; l<8; l++) {
        int4 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);
        diod[j*8+l] = (diod[j*8+l] << 4) + word;
      }

  //for (j=0; j<48; j++) if (diod[j]>2047) diod[j] -= 4096;
  for (j=0; j<48; j++)  diod[j] -= 2048;
  
}


void SSTHandler::PermutDiode()
{
  int j0 = diodeHistLength-(nb_per_block*2);
  
  // Decalage vers la gauche de la taille d'un bloc
  memcpy(diodeT, diodeT + (nb_per_block*2)*nb_photo_diodes, j0*nb_photo_diodes);
  
  for (int j=0; j<nb_per_block*2; j++) {
    // permutation des diodes
    for (int i=0; i<46; i++)
      diode(j+j0,i) = diodeRaw[j][diodpermut[i]];
  
    /*
    // calcul d'un fond sur la rangee. Moyenne clippee.
    float m = 0; float sig = 1.e10;
    for (int k=0; k<2; k++) {
      float s=0; float s2=0; int n=0;
      for (int i=0; i<46; i++) {
        if (fabs(diode(j+j0,i)-m)<3*sig+1) {
          s += diode(j+j0,i); s2 += diode(j+j0,i)*diode(j+j0,i); n++;
        }
      }
      if (n>0) {
        m = s/n; sig = sqrt(s2/n - m*m);
      } else {
        m = 0; break;
      }
    }
    for (int i=0; i<46; i++) 
      diode(j+j0,i) -= m;
    */
  }
}

int SSTHandler::getRawSignal(int imesure, int idiode) // for last block
{
  if (imesure<0 || imesure>=nb_per_block*2 || idiode<0 || idiode>=48) return -99999;
  return diodeRaw[imesure][idiode];
}

int SSTHandler::getSignal(int imesure, int idiode) // for last block
{
  int j0 = diodeHistLength-(nb_per_block*2);
  if (imesure+j0<0 || imesure>=nb_per_block*2 || 
      idiode<0 || idiode>=nb_photo_diodes) return -99999;
  return diode(imesure+j0, idiode);
}

int SSTHandler::FindStars(block_type_sst* blk) {
	NbStarInBlock=0;
	LastBlockStarVec.clear();
	
	int NoFirstSpInBlock=FirstSplNb(blk);
  	int jd0 = diodeHistLength-NbSampleBlock;

	bool FlagLBlockPerdu=false;
	int ThisBlockNumb=numero_block(blk);
	
	if ((LastBlockSSTNb+1)==ThisBlockNumb) LastBlockSSTNb++;
	else {
		FlagLBlockPerdu=true;
		LastBlockSSTNb=ThisBlockNumb;
	}

//	On cherche les etoiles	
	for(int NoDiode=0;NoDiode<NbPhotDiodBarette; NoDiode++) {	
		pPiste=PisteBar[NoDiode];
		
	// Traitons une diode pendant un block
		offseDataDiod=0;
		for (int i=0; i<NbSampleBlock; i++) Diodedata[i]=diode(jd0+i,NoDiode);

		if (FlagLBlockPerdu)  {
			pPiste->fill(Diodedata+offseDataDiod,FirstSplNb(blk));
			offseDataDiod+=PhDiodTabLong;
		}
		else {
			pPiste->push(Diodedata);
			offseDataDiod+=Pousslong;
		}
		
	// Recherchons les etoiles
		while (true) {
			if (pPiste->traque()) {				// On a trouve!
				LastStar=pPiste->DonneEtoile();
/*				double dum= LastStar.TEchan;
				pair<const double, SSTEtoile> unepaire=make_pair(dum,LastStar);
				StarHistoryMap.insert(unepaire);	
*/
				StarHistoryMap[LastStar.TEchan]=LastStar;
					//On empile sur la map.
				NbStarInBlock++;
#ifdef SST_DEBUG			
			// On écrit les pistes ayant déclénchées
				int NoDiodeEvt=LastStar.NoDiode;
				if(pPisteDump[NoDiodeEvt]->is_open())	
					for(int noSamp=0; noSamp<NbSampleBlock;noSamp++)
						(*pPisteDump[NoDiodeEvt])<<noStarDet<<'\t'<<NoDiode<<'\t'<<NoFirstSpInBlock+noSamp<<'\t'<<Diodedata[noSamp]<<endl;
#endif
				noStarDet++;
			}
		// Est-on en bout de Piste?
			if(offseDataDiod+Pousslong>=NbSampleBlock) break;
			else {
				pPiste->push(Diodedata+offseDataDiod);
				offseDataDiod+=Pousslong;
			}
		}
	}
	
//	On ne garde que les NbEtInSetMax dernières étoiles
	int nbEtoileInMap=StarHistoryMap.size();
	if(nbEtoileInMap>NbEtInMapMax) {
		int nbErase=nbEtoileInMap-NbEtInMapMax;
		StarHistIter StarIter=StarHistoryMap.begin();
		for(int i=0;i<nbErase;i++){
			StarHistoryMap.erase(StarIter);
			StarIter++;
		}
		nbEtoileInMap-=nbErase;
	}			
	
//	On stocke les etoiles du block dans LastBlockStarVec
	map<double,SSTEtoile>::reverse_iterator StarHistReIter= StarHistoryMap.rbegin() ;
	for(int i=0; i<NbStarInBlock; i++, StarHistReIter++) 
		LastBlockStarVec.push_back((*StarHistReIter).second);
	
//#ifdef SST_DEBUG
	if(NbStarInBlock>0) {
			
	// On écrit les étoiles detectées
		vector<SSTEtoile>::reverse_iterator StarVecRevIter;
		StarVecRevIter=LastBlockStarVec.rbegin();
		for(int i=0; i<NbStarInBlock;i++,StarVecRevIter++) 
			(*StarVecRevIter).print(sstchass);
	}
		
//#endif

	return NbStarInBlock;
}

void SSTHandler::findStarConstruct() {
	PisteBar=new PisteEtoile*[NbPhotDiodBarette];
	for(int i=0; i<NbPhotDiodBarette; i++) PisteBar[i]=new PisteEtoile(i);
	StarHistoryMap.clear();
	sstchass<<LastStar.printHeader()<<endl;
	
#ifdef SST_DEBUG	
	char s[32];
	string fileName;
//	if(pPisteDump==NULL)
		pPisteDump= new ofstream* [NbChanDump];
	
	for (int pistNumb=0; pistNumb<NbChanDump; pistNumb++) {
		sprintf(s,"%i", pistNumb);	
		string piste=s;
		fileName="pisteno"+piste+"dump";
/*		
		if ((*pPisteDump[pistNumb]).is_open()) {
			cerr<<"Erreur: le fichier "<<fileName<<" devrait être libre"<<endl;
				//On libère
			(*pPisteDump[pistNumb]).close();
			delete pPisteDump[pistNumb];
		}
*/			
		pPisteDump[pistNumb]=new ofstream(fileName.c_str(),ios::out|ios::trunc);
		
		if(!(*pPisteDump[pistNumb]).is_open()) {
			cerr<<"Warning Echec à l'ouverture du fichier: "<<fileName<<endl;
		}
	}
#endif
	return;
}

void SSTHandler::findStarDestruct() {
	for(int i=0; i<NbPhotDiodBarette; i++) {
		delete PisteBar[i];
	}
	delete[] PisteBar;
		
#ifdef SST_DEBUG	
	for (int pistNumb=0; pistNumb<NbChanDump; pistNumb++) {
		pPisteDump[pistNumb]->close();
		delete pPisteDump[pistNumb];
	}
	delete 	pPisteDump;
#endif

	return;
}

int SSTHandler::getNumbStar(int iSampl) {
	StarHistIter IterLow=StarHistoryMap.lower_bound((double)iSampl);
	StarHistIter IterHigh=StarHistoryMap.upper_bound((double)(iSampl+1.));
	int Compteur=0;
	while (!(IterLow==IterHigh)) {IterLow++; Compteur++;}
	return Compteur;
}

double SSTHandler::getStarF(int iSampl, int istar){ 
	if( (getNumbStar(iSampl)==0)||(getNumbStar(iSampl)<istar)) return -1;
	else {
		StarHistIter IterLow=StarHistoryMap.lower_bound((double)iSampl);
		while(!(istar==0)) {
			IterLow++;
			istar--;
		}
	return (double) ((*IterLow).second).InpCurrent;
	}
}


int SSTHandler::getStarZ(int iSampl, int istar) {
	if( (getNumbStar(iSampl)==0)||(getNumbStar(iSampl)<istar)) return -1;
	StarHistIter IterLow=StarHistoryMap.lower_bound((double)iSampl);
	while(!(istar==0)) {
		IterLow++;
		istar--;
	}
	return ((*IterLow).second).NoDiode;
}

double SSTHandler::getStarTime(int iSampl, int istar) {
	if( (getNumbStar(iSampl)==0)||(getNumbStar(iSampl)<istar)) return -1;
	StarHistIter IterLow=StarHistoryMap.lower_bound((double)iSampl);
	while(!(istar==0)) {
		IterLow++;
		istar--;
	}
	return ((*IterLow).second).TEchan;
}

int SSTHandler::FirstSplNb(block_type_sst* blk){
	return NbSampleBlock*numero_block(blk);	// BUGG	A verifier
}


void SSTHandler::ProcessBlock(block_type_sst* blk)
{
  lastBlkNum = numero_block(blk);
  for (int i = 0; i<nb_per_block*2; i++) {
    DecodeTMBlock(blk, i, diodeRaw[i]);
  }
  if (prcTodo & permDiode) {
    PermutDiode();
  }
  if (prcTodo & findStars) {
    FindStars(blk);
  }
}