#include <smoothers/CEigenSmoother.h>

//=====================================================================================================================
CEigensmoother::CEigensmoother( double factor, bool normalize, bool invert, int iterations, double exponent ) :
        CSmoother(),
        dFactor( factor ),
        dExponent( exponent ),
        bNormalize( normalize ),
        bInvert( invert ),
        iIterations( iterations )
{
}

//=====================================================================================================================
CEigensmoother::~CEigensmoother()
{
}

//=====================================================================================================================
bool    CEigensmoother::smooth( CSmoothingMesh * mesh )
{
    tVertPtrList             nverts;
    tVertPtrList::iterator   nvert_it;

    for ( int i = 0; i < this->iIterations; i++ )
    {
        vctl::MCVerticeS    * vertices = mesh->GetVerticeS();
        vctl::MCVertex      * vertex   = vertices->GetFirst();
        vctl::MCPoint3D       smoothed, difference;

        std::vector< vctl::MCPoint3D >              smooth_coords( vertices->GetNumber() );
        std::vector< vctl::MCPoint3D >::iterator    smooth_coords_it = smooth_coords.begin();

        while ( vertex )
        {
            smoothed.setXYZ( 0, 0, 0 );

            this->smoothVertex( mesh, vertex, smoothed );

            smooth_coords_it->SetX( smoothed.GetX() );
            smooth_coords_it->SetY( smoothed.GetY() );
            smooth_coords_it->SetZ( smoothed.GetZ() );

            vertex = vertex->GetNext();
            smooth_coords_it++;
         }

         vertex = vertices->GetFirst();
         smooth_coords_it = smooth_coords.begin();

         while ( vertex )
         {
            vertex->SetX( smooth_coords_it->GetX() );
            vertex->SetY( smooth_coords_it->GetY() );
            vertex->SetZ( smooth_coords_it->GetZ() );
            vertex = vertex->GetNext();
            smooth_coords_it++;
         }
     }
     return true;
}

double  getSimilarity( vctl::MCVertex * v1, vctl::MCVertex * v2 )
{
  CVertexInfo *   in1 = reinterpret_cast< CVertexInfo * >( v1->GetValuePtr() );
  CVertexInfo *   in2 = reinterpret_cast< CVertexInfo * >( v2->GetValuePtr() );

  CVertexInfoMV * info1 = dynamic_cast< CVertexInfoMV * >( in1 );
  CVertexInfoMV * info2 = dynamic_cast< CVertexInfoMV * >( in2 );


  osg::Vec3    vv1 = conv::convert< mds::math::CDVector, osg::Vec3 >( info1->getVector() );
  osg::Vec3    vv2 = conv::convert< mds::math::CDVector, osg::Vec3 >( info2->getVector() );

  vv1.normalize();
  vv2.normalize();

  double d = fabs( vv1[0] - vv2[0] ) + fabs( vv1[1] - vv2[1] ) + fabs( vv1[2] - vv2[2] );

 // std::cerr << d << std::endl;
  return ( 1.0 - d ) * ( 1.0 - d ) ;

}

//=====================================================================================================================
bool    CEigensmoother::smoothVertex( CSmoothingMesh * mesh, vctl::MCVertex * actual_vertex, vctl::MCPoint3D & smoothed_point )
{
  std::vector<vctl::MCVertex *>             neighbour_vertex;               // vektor sousednich vrcholu pro aktualni vrchol
  std::vector<vctl::MCVertex *>::iterator   neighbour_vertex_iter;          // iterator vektoru sousednich vrcholu
  double                                    dx = 0, dy = 0, dz = 0;         // point coordinate accumulation variables

  vctl::MCPoint3D   difference_vector;


  // inicializace vyhlazeneho vrcholu
  smoothed_point.SetXYZ(0.0, 0.0, 0.0);

  // ziskani okolnich vrcholu pro aktualni vrchol
  mesh->getNeighbourVert( actual_vertex, neighbour_vertex, 1 );

  // cyklus okolnich vrcholu
  for (neighbour_vertex_iter = neighbour_vertex.begin(); neighbour_vertex_iter != neighbour_vertex.end(); ++neighbour_vertex_iter)
  {
    // point coordinate accumulation
   //  double d = getSimilarity( actual_vertex, (*neighbour_vertex_iter) );

    dx += ((*neighbour_vertex_iter)->GetX() - actual_vertex->GetX());// * d;
    dy += ((*neighbour_vertex_iter)->GetY() - actual_vertex->GetY());// * d;
    dz += ((*neighbour_vertex_iter)->GetZ() - actual_vertex->GetZ());// * d;
  }

  difference_vector.SetX( dx / neighbour_vertex.size() );
  difference_vector.SetY( dy / neighbour_vertex.size() );
  difference_vector.SetZ( dz / neighbour_vertex.size() );




  if ( CVertexInfo *   in = reinterpret_cast< CVertexInfo * >( actual_vertex->GetValuePtr() ) )
  {
    if ( CVertexInfoMV * info = dynamic_cast< CVertexInfoMV * >( in ) )
    {
		osg::Matrix  A = conv::convert< mds::math::CDMatrix, osg::Matrix >( info->getMatrix() );
		osg::Vec3    v = conv::convert< mds::math::CDVector, osg::Vec3 >( info->getVector() );

		if ( this->bInvert )
		{
		   v[0] = 1 / exp( this->dExponent * log( v[0] ) );
		   v[1] = 1 / exp( this->dExponent * log( v[1] ) );
		   v[2] = 1 / exp( this->dExponent * log( v[2] ) );
		}

		if ( this->bNormalize )
		{
			v.normalize();
		}

		v =  v * this->dFactor;

		osg::Matrix iA = osg::Matrix::inverse( A );
		osg::Matrix  S = osg::Matrix::scale( v );
		osg::Vec3    d = osg::Vec3( difference_vector.getX(),difference_vector.getY(),difference_vector.getZ() );
		osg::Vec3    t = iA * S * A * d;

		difference_vector.SetX( t[0] );
		difference_vector.SetY( t[1] );
		difference_vector.SetZ( t[2] );
     }
   }

  smoothed_point.SetX( actual_vertex->GetX() + difference_vector.GetX() );
  smoothed_point.SetY( actual_vertex->GetY() + difference_vector.GetY() );
  smoothed_point.SetZ( actual_vertex->GetZ() + difference_vector.GetZ() );

  return true;
}

