#include <smoothers/CHCSmoother.h>

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

//=====================================================================================================================
bool    CHCSmoother::smooth( CSmoothingMesh * mesh )
{  
  int smooth_cycle = this->iIterations;
  double smooth_factor_2 = this->dFactor2;
  double smooth_factor_1 = this->dFactor1;

  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>             start_point_vector;     // vektor bodu s pocatecnimi pozicemi vrcholu, zachovani poradi podle vrcholu
  std::vector<vctl::MCPoint3D>::iterator   point_vector_iter;      // iterator vektoru bodu
  std::vector<vctl::MCPoint3D>::iterator   start_point_vector_iter;// iterator vektoru starovnich souradnic bodu


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

  // inicializace iteratoru puvodnich souradnic na zacatek
  start_point_vector_iter = start_point_vector.begin();

  // cyklus vrcholu site pro ulozeni pocatecnich pozic vrcholu site
  while (actual_vertex != NULL)
  {
    // ulozeni pozice vrcholu do vektoru
    (*start_point_vector_iter).SetPoint3D(*actual_vertex);

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

  // cyklus opakovani vyhlazovani site
  for (int smooth_iter = 0; smooth_iter < smooth_cycle; smooth_iter++)
  {
    // inicializace iteratoru novych souradnic na zacatek
    point_vector_iter = point_vector.begin();
    // inicializace iteratoru puvodnich souradnic na zacatek
    start_point_vector_iter = start_point_vector.begin();
    // nastaveni ukazatele na prvni vrchol site
    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
      smoothVertexStep1(actual_vertex, *point_vector_iter, *start_point_vector_iter, smooth_factor_1);

      // ziskani dalsiho vrcholu site a dalsiho bodu
      actual_vertex = actual_vertex->GetNext();
      ++point_vector_iter;
      ++start_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;
    }


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

    // cyklus vrcholu site pro druhy krok vypoctu jejich nove pozice
    while (actual_vertex != NULL)
    {
      // vypocet nove pozice pro aktualni vrchol podle okolnich tri
      smoothVertexStep2(actual_vertex, *point_vector_iter, *start_point_vector_iter, smooth_factor_2);

      // ziskani dalsiho vrcholu site a dalsiho bodu
      actual_vertex = actual_vertex->GetNext();
      ++point_vector_iter;
      ++start_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;
    }
  }
  return true;
}

//=====================================================================================================================
bool     CHCSmoother::smoothVertexStep1(vctl::MCVertex * actual_vertex, vctl::MCPoint3D & smoothed_point, vctl::MCPoint3D & start_point, const double & smooth_factor)
{
  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


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

  // ziskani okolnich vrcholu pro aktualni vrchol  
  CSmoothingMesh::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
    dx += (*neighbour_vertex_iter)->GetX();
    dy += (*neighbour_vertex_iter)->GetY();
    dz += (*neighbour_vertex_iter)->GetZ();
  }

  // vypocet souradnic vyhlazeneho vrcholu
  dx = dx / neighbour_vertex.size();
  dy = dy / neighbour_vertex.size();
  dz = dz / neighbour_vertex.size();

  // vraceni pozice bodu o cast vektoru vuci jeho puvodni poloze
  smoothed_point.SetX( dx - (smooth_factor * start_point.GetX() + (1 - smooth_factor) * actual_vertex->GetX()) );
  smoothed_point.SetY( dy - (smooth_factor * start_point.GetY() + (1 - smooth_factor) * actual_vertex->GetY()) );
  smoothed_point.SetZ( dz - (smooth_factor * start_point.GetZ() + (1 - smooth_factor) * actual_vertex->GetZ()) );

  // ulozeni souradnice aktualniho bodu po Laplace vyhlazeni
  start_point.SetXYZ(dx, dy, dz);

  return true;
}

//=====================================================================================================================
bool     CHCSmoother::smoothVertexStep2(vctl::MCVertex * actual_vertex, vctl::MCPoint3D & smoothed_point, vctl::MCPoint3D & start_point, const double & smooth_factor)
{
  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


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

  // ziskani okolnich vrcholu pro aktualni vrchol
  CSmoothingMesh::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
    dx += (*neighbour_vertex_iter)->GetX();
    dy += (*neighbour_vertex_iter)->GetY();
    dz += (*neighbour_vertex_iter)->GetZ();
  }

  // vypocet souradnic vyhlazeneho vrcholu
  dx = (1.0 - smooth_factor) * dx / neighbour_vertex.size();
  dy = (1.0 - smooth_factor) * dy / neighbour_vertex.size();
  dz = (1.0 - smooth_factor) * dz / neighbour_vertex.size();

  // vypocet souradnic vyhlazeneho vrcholu
  smoothed_point.SetX( start_point.GetX() - (smooth_factor * actual_vertex->GetX() + dx) );
  smoothed_point.SetY( start_point.GetY() - (smooth_factor * actual_vertex->GetY() + dy) );
  smoothed_point.SetZ( start_point.GetZ() - (smooth_factor * actual_vertex->GetZ() + dz) );

  return true;
}


