#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include "TEnv.h"
#include "Stntuple/alg/TStntuple.hh"
#include "Stntuple/obj/TDcasDataBlock.hh"

Int_t         TStntuple::fgLut_Dat[6][4096];
//_____________________________________________________________________________
Int_t TStntuple::Load_LUT(){
  int itemp;
  int ipair;
  std::string pair;

  static int first_time(1);

  if (first_time == 0) return 0;

  const char* project_dir = gEnv->GetValue("Stntuple.ProjectDir","");
  if (strcmp(project_dir,"") == 0) {
    project_dir = getenv ("PROJECT_DIR");
  }

  std::string filename(project_dir);
  filename += "/CalTrigger/memories/LUTblock.file" ;
  std::cout << " LUT filename = " << filename << std::endl;
  std::ifstream asciiFile(filename.c_str());

  if (!asciiFile) {
    std::cout<<" pfred: Error opening LUTblock.file! "<<std::endl;
    return 1;
  }
  else {
    for(int i=0; i<3; i++) {
      asciiFile >>  pair >> ipair;
      std::cout <<" load_lut " << pair<<" " <<ipair<< std::endl;
      for (int nw=0; nw<4096; nw++) {
	asciiFile >> itemp;
	fgLut_Dat[2*i+1][nw] = (itemp>>12) & 0xfff;
	fgLut_Dat[2*i][nw] = itemp & 0xfff;
      }
    }
  }

  first_time = 0;
  return 0;
}

//_____________________________________________________________________________
void TStntuple::L1Met(TDcasDataBlock* _DcasData, int* Tl1d, Float_t &sumet){
  // returns in Tl1d "calculated-on-the-fly" L1 Etx and Ety (in units of GeV)

  int et[24] ,et_wedge[24][12], sum_dirac[12][4], etcs[12][2];
  int sumb[2][6];
  int sumc[2][6];
  int sign;
  int z, sum_s,sumd;
  int lut_add;
  float scf =2.;
  int sume[] = {0,0};
  int abssume[] = {0,0};
  int sum_etxy[2];
  Float_t _EtSum;

  lut_add = 0;

  for ( int i = 0; i < 24; i++)
    {
      for(int j = 0; j < 12; j++)
	{
	  et_wedge[i][j] = 0;
	  sum_dirac[i/2][j/3] = 0;
	}
      etcs[i/2][0]=0;
      etcs[i/2][1]=0;
    }

  for(int i=0; i<24; i++) 
    {
                                        // initialize transverse energy for wedge
      et[i] = 0;
      
      for(int j=0; j<24; j++) 
	{ 	                        //rationalize tower Et from dcas
                                        //sum em+had after dropping 2 lsb (.5GeV units)

	  et_wedge[i][j/2] += ((_DcasData->GetEmEnergy(i,j)>>2) + (_DcasData->GetHdEnergy(i,j)>>2)); 
	}
	  
      for( int j = 0; j < 12; j++)
	{
	                                // make dirac card output
	  sum_dirac[i][j/3] += (et_wedge[i][j]>>1); 

	                                // calculate cratesum sums
	                                // should be shifted by 1 since the threshold 1 GeV
	                                // but would like to test with 0.5 Gev
	  etcs[i/2][i%2] += (et_wedge[i][j]>>1); 
	  //	      etcs[i/2][i%2] += (et_wedge[i][j]);
	}
    }
                                        // rearange to do prefred summation
  for (int i = 0; i<12; i++)
    {

      if (((i>=3)&&(i<=5))||(i>=9))
	{
	  et[i]    = etcs[i][1] ;
	  et[i+12] = etcs[i][0]; 
	} 
      if(((i>=6)&&(i<=8))||(i<=2))
	{
	  et[i]    = etcs[i][0] ;
	  et[i+12] = etcs[i][1]; 
	}
      _EtSum += et[i] + et[i+12];
    }
                                        //do pfred met simulation
  
                                        // start calculation for missing Et

  sumb[0][0] = et[0] + et[11] - (et[5] + et[6]);
  sumb[0][1] = et[1] + et[10] - (et[4] + et[7]);
  sumb[0][2] = et[2] + et[9] - (et[3] + et[8]);
  sumb[0][3] = et[0] + et[5] - (et[6] + et[11]);
  sumb[0][4] = et[1] + et[4] - (et[7] + et[10]);
  sumb[0][5] = et[2] + et[3] - (et[8] + et[9]);
  sumb[1][0] = et[14] + et[15] - (et[20] + et[21]);
  sumb[1][1] = et[13] + et[16] - (et[19] + et[22]);
  sumb[1][2] = et[12] + et[17] - (et[18] + et[23]);
  sumb[1][3] = et[14] + et[21] - (et[15] + et[20]);
  sumb[1][4] = et[13] + et[22] - (et[16] + et[19]);
  sumb[1][5] = et[12] + et[23] - (et[17] + et[18]);
  
                                        // Etx and Ety calculation done via lookup tables,
                                        // with lut_data = 2*Et*cos(phi) at address Et. 
                                        // The calculation is done with twice the precision of the incoming 
                                        // Ets from the cratesum  

  for( int i = 0; i < 2; i++)
    {
      for(int j = 0; j < 6; j++)
	{
	  
	  lut_add = sumb[i][j] & 0xfff;
	  if(lut_add > 4095) lut_add = 4095;
	  sign = 1;
	  if(lut_add > 2047) 
	    {
	      sign = -1;
	    }
	      
	                                // indecies follow definition in Altera code

	  int sum = fgLut_Dat[j][lut_add]; 
	  
	  if(sign == -1 ) sum = -(4096 - sum);
	  sumb[i][j] = sum;
	  z = ( 2<< 11);
	  if (TMath::Abs(sumb[i][j]) > z )
	    {
	      if(sumb[i][j] > 0)
		{
		  sum_s = z -1;
		}
	      else 
		{
		  sum_s = 1 - z;
		}                           
	      
	      sumb[i][j] = sum_s;
	    }
	}
    }     

                                        // do sumex

  sumc[0][0] = sumb[0][0] + sumb[1][3];
  sumc[0][1] = sumb[0][1] + sumb[1][4];
  sumc[0][2] = sumb[0][2] + sumb[1][5];
  
                                        // do sumey

  sumc[1][0] = sumb[0][3] + sumb[1][2];
  sumc[1][1] = sumb[0][4] + sumb[1][1];
  sumc[1][2] = sumb[0][5] + sumb[1][0];
  
  for( int i = 0; i < 2; i++)
    {
      sumd = sumc[i][1] + sumc[i][2];
      sume[i] = sumd + sumc[i][0];
      
	                                // Conditions for the result to be saturated

      if (TMath::Abs(sumc[i][0]) >= 2048 || 
	  TMath::Abs(sumc[i][1]) >= 2048 ||
	  TMath::Abs(sumc[i][2]) >= 2048 || 
	  TMath::Abs(sumd) >= 2048 ||
	  TMath::Abs(sume[i]) >= 1024)
	{
	  sume[i] = 1023;
	} 
      else 
	{
	  abssume[i] = TMath::Abs(sume[i]); 
	  if(scf == 2)  abssume[i] /=2;        
	                                // get back to initial Et units
                                        // check for loss of precision (met will appear twice as big)
	}
      sum_etxy[i] = sume[i];
      if(scf ==2) sum_etxy[i]/=2;
      
      if (sume[i] >= 0)
	{
	  sume[i] = abssume[i];
	}
      else 
	{
	  sume[i] = (abssume[i] + 512) & 0x3ff;
	}
    }
                                        // by default  1 count = 1 GeV
                                        // if work with double precision convert from counts to GeV (*0.5)
      // _exSum  = sum_etxy[0]*0.5; 
      // _eySum  = sum_etxy[1]*0.5;
      // _EtSum  *= 0.5;

  sumet    = _EtSum;
  Tl1d[0]  = sum_etxy[0]; 
  Tl1d[1]  = sum_etxy[1];
 
  return;
}