#ifndef __CPM__DATASTRUCTURES__SIMPLEXMESH__HXX__
#define __CPM__DATASTRUCTURES__SIMPLEXMESH__HXX__

#include <cmath>
#include <vnl/vnl_math.h>
#include <itkMatrix.h>

// -------------------------------------------------------------------------
template< typename P, unsigned int K, unsigned int D, typename T >
bool cpm::DataStructures::SimplexMesh< P, K, D, T >::
ComputeSimplexParameters(
  const PointIdentifier& pId,
  VectorType& A,
  VectorType& B,
  VectorType& C,
  VectorType& N,
  BarycenterVector& barycenter,
  ValueType& r,
  ValueType& d,
  ValueType& dp,
  ValueType& phi
  ) const
{
  typedef itk::Matrix< ValueType, 4, 4 > TTetraMatrix;

  static const ValueType _0 = ValueType( 0 );
  static const ValueType _1 = ValueType( 1 );
  static const ValueType _2 = ValueType( 2 );
  static const ValueType _3 = ValueType( 3 );
  static const ValueType _4 = ValueType( 4 );

  // Get topological neighborhood
  const TPrimalEdge* eA = this->FindEdge( pId );
  if( eA == NULL )
    return( false );
  const TPrimalEdge* eB = eA->GetOnext( );
  if( eB == NULL )
    return( false );
  const TPrimalEdge* eC = eB->GetOnext( );
  if( eC == NULL )
    return( false );

  // Get geometrical neighborhood
  VectorType Q;
  Q = this->GetPoint( eA->GetOrigin( ) ).GetVectorFromOrigin( );
  A = this->GetPoint( eA->GetDestination( ) ).GetVectorFromOrigin( );
  B = this->GetPoint( eB->GetDestination( ) ).GetVectorFromOrigin( );
  C = this->GetPoint( eC->GetDestination( ) ).GetVectorFromOrigin( );

  // Auxiliary values
  VectorType BA = B - A;
  VectorType CA = C - A;
  VectorType BAxCA = itk::CrossProduct( BA, CA );
  ValueType nBAxCA = BAxCA.GetNorm( );
  if( !( _0 < nBAxCA ) )
    return( false );

  // More auxiliary values
  VectorType QA = Q - A;

  // Compute barycenter
  ValueType nBAxCA2 = nBAxCA * nBAxCA;
  barycenter[ 2 ] = ( itk::CrossProduct( BA, QA ) * BAxCA ) / nBAxCA2;
  barycenter[ 1 ] = ( itk::CrossProduct( QA, CA ) * BAxCA ) / nBAxCA2;
  barycenter[ 0 ] = _1 - barycenter[ 2 ] - barycenter[ 1 ];

  // Compute Q projection onto neighboring plane
  VectorType Qp =
    ( A * barycenter[ 0 ] ) +
    ( B * barycenter[ 1 ] ) +
    ( C * barycenter[ 2 ] );

  // Compute normal
  N = BAxCA / nBAxCA;

  // Compute circumcircle center and radius (r)
  VectorType CC = BA * CA.GetSquaredNorm( );
  CC -= CA * BA.GetSquaredNorm( );
  CC = itk::CrossProduct( BAxCA, CC );
  CC /= _2 * nBAxCA2;
  r = CC.GetNorm( );
  CC += A;

  // Barycenter distance
  d = ( Qp - CC ).GetNorm( );

  // Desired barycenter distance
  dp = ( ( ( A + B + C ) / _3 ) - CC ).GetNorm( );

  // Compute tetrahedron's circumsphere
  TTetraMatrix mA;
  for( unsigned int dId = 0; dId < 3; dId++ )
  {
    mA[ 0 ][ dId ] = A[ dId ];
    mA[ 1 ][ dId ] = B[ dId ];
    mA[ 2 ][ dId ] = C[ dId ];
    mA[ 3 ][ dId ] = Q[ dId ];

  } // rof
  mA[ 0 ][ 3 ] = _1;
  mA[ 1 ][ 3 ] = _1;
  mA[ 2 ][ 3 ] = _1;
  mA[ 3 ][ 3 ] = _1;

  ValueType dA = vnl_determinant( mA.GetVnlMatrix( ) );
  if( dA > _0 ) // Tetrahedron IS NOT planar
  {
    TTetraMatrix mD;
    mD[ 0 ][ 3 ] = _1;
    mD[ 1 ][ 3 ] = _1;
    mD[ 2 ][ 3 ] = _1;
    mD[ 3 ][ 3 ] = _1;
    mD[ 0 ][ 0 ] = A.GetSquaredNorm( );
    mD[ 1 ][ 0 ] = B.GetSquaredNorm( );
    mD[ 2 ][ 0 ] = C.GetSquaredNorm( );
    mD[ 3 ][ 0 ] = Q.GetSquaredNorm( );

    mD[ 0 ][ 1 ] = A[ 1 ];
    mD[ 1 ][ 1 ] = B[ 1 ];
    mD[ 2 ][ 1 ] = C[ 1 ];
    mD[ 3 ][ 1 ] = Q[ 1 ];
    mD[ 0 ][ 2 ] = A[ 2 ];
    mD[ 1 ][ 2 ] = B[ 2 ];
    mD[ 2 ][ 2 ] = C[ 2 ];
    mD[ 3 ][ 2 ] = Q[ 2 ];
    ValueType Dx = vnl_determinant( mD.GetVnlMatrix( ) );

    mD[ 0 ][ 1 ] = A[ 0 ];
    mD[ 1 ][ 1 ] = B[ 0 ];
    mD[ 2 ][ 1 ] = C[ 0 ];
    mD[ 3 ][ 1 ] = Q[ 0 ];
    mD[ 0 ][ 2 ] = A[ 2 ];
    mD[ 1 ][ 2 ] = B[ 2 ];
    mD[ 2 ][ 2 ] = C[ 2 ];
    mD[ 3 ][ 2 ] = Q[ 2 ];
    ValueType Dy = vnl_determinant( mD.GetVnlMatrix( ) );

    mD[ 0 ][ 1 ] = A[ 0 ];
    mD[ 1 ][ 1 ] = B[ 0 ];
    mD[ 2 ][ 1 ] = C[ 0 ];
    mD[ 3 ][ 1 ] = Q[ 0 ];
    mD[ 0 ][ 2 ] = A[ 1 ];
    mD[ 1 ][ 2 ] = B[ 1 ];
    mD[ 2 ][ 2 ] = C[ 1 ];
    mD[ 3 ][ 2 ] = Q[ 1 ];
    ValueType Dz = vnl_determinant( mD.GetVnlMatrix( ) );

    mD[ 0 ][ 2 ] = A[ 2 ];
    mD[ 1 ][ 2 ] = B[ 2 ];
    mD[ 2 ][ 2 ] = C[ 2 ];
    mD[ 3 ][ 2 ] = Q[ 2 ];
    ValueType dC = vnl_determinant( mD.GetVnlMatrix( ) );

    // Circumcenter
    VectorType CS;
    CS[ 0 ] = Dx;
    CS[ 1 ] = Dy * -_1;
    CS[ 2 ] = Dz;
    CS /= _2 * dA;

    // Circumradius
    /*
     * WARNING: this is not necessary since sin(phi) and cos(phi)
     * depend on R as divisor:
     *
     * ValueType R = ( Dx * Dx ) + ( Dy * Dy ) + ( Dz * Dz );
     * R -= _4 * dA * dC;
     * R = ValueType( std::sqrt( double( R ) ) );
     * R /= _2 * ValueType( std::fabs( double( dA ) ) );
     */

    // Phi sin and cos
    ValueType sphi = r;
    VectorType CCCS = CC - CS;
    ValueType cphi = CCCS.GetNorm( ); 
    if( ( QA * N ) < _0 )
      sphi *= -_1;
    if( ( CCCS * N ) < _0 )
      cphi *= -_1;
    phi = ValueType( std::atan2( double( sphi ), double( cphi ) ) );
  }
  else // Tetrahedron IS planar
    phi = _0;

  return( true );
}

// -------------------------------------------------------------------------
template< typename P, unsigned int K, unsigned int D, typename T >
bool cpm::DataStructures::SimplexMesh< P, K, D, T >::
VerifySimplexOrder( ) const
{
  if( this->m_OnextRings.size( ) == 0 )
    return( true );

  typename Superclass::CntOnextRings::const_iterator rIt =
    this->m_OnextRings.begin( );
  unsigned int minOrder = ( *rIt )->GetOnextRingSize( );
  unsigned int maxOrder = minOrder;
  ++rIt;
  for( ; rIt != this->m_OnextRings.end( ); ++rIt )
  {
    unsigned int order = ( *rIt )->GetOnextRingSize( );
    minOrder = ( order < minOrder )? order: minOrder;
    maxOrder = ( maxOrder < order )? order: minOrder;

  } // rof

  if( K == 1 )
    return(
      ( minOrder == 1 || minOrder == 2 ) &&
      ( maxOrder == 1 || maxOrder == 2 )
      );
  else if( K == 2 )
    return( ( minOrder == maxOrder ) && ( minOrder == K + 1 ) );
  else
    return( false );
}

// -------------------------------------------------------------------------
template< typename P, unsigned int K, unsigned int D, typename T >
bool cpm::DataStructures::SimplexMesh< P, K, D, T >::
OperatorDeleteEdge( const PointIdentifier& a, const PointIdentifier& b )
{
  TPrimalEdge* e0 = this->FindEdge( a, b );
  if( e0 == NULL )
    return( false );

  // Name edges and faces
  TPrimalEdge* e1 = e0->GetOnext( );
  TPrimalEdge* e2 = e1->GetOnext( );
  TPrimalEdge* e3 = e0->GetLnext( );
  TPrimalEdge* e4 = e0->GetRprev( );
  TPrimalEdge* e5 = e2->GetSym( )->GetOprev( );
  TPrimalEdge* e6 = e4->GetLnext( );

  CellIdentifier f0 = e0->GetLeft( );
  CellIdentifier f1 = e0->GetRight( );
  CellIdentifier f2 = e1->GetLeft( );
  CellIdentifier f3 = e3->GetRight( );

  // Disconnect edges e0, e2, e4 and origins of e1 and e3
  TPrimalEdge::Splice( e2, e0 );
  TPrimalEdge::Splice( e2, e1 );
  TPrimalEdge::Splice( e5, e2->GetSym( ) );
  TPrimalEdge::Splice( e3, e0->GetSym( ) );
  TPrimalEdge::Splice( e3, e4 );
  TPrimalEdge::Splice( e6, e4->GetSym( ) );

  // Connect e1 and e3 origins
  TPrimalEdge::Splice( e5, e1 );
  TPrimalEdge::Splice( e6, e3 );

  // Change e1 and e3 origins
  e1->SetOrigin( e5->GetOrigin( ) );
  e3->SetOrigin( e6->GetOrigin( ) );

  // Assign face id (even if f1 is a "not face")
  typename TPrimalEdge::Iterator eIt = e3->BeginLnext( );
  while( eIt != e3->EndLnext( ) && *eIt != e1->GetSym( ) )
  {
    ( *eIt )->SetLeft( f1 );
    eIt++;

  } // elihw

  // Update points
  this->m_OnextRings[ e5->GetOrigin( ) ] = e5;
  this->m_OnextRings[ e6->GetOrigin( ) ] = e6;

  // Update cells entry points
  CellAutoPointer c1, c2, c3;
  this->GetCell( f1, c1 );
  this->GetCell( f2, c2 );
  this->GetCell( f3, c3 );

  TQuadEdgeCell* ec1 = dynamic_cast< TQuadEdgeCell* >( c1.GetPointer( ) );
  TQuadEdgeCell* ec2 = dynamic_cast< TQuadEdgeCell* >( c2.GetPointer( ) );
  TQuadEdgeCell* ec3 = dynamic_cast< TQuadEdgeCell* >( c3.GetPointer( ) );

  ec1->SetEntryPrimalEdge( e3 );
  ec2->SetEntryPrimalEdge( e1 );
  ec3->SetEntryPrimalEdge( e3->GetSym( ) );

  // Delete face f0
  this->_DeleteFace( f0 );

  // Delete e0 origin and destination
  this->_DeletePoint( e0->GetOrigin( ) );
  this->_DeletePoint( e0->GetDestination( ) );

  // Delete edges e0, e2 and e4
  this->_DeleteEdge( e0 );
  this->_DeleteEdge( e2 );
  this->_DeleteEdge( e4 );

  return( true );
}

// -------------------------------------------------------------------------
template< typename P, unsigned int K, unsigned int D, typename T >
cpm::DataStructures::SimplexMesh< P, K, D, T >::
SimplexMesh( )
  : Superclass( )
{
}

// -------------------------------------------------------------------------
template< typename P, unsigned int K, unsigned int D, typename T >
cpm::DataStructures::SimplexMesh< P, K, D, T >::
~SimplexMesh( )
{
}

// -------------------------------------------------------------------------
template< typename P, unsigned int K, unsigned int D, typename T >
typename cpm::DataStructures::SimplexMesh< P, K, D, T >::
VectorType cpm::DataStructures::SimplexMesh< P, K, D, T >::
_ComputePointNormal( const TPrimalEdge* e ) const
{
  VectorType N( ValueType( 0 ) );
  if( K == 1 )
  {
    /* TODO
       PointIdentifier iQp0 = e->GetOrigin( );
       PointIdentifier iQp1 = e->GetDestination( );
       PointIdentifier iQm1 = e->GetOnext( )->GetDestination( );
       PointIdentifier iQp2 = e->GetSym( )->GetOnext( )->GetDestination( );
       PointIdentifier iQm2 =
       e->GetOnext( )->GetSym( )->GetOnext( )->GetDestination( );

       VectorType Qp0 = this->GetPoint( iQp0 ).GetVectorFromOrigin( );
       VectorType Qp1 = this->GetPoint( iQp1 ).GetVectorFromOrigin( );
       VectorType Qm1 = this->GetPoint( iQm1 ).GetVectorFromOrigin( );
       VectorType Qp2 = this->GetPoint( iQp2 ).GetVectorFromOrigin( );
       VectorType Qm2 = this->GetPoint( iQm2 ).GetVectorFromOrigin( );

       // Compute tangent
       VectorType T( ValueType( 0 ) );
       T = Qp1 - Qm1;
       ValueType nT = T.GetNorm( );
       if( nT > ValueType( 0 ) )
       T /= nT;

       // Compute auxiliary vector
       VectorType R( ValueType( 0 ) );
       R = itk::CrossProduct( T, Qm1 - Qm2 );
       R = itk::CrossProduct( R, Qp2 - Qp1 );
       ValueType nR = R.GetNorm( );
       if( nR > ValueType( 0 ) )
       R /= nR;
    */
    // TODO : psi??? see documentation
  }
  else if( K == 2 )
  {
    PointIdentifier iA = e->GetDestination( );
    PointIdentifier iB = e->GetOnext( )->GetDestination( );
    PointIdentifier iC = e->GetOnext( )->GetOnext( )->GetDestination( );

    VectorType A = this->GetPoint( iA ).GetVectorFromOrigin( );
    VectorType B = this->GetPoint( iB ).GetVectorFromOrigin( );
    VectorType C = this->GetPoint( iC ).GetVectorFromOrigin( );

    N  = itk::CrossProduct( B - A, C - A );
    ValueType nN = N.GetNorm( );
    if( nN > ValueType( 0 ) )
      N /= nN;

  } // fi
  return( N );
}

#endif // __CPM__DATASTRUCTURES__SIMQLEXMESH__HXX__

// eof - $RCSfile$