#include <smoothers/CBMDSSmoother.h>


//=====================================================================================================================
CBMDSSmoother::CBMDSSmoother( double factor1, double factor2, int iterations, int layers )
{
    dFactor1 = factor1;
    dFactor2 = factor2;
    iIterations = iterations;
    iLayers = layers;
}

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

//=====================================================================================================================
bool    CBMDSSmoother::smooth( CSmoothingMesh * mesh )
{

  vctl::MCVertex                           * actual_vertex = (mesh->GetVerticeS())->GetFirst();      // ukazatel na aktualni vrchol site
  std::vector<vctl::MCPoint3D>             point_vector;           // vektor bodu s novymi pozicemi vrcholu, zachovani poradi podle vrcholu
  std::vector<vctl::MCPoint3D>::iterator   point_vector_iter;      // iterator vektoru bodu

  mesh->disposeVertexInfo();

  // inicializace vektoru bodu novych pozic vrcholu
  point_vector.resize(mesh->GetVerticeS()->GetNumber());

  // cyklus opakovani vyhlazovani site
  for ( int smooth_iter = 0; smooth_iter < iIterations; smooth_iter++)
  {
    // generovani prumernych normal vrcholu site
    mesh->precomputeVerticesNormals();

    dFactor1 = dFactor1 * dFactor1;
    dFactor2 = dFactor2 * dFactor2;

    // inicializace iteratoru novych souradnic na zacatek
    point_vector_iter = point_vector.begin();
    // nastaveni ukazatele na prvni vrchol site a iteratoru vektoru
    actual_vertex = (mesh->GetVerticeS())->GetFirst();



    // cyklus vrcholu site pro vypocet jejich nove pozice
    while (actual_vertex != NULL)
    {
      // vypocet nove pozice pro aktualni vrchol podle okolnich tri
      if ( !this->smoothVertex( mesh, actual_vertex, *point_vector_iter ) )
      {
          dFactor1 = sqrt( dFactor1 );
          dFactor2 = sqrt( dFactor2 );
          return false;
      }
      // ziskani dalsiho vrcholu site a dalsiho bodu
      actual_vertex = actual_vertex->GetNext();
      ++point_vector_iter;
    }

    // nastaveni ukazatele na prvni vrchol site a iteratoru vektoru
    actual_vertex = (mesh->GetVerticeS())->GetFirst();
    point_vector_iter = point_vector.begin();

    // cyklus vrcholu site pro nastaveni vypoctene pozice
    while (actual_vertex != NULL)
    {
      // nastaveni vypoctene nove pozice vrcholu jako aktualni
      actual_vertex->SetPoint3D(*point_vector_iter);

      // ziskani dalsiho vrcholu site a dalsiho bodu
      actual_vertex = actual_vertex->GetNext();
      ++point_vector_iter;
    }

    dFactor1 = sqrt( dFactor1 );
    dFactor2 = sqrt( dFactor2 );
  }

  // zruseni normal vrcholu a tri site
  mesh->removeVerticesNormals();

  return true;
}

//=====================================================================================================================
bool    CBMDSSmoother::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
  vctl::MCVector3D                          actual_neighbour_vector;        // vektor mezi aktualnim a sousednim vrcholem
  vctl::MCVector3D                          * actual_vertex_normal;         // ukazatel na normalovy vektor aktualniho vrcholu
  double                                    actual_neighbour_distance;      // vzdalenost mezi aktualnim a sousednim vrcholem
  double                                    plane_neighbour_distance;       // vzdalenost mezi normalovou rovinou aktualniho vrcholu a sousednim vrcholem
  double                                    actual_vertex_weight = 0.0;     // vaha pro vypocet nove polohy vrcholu
  double                                    normalisation_weight = 0.0;     // celkova normalizacni vaha souradnic vyhlazeneho vrcholu
  double                                    weight_1, weight_2;             // gausovske vahy vzdalenosti na aktualnim vrcholu
  double                                    squared_smooth_factor_1 = dFactor1;
  double                                    squared_smooth_factor_2 = dFactor2;

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

  // ziskani normaly aktualniho vrcholu
  actual_vertex_normal = (vctl::MCVector3D *) actual_vertex->GetValuePtr();

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

  // cyklus okolnich vrcholu
  for (neighbour_vertex_iter = neighbour_vertex.begin(); neighbour_vertex_iter != neighbour_vertex.end(); ++neighbour_vertex_iter)
  {
    // vypocet vah pro vyhlazeni vrcholu
    actual_neighbour_vector.Make(*actual_vertex, *(*neighbour_vertex_iter));
    actual_neighbour_distance = actual_neighbour_vector.Length();
    plane_neighbour_distance = (*actual_vertex_normal) * actual_neighbour_vector;
    weight_1 = exp((actual_neighbour_distance * actual_neighbour_distance) / (-2.0 * squared_smooth_factor_1));
    weight_2 = exp((plane_neighbour_distance * plane_neighbour_distance) / (-2.0 * squared_smooth_factor_2));
    actual_vertex_weight += weight_1 * weight_2 * plane_neighbour_distance;
    normalisation_weight += weight_1 * weight_2;
  }

  // vypocet souradnic vyhlazenho vrcholu
  smoothed_point.SetX( actual_vertex->GetX() + (actual_vertex_normal->GetX() * (actual_vertex_weight / normalisation_weight)) );
  smoothed_point.SetY( actual_vertex->GetY() + (actual_vertex_normal->GetY() * (actual_vertex_weight / normalisation_weight)) );
  smoothed_point.SetZ( actual_vertex->GetZ() + (actual_vertex_normal->GetZ() * (actual_vertex_weight / normalisation_weight)) );

  return true;
}




