// 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"

// 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()
{
  diodeHistLength = nb_per_block*2+10;
  diodeT = new int[diodeHistLength*nb_photo_diodes];
  starHistLength = 300;
  stars = new (vector<star>[starHistLength]);
  lastBlkNum = -1;
  
  //Has2Bars(false);
  prcTodo=0;
  seuil=100;
  
}

SSTHandler::SSTHandler(SSTHandler const& x)
{
  diodeHistLength = x.diodeHistLength;
  diodeT = new int[diodeHistLength*nb_photo_diodes];
  memcpy(diodeT, x.diodeT, diodeHistLength*nb_photo_diodes);
  starHistLength = x.starHistLength;
  stars = new (vector<star>[starHistLength]);
  for (int i=0; i<starHistLength; i++)
    stars[i] = x.stars[i];
    
  prcTodo    = x.prcTodo;
 // has2bars   = x.has2bars;
 // elecOffset = x.elecOffset;
  lastBlkNum = x.lastBlkNum;
  seuil      = x.seuil;
}

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

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

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

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

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

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 & rmveOffset) {
    RemoveOffset();
  }
  if (prcTodo & findStars) {
    FindStars();
  }
}


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::RemoveOffset()
{
  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::getStarZ(int imesure, int istar) // for last block
{
  int j0 = starHistLength-(nb_per_block*2);
  if (imesure+j0<0 || imesure>=nb_per_block*2) return -99999;
  if (istar < 0 || istar >= stars[imesure+j0].size()) return -99999;
  return stars[imesure+j0][istar].z;
}

int SSTHandler::getStarF(int imesure, int istar) // for last block
{
  int j0 = starHistLength-(nb_per_block*2);
  if (imesure+j0<0 || imesure>=nb_per_block*2) return -99999;
  if (istar < 0 || istar >= stars[imesure+j0].size()) return -99999;
  return stars[imesure+j0][istar].m;
}


void SSTHandler::FindStars()  // sur les 72 derniers echantillons dans diode.
{
  int js0 = starHistLength-(nb_per_block*2);
  int jd0 = diodeHistLength-(nb_per_block*2);
  // Decalage vers la gauche de la taille d'un bloc
  for (int i=0; i<js0; i++) {
    stars[i] = stars[i+(nb_per_block*2)];
  }
  
  // Recherche d'etoiles
  for (int i=0; i<(nb_per_block*2); i++) {
    stars[js0+i].clear();
    for (int idiode=0; idiode<46; idiode++) {
      if (diode(jd0+i,idiode) < -seuil) {
        if (has2bars) {
          for (int j=-2; j>=-5; j--) {
            if (diode(jd0+i+j,(idiode+elecOffset)%46) < -seuil) {
              star s;
              s.z = idiode;
              s.m = -diode(jd0+i,idiode);
              s.t = i; // $CHECK$ need absolute time...
              stars[js0+i].push_back(s);
              break;
            } else if (idiode<45 && diode(jd0+i+j,(idiode+1+elecOffset)%46) > seuil) {
              star s;
              s.z = idiode+.5;
              s.m = -diode(jd0+i,idiode);
              s.t = i; // $CHECK$ need absolute time...
              stars[js0+i].push_back(s);
              break;
            }
          }
        } else {
          if (diode(jd0+i,idiode) > diode(jd0+i-1,idiode)) {
            star s;
            s.z = idiode;
            s.m = -diode(jd0+i-1,idiode);
            s.t = i; // $CHECK$ need absolute time...
            stars[js0+i].push_back(s);
          }
        }
      }
    }
  }


}