#include <iostream>
#include <iomanip>
#include <cstdio>
#include <cstring>

#include <TMath.h>

//#include "../include/CMSpixelClust.h"

static int findBin( int adc, int nlevel, int* level );

static bool convertDcolToCol( const int dcol, const int pix,
                              int & col, int & row );

static std::vector<cluster> getHits(int fNpix, pixel pb[4160]);

CMSPixelReader::CMSPixelReader():
	chip(0), word(0)
{
	// flevel, amax, gain, horiz and vert are initialized at open()
}

//----------------------------------------------------------------------------
// read CMS pixel data from mtb.bin

std::vector<cluster> CMSPixelReader::getCMSclust( unsigned long & cmsTimeStamp, int& fNpix ) {

  std::vector<cluster> clust;
  fNpix = 0;

  bool ldb = 0;

  static unsigned long Time0 = 0; // TODO: Should become a member variable

  if( !mtbStream.eof() ) {

    int head[4];
    int h = 0;
    head[h] = word;//read at end of previous event

    // decode header:
    // hex 8004 = trigger
    // hex 8002 = trigger
    // hex 8001 = data
    // hex 8008 = reset

    bool validHeader = 0;
    if( head[0] == 0x8004 ) validHeader = 1; // trigger
    if( head[0] == 0x8008 ) validHeader = 1; // DP reset
    if( head[0] == 0x8081 ) validHeader = 1; // DP ?

    unsigned char a;//one byte
    unsigned char b;// 2nd byte
    char buf[100];

    if( validHeader ) {

      // read 3 more words after header:

      a = mtbStream.get();
      b = mtbStream.get();
      word = (b << 8) | a;//next word

      for( h = 1; h < 4 && !mtbStream.eof(); h++ ){

	head[h] = word;

	a = mtbStream.get();
	b = mtbStream.get();
	word = (b << 8) | a;//next word

      }//loop over 3 more header words

      if( h != 4 ) std::cout << "ERROR: header length not 4: " << h << std::endl;

      sprintf( buf, "%4x %4x %4x %4x", head[0], head[1], head[2], head[3] );
      //std::cout << buf << std::endl;

    }//validHeader

    else{
      sprintf( buf, "? %4x", head[0] );
      std::cout << buf << std::endl;
    }

    // next should be data:

    head[0] = word;
    //DP if( !mtbStream.eof() && head[0] == 0x8001 ){ // 8001 = data
    if( !mtbStream.eof() && ( head[0] == 0x8001 || head[0] == 0x8081 ) ){ // 8001 = data

      // read 3 more words after header:

      a = mtbStream.get();
      b = mtbStream.get();
      word = (b << 8) | a;//next word
      head[1] = word;

      a = mtbStream.get();
      b = mtbStream.get();
      word = (b << 8) | a;//next word
      head[2] = word;

      a = mtbStream.get();
      b = mtbStream.get();
      word = (b << 8) | a;//next word
      head[3] = word;

      sprintf( buf, "%4x %4x %4x %4x", head[0], head[1], head[2], head[3] );
      if( ldb ) std::cout << buf;

      unsigned short t0 = head[1];
      unsigned short t1 = head[2];
      unsigned short t2 = head[3];
      //unsigned long UpperTime = t0;
      //unsigned long LowerTime = (t1 << 16) | t2;
      unsigned int UpperTime = t0;
      unsigned int LowerTime = (t1 << 16) | t2;
      unsigned long Time = ( (unsigned long) UpperTime << 32 ) + LowerTime;
      if( Time0 == 0 ) {
	Time0 = Time; // first trig
      }
      static unsigned long prevTime = 0; // TODO: Should become a member variable
      //static unsigned long prevEuTime = 0;

      if( ldb ) std::cout << " (" << Time << " clocks, "
                          << std::setprecision(9) << ( Time - Time0 )/40E6 << " s)"
                          << " dt = " << (Time-prevTime)/40E0 << " us";

      cmsTimeStamp = Time;

      //prevEuTime = EuTime;

      int k = 0;
      int adc[65001];

      // read ROC data until next reset or next trig

      a = mtbStream.get();
      b = mtbStream.get();
      word = (b << 8) | a;//next word

      while( word != 0x8008 && word != 0x8004 && word != 0x8002 && !mtbStream.eof() ){ // 8008 = reset, 8004=trig

	if( k < 65000 ) {
	  int value = word & 0x0fff;
	  if( value & 0x0800 ) value -= 4096;//hex 800 = 2048
	  adc[k] = value;
	}
	k++;

	a = mtbStream.get();
	b = mtbStream.get();
	word = (b << 8) | a;//next word

      }//loop over ADC data

      // we have k data values. expect at least 10 for valid ROC data

      fNpix = ( k - empty ) / 6; //for cluster search

      if( k >= empty ){

	if( (k-empty) % 6 != 0 ) { // wrong size

	  std::cout << " :";
	  int m = std::min( 650000, k );
	  for( int i = 0; i < m; ++i ){
	    sprintf( buf," %4x", adc[i] );
	    std::cout << buf;
	    if( (i+1)%600 == 0 ) std::cout << std::endl;
	  }
	  std::cout << " (wrong data length " << k << " = " << fNpix << " pixels + " << k - 6*fNpix << ")" << std::endl;

	}
	else if( k > 2222  ) {

	  std::cout << " :";
	  int m = std::min( 650000, k );
	  for( int i = 0; i < m; ++i ){
	    sprintf( buf," %4x", adc[i] );
	    std::cout << buf;
	    if( (i+1)%600 == 0 ) std::cout << std::endl;
	  }
	  std::cout << " (" << k << " data words: big event skipped!)" << std::endl;
	}
	else { // event OK

	  if( ldb ) std::cout << " :";
	  //
	  // FPGA UB B LD
	  //
	  sprintf( buf, "%4d : %4d %4d %4d : ", adc[0], adc[1], adc[2], adc[3] );
	  if( ldb ) std::cout << buf;

	  int i = 4;
	  int ipx = 0;

	  // pixel data from ROC:
	  pixel pb[4160];
	  memset(pb, 0, sizeof(pb)); // zero-init the buffer

	  while( i < k - tail ){

	    // hits:       C1  C0  A2  A1  A0 Aout
	    sprintf( buf,"%4d %4d %4d %4d %4d %4d :",
		     adc[i], adc[i+1], adc[i+2], adc[i+3], adc[i+4], adc[i+5] );
	    if( ldb ) std::cout << buf;

	    int c1 = findBin( adc[i+0], 6, flevel );
	    int c0 = findBin( adc[i+1], 6, flevel );
	    int a2 = findBin( adc[i+2], 6, flevel );
	    int a1 = findBin( adc[i+3], 6, flevel );
	    int a0 = findBin( adc[i+4], 6, flevel );
	    int aa = adc[i+5]; //Aout

	    int dcol =  c1*6 + c0;
	    int drow =  a2*36 + a1*6 + a0;

	    int col = -1;
	    int row = -1;
	    bool addressOK = convertDcolToCol( dcol, drow, col, row );
	    int roc = 0;

	    if( chip == 7 && col == 48 && row == 79 ) addressOK = 0;//mask

	    if( ldb ) {
	      std::cout << " pix " << ipx;
	      std::cout << ", col " << col;
	      std::cout << ", row " << row;
	      std::cout << ", OK " << addressOK;
	    }

	    if( addressOK ) {

	      double Aout = aa / ff; // Aout in no-TBM units!

	      double Ared = Aout - vert[col][row]; // sub vert off, see gaintanh2ps.C

	      double ma9 = amax[col][row];

	      if( Ared >  ma9-1 ) ma9 = Ared + 2;
	      if( Ared < -ma9+1 ) ma9 = Ared - 2;

	      if( ldb ) {
		std::cout << ", Ared " << Ared;
		std::cout << ", Amax " << amax[col][row];
		std::cout << ", ma9 " << ma9;
	      }

	      // calibrate into ke units:

	      if( ma9 > 1 ){
	        pb[ipx].roc = roc;
	        pb[ipx].ana = aa;
	        pb[ipx].col = col;
	        pb[ipx].row = row;
	        pb[ipx].anaVcal = ( TMath::ATanH( Ared / ma9 ) * gain[col][row] + horz[col][row] ) * 0.45 ; // [ke]
	        ipx++;
	      }

	    }//address OK

	    i+= 6;
	    if( (i-4)%600 == 0 && ldb ) std::cout << std::endl;

	  }//while < k-tail

	  // FPGA trailer: 6 words, same with/without tbm

	  for( int j = i; j < i + tail; ++j ){
	    sprintf( buf," %4d", adc[j] );
	    if( ldb ) std::cout << buf;
	  }
	  if( ldb ) std::cout << std::endl;

	  // find clusters in pb:

	  fNpix = ipx; // DP 11.1.2012

	  clust = getHits(fNpix, pb);

	  if( ldb ) std::cout << "(" << fNpix << " pixels, " << clust.size() << " clusters)";

	}//OK

	if( ldb ) std::cout << std::endl;
 
      }//not empty

    }// head = 8001 = data
    else{
      sprintf( buf, "@ %4x", head[0] );
      std::cout << buf << std::endl;
    }
  }//not eof

  return clust;
}

//------------------------------------------------------------------------------
// from psi/slc5/offline/b2h.cc

static int findBin( int adc, int nlevel, int* level ) {
  //
  //   |   |   |
  //   0   1   2
  //
  if( adc < level[0] ) return 0;//underflow

  for( int i = 0; i < nlevel; i++ ){

    if( adc >= level[i] && adc < level[i+1] ) return i;//inside

  }
  return nlevel;//overflow
}

/*****************************************************************/
// from psi/slc5/offline/b2h.cc
//                                        from Daneks DecodeRawPacket
// Convert dcol&pix to col&row  
// Decodeing from "Weber" pixel addresses to rows for PSI46
// dcol = 0 - 25
// pix = 2 - 161 zigzag pattern.
// col = 0-51
// row = 0-79
//
static bool convertDcolToCol( const int dcol, const int pix,
                              int & col, int & row )
{
  const int ROCNUMDCOLS = 26;
  const int ROCNUMCOLS = 52;
  const int ROCNUMROWS = 80;
  const int printWarning = 1;

  if( dcol < 0 || dcol >= ROCNUMDCOLS || pix < 2 || pix > 161) {

    if( printWarning ) std::cout << "wrong dcol or pix " << dcol << " " << pix << std::endl;

    row = -1;
    col = -1;
    return false;
  }

  int colEvenOdd = pix%2;  // 0 = 1st col, 1 = 2nd col of a double column

  col = dcol * 2 + colEvenOdd; // col address, starts from 0

  row = abs( int(pix/2) - 80);  // row address, starts from 0

  if( col < 0 || col > ROCNUMCOLS || row < 0 || row > ROCNUMROWS ) {

    if( printWarning ) {
      std::cout << "wrong col or row in user_decode " << col << " " << row << std::endl;
      std::cout << "wrong dcol or pix in user_decode " << dcol << " " << pix << std::endl;
    }
    row = -1;
    col = -1;
    return false;
  }
  return true;
}

// ----------------------------------------------------------------------
// from psi/slc5/offline/b2h.cc

static std::vector<cluster> getHits(int fNpix, pixel pb[4160]) {

  // returns clusters with local coordinates
  // decodePixels should have been called before to fill pixel buffer pb 
  // simple clusterization
  // cluster search radius fCluCut ( allows fCluCut-1 empty pixels)

  int fCluCut = 2;//clustering: 2 = allow gap of 1

  std::vector<cluster> v;
  if( fNpix==0 ) return v;

  int* gone = new int[fNpix];

  for( int i=0; i<fNpix; i++){
    gone[i] = 0;
  }

  int seed = 0;

  while( seed < fNpix ) {

    // start a new cluster

    cluster c;
    c.vpix.push_back(pb[seed]);
    gone[seed]=1;
    c.sumA = 0;
    c.charge = 0;
    c.size = 0;
    c.col = 0;
    c.row = 0;
    c.xy[0] = 0;
    c.xy[1] = 0;

    // let it grow as much as possible:

    int growing;
    do{
      growing = 0;
      for( int i = 0; i < fNpix; i++ ){
        if( !gone[i] ){//unused pixel
          for( unsigned int p = 0; p < c.vpix.size(); p++ ){//vpix in cluster so far
            int dr = c.vpix.at(p).row - pb[i].row;
            int dc = c.vpix.at(p).col - pb[i].col;
            if( (   dr>=-fCluCut) && (dr<=fCluCut) 
		&& (dc>=-fCluCut) && (dc<=fCluCut) ) {
              c.vpix.push_back(pb[i]);
	      gone[i] = 1;
              growing = 1;
              break;//important!
            }
          }//loop over vpix
        }//not gone
      }//loop over all pix
    }while( growing);

    // added all I could. determine position and append it to the list of clusters:

    for( std::vector<pixel>::iterator p=c.vpix.begin();  p!=c.vpix.end();  p++){
      double Qpix = p->anaVcal;//shifted and scaled
      c.charge += Qpix;
      c.sumA += p->ana;//Aout
      c.col += (*p).col*Qpix;
      c.row += (*p).row*Qpix;
      c.xy[0] += (*p).xy[0]*Qpix;
      c.xy[1] += (*p).xy[1]*Qpix;
    }

    c.size = c.vpix.size();

    //std::cout << "(cluster with " << c.vpix.size() << " pixels)" << std::endl;

    if( !(c.charge==0)){
      c.col = c.col/c.charge;
      c.row = c.row/c.charge;
      c.xy[0] /= c.charge;
      c.xy[1] /= c.charge;
    }else{
      c.col = (*c.vpix.begin()).col;
      c.row = (*c.vpix.begin()).row;
      c.xy[0] = (*c.vpix.begin()).xy[0];
      c.xy[1] = (*c.vpix.begin()).xy[1];
      std::cout << "GetHits: cluster with zero charge" << std::endl;
    }

    v.push_back(c);//add cluster to vector

    //look for a new seed = unused pixel:

    while( (++seed<fNpix) && (gone[seed]) );

  }//while over seeds

  // nothing left,  return clusters

  delete gone;
  return v;
}