#include "L1Grid.h"

#include "CbmL1Def.h"
#include <algorithm>
#include <string.h>

#include <assert.h>
#include <cstdio>
#include "omp.h"
#include "L1Algo.h"


using namespace std;

  /// Copy to memory block [@dest, @dest+@num] num number of times the value of i of type @T with size @typesize.
  /// uses binary expansion of copied volume for speed up
template< typename T>

inline void memset( T *dest, T i, size_t num ) {
  const size_t tsize = sizeof(T);
  unsigned int lastBin = 0;
  dest[0] = i;
  while ( lastBin+1 < num ) {
    unsigned int l = lastBin + 1;
    l = lastBin + l + 1 > num ? num - lastBin - 1 : l;
    memcpy( dest + lastBin + 1, dest, l*tsize );
    lastBin += l;
  }
}


void L1Grid::UpdateIterGrid(int Nelements, L1StsHit* hits,  vector <THitI>* indicesBuf, THitI* indices,  vector <THitI>* indicesBuf2, vector <L1StsHit>* hits2, vector <L1HitPoint>* pointsBuf, L1HitPoint* points, int &NHitsOnStation)
{
 
   int SavedHits = 0 ;

   int Saved[fNThreads];
   int Saved2[fNThreads];
   int fin = fN;
   
  #pragma omp parallel for firstprivate(SavedHits, fin)
  for (int j=0; j<fNThreads; j++)
  {
      Saved[j]=0;
      Saved2[j]=0;
    
 if (j!=fNThreads-1) fin = (fN/fNThreads)*(j+1);

 for (int i=j*(fN/fNThreads); i<fin; i++)
 {
 
  (*indicesBuf2)[i]=SavedHits;
 
   
   for (int l=FirstHitInBin(i); l<FirstHitInBin(i+1); l++)
 {   
    const L1StsHit &hit = hits[l];
  
   if (!hit.used) { 
     fFirstHitInBin[omp_get_thread_num()][SavedHits]=l;
     SavedHits++;
   }
  }
  }
 Saved[j]= SavedHits;
  }

   int NHitsBefore =0;
   
    for (int p=0;p<fNThreads;p++)
  {
   Saved2[p]=NHitsBefore;
   NHitsBefore += Saved[p];
  }
  
  fFirstHitInBinArray[fN/fBinInGrid][fN%fBinInGrid]=NHitsBefore;
 
   #pragma omp parallel for  firstprivate(fin)
  for (int j=0; j<fNThreads; j++)
  {
    
   if (j!=fNThreads-1) fin = (fN/fNThreads)*(j+1);

  for (int i=j*(fN/fNThreads); i<fin; i++)
   
 {
  
  fFirstHitInBinArray[i/fBinInGrid][i%fBinInGrid]=(*indicesBuf2)[i]+Saved2[j];

  if (i!=0)
  for (int l=fFirstHitInBinArray[(i-1)/fBinInGrid][(i-1)%fBinInGrid]; l<fFirstHitInBinArray[i/fBinInGrid][i%fBinInGrid]; l++)
  {
      const THitI &end=fFirstHitInBinArray[(i)/fBinInGrid][(i)%fBinInGrid];    
      const THitI &oldIndex=fFirstHitInBin[omp_get_thread_num()][(*indicesBuf2)[i]-end+l];
     
    //  (*indicesBuf2)[l+NHitsOnStation]=newIndex;
      (*hits2)[l+NHitsOnStation] = hits[oldIndex];
      (*indicesBuf)[l+NHitsOnStation]=indices[oldIndex];
      (*pointsBuf)[l+NHitsOnStation]=points[oldIndex];
  }
 
 }
      
      if (fin>0)

     for (int l=fFirstHitInBinArray[(fin-1)/fBinInGrid][(fin-1)%fBinInGrid]; l<fFirstHitInBinArray[(fin)/fBinInGrid][(fin)%fBinInGrid]; l++)
  {
      const THitI &end=fFirstHitInBinArray[(fin)/fBinInGrid][(fin)%fBinInGrid];
      const THitI &oldIndex=fFirstHitInBin[omp_get_thread_num()][Saved[j]+l-end];

   //   (*indicesBuf2)[l+NHitsOnStation]=newIndex;
      (*hits2)[l+NHitsOnStation] = hits[oldIndex];
      (*indicesBuf)[l+NHitsOnStation]=indices[oldIndex];
      (*pointsBuf)[l+NHitsOnStation]=points[oldIndex];
  }
  
 
}


    
    NHitsOnStation+=NHitsBefore;

}


void L1Grid::fill(int Nelements, L1StsHit* hits,  vector <THitI>* indicesBuf)
{

int SavedHits=0;

  for (int j=0; j<fN; j++)
  {

    
     
     
      (*indicesBuf)[j]=SavedHits;
   
   for (int l=FirstHitInBin(j); l<FirstHitInBin(j+1); l++)
 {
   
    const L1StsHit &hit = hits[l];
   if (!hit.used) SavedHits++; 
   
   

 }
 
 
 
 }
 
 for (int j=0; j<fN; j++)
 fFirstHitInBinArray[j/fBinInGrid][j%fBinInGrid]=(*indicesBuf)[j];
 
  fFirstHitInBinArray[fN/fBinInGrid][fN%fBinInGrid]=SavedHits;
  
}


void L1Grid::Initial1( int NThreads )
{

    fNThreads = NThreads*1;
    // fNThreads = NThreads;

    fFirstHitInBinArray.resize(fNThreads, 0);
    fFirstHitInBin.resize(fNThreads, 0);
    
    for( int i = 0; i < fNThreads; i++ ) 
    {
      delete[] fFirstHitInBinArray[i];
      delete[] fFirstHitInBin[i];
    fFirstHitInBinArray[i] = new THitI[8000000];// TODO calculate safe number of bins
    fFirstHitInBin[i] = new THitI[8000000];
    }
    fOffsets.resize(fNThreads +1, 0);
    fNumberHitsInBin.resize(8000000, 0);
    lock.resize(8000000);
    
   // fFirstHitInBin = new THitI[8000000];
    
    for (int i=0; i<8000000; i++)
    {   omp_init_lock(&(lock[i]));
      //  fFirstHitInBin[i]=0;
    }

}


void L1Grid::CreatePar0( float yMin, float yMax, float zMin, float zMax, float sy, float sz)
{
  fStepYInv = 1.f / sy;
  fStepZInv = 1.f / sz;

  fYMinOverStep = yMin * fStepYInv;
  fZMinOverStep = zMin * fStepZInv;

  fNy = static_cast<unsigned short>( yMax * fStepYInv - fYMinOverStep + 1.f );
  fNz = static_cast<unsigned short>( zMax * fStepZInv - fZMinOverStep + 1.f );
 // unsigned int Nold = fN;

  fN = fNy * fNz;
  fBinInGrid = (((fN)/fNThreads)+1);
    
//      #pragma omp critical
//     cout<<fN<<" fN "<<fBinInGrid<<" fBinInGrid "<<endl;
}


//THitI recursiveSumBody(THitI * begin, THitI * end, int fN){
//    
//    size_t length  = end - begin;
//    size_t mid     = length/2;
//    int    sum     = 0;
//    const int baseLength = fN/200;
//    
//    if ( length < baseLength ) {
//        for(size_t ii = 1; ii < length; ii++ ){
//            begin[ii] += begin[ii-1];
//        }
//    } else {
//#pragma omp task shared(sum)
//        {
//            sum = recursiveSumBody(begin    ,begin+mid, fN);
//        }
//#pragma omp task
//        {
//            recursiveSumBody(begin+mid,end, fN);
//        }
//#pragma omp taskwait
//        
//#pragma omp parallel for
//        for(size_t ii = mid; ii < length; ii++) {
//            begin[ii] += sum;
//        }
//        
//    }
//    return begin[length-1];
//}
//
//void recursiveSum(THitI * begin, THitI * end, int fN){
//    
//#pragma omp single
//    {
//        recursiveSumBody(begin,end, fN);
//    }    
//}



__m128 scan_SSE(__m128 x) {
    x = _mm_add_ps(x, _mm_castsi128_ps(_mm_slli_si128(_mm_castps_si128(x), 4)));
    x = _mm_add_ps(x, _mm_shuffle_ps(_mm_setzero_ps(), x, 0x40));
    return x;
}

float pass1_SSE(float *a, float *s, const int n) {
    __m128 offset = _mm_setzero_ps();
#pragma omp for schedule(static) nowait
    for (int i = 0; i < n / 4; i++) {
        __m128 x = _mm_load_ps(&a[4 * i]);
        __m128 out = scan_SSE(x);
        out = _mm_add_ps(out, offset);
        _mm_store_ps(&s[4 * i], out);
        offset = _mm_shuffle_ps(out, out, _MM_SHUFFLE(3, 3, 3, 3));
    }
    float tmp[4];
    _mm_store_ps(tmp, offset);
    return tmp[3];
}

void pass2_SSE(float *s, __m128 offset, const int n) {
#pragma omp for schedule(static)
    for (int i = 0; i<n/4; i++) {
        __m128 tmp1 = _mm_load_ps(&s[4 * i]);
        tmp1 = _mm_add_ps(tmp1, offset);
        _mm_store_ps(&s[4 * i], tmp1);
    }
}

void scan_omp_SSEp2_SSEp1_chunk(float a[], float s[], int n) {
    float *suma;
    const int chunk_size = 1<<18;
    const int nchunks = n%chunk_size == 0 ? n / chunk_size : n / chunk_size + 1;
    //printf("nchunks %d\n", nchunks);
#pragma omp parallel
    {
        const int ithread = omp_get_thread_num();
        const int nthreads = omp_get_num_threads();
        
#pragma omp single
        {
            suma = new float[nthreads + 1];
            suma[0] = 0;
        }
        
        float offset2 = 0.0f;
        for (int c = 0; c < nchunks; c++) {
            const int start = c*chunk_size;
            const int chunk = (c + 1)*chunk_size < n ? chunk_size : n - c*chunk_size;
            suma[ithread + 1] = pass1_SSE(&a[start], &s[start], chunk);
#pragma omp barrier
#pragma omp single
            {
                float tmp = 0;
                for (int i = 0; i < (nthreads + 1); i++) {
                    tmp += suma[i];
                    suma[i] = tmp;
                }
            }
            __m128 offset = _mm_set1_ps(suma[ithread]+offset2);
            pass2_SSE(&s[start], offset, chunk);
#pragma omp barrier
            offset2 = s[start + chunk-1];
        }
    }
    delete[] suma;
}

void L1Grid::CreateParSIMD(L1HitPoint* points, THitI nhits, vector <L1HitPoint>* pointsBuf, vector <L1StsHit>* hitsBuf, const L1StsHit* hits, vector <THitI>* indices, vector <THitI>* indicesBuf, char iS, L1Algo &Algo, THitI n, const vector< L1Strip > *vStsStrips, const vector< L1Strip > *vStsStripsB, const vector< fscal >   *vStsZPos)
{
    fOffsets.resize(fNThreads +1, 0);
    fNumberHitsInBin.resize(8000000, 0);
    
    fvec xv, yv, z;
    fvec u, v;
    fscal xs, ys;
    #pragma omp parallel for schedule(dynamic, 250)
    for( THitI x = 0; x < (fN+1); x+=fvecLen ) {
        
        reinterpret_cast<int_v&>(fNumberHitsInBin[x])=int_v(Vc::Zero);
        
    }
    
    
    
    
    #pragma omp parallel for schedule(dynamic, 250) firstprivate(xs, ys)
    for( THitI x = 0; x < nhits; x+=fvecLen ) {
        
        
        Algo.GetHitCoor((hits)[x], xs, ys, iS);

        
        for (int i=0; i<fvecLen; i++)
        {
        u[i] = float((*vStsStrips)[(hits)[x+i].f]);
        v[i] = float((*vStsStripsB)[(hits)[x+i].b]);
        z[i] = float((*vStsZPos)[(hits)[x+i].iz]);
        }
        
        Algo.StripsToCoor(u, v, xv, yv, z, iS);
        
        
        for (int i=0; i<fvecLen; i++)
        {
             #pragma omp atomic
            fNumberHitsInBin[GetBinBounded(xv[i], yv[i])]++;
        };
        
    }
    

    
#pragma omp parallel
    {
        const short int id = omp_get_thread_num();
        const short int Nthreads = omp_get_num_threads();
        const int istart=(omp_get_thread_num())*fN/Nthreads;
        int iend =(omp_get_thread_num()+1)*fN/Nthreads;
        if (omp_get_thread_num() == Nthreads - 1) iend = fN+1;
        
        
        THitI prefix = 0;

        
        for(int i=istart+1;i<iend;i++)
        {
            prefix += fNumberHitsInBin[i-1];
            
            fFirstHitInBinArray[i/fBinInGrid][i%fBinInGrid]=prefix;
        }
        
        
        
        int Add = 0;
        
#pragma omp barrier
        
        for (int p=0;p<omp_get_thread_num();p++)
        {
            
            
            const int &index = (((p+1)*fN/Nthreads)-1);
            
            if (index<0) break;
            
            
            Add += fFirstHitInBinArray[index/fBinInGrid][index%fBinInGrid] +
            fNumberHitsInBin[index];
            
        }
        

        
#pragma omp barrier
        

        for(int k=istart;k<iend;k++)
        {
            
            
            fFirstHitInBinArray[k/fBinInGrid][k%fBinInGrid]+=Add;

        }
    }

    
}


void L1Grid::CreatePar(L1HitPoint* points, THitI nhits, vector <L1HitPoint>* pointsBuf, vector <L1StsHit>* hitsBuf, const L1StsHit* hits, vector <THitI>* indices, vector <THitI>* indicesBuf, char iS, L1Algo &Algo, THitI n)
{
    fOffsets.resize(fNThreads +1, 0);
    fNumberHitsInBin.resize(8000000, 0);
    
   fscal xs, ys;
   #pragma omp parallel for schedule(dynamic, 250) 
  for( THitI x = 0; x < (fN+1); x++ ) {

  fNumberHitsInBin[x]=0;

  }
   
   
   
   
 #pragma omp parallel for schedule(dynamic, 250) firstprivate(xs, ys)
  for( THitI x = 0; x < nhits; x++ ) {
    Algo.GetHitCoor((hits)[x], xs, ys, iS);
  #pragma omp atomic
  fNumberHitsInBin[GetBinBounded(xs, ys)]++;

  }
    
//    fFirstHitInBin[0]=0;
// //   vector<THitI> a(fN,0);
    
//#pragma omp parallel
//    {
//#pragma omp for
//        for(int ii=1; ii < fN; ii++) {
//            fFirstHitInBin[ii] = fNumberHitsInBin[ii-1];
//        }
//        
//        recursiveSum(&fFirstHitInBin[0],&fFirstHitInBin[fN], fN);
//        
//    }

//  int  NTreads = fNThreads/3;
// 
//   fOffsets[0]=0;
//   #pragma omp parallel for 
//     for( int i = 1; i < NTreads; i++ ) 
//    fOffsets[i]=i*fN/(NTreads);
//    fOffsets[NTreads]=fN;
//    
//    #pragma omp parallel for //schedule(static, 1)//firstprivate(first, last, mid)
//    for( THitI x = 0; x < NTreads; x++ ) 
//    {
//      fFirstHitInBin[fOffsets[x]] = 0;
//      for( THitI iBin = fOffsets[x]+1; iBin < fOffsets[x+1]; iBin++ ) 
//        fFirstHitInBin[iBin]=fFirstHitInBin[iBin-1]+ fNumberHitsInBin[iBin-1];
//    }
//    
//    int Add[NTreads];
//    
// 
//      Add[0]=0;
//      for (int p=1;p<NTreads;p++)
//      Add[p]=Add[p-1]+fFirstHitInBin[((p)*fN/NTreads)-1]+fNumberHitsInBin[((p)*fN/NTreads)-1];
//    
//     #pragma omp parallel for //schedule(static, 1)//firstprivate(first, last, mid)
//    for( THitI x = 0; x < NTreads; x++ ) 
//    {
//      for( THitI iBin = fOffsets[x]; iBin < fOffsets[x+1]; iBin++ ) 
//       fFirstHitInBin[iBin]=fFirstHitInBin[iBin]+ Add[x];
//    }
  
   #pragma omp parallel
  {
    const short int id = omp_get_thread_num();
    const short int Nthreads = omp_get_num_threads();
    const int istart=(omp_get_thread_num())*fN/Nthreads;
    int iend =(omp_get_thread_num()+1)*fN/Nthreads;
    if (omp_get_thread_num() == Nthreads - 1) iend = fN+1;
      
    
    THitI prefix = 0;
//      #pragma omp critical
//      cout<<(istart + 1)<<" start 1 "<<iend<<" end "<<endl;
    
    for(int i=istart+1;i<iend;i++)
    {
      prefix += fNumberHitsInBin[i-1];
      
        fFirstHitInBinArray[i/fBinInGrid][i%fBinInGrid]=prefix;
    }
    

    
      int Add = 0;
    
    #pragma omp barrier
    
    for (int p=0;p<omp_get_thread_num();p++)
    {
      
 
        const int &index = (((p+1)*fN/Nthreads)-1);
        
        if (index<0) break;

        
        Add += fFirstHitInBinArray[index/fBinInGrid][index%fBinInGrid] +
        fNumberHitsInBin[index];
        
    }
      
//#pragma omp critical
//      cout<<omp_get_thread_num()<<" thread "<<Add<<" Add "<<endl;
      
    #pragma omp barrier
      
//#pragma omp critical
//      cout<<istart<<" istart "<<iend<<" iend "<<endl;

    for(int k=istart;k<iend;k++)
    {
        
     
        fFirstHitInBinArray[k/fBinInGrid][k%fBinInGrid]+=Add;
      //  fFirstHitInBin[k/fBinInGrid][k%fBinInGrid]=fFirstHitInBinArray[k/fBinInGrid][k%fBinInGrid];
       // cout<<fFirstHitInBin[k/fBinInGrid][k%fBinInGrid]<<endl;
       }
  }
 
    
    
    for(int k=0;k<(fN+1);k++)
    {
        
        
      //  cout<<fFirstHitInBinArray[k/fBinInGrid][k%fBinInGrid]<<" "<<k<<endl;
        //  fFirstHitInBin[k/fBinInGrid][k%fBinInGrid]=fFirstHitInBinArray[k/fBinInGrid][k%fBinInGrid];
        // cout<<fFirstHitInBin[k/fBinInGrid][k%fBinInGrid]<<endl;
    }
 
}

void L1Grid::HitsSortSIMD(L1HitPoint* pointsBuf, L1StsHit* hitsBuf, const L1StsHit* hits, THitI* indices, THitI* indicesBuf, L1HitPoint* points, THitI n, THitI nhits, char iS, L1Algo &Algo, L1StsHitV* vStsHitsUnusedV,  const vector< fscal >   *vStsZPos, const vector< L1Strip >  *vStsStrips, const vector< L1Strip >  *vStsStripsB){
    
    fscal xs, ys;
    
#pragma omp parallel for schedule(dynamic, 250) firstprivate(xs, ys)
    for(THitI x = 0; x < nhits; x++ ) {
        
        // const THitI &index2 = x+n;
        //   const THitI  &bin = GetBinBounded((points)[x].Xs(), (points)[x].Ys());
        //  const THitI &index1 =fNumberHitsInBin[bin];
        
        
        Algo.GetHitCoor((hits)[x], xs, ys, iS);
        
        
        const THitI  &bin = GetBinBounded(xs, ys);
        
        if (fNumberHitsInBin[bin] == 1)
            
        {
            const THitI &index =fFirstHitInBinArray[bin/fBinInGrid][bin%fBinInGrid];
            

            (indices)[index]=x+n;
            (*vStsHitsUnusedV).setHit(hits[x], index);
            (*vStsHitsUnusedV).z[index]=(*vStsZPos)[hits[x].iz];
            (*vStsHitsUnusedV).u[index]=(*vStsStrips)[hits[x].f];
            (*vStsHitsUnusedV).v[index]=(*vStsStripsB)[hits[x].b];
           
            
        }
        
        else
        {
            
            
            
            omp_set_lock(&(lock[bin]));
            
            fNumberHitsInBin[bin]--;
            
            const THitI &index1 =fNumberHitsInBin[bin]+fFirstHitInBinArray[bin/fBinInGrid][bin%fBinInGrid];
            
            
            (indices)[index1]=x+n;
            
            (*vStsHitsUnusedV).setHit(hits[x], index1);
            (*vStsHitsUnusedV).z[index1]=(*vStsZPos)[hits[x].iz];
            (*vStsHitsUnusedV).u[index1]=(*vStsStrips)[hits[x].f];
            (*vStsHitsUnusedV).v[index1]=(*vStsStripsB)[hits[x].b];
            
            omp_unset_lock(&(lock[bin]));
            
        }
        
        
        
    }
    
}

void L1Grid::HitsSort(L1HitPoint* pointsBuf, L1StsHit* hitsBuf, const L1StsHit* hits, THitI* indices, THitI* indicesBuf, L1HitPoint* points, THitI n, THitI nhits, char iS, L1Algo &Algo, L1StsHitV* vStsHitsUnusedV,  const vector< fscal >   *vStsZPos, const vector< L1Strip >  *vStsStrips, const vector< L1Strip >  *vStsStripsB){
    
    fscal xs, ys;

    #pragma omp parallel for schedule(dynamic, 250) firstprivate(xs, ys)
    for(THitI x = 0; x < nhits; x++ ) {

        // const THitI &index2 = x+n;
      //   const THitI  &bin = GetBinBounded((points)[x].Xs(), (points)[x].Ys());
       //  const THitI &index1 =fNumberHitsInBin[bin];
       
          
    Algo.GetHitCoor((hits)[x], xs, ys, iS);

       
       const THitI  &bin = GetBinBounded(xs, ys);
        
        if (fNumberHitsInBin[bin] == 1)
            
        {
      const THitI &index =fFirstHitInBinArray[bin/fBinInGrid][bin%fBinInGrid];
        
      //  (pointsBuf)[fFirstHitInBin[bin]]=points[x];
        (hitsBuf)[index]=hits[x];
        (indices)[index]=x+n;
        (*vStsHitsUnusedV).setHit(hits[x], index);
        (*vStsHitsUnusedV).z[index]=(*vStsZPos)[hits[x].iz];
        (*vStsHitsUnusedV).u[index]=(*vStsStrips)[hits[x].f];
        (*vStsHitsUnusedV).v[index]=(*vStsStripsB)[hits[x].b];
          //  a[fFirstHitInBinArray[bin/fBinInGrid][bin%fBinInGrid]]=x+n; 
            
        }
        
        else
        {
          
           
            
            omp_set_lock(&(lock[bin]));
            
            fNumberHitsInBin[bin]--;
           
            const THitI &index1 =fNumberHitsInBin[bin]+fFirstHitInBinArray[bin/fBinInGrid][bin%fBinInGrid];
//             cout<<index1<<" "<<bin<<" "<< fBinInGrid<<" "<<fFirstHitInBinArray[bin/fBinInGrid][bin%fBinInGrid]<<" "<<fNumberHitsInBin[bin]<<endl;

         //   (pointsBuf)[index1]=points[x];
            (hitsBuf)[index1]=hits[x];
          //  (indicesBuf)[index1]=(indices)[x];
           
            (indices)[index1]=x+n;
            
             (*vStsHitsUnusedV).setHit(hits[x], index1);
             (*vStsHitsUnusedV).z[index1]=(*vStsZPos)[hits[x].iz];
             (*vStsHitsUnusedV).u[index1]=(*vStsStrips)[hits[x].f];
             (*vStsHitsUnusedV).v[index1]=(*vStsStripsB)[hits[x].b];
            
            omp_unset_lock(&(lock[bin]));
            
        }
        
        
       
   }
    
}



void L1Grid::Create( float yMin, float yMax, float zMin, float zMax, float sy, float sz )
{
    //* Create the grid
    fStepYInv = 1.f / sy;
    fStepZInv = 1.f / sz;
    
    fYMinOverStep = yMin * fStepYInv;
    fZMinOverStep = zMin * fStepZInv;
    
    fNy = static_cast<unsigned short>( yMax * fStepYInv - fYMinOverStep + 1.f );
    fNz = static_cast<unsigned short>( zMax * fStepZInv - fZMinOverStep + 1.f );
    unsigned int Nold = fN;
    
    fN = fNy * fNz;
//     if (Nold < fN || Nold == 0) {
//         delete[] fFirstHitInBin;
//         fFirstHitInBin = new THitI[fN+1]; // one extra bin is needed by area to determine number of hits in the last bin
//     }
  
}

void L1Grid::FillPar( const L1HitPoint* points, THitI n) // call after sort, hits and number of hits are given as an input
{
 
   
   
    fOffsets[0] = 0;
   
    
  // #pragma omp parallel for schedule(dynamic)//firstprivate(first, last, mid)
   for( THitI x = 1; x < fNThreads+1; x++ ) {
    THitI first = 0;
    THitI last = n;                   
    THitI mid = first + (last - first) / 2; 
 
    if ((GetBinBounded(points[0].Xs(), points[0].Ys())*fBinInGrid) > x)
    {
         fOffsets[x] = 0;
    } 
    else if (GetBinBounded(points[n].Xs(), points[n].Ys()/fBinInGrid) < x)
    {
          fOffsets[x] = n;
    }
    while (first < last)
    {
 
 
        if (x <= GetBinBounded(points[mid].Xs(), points[mid].Ys())/fBinInGrid)
        {
            last = mid;
        }
        else
        {
            first = mid + 1;
        }
        mid = first + (last - first) / 2;
    }

      fOffsets[x] = last;
   }

 
  unsigned int currBin;
  int lastBin = 0; 
  
    
  #pragma omp parallel for firstprivate(lastBin, currBin)
  for( int j = 0; j < fNThreads; j++ ) {

  fFirstHitInBinArray[j][lastBin] = fOffsets[j];

  for( THitI i = fOffsets[j]; i < fOffsets[j+1]; i++ ) {

    currBin = GetBinBounded(points[i].Xs(), points[i].Ys())%(fBinInGrid);
   // L1_assert( lastBin <= currBin );
    memset( fFirstHitInBinArray[j] + lastBin + 1, i, currBin - lastBin );
    lastBin = currBin;    
  }
  memset( fFirstHitInBinArray[j] + lastBin + 1, fOffsets[j+1],  fBinInGrid - lastBin );     
    }
  

}
void L1Grid::Fill( const L1HitPoint* points, THitI n ) // call after sort, hits and number of hits are given as an input
{
    
   
    
    unsigned int lastBin = 0;
  //  fFirstHitInBin[lastBin] = 0;
    for( THitI i = 0; i < n; i++ ) {
        const unsigned int currBin = GetBinBounded(points[i].Xs(), points[i].Ys());
        L1_assert( lastBin <= currBin );
        
     //   memset( fFirstHitInBin + lastBin + 1, i, currBin - lastBin );
        lastBin = currBin;
    }
  //  memset( fFirstHitInBin + lastBin + 1, n, fN - lastBin );
 
   

  
}