#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, char iS, L1Algo &Algo, const vector< unsigned char > *vSFlag, const vector< unsigned char > *vSFlagB)
{

    fFirstHitInBin.assign(fN+1, 0);

    fscal xs, ys;
    
#pragma omp parallel for schedule(dynamic, 250) firstprivate(xs, ys)
    for(THitI x = 0; x < Nelements; x++ ) {
        
        const L1StsHit &hit = hits[x];
        
        if (!L1Algo::GetFUsed( (*vSFlag)[hit.f] | (*vSFlagB)[hit.b] ))
        {
            Algo.GetHitCoor(hit, xs, ys, iS);

            const THitI  &bin = GetBinBounded(xs, ys, hit.t_reco);
                
            fHitsInBin[x]=fFirstHitInBin[bin+1];
            fFirstHitInBin[bin+1]++;
     
        } 
    }
    
    
    
    
            int kk = 2;
            /* Up-Sweep Phase */
            for (int k = 1; k < fN+1; k = kk) { kk = k << 1;
#pragma omp parallel for //schedule(guided)
                for (int i = kk - 1; i < fN+1; i += kk) { fFirstHitInBin[i] = fFirstHitInBin[i - k]+fFirstHitInBin[i];
                    
                } }
            
            /* Down-Sweep Phase */
            for (int k = kk >> 1; k > 1; k = kk) { kk = k >> 1;
#pragma omp parallel for //schedule(guided)
                for (int i = k - 1; i < fN+1 - kk; i += k) {
                    fFirstHitInBin[i + kk] = fFirstHitInBin[i]+fFirstHitInBin[i + kk];
                    
                } }

    
#pragma omp parallel for schedule(dynamic, 250) firstprivate(xs, ys)
    for(THitI x = 0; x < Nelements; x++ ) {
        
        const L1StsHit &hit = hits[x];
        if (!L1Algo::GetFUsed( (*vSFlag)[hit.f] | (*vSFlagB)[hit.b] ))
        {
        Algo.GetHitCoor(hit, xs, ys, iS);
        
        
        const THitI  &bin = GetBinBounded(xs, ys, hit.t_reco);
        
        {
   
            const THitI &index1 =fHitsInBin[x]+fFirstHitInBin[bin];
            
            (*hits2)[index1+NHitsOnStation] = hits[x];
            (*indicesBuf)[index1+NHitsOnStation]=indices[x];
            (*pointsBuf)[index1+NHitsOnStation]=points[x];
            
        }
        }
    
        
    }

    NHitsOnStation+=fFirstHitInBin[fN];

}


void L1Grid::AllocateMemory( int NThreads )
{
    
    fNThreads = NThreads*1;
    
    fFirstHitInBinArray.resize(fNThreads, 0);
    
    
    int binsGrid = 400000;
    
    fFirstHitInBin.resize(binsGrid, 0);
    fHitsInBin.resize(binsGrid, 0);
    
    for( int i = 0; i < fNThreads; i++ )
    {
        delete[] fFirstHitInBinArray[i];
       // delete[] fFirstHitInBin[i];
        fFirstHitInBinArray[i] = new THitI[binsGrid];// TODO calculate safe number of bins
       // fFirstHitInBin[i] = new THitI[binsGrid];
    }
    fOffsets.resize(fNThreads +1, 0);
    fNumberHitsInBin.resize(binsGrid, 0);
    
}




void L1Grid::BuildBins( float yMin, float yMax, float zMin, float zMax, float tMin, float tMax, float sy, float sz, float st)
{
    
    fStepYInv = 1.f / sy;
    fStepZInv = 1.f / sz;
    fStepTInv = 1.f / st;
    
    fYMinOverStep = yMin * fStepYInv;
    fZMinOverStep = zMin * fStepZInv;
    fTMinOverStep = tMin * fStepTInv;
    
    fNy = ( yMax * fStepYInv - fYMinOverStep + 1.f );
    fNz = ( zMax * fStepZInv - fZMinOverStep + 1.f );
    fNt = ( tMax * fStepTInv - fTMinOverStep + 1.f );
    // unsigned int Nold = fN;
   
    fN = fNy * fNz * fNt;
   // cout<<fNThreads<<" fN "<<fN<<" fBinInGrid "<<fNy<<" "<<fNz<<" "<<fNt<<endl;
  //   cout<<fN<<endl;
    fBinInGrid = (((fN)/fNThreads)+1);
    
    //      #pragma omp critical
    //     cout<<fN<<" fN "<<fBinInGrid<<" fBinInGrid "<<endl;
}


void L1Grid::StoreHits(THitI nhits, const L1StsHit* hits, char iS, L1Algo &Algo, THitI n, L1StsHit* hitsBuf1, const L1StsHit* hits1, THitI* indices1)
{
    
    fscal xs, ys;

    fFirstHitInBin.assign(fN+1, 0);
    
    
#pragma omp parallel for firstprivate(xs, ys)
    for( THitI x = 0; x < nhits; x++ ) {
        Algo.GetHitCoor((hits)[x], xs, ys, iS);

        
        fHitsInBin[x]=fFirstHitInBin[GetBinBounded(xs, ys, (hits)[x].t_reco )+1];
#pragma omp atomic
        fFirstHitInBin[GetBinBounded(xs, ys, (hits)[x].t_reco )+1]++;

        
    }


    
    int kk = 2;
    /* Up-Sweep Phase */
    for (int k = 1; k < fN+1; k = kk) { kk = k << 1;
#pragma omp parallel for //schedule(guided)
        for (int i = kk - 1; i < fN+1; i += kk) { fFirstHitInBin[i] = fFirstHitInBin[i - k]+fFirstHitInBin[i];
            
        } }
    
    /* Down-Sweep Phase */
    for (int k = kk >> 1; k > 1; k = kk) { kk = k >> 1;
#pragma omp parallel for //schedule(guided)
        for (int i = k - 1; i < fN+1 - kk; i += k) {
            fFirstHitInBin[i + kk] = fFirstHitInBin[i]+fFirstHitInBin[i + kk];
            
        } }

    
#pragma omp parallel for firstprivate(xs, ys)
    for(THitI x = 0; x < nhits; x++ ) {
        
        Algo.GetHitCoor((hits1)[x], xs, ys, iS);
        
        
        const THitI  &bin = GetBinBounded(xs, ys, (hits1)[x].t_reco);
        
        {
            
            const THitI &index1 =fHitsInBin[x]+fFirstHitInBin[bin];
            
            (hitsBuf1)[index1]=hits[x];
            
            
            (indices1)[index1]=x+n;
            
        }
        
        
        
    }

    
}