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

#define DIODE_UNUSED_1 3
#define DIODE_UNUSED_2 7

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

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;
}

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;
}

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;
  
  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;
}

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();
  }
  
}


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 = 28-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++) {
        long word = blk->sst[i][j*3+k];
        word = (word >> (28-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;
      }
}


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++) {
    // suppression des positions non utilisees. 3 et 7 ?
    for (int i=0; i<DIODE_UNUSED_1; i++)
      diode(j+j0,i) = diodeRaw[j][i];
    for (int i=DIODE_UNUSED_1; i<46; i++)
      diode(j+j0,i) = diodeRaw[j][ i<DIODE_UNUSED_2-1 ? i+1 : i+2 ];
  
    // 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);
}