/*
    Copyright (C) 2009 Matthias Kretz <kretz@kde.org>

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) version 3.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public License
    along with this library; see the file COPYING.LIB.  If not, write to
    the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
    Boston, MA 02110-1301, USA.

*/

#include "AliHLTTPCCASliceData.h"
#include "AliHLTTPCCAClusterData.h"
#include "AliHLTTPCCAMath.h"
#include "AliHLTArray.h"
#include "AliHLTTPCCAHit.h"
#include "AliHLTTPCCAParam.h"
#include "MemoryAssignmentHelpers.h"
#include <iostream>

// calculates an approximation for 1/sqrt(x)
// Google for 0x5f3759df :)
static inline float fastInvSqrt( float _x )
{
  union { float f; int i; } x = { _x };
  const float xhalf = 0.5f * x.f;
  x.i = 0x5f3759df - ( x.i >> 1 );
  x.f = x.f * ( 1.5f - xhalf * x.f * x.f );
  return x.f;
}

inline void AliHLTTPCCASliceData::createGrid( AliHLTTPCCARow *row, const AliHLTTPCCAClusterData &data )
{
  if ( row->NHits() <= 0 ) { // no hits or invalid data
    // grid coordinates don't matter, since there are no hits
    row->fGrid.CreateEmpty();
    return;
  }

  float yMin =  1.e3f;
  float yMax = -1.e3f;
  float zMin =  1.e3f;
  float zMax = -1.e3f;
  for ( int i = row->fHitNumberOffset; i < row->fHitNumberOffset + row->fNHits; ++i ) {
    const float y = data.Y( i );
    const float z = data.Z( i );
    if ( yMax < y ) yMax = y;
    if ( yMin > y ) yMin = y;
    if ( zMax < z ) zMax = z;
    if ( zMin > z ) zMin = z;
  }

  const float norm = fastInvSqrt( row->fNHits );
  row->fGrid.Create( yMin, yMax, zMin, zMax,
                     CAMath::Max( ( yMax - yMin ) * norm, 2.f ),
                     CAMath::Max( ( zMax - zMin ) * norm, 2.f ) );
}

inline void AliHLTTPCCASliceData::PackHitData( AliHLTTPCCARow *row, const AliHLTArray<AliHLTTPCCAHit> &binSortedHits )
{
  for ( int hitIndex = 0; hitIndex < row->fNHits; ++hitIndex ) {
    // bin sorted index!
    const int globalHitIndex = row->fHitNumberOffset + hitIndex;
    const AliHLTTPCCAHit &hh = binSortedHits[globalHitIndex];
    // HitData is bin sorted
    fHitDataY[row->fHitNumberOffset + hitIndex] = hh.Y();
    fHitDataZ[row->fHitNumberOffset + hitIndex] = hh.Z();
  }
}

void AliHLTTPCCASliceData::Clear()
{
  fNumberOfHits = 0;
}

void AliHLTTPCCASliceData::InitializeRows( const AliHLTTPCCAParam &p )
{
  for ( int i = 0; i < p.NRows(); ++i ) {
    fRows[i].fX = p.RowX( i );
    fRows[i].fMaxY = CAMath::Tan( p.DAlpha() / 2. ) * fRows[i].fX;
  }
}

void AliHLTTPCCASliceData::InitFromClusterData( const AliHLTTPCCAClusterData &data )
{
  ////////////////////////////////////
  // 1. prepare arrays
  ////////////////////////////////////

  fNumberOfHits = data.NumberOfClusters();

  /* TODO Vectorization
  for ( int rowIndex = data.FirstRow(); rowIndex <= data.LastRow(); ++rowIndex ) {
    int NumberOfClusters( int rowIndex ) const;
  }
  const int memorySize = fNumberOfHits * sizeof( short_v::Type )
  */
  const int numberOfRows = data.LastRow() - data.FirstRow();
  enum { VectorAlignment = sizeof( int ) };
  const int numberOfHitsPlusAlignment = NextMultipleOf < VectorAlignment / sizeof( int ) > ( fNumberOfHits );
  const int memorySize =
    // LinkData
    numberOfHitsPlusAlignment * 2 * sizeof( short ) +
    // HitData
    numberOfHitsPlusAlignment * 2 * sizeof( float ) +
    // FirstHitInBin
    NextMultipleOf<VectorAlignment>( ( 23 * numberOfRows + 4 * fNumberOfHits ) * sizeof( int ) ) +
    // HitWeights, ClusterDataIndex
    numberOfHitsPlusAlignment * 2 * sizeof( int );

  if ( fMemorySize < memorySize ) {
    fMemorySize = memorySize;
    delete[] fMemory;
    fMemory = new char[fMemorySize + 4];// VectorAlignment];
  }

  char *mem = fMemory;
  AssignMemory( fLinkUpData,   mem, numberOfHitsPlusAlignment );
  AssignMemory( fLinkDownData, mem, numberOfHitsPlusAlignment );
  AssignMemory( fHitDataY,     mem, numberOfHitsPlusAlignment );
  AssignMemory( fHitDataZ,     mem, numberOfHitsPlusAlignment );
  AssignMemory( fFirstHitInBin,  mem, 23 * numberOfRows + 4 * fNumberOfHits );
  AssignMemory( fHitWeights,   mem, numberOfHitsPlusAlignment );
  AssignMemory( fClusterDataIndex, mem, numberOfHitsPlusAlignment );

  ////////////////////////////////////
  // 2. fill HitData and FirstHitInBin
  ////////////////////////////////////

  for ( int rowIndex = 0; rowIndex < data.FirstRow(); ++rowIndex ) {
    AliHLTTPCCARow &row = fRows[rowIndex];
    row.fGrid.CreateEmpty();
    row.fNHits = 0;
    row.fFullSize = 0;
    row.fHitNumberOffset = 0;
    row.fFirstHitInBinOffset = 0;
  }
  for ( int rowIndex = data.LastRow() + 1; rowIndex < AliHLTTPCCAParameters::NumberOfRows2; ++rowIndex ) {
    AliHLTTPCCARow &row = fRows[rowIndex];
    row.fGrid.CreateEmpty();
    row.fNHits = 0;
    row.fFullSize = 0;
    row.fHitNumberOffset = 0;
    row.fFirstHitInBinOffset = 0;
  }

  AliHLTResizableArray<AliHLTTPCCAHit> binSortedHits( fNumberOfHits );

  int gridContentOffset = 0;

  int binCreationMemorySize = 103 * 2 + fNumberOfHits;
  AliHLTResizableArray<unsigned short> binCreationMemory( binCreationMemorySize );

  for ( int rowIndex = data.FirstRow(); rowIndex <= data.LastRow(); ++rowIndex ) {
    AliHLTTPCCARow &row = fRows[rowIndex];

    row.fNHits = data.NumberOfClusters( rowIndex );
    assert( row.fNHits < ( 1 << sizeof( unsigned short ) * 8 ) );
    row.fHitNumberOffset = data.RowOffset( rowIndex );
    row.fFirstHitInBinOffset = gridContentOffset;

    createGrid( &row, data );
    const AliHLTTPCCAGrid &grid = row.fGrid;
    const int numberOfBins = grid.N();

    int binCreationMemorySizeNew;
    if ( ( binCreationMemorySizeNew = numberOfBins * 2 + 6 + row.fNHits ) > binCreationMemorySize ) {
      binCreationMemorySize = binCreationMemorySizeNew;
      binCreationMemory.Resize( binCreationMemorySize );
    }

    AliHLTArray<unsigned short> c = binCreationMemory;           // number of hits in all previous bins
    AliHLTArray<unsigned short> bins = c + ( numberOfBins + 3 ); // cache for the bin index for every hit in this row
    AliHLTArray<unsigned short> filled = bins + row.fNHits;      // counts how many hits there are per bin

    for ( unsigned int bin = 0; bin < row.fGrid.N() + 3; ++bin ) {
      filled[bin] = 0; // initialize filled[] to 0
    }

    for ( int hitIndex = 0; hitIndex < row.fNHits; ++hitIndex ) {
      const int globalHitIndex = row.fHitNumberOffset + hitIndex;
      const unsigned short bin = row.fGrid.GetBin( data.Y( globalHitIndex ), data.Z( globalHitIndex ) );
      bins[hitIndex] = bin;
      ++filled[bin];
    }

    unsigned short n = 0;
    for ( int bin = 0; bin < numberOfBins + 3; ++bin ) {
      c[bin] = n;
      n += filled[bin];
    }

    for ( int hitIndex = 0; hitIndex < row.fNHits; ++hitIndex ) {
      const unsigned short bin = bins[hitIndex];
      --filled[bin];
      const unsigned short ind = c[bin] + filled[bin]; // generate an index for this hit that is >= c[bin] and < c[bin + 1]
      const int globalBinsortedIndex = row.fHitNumberOffset + ind;
      const int globalHitIndex = row.fHitNumberOffset + hitIndex;

      // allows to find the global hit index / coordinates from a global bin sorted hit index
      fClusterDataIndex[globalBinsortedIndex] = globalHitIndex;
      binSortedHits[globalBinsortedIndex].SetCoordinates( data.Y( globalHitIndex ), data.Z( globalHitIndex ) );
    }

    PackHitData( &row, binSortedHits );

    for ( int i = 0; i < numberOfBins; ++i ) {
      fFirstHitInBin[row.fFirstHitInBinOffset + i] = c[i]; // global bin-sorted hit index
    }
    const unsigned short a = c[numberOfBins];
    // grid.N is <= row.fNHits
    const int nn = numberOfBins + grid.Ny() + 3;
    for ( int i = numberOfBins; i < nn; ++i ) {
      assert( static_cast<int>( row.fFirstHitInBinOffset + i ) < 23 * numberOfRows + 4 * fNumberOfHits );
      fFirstHitInBin[row.fFirstHitInBinOffset + i] = a;
    }

    row.fFullSize = nn;
    gridContentOffset += nn;
  }
}

void AliHLTTPCCASliceData::ClearHitWeights()
{
#ifdef ENABLE_VECTORIZATION
  const int_v v0( Zero );
  const int *const end = fHitWeights + fNumberOfHits;
  for ( int *mem = fHitWeights; mem < end; mem += v0.Size ) {
    v0.store( mem );
  }
#else
  for ( int i = 0; i < fNumberOfHits; ++i ) {
    fHitWeights[i] = 0;
  }
#endif
}

void AliHLTTPCCASliceData::ClearLinks()
{
#ifdef ENABLE_VECTORIZATION
  const short_v v0( -1 );
  const short *const end1 = fLinkUpData + fNumberOfHits;
  for ( short *mem = fLinkUpData; mem < end; mem += v0.Size ) {
    v0.store( mem );
  }
  const short *const end2 = fLinkDownData + fNumberOfHits;
  for ( short *mem = fLinkDownData; mem < end; mem += v0.Size ) {
    v0.store( mem );
  }
#else
  for ( int i = 0; i < fNumberOfHits; ++i ) {
    fLinkUpData[i] = -1;
  }
  for ( int i = 0; i < fNumberOfHits; ++i ) {
    fLinkDownData[i] = -1;
  }
#endif
}

///////////////////////
// for debugging
///////////////////////

#include "BinaryStoreHelper.h"

void AliHLTTPCCASliceData::StoreToFile( FILE *f ) const
{
  for ( int i = 0; i < 200; ++i ) {
    BinaryStoreWrite( fRows[i], f );
  }
  BinaryStoreWrite( fNumberOfHits, f );

  const char *startPointer = reinterpret_cast<const char *>( fLinkUpData );
  const int size = fMemorySize - ( startPointer - fMemory );
  assert( size >= 0 );
  assert( size <= fMemorySize );
  BinaryStoreWrite( size, f );
  BinaryStoreWrite( static_cast<Byte_t>( reinterpret_cast<unsigned long>( startPointer ) & 0xff ), f );
  BinaryStoreWrite( startPointer, size, f );

  BinaryStoreWrite( fLinkUpData, startPointer, f );
  BinaryStoreWrite( fLinkDownData, startPointer, f );
  BinaryStoreWrite( fHitDataY, startPointer, f );
  BinaryStoreWrite( fHitDataZ, startPointer, f );
  BinaryStoreWrite( fFirstHitInBin, startPointer, f );
  BinaryStoreWrite( const_cast<const uint_v *>( fHitWeights ), startPointer, f );
  BinaryStoreWrite( fClusterDataIndex, startPointer, f );
}

void AliHLTTPCCASliceData::RestoreFromFile( FILE *f )
{
  for ( int i = 0; i < 200; ++i ) {
    BinaryStoreRead( fRows[i], f );
  }
  BinaryStoreRead( fNumberOfHits, f );

  BinaryStoreRead( fMemorySize, f );
  Byte_t alignment;
  BinaryStoreRead( alignment, f );
  fMemory = new char[fMemorySize + 256];
  const Byte_t offset = alignment - static_cast<Byte_t>( reinterpret_cast<unsigned long>( fMemory ) & 0xff );
  BinaryStoreRead( fMemory + offset, fMemorySize, f );

  BinaryStoreRead( fLinkUpData, fMemory, f, offset );
  BinaryStoreRead( fLinkDownData, fMemory, f, offset );
  BinaryStoreRead( fHitDataY, fMemory, f, offset );
  BinaryStoreRead( fHitDataZ, fMemory, f, offset );
  BinaryStoreRead( fFirstHitInBin, fMemory, f, offset );
  BinaryStoreRead( fHitWeights, fMemory, f, offset );
  BinaryStoreRead( fClusterDataIndex, fMemory, f, offset );
}