// @(#) $Id: AliHLTTPCCANeighboursFinder.cxx,v 1.12 2010/08/27 20:01:33 ikulakov Exp $
// **************************************************************************
// This file is property of and copyright by the ALICE HLT Project          *
// ALICE Experiment at CERN, All rights reserved.                           *
//                                                                          *
// Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
//                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
//                  for The ALICE HLT Project.                              *
//                                                                          *
// Developed by:   Igor Kulakov <I.Kulakov@gsi.de>                          *
//                 Maksym Zyzak <M.Zyzak@gsi.de>                            *
//                                                                          *
// Permission to use, copy, modify and distribute this software and its     *
// documentation strictly for non-commercial purposes is hereby granted     *
// without fee, provided that the above copyright notice appears in all     *
// copies and that both the copyright notice and this permission notice     *
// appear in the supporting documentation. The authors make no claims       *
// about the suitability of this software for any purpose. It is            *
// provided "as is" without express or implied warranty.                    *
//                                                                          *
//***************************************************************************

#define USE_ARBB // there is only a small difference
//#define USE_CALL

#include "AliHLTTPCCANeighboursFinder.h"
#include "AliHLTTPCCAMath.h"
#include "AliHLTTPCCAHitArea.h"
#include "AliHLTTPCCATracker.h"
#include "AliHLTArray.h"
#include "AliHLTVector.h"

#ifndef HLTCA_STANDALONE
#include "AliHLTTPCCADisplay.h"
#endif

#ifdef USE_TBB
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#endif //USE_TBB

#ifdef USE_ARBB
#include <arbb.hpp>
using namespace arbb;
#endif // USE_ARBB

// #ifdef DRAW
// #include "AliHLTTPCCADisplay.h"
// #endif

#include "debug.h"

#ifndef NVALGRIND
#include <valgrind/memcheck.h>
#endif

using namespace Vc;

bool DRAW_EVERY_LINK = false;

#ifdef USE_TBB
inline void AliHLTTPCCATracker::NeighboursFinder::operator()( const tbb::blocked_range<int> &r ) const
{
  for ( int i = r.begin(); i < r.end(); ++i ) {
    executeOnRow( i );
  }
}
#endif //USE_TBB

void AliHLTTPCCATracker::NeighboursFinder::FindNeighboursOnRow_ArBB( nested_my<AliHLTTPCCARowHit>& rows, const dense<f32>& rowsX, const u8 rowIndex, const FindNeighboursOnRowParam& param )
{
  const u8 rowStep = AliHLTTPCCAParameters::RowStep;
  
  AliHLTTPCCARowHit::TDense<1> rowData   = rows.segment( rowIndex );
  AliHLTTPCCARowHit::TDense<1> rowUpData = rows.segment( rowIndex + rowStep );
  AliHLTTPCCARowHit::TDense<1> rowDnData = rows.segment( rowIndex - rowStep );


  const THitI_ArBB NHits = rowData.length();
  const THitI_ArBB NHitsUp = rowUpData.length();
  const THitI_ArBB NHitsDown = rowDnData.length();


  // the axis perpendicular to the rows
  const f32 xDn = rowsX[rowIndex - rowStep];
  const f32 x   = rowsX[rowIndex];
  const f32 xUp = rowsX[rowIndex + rowStep];

  // distance of the rows (absolute and relative)
  const f32 UpDx = xUp - x;
  const f32 DnDx = xDn - x;
  const f32 UpTx = xUp / x;
  const f32 DnTx = xDn / x;

  static const f32 kAreaSizeY = param.neighbourAreaSizeTgY;//*4;
  static const f32 kAreaSizeZ = param.neighbourAreaSizeTgZ;//*4;
  static const usize kMaxN = 20; // TODO minimaze

//#define USE_CURV_CUT // TODO don't work, problem with error estimation
#ifdef USE_CURV_CUT  
  f32 curv2Cut =  AliHLTTPCCAParameters::NeighbourCurvCut[iter] * .5 * ( UpDx * DnDx * ( UpDx - DnDx ) ); // max curv^2*(dx1*dx2*(dx1+dx2))^2 = (dslopeY^2+dslopeZ^2)*(dx1*dx2)^2 = (dy1*dx2-dy2*dx1)^2+(dz1*dx2-dz2*dx1)^2
  curv2Cut *= curv2Cut;
  const f32 UpErr2 = (rowIndex - rowStep < AliHLTTPCCAParameters::NumberOfInnerRows) ? 0.06*0.06 : 0.12*0.12,
              DnErr2 = (rowIndex + rowStep < AliHLTTPCCAParameters::NumberOfInnerRows) ? 0.06*0.06 : 0.12*0.12;

//  std::cout << UpDx << " " <<  DnDx << std::endl;
//  std::cout << "before " << 4.f * ( UpDx * UpDx + DnDx * DnDx ) << " now: " << 1/4.f * ( UpDx * DnDx * ( UpDx - DnDx ) ) * ( UpDx * DnDx * ( UpDx - DnDx ) ) << std::endl;
#else // USE_CURV_CUT  
  const f32 chi2Cut = param.neighbourChiCut*param.neighbourChiCut * 4.f * ( UpDx * UpDx + DnDx * DnDx );
#endif // USE_CURV_CUT  

    // coordinates of the hit in the current row
  dense<f32> y, z, yUp, zUp, yDn, zDn;

  y = rowData.Y();
  z = rowData.Z();

  yUp = y * UpTx; // suppose vertex at (0,0,0)
  yDn = y * DnTx; // TODO change name
  zUp = z * UpTx;
  zDn = z * DnTx;

  // UpTx/DnTx is used to move the HitArea such that central events are preferred (i.e. vertices
  // coming from y = 0, z = 0).

  /* find the connection between hits in the row below, this hit and a hit in the row above
   * minimizing the difference in slope, i.e.
   * ______________________________________
   *  \                           |dy1
   *   \                          |       dx1
   * ___\_________________________|________
   *     \     is better than      \      dx2
   * _____\______________________dy2\______
   *
   *
   * The slope is easily calculated as dy/dx ( dz/dx ).
   * Comparison: dy1/dx1 - dy2/dx2 = dslopeY
   * => dy1 * dx2 - dy2 * dx1 = dslopeY * dx1 * dx2
   *
   * y+z Comparison is done with:
   * dslopeY^2 + dslopeZ^2
   *
   * The constant ( dx1^2 + dx2^2 ) is multiplied into the chi2Cut
   */

  
#ifdef USE_CURV_CUT
  const dense<f32> UpDy = yUp - y,
    DnDy = yDn - y;
  const dense<f32> UpR2 = UpDy*UpDy + UpDx*UpDx,
    DnR2 = DnDy*DnDy + DnDx*DnDx;
  dense<f32> chi2Cut = curv2Cut + (UpErr2/UpR2 + DnErr2/DnR2)*(UpDx*DnDx)*(UpDx*DnDx); // additional error in dSlopes TODO Take into account z. TODO Use more precise error estimation // TODO change name // TODO optimize
    // std::cout << rowIndex << " " << curv2Cut << UpErr2/UpR2*(UpDx*DnDx)*(UpDx*DnDx) << DnErr2/DnR2*(UpDx*DnDx)*(UpDx*DnDx) << std::endl;
    // std::cout << UpErr2 << " " << DnErr2 << " " << UpDy << " " << DnDy << " " << UpR2 << " " << DnR2 << std::endl;
#endif //  USE_CURV_CUT

  const dense<arbb::boolean> validHitsMask = fill<arbb::boolean>(true, NHits) && ( rowData.IsUsed() == 0 );

    // get all information about matched upper hits
  AliHLTTPCCAHitArea_ArBB areaUp( rowUpData, yUp, zUp, UpDx*kAreaSizeY, UpDx*kAreaSizeZ, validHitsMask );
    
  // dense<u16> maxUpperNeighbourIndex = fill<u8>(0,NHits);
  usize maxCombUp = 0;
  typedef AliHLTTPCCAHitArea_ArBB::NeighbourData NeighbourData;
  NeighbourData::TDense<2> neighsUp;

  NeighbourData::TDense<1> neighUp_tmp = areaUp.GetNext();
  NeighbourData::TDense<1> neighsUp_tmp; // for neighsUp filling 
  _if ( any( neighUp_tmp.Valid() ) ) {
    neighsUp_tmp = neighUp_tmp;
    _for (,true,) {
      // maxUpperNeighbourIndex = select( neighUp_tmp.Valid(), maxUpperNeighbourIndex + 1, maxUpperNeighbourIndex );
      ++maxCombUp;
      _if ( maxCombUp >= kMaxN ) {   _break;   } _end_if;
    
      neighUp_tmp = areaUp.GetNext();
      const arbb::boolean isAny = any( neighUp_tmp.Valid() );
      _if ( !isAny ) { _break; } _end_if;
      neighsUp_tmp = cat( neighsUp_tmp, neighUp_tmp );
    } _end_for;
    neighsUp = reshape( neighsUp_tmp, NHits, maxCombUp );

      // get all information about matched lower hits

    AliHLTTPCCAHitArea_ArBB areaDn( rowDnData, yDn, zDn, -DnDx*kAreaSizeY, -DnDx*kAreaSizeZ, validHitsMask );
    usize maxCombDn = 0;
    NeighbourData::TDense<2> neighsDn;

    NeighbourData::TDense<1> neighDn_tmp = areaDn.GetNext();
    NeighbourData::TDense<1> neighsDn_tmp; // for neighsDn filling
    _if ( any( neighDn_tmp.Valid() ) ) {
      neighsDn_tmp = neighDn_tmp;
      _for (,true,) {
        ++maxCombDn;
        _if ( maxCombDn >= kMaxN ) {   _break;   } _end_if;
    
        neighDn_tmp = areaDn.GetNext();
        const arbb::boolean isAny = any( neighDn_tmp.Valid() );
        _if ( !isAny ) { _break; } _end_if;
        neighsDn_tmp = cat( neighsDn_tmp, neighDn_tmp );
      } _end_for;
      neighsDn = reshape( neighsDn_tmp, NHits, maxCombDn );


        // calculate parts of formula
      { 
        const dense<f32,2> y_2D = repeat_row(y, maxCombUp);
        const dense<f32,2> z_2D = repeat_row(z, maxCombUp);    
        neighsUp.SetY( DnDx * ( neighsUp.Y() - y_2D ) ); // the faster one
        neighsUp.SetZ( DnDx * ( neighsUp.Z() - z_2D ) );
          // const dense<THitI_ArBB> dMinusOne = fill(-1,   NHits);
          // const dense<arbb::boolean>    dTrue     = fill(true, NHits);
          // const dense<NeighbourData> yz_tmp   = y.zip(z).zip(dMinusOne).zip(dTrue);
    
          // neighsUp -= repeat_row( yz_tmp,   maxCombUp );
          // neighsUp *= repeat_row( fill<const f32>(DnDx, NHits), maxCombUp );
      }
      { 
        const dense<f32,2> y_2D = repeat_row(y, maxCombDn);
        const dense<f32,2> z_2D = repeat_row(z, maxCombDn);    
        neighsDn.SetY( UpDx * ( neighsDn.Y() - y_2D ) ); // the faster one
        neighsDn.SetZ( UpDx * ( neighsDn.Z() - z_2D ) );
          // const dense<THitI_ArBB> dMinusOne = fill(-1,   NHits);
          // const dense<arbb::boolean>    dTrue     = fill(true, NHits);
          // const dense<NeighbourData> yz_tmp   = y.zip(z).zip(dMinusOne).zip(dTrue);
    
          // neighsDn -= repeat_row( yz_tmp,   maxCombDn );
          // neighsDn *= repeat_row( fill<const f32>(UpDx, NHits), maxCombDn );
      }
      
        // expand into 3D versions
      const NeighbourData::TDense<3> neighsUp_3D = repeat_page( neighsUp, maxCombDn );
      const NeighbourData::TDense<3> neighsDn_3D = repeat_row ( neighsDn, maxCombUp );
      

        // get curvatures
      const dense<f32,3> dy = neighsDn_3D.Y() - neighsUp_3D.Y();
      const dense<f32,3> dz = neighsDn_3D.Z() - neighsUp_3D.Z();
      dense<f32,3> d = dy * dy + dz * dz;
      const dense<arbb::boolean,3> validCombMask = neighsUp_3D.Valid() && neighsDn_3D.Valid() && repeat_page( repeat_row( validHitsMask, maxCombUp), maxCombDn);
      d = select( validCombMask, d, chi2Cut );
      
        // find the minimum
      dense<usize> bestIndicesAtPages;
      dense<usize> bestIndicesAtColumns;        
      const dense<f32> bestNewD = min_reduce_my(d, bestIndicesAtColumns, bestIndicesAtPages);
      const dense<arbb::boolean> minNewMask = bestNewD < chi2Cut;
      const dense<array<usize, 3>, 1> bestIndices_tmp = bestIndicesAtPages.zip( bestIndicesAtColumns ).zip( indices<usize>(0,NHits,1) );
      const dense<array<usize, 3>, 3> bestIndices = repeat_page( repeat_row( bestIndices_tmp, 1 ), 1);
      
        // collect the best links
      const dense<THitI_ArBB> bestNewUp = gather( neighsUp_3D.Links(), bestIndices, -1 ).page(0).row(0); // TODO mask
      const dense<THitI_ArBB> bestNewDn = gather( neighsDn_3D.Links(), bestIndices, -1 ).page(0).row(0); // TODO mask
      dense<THitI_ArBB> bestUp = select( minNewMask, bestNewUp,  -1 );
      dense<THitI_ArBB> bestDn = select( minNewMask, bestNewDn, -1 );
    
        // store the link indices we found
      rowData.SetLinkUp( select( validHitsMask, bestUp, rowData.LinkUp() ) );
      rowData.SetLinkDn( select( validHitsMask, bestDn, rowData.LinkDn() ) );

    } _end_if; // isAny lower neighbour
  } _end_if; // isAny upper neighbour

  
  rows = replace_segment( rows, rowIndex, rowData );
}


inline void AliHLTTPCCATracker::NeighboursFinder::executeOnRow( TRowI_ArBB rowIndex )
{
  
  const TRowI_ArBB rowStep = AliHLTTPCCAParameters::RowStep;

  _if ( fRows_ArBB.segment(rowIndex          ).length() > 0 &&
        fRows_ArBB.segment(rowIndex + rowStep).length() > 0 &&
        fRows_ArBB.segment(rowIndex - rowStep).length() > 0     ) {

    FindNeighboursOnRowParam callParam;
    callParam.neighbourAreaSizeTgY = AliHLTTPCCAParameters::NeighbourAreaSizeTgY[fIter];
    callParam.neighbourAreaSizeTgZ = AliHLTTPCCAParameters::NeighbourAreaSizeTgZ[fIter];
    callParam.neighbourChiCut      = AliHLTTPCCAParameters::NeighbourChiCut[fIter];

    FindNeighboursOnRow_ArBB( fRows_ArBB, fRowsX, rowIndex, callParam );
    
  } _end_if;

}

void AliHLTTPCCATracker::NeighboursFinder::execute()
{
  const TRowI_ArBB numberOfRows = AliHLTTPCCAParameters::NumberOfRows;

  //tbb::affinity_partitioner partitioner;
  const TRowI_ArBB rowStep = AliHLTTPCCAParameters::RowStep;
// #ifdef USE_TBB  
//   tbb::parallel_for( tbb::blocked_range<int>( rowStep, numberOfRows - rowStep, 1000 ), *this );//, partitioner );
// #else  //USE_TBB
  
  _for ( TRowI_ArBB iRow = rowStep, iRow < numberOfRows - rowStep, ++iRow ) {
    executeOnRow( iRow );
  } _end_for;
// #endif //USE_TBB  

}