/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkQuadEdgeMeshParam.txx,v $
  Language:  C++
  Date:      $Date: 2008-10-03 22:03:41 $
  Version:   $Revision: 1.3 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#ifndef __itkQuadEdgeMeshParam_txx
#define __itkQuadEdgeMeshParam_txx

#include "itkQuadEdgeMeshParam.h"

namespace itk
{

// ---------------------------------------------------------------------
template< typename TInputMesh, typename TOutputMesh, typename TSolverTraits >
QuadEdgeMeshParam< TInputMesh, TOutputMesh, TSolverTraits >
::QuadEdgeMeshParam()
{
  this->m_CoefficientsMethod = 0;
  this->m_BorderTransform = 0;
}

// ---------------------------------------------------------------------
template< typename TInputMesh, typename TOutputMesh, typename TSolverTraits >
void
QuadEdgeMeshParam< TInputMesh, TOutputMesh, TSolverTraits >
::CopyToOutputBorder( )
{
  OutputMeshType* output = this->GetOutput( );

  for( InputMapPoinIdentifierIterator it = m_BoundaryPtMap.begin( );
       it != m_BoundaryPtMap.end( );
       ++it )
    {
    output->SetPoint( it->first, m_Border[it->second] );
    }
}

// ---------------------------------------------------------------------
template< typename TInputMesh, typename TOutputMesh, typename TSolverTraits >
void
QuadEdgeMeshParam< TInputMesh, TOutputMesh, TSolverTraits >
::ComputeListOfInteriorVertices( )
{
  InputMeshPointer input = this->GetInput( );

  typename InputPointsContainer::Pointer points = input->GetPoints( );

  InputPointIdentifier k( 0 );
  InputPointIdentifier id( 0 );

  for( InputPointsContainerConstIterator it = points->Begin( );
        it != points->End( );
        ++it )
    {
    id = it->Index( );

    // if the id point is not in m_BoundaryPtMap
    // add this id point to m_InternalPtMap
    if( m_BoundaryPtMap.find( id ) == m_BoundaryPtMap.end( ) )
      {
      m_InternalPtMap[id] = k++;
      }
    }
}

// ---------------------------------------------------------------------
template< typename TInputMesh, typename TOutputMesh, typename TSolverTraits >
void
QuadEdgeMeshParam< TInputMesh, TOutputMesh, TSolverTraits >
::SolveLinearSystems( const MatrixType& iM, const VectorType& iBx,
       const VectorType& iBy, VectorType& oX, VectorType& oY )
{
  SolverTraits traits;
  traits.Solve( iM, iBx, iBy, oX, oY );
}

// ---------------------------------------------------------------------
template< typename TInputMesh, typename TOutputMesh, typename TSolverTraits >
void
QuadEdgeMeshParam< TInputMesh, TOutputMesh, TSolverTraits >
::FillMatrix( MatrixType& iM, VectorType& ioBx, VectorType& ioBy )
{
  InputMeshPointer input   = this->GetInput( );
  OutputMeshPointer output = this->GetOutput( );

  SolverTraits traits;

  InputCoordRepType value;

  InputMapPoinIdentifierIterator it;

  InputPointIdentifier id1, id2;
  InputPointIdentifier InternalId1, InternalId2;

  OutputPointType pt2;

  ValueType k[2];

  for( InputMapPoinIdentifierIterator
       InternalPtIterator = m_InternalPtMap.begin( );
       InternalPtIterator != m_InternalPtMap.end( );
       ++InternalPtIterator )
    {
    id1 = InternalPtIterator->first;
    InternalId1 = InternalPtIterator->second;

    k[0] = 0.;
    k[1] = 0.;

    InputQEType* edge = input->FindEdge( id1 );
    InputQEType* temp = edge;

    do
      {
      id2 = temp->GetDestination( );

      it = m_BoundaryPtMap.find( id2 );

      if( it != m_BoundaryPtMap.end( ) )
        {
        value = ( *m_CoefficientsMethod )( input, temp );
        pt2 = output->GetPoint( it->first );
        traits.AddToMatrix( iM, InternalId1, InternalId1, value );
        k[0] += static_cast< ValueType >( pt2[0] * value );
        k[1] += static_cast< ValueType >( pt2[1] * value );
        }
      else
        {
        InternalId2 = m_InternalPtMap[id2];
        if( InternalId1 < InternalId2 )
          {
          value = ( *m_CoefficientsMethod )( input, temp );
          traits.FillMatrix( iM, InternalId1, InternalId2, -value );
          traits.FillMatrix( iM, InternalId2, InternalId1, -value );
          traits.AddToMatrix( iM, InternalId1, InternalId1, value );
          traits.AddToMatrix( iM, InternalId2, InternalId2, value );
          }
       }

      temp = temp->GetOnext( );

      } while( temp != edge );

    ioBx[InternalId1] = k[0];
    ioBy[InternalId1] = k[1];
    }
}

// ---------------------------------------------------------------------
template< typename TInputMesh, typename TOutputMesh, typename TSolverTraits >
void
QuadEdgeMeshParam< TInputMesh, TOutputMesh, TSolverTraits >
::GenerateData( )
{
  Superclass::GenerateData( );

  InputMeshPointer input = this->GetInput( );
  OutputMeshType* output = this->GetOutput( );

  if( m_BorderTransform.IsNotNull( ) )
    {
    m_BorderTransform->Update( );
    m_Border = m_BorderTransform->GetBorder( );
    m_BoundaryPtMap = m_BorderTransform->GetBoundaryPtMap( );
    }

  assert( ( m_BoundaryPtMap.size( ) > 2 ) && ( m_Border.size( ) > 2 ) );
  CopyToOutputBorder( );

  ComputeListOfInteriorVertices( );

  size_t NbOfInteriorPts = m_InternalPtMap.size( );

  SolverTraits traits;

  MatrixType Matrix = traits.InitializeSparseMatrix( NbOfInteriorPts );
  VectorType Bx = traits.InitializeVector( NbOfInteriorPts );
  VectorType By = traits.InitializeVector( NbOfInteriorPts );
  VectorType X = traits.InitializeVector( NbOfInteriorPts );
  VectorType Y = traits.InitializeVector( NbOfInteriorPts );

  FillMatrix( Matrix, Bx, By );

  SolveLinearSystems( Matrix, Bx, By, X, Y );

  OutputPointType OutputPt;

  for( InputMapPoinIdentifierIterator PtIterator = m_InternalPtMap.begin( );
       PtIterator != m_InternalPtMap.end( );
       ++PtIterator )
    {
    OutputPointIdentifier id = static_cast< OutputPointIdentifier >( PtIterator->first );
    InputPointIdentifier InternalId = PtIterator->second;

    OutputPt[0] = X[InternalId];
    OutputPt[1] = Y[InternalId];
    OutputPt[2] = 0.;

    output->SetPoint( id, OutputPt );
    }
}

} // end namespace itk

#endif