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

  Program: 3D Slicer

  Portions (c) Copyright Brigham and Women's Hospital (BWH) All Rights Reserved.

  See COPYRIGHT.txt
  or http://www.slicer.org/copyright/copyright.txt for details.

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

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

// SkeletalRepresentationInitializer Logic includes
#include "vtkSlicerSkeletalRepresentationInitializerLogic.h"

#include "vtkSmartPointer.h"
#include "vtkPolyDataReader.h"
#include "vtkPolyData.h"
// MRML includes
#include <vtkMRMLScene.h>
#include <vtkMRMLModelNode.h>
#include <vtkMRMLModelDisplayNode.h>
#include <vtkMRMLDisplayNode.h>
#include <vtkMRMLMarkupsDisplayNode.h>
#include <vtkMRMLMarkupsFiducialNode.h>
#include <vtkMRMLMarkupsNode.h>
#include "vtkSlicerMarkupsLogic.h"

// VTK includes
#include <vtkIntArray.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkDoubleArray.h>
#include <vtkCenterOfMass.h>
#include <vtkObjectFactory.h>
#include <vtkCurvatures.h>
#include <vtkVector.h>
#include <vtkWindowedSincPolyDataFilter.h>
#include <vtkSmoothPolyDataFilter.h>
#include <vtkParametricEllipsoid.h>
#include <vtkParametricFunctionSource.h>
#include <vtkImplicitPolyDataDistance.h>
#include <vtkPolyDataNormals.h>

#include <vtkPoints.h>
#include <vtkLine.h>
#include <vtkQuad.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkDataArray.h>
#include <vtkDoubleArray.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkMassProperties.h>

// Eigen includes
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

// STD includes
#include <cassert>
#include <iostream>

// vtk system tools
#include <vtksys/SystemTools.hxx>

#include "vtkBackwardFlowLogic.h"
#include "qSlicerApplication.h"
#include <QString>

#define MAX_FILE_NAME  256
//----------------------------------------------------------------------------
vtkStandardNewMacro(vtkSlicerSkeletalRepresentationInitializerLogic);

//----------------------------------------------------------------------------
vtkSlicerSkeletalRepresentationInitializerLogic::vtkSlicerSkeletalRepresentationInitializerLogic()
{
}

//----------------------------------------------------------------------------
vtkSlicerSkeletalRepresentationInitializerLogic::~vtkSlicerSkeletalRepresentationInitializerLogic()
{
}

//----------------------------------------------------------------------------
void vtkSlicerSkeletalRepresentationInitializerLogic::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}

//---------------------------------------------------------------------------
void vtkSlicerSkeletalRepresentationInitializerLogic::SetMRMLSceneInternal(vtkMRMLScene * newScene)
{
  vtkNew<vtkIntArray> events;
  events->InsertNextValue(vtkMRMLScene::NodeAddedEvent);
  events->InsertNextValue(vtkMRMLScene::NodeRemovedEvent);
  events->InsertNextValue(vtkMRMLScene::EndBatchProcessEvent);
  this->SetAndObserveMRMLSceneEventsInternal(newScene, events.GetPointer());
}

//-----------------------------------------------------------------------------
void vtkSlicerSkeletalRepresentationInitializerLogic::RegisterNodes()
{
  assert(this->GetMRMLScene() != 0);
}

//---------------------------------------------------------------------------
void vtkSlicerSkeletalRepresentationInitializerLogic::UpdateFromMRMLScene()
{
  assert(this->GetMRMLScene() != 0);
}

//---------------------------------------------------------------------------
void vtkSlicerSkeletalRepresentationInitializerLogic
::OnMRMLSceneNodeAdded(vtkMRMLNode* vtkNotUsed(node))
{
}

//---------------------------------------------------------------------------
void vtkSlicerSkeletalRepresentationInitializerLogic
::OnMRMLSceneNodeRemoved(vtkMRMLNode* vtkNotUsed(node))
{
}

// flow surface in one step
// basic idea: When the user select a mesh file, make a copy of vtk file in the application path.
// In each step of flow, read in that copy, flow it and save it the same place with same name.
// TODO: cleanup the hard disk when the module exits
int vtkSlicerSkeletalRepresentationInitializerLogic::FlowSurfaceOneStep(double dt, double smooth_amount)
{
    std::cout << "flow one step : dt-" << dt << std::endl;
    std::cout << "flow one step : smooth amount-" << smooth_amount << std::endl;

    std::string tempFileName(this->GetApplicationLogic()->GetTemporaryPath());
    tempFileName += "/temp_output.vtk";

    vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(tempFileName.c_str());
    reader->Update();
    vtkSmartPointer<vtkPolyData> mesh = reader->GetOutput();
    if(mesh == NULL)
    {
        vtkErrorMacro("No mesh has read in this module. Please select input mesh file first.");
        return -1;
    }
    vtkSmartPointer<vtkMassProperties> mass_filter =
        vtkSmartPointer<vtkMassProperties>::New();
    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double original_volume = mass_filter->GetVolume();
//    std::cout << "Original Volume: " << original_volume << std::endl;
    vtkSmartPointer<vtkWindowedSincPolyDataFilter> smooth_filter =
        vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
    smooth_filter->SetPassBand(smooth_amount);
    smooth_filter->NonManifoldSmoothingOn();
    smooth_filter->NormalizeCoordinatesOn();
    smooth_filter->SetNumberOfIterations(20);
    smooth_filter->FeatureEdgeSmoothingOff();
    smooth_filter->BoundarySmoothingOff();
    smooth_filter->SetInputData(mesh);
    smooth_filter->Update();
    if(smooth_amount > 0) {
        mesh = smooth_filter->GetOutput();
    }

//    normal filter
    vtkSmartPointer<vtkPolyDataNormals> normal_filter =
        vtkSmartPointer<vtkPolyDataNormals>::New();
    normal_filter->SplittingOff();
    normal_filter->ComputeCellNormalsOff();
    normal_filter->ComputePointNormalsOn();
    normal_filter->SetInputData(mesh);
    normal_filter->Update();
    // curvature filter
    vtkSmartPointer<vtkCurvatures> curvature_filter =
        vtkSmartPointer<vtkCurvatures>::New();
    curvature_filter->SetCurvatureTypeToMean();
    curvature_filter->SetInputData(mesh);
    curvature_filter->Update();

    vtkSmartPointer<vtkDoubleArray> H =
        vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Mean_Curvature"));
    if(H == NULL) {
        vtkErrorMacro("error in getting mean curvature");
        return -1;
    }
    vtkDataArray* N = normal_filter->GetOutput()->GetPointData()->GetNormals();
    if(N == NULL) {
        vtkErrorMacro("error in getting normals");
        return -1;
    }

    // perform the flow
    vtkSmartPointer<vtkPoints> points = mesh->GetPoints();

    for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
        double p[3];
        points->GetPoint(i, p);
        double curr_N[3];
        N->GetTuple(i, curr_N);
        double curr_H = H->GetValue(i);

        for(int idx = 0; idx < 3; ++idx) {
            p[idx] -= dt * curr_H * curr_N[idx];
        }
        points->SetPoint(i, p);
    }
    points->Modified();

    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double curr_volume = mass_filter->GetVolume();
    for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
        double p[3];
        points->GetPoint(i, p);
        for(int j = 0; j < 3; ++j) {
            p[j] *= std::pow( original_volume / curr_volume , 1.0 / 3.0 );
        }
//        points->SetPoint(i, p);
    }
    points->Modified();

    // firstly get other intermediate result invisible
    HideNodesByNameByClass("curvature_flow_result","vtkMRMLModelNode");
    HideNodesByNameByClass("best_fitting_ellipsoid_polydata", "vtkMRMLModelNode");

    // then add this new intermediate result
    const std::string modelName("curvature_flow_result");
    AddModelNodeToScene(mesh, modelName.c_str(), true);

    vtkSmartPointer<vtkPolyDataWriter> writer =
        vtkSmartPointer<vtkPolyDataWriter>::New();
    writer->SetInputData(mesh);
    writer->SetFileName(tempFileName.c_str());
    writer->Update();

    // compute the fitting ellipsoid
    vtkSmartPointer<vtkCenterOfMass> centerMassFilter =
        vtkSmartPointer<vtkCenterOfMass>::New();
    centerMassFilter->SetInputData(mesh);
    centerMassFilter->SetUseScalarsAsWeights(false);
    centerMassFilter->Update();
    double center[3];
    centerMassFilter->GetCenter(center);

//    ShowFittingEllipsoid(points, curr_volume, center);
    return 0;
}
void vtkSlicerSkeletalRepresentationInitializerLogic::SetInputFileName(const std::string &filename)
{
    vtkSmartPointer<vtkPolyDataReader> reader =
        vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(filename.c_str());
    reader->Update();

    vtkSmartPointer<vtkPolyData> mesh;
    mesh = reader->GetOutput();
    // output the original mesh
    const std::string modelName("original");
    AddModelNodeToScene(mesh, modelName.c_str(), true, 0.88, 0.88, 0.88);

    // save
    const std::string tempDir(this->GetApplicationLogic()->GetTemporaryPath());
    std::string tempFileName;
    tempFileName = tempDir + "/initial_surface.vtk";

    vtkSmartPointer<vtkPolyDataWriter> writer =
        vtkSmartPointer<vtkPolyDataWriter>::New();
    writer->SetInputData(mesh);
    writer->SetFileName(tempFileName.c_str());
    writer->Update();
}

// flow surface to the end: either it's ellipsoidal enough or reach max_iter
int vtkSlicerSkeletalRepresentationInitializerLogic::FlowSurfaceMesh(const std::string &filename, double dt, double smooth_amount, int max_iter, int freq_output)
{
    vtkSmartPointer<vtkPolyDataReader> reader =
        vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(filename.c_str());
    reader->Update();

    vtkSmartPointer<vtkPolyData> mesh =
        vtkSmartPointer<vtkPolyData>::New();
    mesh = reader->GetOutput();

    vtkSmartPointer<vtkMassProperties> mass_filter =
        vtkSmartPointer<vtkMassProperties>::New();
    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double original_volume = mass_filter->GetVolume();
    // default parameters
    // double dt = 0.001;
    // double smooth_amount = 0.03;
    // int max_iter = 500;
    int iter = 0;
    double tolerance = 0.05;
    double q = 1.0;

    // create folder if not exist
    const std::string tempFolder(this->GetApplicationLogic()->GetTemporaryPath());
    std::string forwardFolder;
    forwardFolder = tempFolder + "/forward";
    std::cout << "forward folder" << forwardFolder << std::endl;
    if (!vtksys::SystemTools::FileExists(forwardFolder, false))
    {
      if (!vtksys::SystemTools::MakeDirectory(forwardFolder))
      {
              std::cout << "Failed to create folder : " << forwardFolder << std::endl;

      }
    }
    while(q > tolerance && iter < max_iter) {
        // smooth filter
        vtkSmartPointer<vtkWindowedSincPolyDataFilter> smooth_filter =
            vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
        smooth_filter->SetPassBand(smooth_amount);
        smooth_filter->NonManifoldSmoothingOn();
        smooth_filter->NormalizeCoordinatesOn();
        smooth_filter->SetNumberOfIterations(20);
        smooth_filter->FeatureEdgeSmoothingOff();
        smooth_filter->BoundarySmoothingOff();
        smooth_filter->SetInputData(mesh);
        smooth_filter->Update();
        if(smooth_amount > 0) {
            mesh = smooth_filter->GetOutput();
        }

        // normal filter
        vtkSmartPointer<vtkPolyDataNormals> normal_filter =
            vtkSmartPointer<vtkPolyDataNormals>::New();
        normal_filter->SplittingOff();
        normal_filter->ComputeCellNormalsOff();
        normal_filter->ComputePointNormalsOn();
        normal_filter->SetInputData(mesh);
        normal_filter->Update();
        // curvature filter
        vtkSmartPointer<vtkCurvatures> curvature_filter =
            vtkSmartPointer<vtkCurvatures>::New();
        curvature_filter->SetCurvatureTypeToMean();
        curvature_filter->SetInputData(mesh);
        curvature_filter->Update();

        vtkSmartPointer<vtkDoubleArray> H =
            vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Mean_Curvature"));
        if(H == NULL) {
            std::cerr << "error in getting mean curvature" << std::endl;
            return EXIT_FAILURE;
        }
        vtkDataArray* N = normal_filter->GetOutput()->GetPointData()->GetNormals();
        if(N == NULL) {
            std::cerr << "error in getting normals" << std::endl;
            return EXIT_FAILURE;
        }

        // perform the flow
        vtkSmartPointer<vtkPoints> points = mesh->GetPoints();
        for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
            double p[3];
            points->GetPoint(i, p);
            double curr_N[3];
            N->GetTuple(i, curr_N);
            double curr_H = H->GetValue(i);
            for(int idx = 0; idx < 3; ++idx) {
                p[idx] -= dt * curr_H * curr_N[idx];
            }
            points->SetPoint(i, p);
        }
        points->Modified();
        mass_filter->SetInputData(mesh);
        mass_filter->Update();
        double curr_volume = mass_filter->GetVolume();
        for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
            double p[3];
            points->GetPoint(i, p);
            for(int j = 0; j < 3; ++j) {
                p[j] *= std::pow( original_volume / curr_volume , 1.0 / 3.0 );
            }
//            points->SetPoint(i, p);
        }
        points->Modified();
        // save the result for the purpose of backward flow
        std::string fileName;
        fileName = forwardFolder + "/" + std::to_string(iter+1) + ".vtk";
        vtkSmartPointer<vtkPolyDataWriter> writer =
            vtkSmartPointer<vtkPolyDataWriter>::New();
        writer->SetInputData(mesh);
        writer->SetFileName(fileName.c_str());
        writer->Update();

        if((iter +1) % freq_output == 0)
        {
            const std::string modelName = "output" + std::to_string(iter+1);
            AddModelNodeToScene(mesh, modelName.c_str(), false);
            vtkSmartPointer<vtkCenterOfMass> centerMassFilter =
                vtkSmartPointer<vtkCenterOfMass>::New();
            centerMassFilter->SetInputData(mesh);
            centerMassFilter->SetUseScalarsAsWeights(false);
            centerMassFilter->Update();
            double center[3];
            centerMassFilter->GetCenter(center);
        }
        q -= 0.0001;
        iter++;
    }
    forwardCount = iter;
    double rx, ry, rz;
    ShowFittingEllipsoid(mesh, rx, ry, rz);

    GenerateSrepForEllipsoid(mesh, 5, 5, forwardCount);
    return 1;
}

void vtkSlicerSkeletalRepresentationInitializerLogic::AddModelNodeToScene(vtkPolyData* mesh, const char* modelName, bool isModelVisible, double r, double g, double b)
{
    std::cout << "AddModelNodeToScene: parameters:" << modelName << std::endl;
    vtkMRMLScene *scene = this->GetMRMLScene();
    if(!scene)
    {
        vtkErrorMacro(" Invalid scene");
        return;
    }

    // model node
    vtkSmartPointer<vtkMRMLModelNode> modelNode;
    modelNode = vtkSmartPointer<vtkMRMLModelNode>::New();
    modelNode->SetScene(scene);

    modelNode->SetName(modelName);
    modelNode->SetAndObservePolyData(mesh);

    // display node
    vtkSmartPointer<vtkMRMLModelDisplayNode> displayModelNode;

    displayModelNode = vtkSmartPointer<vtkMRMLModelDisplayNode>::New();
    if(displayModelNode == NULL)
    {
        vtkErrorMacro("displayModelNode is NULL");
        return;
    }
    displayModelNode->SetColor(r, g, b);
    displayModelNode->SetScene(scene);
    displayModelNode->SetLineWidth(2.0);
    displayModelNode->SetBackfaceCulling(0);
    displayModelNode->SetRepresentation(1);
    if(isModelVisible)
    {
        // make the 1st mesh after flow visible
        displayModelNode->SetVisibility(1);
    }
    else
    {
        displayModelNode->SetVisibility(0);
    }

    scene->AddNode(displayModelNode);
    modelNode->AddAndObserveDisplayNodeID(displayModelNode->GetID());

    scene->AddNode(modelNode);

}
void vtkSlicerSkeletalRepresentationInitializerLogic::ShowFittingEllipsoid(vtkPolyData* mesh, double &rx, double &ry, double &rz)
{
    vtkSmartPointer<vtkPoints> points = mesh->GetPoints();
    Eigen::MatrixXd point_matrix(points->GetNumberOfPoints(), 3);
    for(int i = 0; i < points->GetNumberOfPoints(); ++i)
    {
        double p[3];
        points->GetPoint(i, p);
        point_matrix.row(i) << p[0], p[1], p[2];
    }
    // compute best fitting ellipsoid
    // For now assume that the surface is centered and rotationally aligned
    // 1. compute the second moment after centering the data points
    Eigen::MatrixXd center = point_matrix.colwise().mean();
    Eigen::MatrixXd centered_point_mat = point_matrix - center.replicate(point_matrix.rows(), 1);
    Eigen::MatrixXd point_matrix_transposed = centered_point_mat.transpose();
    Eigen::Matrix3d second_moment = point_matrix_transposed * centered_point_mat;
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(second_moment);
    Eigen::VectorXd radii = es.eigenvalues();
    radii(0) = sqrt(radii(0));
    radii(1) = sqrt(radii(1));
    radii(2) = sqrt(radii(2));

    double ellipsoid_volume = 4 / 3.0 * vtkMath::Pi() * radii(0) * radii(1) * radii(2);
    vtkSmartPointer<vtkMassProperties> mass =
        vtkSmartPointer<vtkMassProperties>::New();
    mass->SetInputData(mesh);
    mass->Update();

    double volume_factor = pow(mass->GetVolume() / ellipsoid_volume, 1.0 / 3.0);
    radii(0) *= volume_factor;
    radii(1) *= volume_factor;
    radii(2) *= volume_factor;
    // obtain the best fitting ellipsoid from the second moment matrix
    vtkSmartPointer<vtkParametricEllipsoid> ellipsoid =
        vtkSmartPointer<vtkParametricEllipsoid>::New();
    ellipsoid->SetXRadius(radii(0));
    ellipsoid->SetYRadius(radii(1));
    ellipsoid->SetZRadius(radii(2));

    vtkSmartPointer<vtkParametricFunctionSource> parametric_function =
        vtkSmartPointer<vtkParametricFunctionSource>::New();
    parametric_function->SetParametricFunction(ellipsoid);
    parametric_function->SetUResolution(30);
    parametric_function->SetVResolution(30);
    parametric_function->Update();
    vtkSmartPointer<vtkPolyData> ellipsoid_polydata = parametric_function->GetOutput();

    using namespace Eigen;
    // Get ellipsoid points into the matrix
    MatrixXd ellipsoid_points_matrix(ellipsoid_polydata->GetNumberOfPoints(), 3);
    for(int i = 0; i < ellipsoid_polydata->GetNumberOfPoints(); ++i) {
        double p[3];
        ellipsoid_polydata->GetPoint(i,p);
        ellipsoid_points_matrix(i,0) = p[0];
        ellipsoid_points_matrix(i,1) = p[1];
        ellipsoid_points_matrix(i,2) = p[2];
    }
    MatrixXd rotation;
    rotation = es.eigenvectors(); // 3 by 3 rotation matrix

    // rotate the points
    MatrixXd rotated_ellipsoid_points = rotation * (ellipsoid_points_matrix.transpose());
    rotated_ellipsoid_points.transposeInPlace(); // n x 3
    // translate the points
    MatrixXd translated_points = rotated_ellipsoid_points + center.replicate(rotated_ellipsoid_points.rows(),1);

    // convert eigen matrix to vtk polydata
    vtkSmartPointer<vtkPolyData> best_fitting_ellipsoid_polydata =
        vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints> best_fitting_ellipsoid_points =
        vtkSmartPointer<vtkPoints>::New();
    for(int i = 0; i < translated_points.rows(); ++i) {
        double p[3] = {translated_points(i,0), translated_points(i,1), translated_points(i,2)};
        best_fitting_ellipsoid_points->InsertNextPoint(p);
    }
    best_fitting_ellipsoid_polydata->SetPoints(best_fitting_ellipsoid_points);
    best_fitting_ellipsoid_polydata->SetPolys(ellipsoid_polydata->GetPolys());
    best_fitting_ellipsoid_polydata->Modified();

    AddModelNodeToScene(best_fitting_ellipsoid_polydata, "best_fitting_ellipsoid", true, 1, 1, 0);
    rx = radii(2); ry = radii(1); rz = radii(0);

    // output to file
    const std::string tempFolder(this->GetApplicationLogic()->GetTemporaryPath());

    const std::string ellipsoidFile = tempFolder + "/ellipsoid.vtk";
    vtkSmartPointer<vtkPolyDataWriter> writer =
        vtkSmartPointer<vtkPolyDataWriter>::New();
    writer->SetInputData(best_fitting_ellipsoid_polydata);
    writer->SetFileName(ellipsoidFile.c_str());
    writer->Update();

}

const double ELLIPSE_SCALE = 0.9;
const double EPS = 0.0001;
void vtkSlicerSkeletalRepresentationInitializerLogic::GenerateSrepForEllipsoid(vtkPolyData *mesh,
                                                                              int nRows, int nCols, int totalNum)
{
    // create folder if not exist
    const std::string tempFolder(this->GetApplicationLogic()->GetTemporaryPath());
    const std::string modelFolder = tempFolder + "/model";
    std::cout << "s-reps folder" << modelFolder << std::endl;
    if (!vtksys::SystemTools::FileExists(modelFolder, false))
    {
      if (!vtksys::SystemTools::MakeDirectory(modelFolder))
      {
        std::cout << "Failed to create folder : " << modelFolder << std::endl;
      }
    }

    // copy surface of ellipsoid to model folder
    const std::string ellSurfaceFile = tempFolder + "/ellipsoid.vtk";
    const std::string newEllSurfaceFile = tempFolder + "/forward/" + std::to_string(totalNum + 1) + ".vtk";
    vtksys::SystemTools::CopyAFile(ellSurfaceFile, newEllSurfaceFile, true);

    using namespace Eigen;
    // the number of rows should be odd number
    double shift = 0.02; // shift fold curve off the inner spokes
    nRows = 5; nCols = 5; // TODO: accept input values from user interface

    // 1. derive the best fitting ellipsoid from the deformed mesh
    vtkSmartPointer<vtkPoints> points = mesh->GetPoints();
    MatrixXd point_matrix(points->GetNumberOfPoints(), 3);
    for(int i = 0; i < points->GetNumberOfPoints(); ++i)
    {
        double p[3];
        points->GetPoint(i, p);
        point_matrix.row(i) << p[0], p[1], p[2];
    }
    MatrixXd center = point_matrix.colwise().mean();
    MatrixXd centered_point_mat = point_matrix - center.replicate(point_matrix.rows(), 1);
    MatrixXd point_matrix_transposed = centered_point_mat.transpose();
    Matrix3d second_moment = point_matrix_transposed * centered_point_mat;
    SelfAdjointEigenSolver<Eigen::MatrixXd> es_obj(second_moment);
    VectorXd radii = es_obj.eigenvalues();

    double rz = sqrt(radii(0));
    double ry = sqrt(radii(1));
    double rx = sqrt(radii(2));
    double ellipsoid_volume = 4 / 3.0 * vtkMath::Pi() * rx * ry * rz;
    vtkSmartPointer<vtkMassProperties> mass =
        vtkSmartPointer<vtkMassProperties>::New();
    mass->SetInputData(mesh);
    mass->Update();

    double volume_factor = pow(mass->GetVolume() / ellipsoid_volume, 1.0 / 3.0);
    rz *= volume_factor;
    ry *= volume_factor;
    rx *= volume_factor;

    double mrx_o = (rx*rx-rz*rz)/rx;
    double mry_o = (ry*ry-rz*rz)/ry;
    double mrb = mry_o * ELLIPSE_SCALE;
    double mra = mrx_o * ELLIPSE_SCALE;

    // 2. compute the skeletal points
    int nCrestPoints = nRows*2 + (nCols-2)*2;
    double deltaTheta = 2*vtkMath::Pi()/nCrestPoints;
    MatrixXd skeletal_points_x(nRows, nCols);
    MatrixXd skeletal_points_y(nRows, nCols);
    //MatrixXd skeletal_points_z(nRows, nCols);
    int r = 0, c = 0;
    for(int i = 0; i < nCrestPoints; ++i)
    {
        double theta = vtkMath::Pi() - deltaTheta * floor(nRows/2) - deltaTheta*i;
        double x = mra * cos(theta);
        double y = mrb * sin(theta);

        // these crest points have no inward points (side or corner of the s-rep)
        skeletal_points_x(r, c) = x;
        skeletal_points_y(r, c) = y;
        //skeletal_points_z(r, c) = z;
        if(i < nCols - 1)
        {
            // top row of crest points
            c += 1;
        }
        else if(i < nCols - 1 + nRows - 1)
        {
            // right side col of crest points ( if the top-left point is the origin)
            r = r + 1;
        }
        else if(i < nCols - 1 + nRows - 1 + nCols - 1)
        {
            // bottom row of crest points
            c = c - 1;
        }
        else
        {
            // left side col of crest points
            r = r - 1;
        }
        if((i < nCols - 1 && i > 0) || (i > nCols + nRows - 2 && i < 2*nCols + nRows - 3))
        {
            // compute skeletal points inward
            double mx_ = (mra * mra - mrb * mrb) * cos(theta) / mra; // this is the middle line
            double my_ = .0;
            double dx_ = x - mx_;
            double dy_ = y - my_;
            int numSteps = floor(nRows/2); // steps from crest point to the skeletal point
            double stepSize = 1.0 / double(numSteps); // step size on the half side of srep
            for(int j = 0; j <= numSteps; ++j)
            {
                double tempX_ = mx_ + stepSize * j * dx_;
                double tempY_ = my_ + stepSize * j * dy_;
                if(i < nCols - 1)
                {
                    // step from medial to top at current iteration on the top line
                    int currR = numSteps - j;
                    skeletal_points_x(currR, c-1) = tempX_;
                    skeletal_points_y(currR, c-1) = tempY_;
                }
                else
                {
                    int currR = j + numSteps;
                    skeletal_points_x(currR, c+1) = tempX_;
                    skeletal_points_y(currR, c+1) = tempY_;
                }

            }

        }
    }

    // 3. compute the head points of spokes
    MatrixXd skeletal_points(nRows*nCols, 3);
    MatrixXd bdry_points_up(nRows*nCols, 3);
    MatrixXd bdry_points_down(nRows*nCols, 3);
    MatrixXd bdry_points_crest(nCrestPoints, 3);
    MatrixXd skeletal_points_crest(nCrestPoints, 3);
    int id_pt = 0; int id_crest = 0;
    MatrixXd shift_dir(nCrestPoints, 3); // shift direction for every crest point
    for(int i = 0; i < nRows; ++i)
    {
        for(int j = 0; j < nCols; ++j)
        {
            double mx = skeletal_points_x(i,j);
            double my = skeletal_points_y(i,j);
            double sB = my * mrx_o;
            double cB = mx * mry_o;
            double l = sqrt(sB*sB + cB*cB);
            double sB_n, cB_n;
            if(l == 0)
            {
                sB_n = sB;
                cB_n = cB;
            }
            else
            {
                sB_n = sB / l;
                cB_n = cB / l;
            }
            double cA = l / (mrx_o * mry_o);
            double sA = sqrt(1 - cA*cA);
            double sx = rx * cA * cB_n - mx;
            double sy = ry * cA * sB_n - my;
            double sz = rz * sA;

            double bx = (sx + mx);
            double by = (sy + my);
            double bz = (sz);

            skeletal_points.row(id_pt) << mx, my, 0.0;
            bdry_points_up.row(id_pt) << bx, by, bz;
            bdry_points_down.row(id_pt) << bx, by, -bz;
            id_pt++;
            // fold curve
            if(i == 0 || i == nRows - 1 || j == 0 || j == nCols - 1)
            {
                double cx = rx * cB_n - mx;
                double cy = ry * sB_n - my;
                double cz = 0;
                Vector3d v, v2, v3;
                v << cx, cy, cz;
                v2 << sx, sy, 0.0;
                double v_n = v.norm();
                v2.normalize(); // v2 is the unit vector pointing out to norm dir
                v3 = v_n * v2;
                double bx = (v3(0) + mx);
                double by = (v3(1) + my);
                double bz = v3(2);
                bdry_points_crest.row(id_crest) << bx, by, bz;
                skeletal_points_crest.row(id_crest) << mx, my, 0.0;
                shift_dir.row(id_crest) << v2(0), v2(1), v2(2);
                id_crest++;
            }
        }
    }

    // 4. transform the s-rep
    MatrixXd transpose_srep = skeletal_points.transpose(); // 3xn
    Matrix3d srep_secondMoment = transpose_srep * skeletal_points; // 3x3
    SelfAdjointEigenSolver<Eigen::MatrixXd> es_srep(srep_secondMoment);

    Matrix3d rotation;
    rotation = es_obj.eigenvectors(); // 3 by 3 rotation relative to deformed object
    Matrix3d rot_srep;
    rot_srep = es_srep.eigenvectors().transpose();
    rotation = rotation * rot_srep;

    // all skeletal points
    MatrixXd trans_srep = (rotation * transpose_srep).transpose();
    MatrixXd transformed_skeletal_points = trans_srep+
            center.replicate(trans_srep.rows(), 1);

    // up spoke head point on the bdry
    MatrixXd transpose_up_pdm = bdry_points_up.transpose();
    MatrixXd trans_up_pdm = (rotation * transpose_up_pdm).transpose();
    MatrixXd transformed_up_pdm =  trans_up_pdm +
            center.replicate(trans_up_pdm.rows(), 1);

    // down spoke head point on the bdry
    MatrixXd transpose_down_pdm = bdry_points_down.transpose();
    MatrixXd trans_down_pdm = (rotation * transpose_down_pdm).transpose();
    MatrixXd transformed_down_pdm = trans_down_pdm +
            center.replicate(trans_down_pdm.rows(), 1);

    // crest head point on the bdry
    MatrixXd transpose_crest_pdm = bdry_points_crest.transpose();
    MatrixXd trans_crest_pdm = (rotation * transpose_crest_pdm).transpose();
    MatrixXd transformed_crest_pdm = trans_crest_pdm + center.replicate(trans_crest_pdm.rows(), 1);

    // crest base point on the skeletal sheet
    MatrixXd transpose_crest_base = skeletal_points_crest.transpose();
    MatrixXd trans_crest_base = (rotation * transpose_crest_base).transpose();
    MatrixXd transformed_crest_base = trans_crest_base + center.replicate(trans_crest_base.rows(), 1);

    // 5. transfer points to polydata
    // srep_poly is supposed to form a mesh grid connecting skeletal points
    vtkSmartPointer<vtkPolyData>  srep_poly       = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints>    skeletal_sheet  = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> skeletal_mesh   = vtkSmartPointer<vtkCellArray>::New();

    vtkSmartPointer<vtkPolyData>  upSpokes_poly      = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints>    upSpokes_pts       = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> upSpokes_lines     = vtkSmartPointer<vtkCellArray>::New();

    vtkSmartPointer<vtkPolyData>  downSpokes_poly      = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints>    downSpokes_pts       = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> downSpokes_lines     = vtkSmartPointer<vtkCellArray>::New();

    // TODO:crest spokes should be a little off the inner spokes
    vtkSmartPointer<vtkPolyData>  crestSpokes_poly      = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints>    crestSpokes_pts       = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> crestSpokes_lines     = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPolyData> foldCurve_poly         = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints>    foldCurve_pts         = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> fold_curve            = vtkSmartPointer<vtkCellArray>::New();

    skeletal_sheet->SetDataTypeToDouble();
    upSpokes_pts->SetDataTypeToDouble();
    downSpokes_pts->SetDataTypeToDouble();
    crestSpokes_pts->SetDataTypeToDouble();
    foldCurve_pts->SetDataTypeToDouble();

    vtkSmartPointer<vtkDoubleArray> upSpokeLengths = vtkSmartPointer<vtkDoubleArray>::New();
    vtkSmartPointer<vtkDoubleArray> downSpokeLengths = vtkSmartPointer<vtkDoubleArray>::New();
    vtkSmartPointer<vtkDoubleArray> crestSpokeLengths = vtkSmartPointer<vtkDoubleArray>::New();
    upSpokeLengths->SetNumberOfComponents(1);
    downSpokeLengths->SetNumberOfComponents(1);
    crestSpokeLengths->SetNumberOfComponents(1);

    upSpokeLengths->SetName("spokeLength");
    downSpokeLengths->SetName("spokeLength");
    crestSpokeLengths->SetName("spokeLength");

    vtkSmartPointer<vtkDoubleArray> upSpokeDirs = vtkSmartPointer<vtkDoubleArray>::New();
    vtkSmartPointer<vtkDoubleArray> downSpokeDirs = vtkSmartPointer<vtkDoubleArray>::New();
    vtkSmartPointer<vtkDoubleArray> crestSpokeDirs = vtkSmartPointer<vtkDoubleArray>::New();

    upSpokeDirs->SetNumberOfComponents(3);
    downSpokeDirs->SetNumberOfComponents(3);
    crestSpokeDirs->SetNumberOfComponents(3);

    upSpokeDirs->SetName("spokeDirection");
    downSpokeDirs->SetName("spokeDirection");
    crestSpokeDirs->SetName("spokeDirection");

    for(int i = 0; i < nRows * nCols; ++i)
    {
        // skeletal points
        double mx = transformed_skeletal_points(i,0);
        double my = transformed_skeletal_points(i,1);
        double mz = transformed_skeletal_points(i,2);
        int id0 = upSpokes_pts->InsertNextPoint(mx,my, mz);

        double bx_up = transformed_up_pdm(i, 0);
        double by_up = transformed_up_pdm(i, 1);
        double bz_up = transformed_up_pdm(i, 2);
        int id1 = upSpokes_pts->InsertNextPoint(bx_up, by_up, bz_up);

        // spoke length and dir
        vtkVector3d upSpoke(bx_up-mx, by_up-my, bz_up-mz);
        double upSpokeLength = upSpoke.Normalize();
        upSpokeLengths->InsertNextTuple1(upSpokeLength);
        upSpokeDirs->InsertNextTuple3(upSpoke.GetX(), upSpoke.GetY(), upSpoke.GetZ());

        // form up spokes
        vtkSmartPointer<vtkLine> up_arrow = vtkSmartPointer<vtkLine>::New();
        up_arrow->GetPointIds()->SetId(0, id0);
        up_arrow->GetPointIds()->SetId(1, id1);
        upSpokes_lines->InsertNextCell(up_arrow);

        // form down spokes
        int id2 = downSpokes_pts->InsertNextPoint(mx, my, mz);
        double bx_down = transformed_down_pdm(i,0);
        double by_down = transformed_down_pdm(i,1);
        double bz_down = transformed_down_pdm(i,2);
        int id3 = downSpokes_pts->InsertNextPoint(bx_down,by_down,bz_down);

        // spoke length and dir
        vtkVector3d downSpoke(bx_down-mx, by_down-my, bz_down-mz);
        double downSpokeLength = downSpoke.Normalize();
        downSpokeLengths->InsertNextTuple1(downSpokeLength);
        downSpokeDirs->InsertNextTuple3(downSpoke.GetX(), downSpoke.GetY(), downSpoke.GetZ());

        vtkSmartPointer<vtkLine> down_arrow = vtkSmartPointer<vtkLine>::New();
        down_arrow->GetPointIds()->SetId(0, id2);
        down_arrow->GetPointIds()->SetId(1, id3);
        downSpokes_lines->InsertNextCell(down_arrow);

    }
    // display up spokes
    upSpokes_poly->SetPoints(upSpokes_pts);
    upSpokes_poly->SetLines(upSpokes_lines);

    upSpokes_poly->GetPointData()->AddArray(upSpokeDirs);
    upSpokes_poly->GetPointData()->SetActiveVectors("spokeDirection");
    upSpokes_poly->GetPointData()->AddArray(upSpokeLengths);
    upSpokes_poly->GetPointData()->SetActiveScalars("spokeLength");
    AddModelNodeToScene(upSpokes_poly, "up spokes for ellipsoid", true, 0, 1, 1);

    // write to file
    vtkSmartPointer<vtkPolyDataWriter> upSpokeWriter = vtkSmartPointer<vtkPolyDataWriter>::New();
    const std::string upFileName = modelFolder + "/up" + std::to_string(totalNum) + ".vtk";
    const std::string downFileName = modelFolder + "/down" + std::to_string(totalNum) + ".vtk";
    const std::string crestFileName = modelFolder + "/crest" + std::to_string(totalNum) + ".vtk";
    upSpokeWriter->SetFileName(upFileName.c_str());
    upSpokeWriter->SetInputData(upSpokes_poly);
    upSpokeWriter->Update();

    // display down spokes
    downSpokes_poly->SetPoints(downSpokes_pts);
    downSpokes_poly->SetLines(downSpokes_lines);

    downSpokes_poly->GetPointData()->AddArray(downSpokeDirs);
    downSpokes_poly->GetPointData()->SetActiveVectors("spokeDirection");
    downSpokes_poly->GetPointData()->AddArray(downSpokeLengths);
    downSpokes_poly->GetPointData()->SetActiveScalars("spokeLength");

    AddModelNodeToScene(downSpokes_poly, "down spokes for ellipsoid", true, 1, 0, 1);

    vtkSmartPointer<vtkPolyDataWriter> downSpokeWriter = vtkSmartPointer<vtkPolyDataWriter>::New();
    downSpokeWriter->SetFileName(downFileName.c_str());
    downSpokeWriter->SetInputData(downSpokes_poly);
    downSpokeWriter->Update();

    // deal with skeletal mesh
    for(int i = 0; i < nRows * nCols; ++i)
    {
        double mx = transformed_skeletal_points(i, 0);
        double my = transformed_skeletal_points(i, 1);
        double mz = transformed_skeletal_points(i, 2);
        int current_id = skeletal_sheet->InsertNextPoint(mx, my, mz);
        int current_row = floor(i / nRows);
        int current_col = i - current_row * nRows;
        if(current_col >= 0 && current_row >= 0
                && current_row < nRows-1 && current_col < nCols - 1)
        {
            vtkSmartPointer<vtkQuad> quad = vtkSmartPointer<vtkQuad>::New();
            quad->GetPointIds()->SetId(0, current_id);
            quad->GetPointIds()->SetId(1, current_id + nCols);
            quad->GetPointIds()->SetId(2, current_id + nCols + 1);
            quad->GetPointIds()->SetId(3, current_id + 1);
            skeletal_mesh->InsertNextCell(quad);
        }
    }
    srep_poly->SetPoints(skeletal_sheet);
    srep_poly->SetPolys(skeletal_mesh);
    AddModelNodeToScene(srep_poly, "skeletal mesh for ellipsoid", true, 0, 0, 0);

    const std::string meshFileName = modelFolder + "/mesh" + std::to_string(totalNum) + ".vtk";
    vtkSmartPointer<vtkPolyDataWriter> meshWriter = vtkSmartPointer<vtkPolyDataWriter>::New();
    meshWriter->SetFileName(meshFileName.c_str());
    meshWriter->SetInputData(srep_poly);
    meshWriter->Update();

    // deal with crest spokes
    for(int i = 0; i < nCrestPoints; ++i)
    {
        // tail point
        double cx_t = transformed_crest_base(i, 0);
        double cy_t = transformed_crest_base(i, 1);
        double cz_t = transformed_crest_base(i, 2);
        // head point (_b means boundary)
        double cx_b = transformed_crest_pdm(i, 0);
        double cy_b = transformed_crest_pdm(i, 1);
        double cz_b = transformed_crest_pdm(i, 2);

        if(shift > 0)
        {
            double shift_x = (cx_b - cx_t) * shift;
            double shift_y = (cy_b - cy_t) * shift;
            double shift_z = (cz_b - cz_t) * shift;

            cx_t += shift_x;
            cy_t += shift_y;
            cz_t += shift_z;
        }

        int id0 = crestSpokes_pts->InsertNextPoint(cx_t, cy_t, cz_t);
        int id1 = crestSpokes_pts->InsertNextPoint(cx_b, cy_b, cz_b);

        vtkSmartPointer<vtkLine> crest_arrow = vtkSmartPointer<vtkLine>::New();
        crest_arrow->GetPointIds()->SetId(0, id0);
        crest_arrow->GetPointIds()->SetId(1, id1);
        crestSpokes_lines->InsertNextCell(crest_arrow);

        vtkVector3d crestSpoke(cx_b-cx_t, cy_b-cy_t, cz_b-cz_t);
        double crestSpokeLength = crestSpoke.Normalize();

        crestSpokeLengths->InsertNextTuple1(crestSpokeLength);
        crestSpokeDirs->InsertNextTuple3(crestSpoke.GetX(), crestSpoke.GetY(), crestSpoke.GetZ());
    }
    crestSpokes_poly->SetPoints(crestSpokes_pts);
    crestSpokes_poly->SetLines(crestSpokes_lines);

    crestSpokes_poly->GetPointData()->AddArray(crestSpokeDirs);
    crestSpokes_poly->GetPointData()->SetActiveVectors("spokeDirection");
    crestSpokes_poly->GetPointData()->AddArray(crestSpokeLengths);
    crestSpokes_poly->GetPointData()->SetActiveScalars("spokeLength");

    AddModelNodeToScene(crestSpokes_poly, "crest spokes for ellipsoid", true, 1, 0, 0);
    vtkSmartPointer<vtkPolyDataWriter> crestSpokeWriter = vtkSmartPointer<vtkPolyDataWriter>::New();
    crestSpokeWriter->SetFileName(crestFileName.c_str());
    crestSpokeWriter->SetInputData(crestSpokes_poly);
    crestSpokeWriter->Update();

    // deal with fold curve
    for(int i = 0; i < nCrestPoints; ++i)
    {
        double cx_t = transformed_crest_base(i, 0);
        double cy_t = transformed_crest_base(i, 1);
        double cz_t = transformed_crest_base(i, 2);
        double cx_b = transformed_crest_pdm(i, 0);
        double cy_b = transformed_crest_pdm(i, 1);
        double cz_b = transformed_crest_pdm(i, 2);

        if(shift > 0)
        {
            double shift_x = (cx_b - cx_t) * shift;
            double shift_y = (cy_b - cy_t) * shift;
            double shift_z = (cz_b - cz_t) * shift;

            cx_t += shift_x;
            cy_t += shift_y;
            cz_t += shift_z;
        }
        int id0 = foldCurve_pts->InsertNextPoint(cx_t, cy_t, cz_t);

        if(id0 > 0 && i < nCols)
        {
            // first row
            vtkSmartPointer<vtkLine> fold_seg = vtkSmartPointer<vtkLine>::New();
            fold_seg->GetPointIds()->SetId(0, id0-1);
            fold_seg->GetPointIds()->SetId(1, id0);
            fold_curve->InsertNextCell(fold_seg);
        }

        if(i > nCols && i < nCols + 2*(nRows-2) + 1 && (i-nCols) % 2 == 1)
        {
            // right side of crest
            vtkSmartPointer<vtkLine> fold_seg = vtkSmartPointer<vtkLine>::New();
            fold_seg->GetPointIds()->SetId(0, id0-2);
            fold_seg->GetPointIds()->SetId(1, id0);
            fold_curve->InsertNextCell(fold_seg);
        }
        if(i > nCols && i < nCols + 2*(nRows-2) + 1 && (i-nCols) % 2 == 0)
        {
            // part of left side
            vtkSmartPointer<vtkLine> fold_seg = vtkSmartPointer<vtkLine>::New();
            fold_seg->GetPointIds()->SetId(0, id0-2);
            fold_seg->GetPointIds()->SetId(1, id0);
            fold_curve->InsertNextCell(fold_seg);
        }

        if(i == nCols)
        {
            // remaining part of left side
            vtkSmartPointer<vtkLine> fold_seg = vtkSmartPointer<vtkLine>::New();
            fold_seg->GetPointIds()->SetId(0, 0);
            fold_seg->GetPointIds()->SetId(1, id0);
            fold_curve->InsertNextCell(fold_seg);
        }
        if(i > nCols + 2*(nRows-2))
        {
            //bottom side
            vtkSmartPointer<vtkLine> fold_seg = vtkSmartPointer<vtkLine>::New();
            fold_seg->GetPointIds()->SetId(0, id0-1);
            fold_seg->GetPointIds()->SetId(1, id0);
            fold_curve->InsertNextCell(fold_seg);
        }
        if(i == nCrestPoints - 1)
        {
            // bottome right
            vtkSmartPointer<vtkLine> fold_seg = vtkSmartPointer<vtkLine>::New();
            fold_seg->GetPointIds()->SetId(0, id0-nCols);
            fold_seg->GetPointIds()->SetId(1, id0);
            fold_curve->InsertNextCell(fold_seg);
        }
    }
    foldCurve_poly->SetPoints(foldCurve_pts);
    foldCurve_poly->SetLines(fold_curve);
    AddModelNodeToScene(foldCurve_poly, "fold curve of ellipsoid", true, 1, 1, 0);

    const std::string curveFileName = modelFolder + "/curve" + std::to_string(totalNum) + ".vtk";
    vtkSmartPointer<vtkPolyDataWriter> curveWriter = vtkSmartPointer<vtkPolyDataWriter>::New();
    curveWriter->SetFileName(curveFileName.c_str());
    curveWriter->SetInputData(foldCurve_poly);
    curveWriter->Update();
}

void vtkSlicerSkeletalRepresentationInitializerLogic::HideNodesByNameByClass(const std::string & nodeName, const std::string &className)
{
    std::cout << "node name:" << nodeName << std::endl;
    std::cout << "class name:" << className << std::endl;
    std::vector<vtkMRMLNode*> vectModelNodes;
    vtkSmartPointer<vtkCollection> modelNodes = this->GetMRMLScene()->GetNodesByClassByName(className.c_str(), nodeName.c_str());
    modelNodes->InitTraversal();
    for(int i = 0; i < modelNodes->GetNumberOfItems(); i++)
    {
        vtkSmartPointer<vtkMRMLModelNode> thisModelNode = vtkMRMLModelNode::SafeDownCast(modelNodes->GetNextItemAsObject());
        vtkSmartPointer<vtkMRMLModelDisplayNode> displayNode;
        displayNode = thisModelNode->GetModelDisplayNode();
        if(displayNode == NULL)
        {
            continue;
        }

        displayNode->SetVisibility(0);

    }

}

void vtkSlicerSkeletalRepresentationInitializerLogic::HideNodesByClass(const std::string &className)
{
    std::cout << "Hide node class name:" << className << std::endl;
    std::vector<vtkMRMLNode*> vectModelNodes;
    vtkSmartPointer<vtkCollection> modelNodes = this->GetMRMLScene()->GetNodesByClass(className.c_str());
    modelNodes->InitTraversal();
    for(int i = 0; i < modelNodes->GetNumberOfItems(); i++)
    {
        vtkSmartPointer<vtkMRMLModelNode> thisModelNode = vtkMRMLModelNode::SafeDownCast(modelNodes->GetNextItemAsObject());
        vtkSmartPointer<vtkMRMLModelDisplayNode> displayNode;
        displayNode = thisModelNode->GetModelDisplayNode();
        if(displayNode == NULL)
        {
            continue;
        }

        displayNode->SetVisibility(0);

    }

}

int vtkSlicerSkeletalRepresentationInitializerLogic::InklingFlow(const std::string &filename, double dt, double smooth_amount, int max_iter, int freq_output, double /*threshold*/)
{
    //std::cout << threshold << std::endl;
    std::cout << filename << std::endl;
    std::cout << dt << std::endl;
    std::cout << smooth_amount << std::endl;
    std::cout << max_iter << std::endl;
    std::cout << freq_output << std::endl;

    std::string tempFileName(this->GetApplicationLogic()->GetTemporaryPath());
    tempFileName += "/temp_output.vtk";

    vtkSmartPointer<vtkPolyDataReader> reader =
        vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(tempFileName.c_str());
    reader->Update();

    vtkSmartPointer<vtkPolyData> mesh =
        vtkSmartPointer<vtkPolyData>::New();
    mesh = reader->GetOutput();

    vtkSmartPointer<vtkMassProperties> mass_filter =
        vtkSmartPointer<vtkMassProperties>::New();
    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double original_volume = mass_filter->GetVolume();

    int iter = 0;
    double tolerance = 0.05;
    double q = 1.0;

    while(q > tolerance && iter < max_iter)
    {
        // smooth filter
        vtkSmartPointer<vtkWindowedSincPolyDataFilter> smooth_filter =
            vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
        smooth_filter->SetPassBand(smooth_amount);
        smooth_filter->NonManifoldSmoothingOn();
        smooth_filter->NormalizeCoordinatesOn();
        smooth_filter->SetNumberOfIterations(20);
        smooth_filter->FeatureEdgeSmoothingOff();
        smooth_filter->BoundarySmoothingOff();
        smooth_filter->SetInputData(mesh);
        smooth_filter->Update();
        if(smooth_amount > 0) {
            mesh = smooth_filter->GetOutput();
        }

        // normal filter
        vtkSmartPointer<vtkPolyDataNormals> normal_filter =
            vtkSmartPointer<vtkPolyDataNormals>::New();
        normal_filter->SplittingOff();
        normal_filter->ComputeCellNormalsOff();
        normal_filter->ComputePointNormalsOn();
        normal_filter->SetInputData(mesh);
        normal_filter->Update();
        vtkDataArray* N = normal_filter->GetOutput()->GetPointData()->GetNormals();
        if(N == NULL) {
            std::cerr << "error in getting normals" << std::endl;
            return EXIT_FAILURE;
        }

        // mean curvature filter
        vtkSmartPointer<vtkCurvatures> curvature_filter =
            vtkSmartPointer<vtkCurvatures>::New();
        curvature_filter->SetCurvatureTypeToMean();
        curvature_filter->SetInputData(mesh);
        curvature_filter->Update();

        vtkSmartPointer<vtkDoubleArray> H =
            vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Mean_Curvature"));

        if(H == NULL) {
            std::cerr << "error in getting mean curvature" << std::endl;
            return EXIT_FAILURE;
        }

        curvature_filter->SetCurvatureTypeToGaussian();
        curvature_filter->Update();
        vtkSmartPointer<vtkDoubleArray> K =
            vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Gauss_Curvature"));

        if(K == NULL) {
            std::cerr << "error in getting Gaussian curvature" << std::endl;
            return EXIT_FAILURE;
        }

        curvature_filter->SetCurvatureTypeToMaximum();
        curvature_filter->Update();
        vtkSmartPointer<vtkDoubleArray> MC =
            vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Maximum_Curvature"));

        if(MC == NULL) {
            std::cerr << "error in getting max curvature" << std::endl;
            return EXIT_FAILURE;
        }

        curvature_filter->SetCurvatureTypeToMinimum();
        curvature_filter->Update();
        vtkSmartPointer<vtkDoubleArray> MinC =
            vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Minimum_Curvature"));
        if(MinC == NULL)
        {
            std::cout << "error in getting min curvature" << std::endl;
            return -1;
        }

        // perform the flow
        vtkSmartPointer<vtkPoints> points = mesh->GetPoints();
        double maxVal = -10000.0;
        double maxIndex = -1;
        for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
            double p[3];
            points->GetPoint(i, p);
            double curr_N[3];
            N->GetTuple(i, curr_N);
            double curr_H = H->GetValue(i);
            //double curr_K = K->GetValue(i);
            double curr_max = MC->GetValue(i);
            double curr_min = MinC->GetValue(i);

            double delta = 0.0;
            double diffK = curr_max - curr_min;
            delta = dt * curr_H; // * 1 / (diffK * diffK);
            diffK = abs(diffK);

            if(diffK > maxVal)
            {
                maxVal = diffK;
                maxIndex = i;
            }
            if(curr_max >= 0 && curr_min >= 0)
            {
                delta = dt * curr_max;
            }
            else if(curr_max < 0 && curr_min < 0)
            {
                delta = dt * curr_min;
            }
            else
            {
                delta = dt * curr_H;
            }
            for(int idx = 0; idx < 3; ++idx)
            {
                p[idx] -= delta * curr_N[idx]; //dt * curr_H * curr_N[idx];
            }
            points->SetPoint(i, p);
        }
        points->Modified();

        mass_filter->SetInputData(mesh);
        mass_filter->Update();
        double curr_volume = mass_filter->GetVolume();

        for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
            double p[3];
            points->GetPoint(i, p);
            for(int j = 0; j < 3; ++j) {
                p[j] *= std::pow( original_volume / curr_volume , 1.0 / 3.0 );
            }
        }
        points->Modified();

        double testRender[3];
        points->GetPoint(maxIndex, testRender);
//        AddPointToScene(testRender[0], testRender[1], testRender[2], 13); // sphere3D

        HideNodesByNameByClass("output_inkling","vtkMRMLModelNode");

        // then add this new intermediate result

//        vtkSmartPointer<vtkPolyDataWriter> writer =
//            vtkSmartPointer<vtkPolyDataWriter>::New();
//        writer->SetInputData(mesh);
//        writer->SetFileName("temp_output.vtk");
//        writer->Update();

//        char modelName[128];
//        sprintf(modelName, "output_inkling");
//        AddModelNodeToScene(mesh, modelName, true);
         if((iter +1) % freq_output == 0)
         {
             std::string modelName("output_inkling");
             modelName += std::to_string(iter+1);
             AddModelNodeToScene(mesh, modelName.c_str(), false);
         }

        q -= 0.0001;
        iter++;
    }

    return 1;

}

void vtkSlicerSkeletalRepresentationInitializerLogic::AddPointToScene(double x, double y, double z, int glyphType, double r, double g, double b)
{
    std::cout << "AddPointToScene: parameters:" << x << ", y:" << y << ", z:" << z << std::endl;
    vtkMRMLScene *scene = this->GetMRMLScene();
    if(!scene)
    {
        vtkErrorMacro(" Invalid scene");
        return;
    }

    // node which controls display properties
    vtkSmartPointer<vtkMRMLMarkupsDisplayNode> displayNode;
    displayNode = vtkSmartPointer<vtkMRMLMarkupsDisplayNode>::New();
//    this->SetDisplayNodeToDefaults(displayNode);
    displayNode->SetGlyphScale(1.10);
    displayNode->SetTextScale(3.40);
    displayNode->SetSelectedColor(r, g, b);

    displayNode->SetGlyphType(glyphType); // 13: sphere3D
    scene->AddNode(displayNode);

    // model node
    vtkSmartPointer<vtkMRMLMarkupsFiducialNode> fidNode;

    fidNode = vtkSmartPointer<vtkMRMLMarkupsFiducialNode>::New();
    if(fidNode == NULL)
    {
        vtkErrorMacro("fidNode is NULL");
        return;
    }
    fidNode->SetAndObserveDisplayNodeID(displayNode->GetID());
    fidNode->SetLocked(true);
    fidNode->SetName("Hi");
    scene->AddNode(fidNode);

    fidNode->AddFiducial(x, y, z);

}

// compute and apply tps transformation matrix
void vtkSlicerSkeletalRepresentationInitializerLogic::ComputePairwiseTps(int totalNum)
{

    typedef itkThinPlateSplineExtended TransformType;
    typedef itk::Point< CoordinateRepType, 3 > PointType;
    typedef TransformType::PointSetType PointSetType;
    typedef PointSetType::PointIdentifier PointIdType;

    // create folder if not exist

    const std::string tempFolder(this->GetApplicationLogic()->GetTemporaryPath());
    const std::string backwardFolder = tempFolder + "/backward";
    std::cout << "backward folder" << backwardFolder << std::endl;
    if (!vtksys::SystemTools::FileExists(backwardFolder, false))
    {
      if (!vtksys::SystemTools::MakeDirectory(backwardFolder))
      {
            std::cout << "Failed to create folder : " << backwardFolder << std::endl;

      }
    }

    // compute transformation matrix by current surface and backward surface

    vtkSmartPointer<vtkPolyDataReader> sourceSurfaceReader = vtkSmartPointer<vtkPolyDataReader>::New();
    vtkSmartPointer<vtkPolyDataReader> targetSurfaceReader = vtkSmartPointer<vtkPolyDataReader>::New();

    std::cout << "Computing pairwise transformation matrix via TPS for " << totalNum << " cases..." << std::endl;
    for(int stepNum = totalNum; stepNum > 1; --stepNum)
    {
        const std::string inputMeshFile = tempFolder + "/forward/" + std::to_string(stepNum) + ".vtk";
        const std::string nextMeshFile = tempFolder + "/forward/" + std::to_string(stepNum - 1) + ".vtk";
        sourceSurfaceReader->SetFileName(inputMeshFile.c_str());
        sourceSurfaceReader->Update();

        // current surface mesh
        vtkSmartPointer<vtkPolyData> polyData_source = sourceSurfaceReader->GetOutput();

        targetSurfaceReader->SetFileName(nextMeshFile.c_str());
        targetSurfaceReader->Update();
        // next surface mesh which back flow to
        vtkSmartPointer<vtkPolyData> polyData_target = targetSurfaceReader->GetOutput();

        PointSetType::Pointer sourceLandMarks = PointSetType::New();
        PointSetType::Pointer targetLandMarks = PointSetType::New();
        PointType p1; PointType p2; // same as double p1[3];
        PointSetType::PointsContainer::Pointer sourceLandMarkContainer
                = sourceLandMarks->GetPoints();
        PointSetType::PointsContainer::Pointer targetLandMarkContainer
                = targetLandMarks->GetPoints();

        PointIdType id_s = itk::NumericTraits< PointIdType >::Zero;
        PointIdType id_t = itk::NumericTraits< PointIdType >::Zero;

        // Read in the source points set
        for(unsigned int i = 0; i < polyData_source->GetNumberOfPoints(); i += 10){
            double p[3];
            polyData_source->GetPoint(i,p);
            p1[0] = p[0];
            p1[1] = p[1];
            p1[2] = p[2];
            sourceLandMarkContainer->InsertElement(id_s, p1);
            id_s++;
        }

        // Read in the target points set
        for(unsigned int i = 0; i < polyData_target->GetNumberOfPoints(); i += 10){
            double p[3];
            polyData_target->GetPoint(i,p);
            p2[0] = p[0];
            p2[1] = p[1];
            p2[2] = p[2];
            targetLandMarkContainer->InsertElement(id_t, p2);
            id_t++;
        }

        TransformType::Pointer tps = TransformType::New();
        tps->SetSourceLandmarks(sourceLandMarks);
        tps->SetTargetLandmarks(targetLandMarks);
        tps->ComputeWMatrix();

        // Apply tps on the srep in future (from the ellipsoid)
        // srep of totalNum == srep of ellipsoid
//        std::cout << "Applying transformation matrix on from step:" << stepNum << " to " << stepNum-1 << std::endl;
        std::string upFileName = tempFolder + "/model/up" + std::to_string(stepNum) + ".vtk";
        std::string downFileName = tempFolder + "/model/down" + std::to_string(stepNum) + ".vtk";
        std::string crestFileName = tempFolder + "/model/crest" + std::to_string(stepNum) + ".vtk";
        // read source srep
        vtkSmartPointer<vtkPolyDataReader> upSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
        upSpokeReader->SetFileName(upFileName.c_str());
        upSpokeReader->Update();
        vtkSmartPointer<vtkPolyData> upSpokes = upSpokeReader->GetOutput();

        vtkSmartPointer<vtkPolyDataReader> downSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
        downSpokeReader->SetFileName(downFileName.c_str());
        downSpokeReader->Update();
        vtkSmartPointer<vtkPolyData> downSpokes = downSpokeReader->GetOutput();

        vtkSmartPointer<vtkPolyDataReader> crestSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
        crestSpokeReader->SetFileName(crestFileName.c_str());
        crestSpokeReader->Update();
        vtkSmartPointer<vtkPolyData> crestSpokes = crestSpokeReader->GetOutput();

        upFileName = tempFolder + "/model/up" + std::to_string(stepNum - 1) + ".vtk";
        std::string upOutputFile(upFileName.c_str());
        TransformNOutput(tps, upSpokes, upOutputFile);
        // display upspokes

        downFileName = tempFolder + "/model/down" + std::to_string(stepNum - 1) + ".vtk";
        std::string downOutputFile(downFileName.c_str());
        TransformNOutput(tps, downSpokes, downOutputFile);

        crestFileName = tempFolder + "/model/crest" + std::to_string(stepNum - 1) + ".vtk";
        std::string crestOutputFile(crestFileName.c_str());
        TransformNOutput(tps, crestSpokes, crestOutputFile);

        // Apply transformation matrix on the skeletal sheet and fold curve
        std::string meshFileName(tempFolder);
        meshFileName = tempFolder + "/model/mesh" + std::to_string(stepNum) + ".vtk";// Transform input
        vtkSmartPointer<vtkPolyDataReader> meshReader = vtkSmartPointer<vtkPolyDataReader>::New();
        meshReader->SetFileName(meshFileName.c_str());
        meshReader->Update();
        vtkSmartPointer<vtkPolyData> meshPoly = meshReader->GetOutput();
        meshFileName = tempFolder + "/model/mesh" + std::to_string(stepNum-1) + ".vtk";// output
        TransformPoints(tps, meshPoly, meshFileName);

        std::string curveFileName(tempFolder);
        curveFileName = tempFolder + "/model/curve" + std::to_string(stepNum) + ".vtk"; // Transform input
        vtkSmartPointer<vtkPolyDataReader> curveReader = vtkSmartPointer<vtkPolyDataReader>::New();
        curveReader->SetFileName(curveFileName.c_str());
        curveReader->Update();
        vtkSmartPointer<vtkPolyData> curvePoly = curveReader->GetOutput();
        curveFileName = tempFolder + "/model/curve" + std::to_string(stepNum-1) + ".vtk"; // output
        TransformPoints(tps, curvePoly, curveFileName);
    }
}

void vtkSlicerSkeletalRepresentationInitializerLogic::BackwardFlow(int totalNum)
{

    // 1. compute pairwise TPS
    ComputePairwiseTps(totalNum);
    std::cout << "Finished computing transformation matrix." << std::endl;

    std::cout << "Finished applying transformation matrix." << std::endl;

    // 2. display the srep for the initial object
    DisplayResultSrep();
}

int vtkSlicerSkeletalRepresentationInitializerLogic::ApplyTps(int totalNum)
{

    std::cout << "Applying transformation matrix to s-reps..." << std::endl;
    const std::string tempFolder(this->GetApplicationLogic()->GetTemporaryPath());
    typedef double CoordinateRepType;
    typedef itkThinPlateSplineExtended TransformType;
    typedef itk::Point< CoordinateRepType, 3 > PointType;
    typedef TransformType::PointSetType PointSetType;
    typedef PointSetType::PointIdentifier PointIdType;

    for(int stepNum = totalNum; stepNum > 0; --stepNum)
    {
        const std::string transformFileName = tempFolder + "/backward/" + std::to_string(stepNum) + ".txt";
        ifstream inFile;
        inFile.open(transformFileName);
        if(!inFile) {
            std::cerr << "Unable to open the file: " << transformFileName << std::endl;
            return -1;
        }

        itkThinPlateSplineExtended::DMatrixType D;
        itkThinPlateSplineExtended::AMatrixType A;
        itkThinPlateSplineExtended::BMatrixType B;
        // first read in the size
        std::string buffer;
        std::getline(inFile,buffer);
        std::istringstream ss1(buffer);
        int nRows = 0;
        int nCols = 0;
        char tmp;
        ss1 >> nRows >> tmp >> nCols;
        D.set_size(nRows,nCols);
        for(int i = 0; i < nRows; i++) {
            for(int j = 0; j < nCols; j++) {
                std::string buffer2;
                std::getline(inFile, buffer2, ',');
                double entry = atof(buffer2.c_str());
                D(i,j) = entry;
            }
        }
        buffer.clear();
        std::getline(inFile, buffer);
        std::getline(inFile, buffer);
        for(unsigned int i = 0; i < A.rows(); i++) {
            for(unsigned int j = 0; j < A.cols(); j++) {
                std::string buffer2;
                std::getline(inFile, buffer2,',');
                double entry = atof(buffer2.c_str());
                A(i,j) = entry;
            }
        }
        buffer.clear();
        std::getline(inFile, buffer);
        std::getline(inFile, buffer);
        for(unsigned int i = 0; i < B.size(); i++) {
            std::string buffer2;
            std::getline(inFile, buffer2, ',');
            double entry = atof(buffer2.c_str());
            B(i) = entry;
        }
        TransformType::Pointer tps = TransformType::New();
        tps->setDMatrix(D);
        tps->setAMatrix(A);
        tps->setBVector(B);

        PointSetType::Pointer sourceLandMarks = PointSetType::New();
        PointSetType::PointsContainer::Pointer sourceLandMarkContainer = sourceLandMarks->GetPoints();
        vtkSmartPointer<vtkPolyDataReader> reader_source = vtkSmartPointer<vtkPolyDataReader>::New();
        vtkSmartPointer<vtkPolyData> polyData_source = vtkSmartPointer<vtkPolyData>::New();

        const std::string sourceLandMarkFileName = tempFolder + "/forward/" + std::to_string(stepNum + 1) + ".vtk";
        reader_source->SetFileName(sourceLandMarkFileName.c_str());
        reader_source->Update();
        polyData_source = reader_source->GetOutput();
        PointIdType id_s = itk::NumericTraits< PointIdType >::Zero;
        PointType p1;
        // Read in the source points set
        for(unsigned int i = 0; i < polyData_source->GetNumberOfPoints(); i += 10){
            double p[3];
            polyData_source->GetPoint(i,p);
            // This is identical to:
            // polydata->GetPoints()->GetPoint(i,p);
            p1[0] = p[0];
            p1[1] = p[1];
            p1[2] = p[2];
            sourceLandMarkContainer->InsertElement(id_s, p1);
            id_s++;
        }
        tps->SetSourceLandmarks(sourceLandMarks);

        std::string upFileName(tempFolder);
        std::string downFileName(tempFolder);
        std::string crestFileName(tempFolder);
        upFileName = upFileName + "/model/up" + std::to_string(stepNum + 1) + ".vtk";
        downFileName = downFileName + "/model/down" + std::to_string(stepNum + 1) + ".vtk";
        crestFileName = crestFileName + "/model/crest" + std::to_string(stepNum + 1) + ".vtk";

        // read source srep
        vtkSmartPointer<vtkPolyDataReader> upSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
        upSpokeReader->SetFileName(upFileName.c_str());
        upSpokeReader->Update();
        vtkSmartPointer<vtkPolyData> upSpokes = upSpokeReader->GetOutput();

        vtkSmartPointer<vtkPolyDataReader> downSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
        downSpokeReader->SetFileName(downFileName.c_str());
        downSpokeReader->Update();
        vtkSmartPointer<vtkPolyData> downSpokes = downSpokeReader->GetOutput();

        vtkSmartPointer<vtkPolyDataReader> crestSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
        crestSpokeReader->SetFileName(crestFileName.c_str());
        crestSpokeReader->Update();
        vtkSmartPointer<vtkPolyData> crestSpokes = crestSpokeReader->GetOutput();

        upFileName = tempFolder + "/model/up" + std::to_string(stepNum);
        std::string upOutputFile(upFileName.c_str());
        TransformNOutput(tps, upSpokes, upOutputFile);
        // display upspokes

        downFileName = tempFolder + "/model/down" + std::to_string(stepNum);
        std::string downOutputFile(downFileName.c_str());
        TransformNOutput(tps, downSpokes, downOutputFile);

        crestFileName = tempFolder + "/model/crest" + std::to_string(stepNum);
        std::string crestOutputFile(crestFileName.c_str());
        TransformNOutput(tps, crestSpokes, crestOutputFile);

    }
    return 0;
}

void vtkSlicerSkeletalRepresentationInitializerLogic::DisplayResultSrep()
{
    // Hide other nodes.
    HideNodesByClass("vtkMRMLModelNode");
    const std::string tempFolder(this->GetApplicationLogic()->GetTemporaryPath());
    const std::string upFileName = tempFolder + "/model/up1.vtk";
    const std::string downFileName = tempFolder + "/model/down1.vtk";
    const std::string crestFileName = tempFolder +"/model/crest1.vtk";
    const std::string meshFileName = tempFolder + "/model/mesh1.vtk";;
    const std::string curveFileName = tempFolder + "/model/curve1.vtk";

    vtkSmartPointer<vtkPolyDataReader> upSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
    upSpokeReader->SetFileName(upFileName.c_str());
    upSpokeReader->Update();
    vtkSmartPointer<vtkPolyData> upSpoke_poly = upSpokeReader->GetOutput();

    vtkSmartPointer<vtkPolyDataReader> downSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
    downSpokeReader->SetFileName(downFileName.c_str());
    downSpokeReader->Update();
    vtkSmartPointer<vtkPolyData> downSpoke_poly = downSpokeReader->GetOutput();

    vtkSmartPointer<vtkPolyDataReader> crestSpokeReader = vtkSmartPointer<vtkPolyDataReader>::New();
    crestSpokeReader->SetFileName(crestFileName.c_str());
    crestSpokeReader->Update();
    vtkSmartPointer<vtkPolyData> crestSpoke_poly = crestSpokeReader->GetOutput();

    vtkSmartPointer<vtkPolyDataReader> meshReader = vtkSmartPointer<vtkPolyDataReader>::New();
    meshReader->SetFileName(meshFileName.c_str());
    meshReader->Update();
    vtkSmartPointer<vtkPolyData> meshPoly = meshReader->GetOutput();

    vtkSmartPointer<vtkPolyDataReader> curveReader = vtkSmartPointer<vtkPolyDataReader>::New();
    curveReader->SetFileName(curveFileName.c_str());
    curveReader->Update();
    vtkSmartPointer<vtkPolyData> curvePoly = curveReader->GetOutput();

    AddModelNodeToScene(upSpoke_poly, "up spokes for initial object", true, 0, 1, 1);
    AddModelNodeToScene(downSpoke_poly, "down spokes for initial object", true, 1, 0, 1);
    AddModelNodeToScene(crestSpoke_poly, "crest spokes for initial object", true, 1, 0, 0);
    AddModelNodeToScene(meshPoly, "skeletal mesh for initial object", true, 0, 0, 0);
    AddModelNodeToScene(curvePoly, "fold curve for initial object", true, 1, 1, 0);

}
void vtkSlicerSkeletalRepresentationInitializerLogic::TransformNOutput(itkThinPlateSplineExtended::Pointer tps, vtkPolyData* spokes, const std::string& outputFileName)
{
    vtkPoints* newPoints = vtkPoints::New();
    newPoints->SetDataTypeToDouble();
    vtkSmartPointer<vtkPolyData> newSpokePoly = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkCellArray> spokeLines     = vtkSmartPointer<vtkCellArray>::New();
    for(int i = 0; i < spokes->GetNumberOfPoints(); i += 2)
    {
        double p[3];
        spokes->GetPoint(i,p);

        // transform medial point by tps
        PointType hub;
        hub[0] = p[0];
        hub[1] = p[1];
        hub[2] = p[2];
        PointType transHub = tps->TransformPoint(hub);
        double newP[3];
        newP[0] = transHub[0];
        newP[1] = transHub[1];
        newP[2] = transHub[2];
        int id0 = newPoints->InsertNextPoint(newP);

        // transform implied boundary point by tps
        double p_bdry[3];
        spokes->GetPoint(i+1, p_bdry);
        PointType bdry;
        bdry[0] = p_bdry[0];
        bdry[1] = p_bdry[1];
        bdry[2] = p_bdry[2];
        PointType transB = tps->TransformPoint(bdry);
        double newB[3];
        newB[0] = transB[0];
        newB[1] = transB[1];
        newB[2] = transB[2];
        int id1 = newPoints->InsertNextPoint(newB);

        vtkSmartPointer<vtkLine> arrow = vtkSmartPointer<vtkLine>::New();
        arrow->GetPointIds()->SetId(0, id0);
        arrow->GetPointIds()->SetId(1, id1);
        spokeLines->InsertNextCell(arrow);

    }
    newSpokePoly->SetPoints(newPoints);
    newSpokePoly->SetLines(spokeLines);

    vtkSmartPointer<vtkPolyDataWriter> writer = vtkSmartPointer<vtkPolyDataWriter>::New();
    writer->SetFileName(outputFileName.c_str());
    writer->SetInputData(newSpokePoly);
    writer->Update();
}

void vtkSlicerSkeletalRepresentationInitializerLogic::TransformPoints(itkThinPlateSplineExtended::Pointer tps,
                                                                     vtkPolyData *poly, const std::string &outputFileName)
{
    vtkSmartPointer<vtkPoints> pts = poly->GetPoints();
    for(int i = 0; i < poly->GetNumberOfPoints(); ++i)
    {
        double pt[3];
        poly->GetPoint(i, pt);

        PointType itkPt;
        itkPt[0] = pt[0];
        itkPt[1] = pt[1];
        itkPt[2] = pt[2];
        PointType transPt = tps->TransformPoint(itkPt);
        pts->SetPoint(i, transPt[0], transPt[1], transPt[2]);
    }
    pts->Modified();
    vtkSmartPointer<vtkPolyDataWriter> writer = vtkSmartPointer<vtkPolyDataWriter>::New();
    writer->SetFileName(outputFileName.c_str());
    writer->SetInputData(poly);
    writer->Update();
}
double vtkSlicerSkeletalRepresentationInitializerLogic::CalculateSpokeLength(PointType tail, PointType tip){
    PointType spokeVector;
    for(unsigned int dim = 0; dim < 3; dim++){
        spokeVector[dim] = tip[dim] - tail[dim];
    }

    // Compute the spoke length
    double spokeRadiu=0;
    for(unsigned int dim=0; dim<3; dim++){
        spokeRadiu += spokeVector[dim] * spokeVector[dim];
    }

    return sqrt(spokeRadiu);
}
void vtkSlicerSkeletalRepresentationInitializerLogic::CalculateSpokeDirection(PointType tail,
                                                                              PointType tip,
                                                                              double *x, double *y, double *z){

    // Compute the spoke length
    double spokeRadiu = CalculateSpokeLength(tail, tip);

    // Normalize the spoke's direction to a unit vector
    *x=((tip[0] - tail[0]) / spokeRadiu);
    *y=((tip[1] - tail[1]) / spokeRadiu);
    *z=((tip[2] - tail[2]) / spokeRadiu);

}


#ifndef NOBODY_UTIL_H_
#define NOBODY_UTIL_H_

#include <Eigen/Core>
#include <cmath>
#include <vector>

namespace nobody {

using VectorF3 = Eigen::Vector3f;
// using Vector = std::vector;

}  // namespace nobody

#endif  // NOBODY_UTIL_H_


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_NEUTRAL_PARAMETER_HPP_
#define CLOTHO_NEUTRAL_PARAMETER_HPP_

#include <boost/property_tree/ptree.hpp>
#include "clotho/utility/clotho_strings.hpp"

#include <boost/random/bernoulli_distribution.hpp>

namespace clotho {
namespace genetics {

template < class RealType >
struct neutral_parameter {
    typedef neutral_parameter< RealType > self_type;

    typedef RealType        real_type;
    typedef boost::random::bernoulli_distribution< real_type >   distribution_type;

    distribution_type   m_dist;

//    static constexpr RealType DEFAULT_NEUTRALITY = 0.5;
#define DEFAULT_NEUTRALITY 0.5

    neutral_parameter( RealType m = DEFAULT_NEUTRALITY ) :
        m_dist( m )
    { }

    neutral_parameter( boost::property_tree::ptree & config ) :
        m_dist( DEFAULT_NEUTRALITY )
    {
        boost::property_tree::ptree lconfig;
        lconfig = config.get_child( NEUTRAL_BLOCK_K, lconfig );

        real_type p = lconfig.get< RealType >( P_K, DEFAULT_NEUTRALITY );

        lconfig.put( P_K, p );
        config.put_child( NEUTRAL_BLOCK_K, lconfig );

        m_dist.param( typename distribution_type::param_type( p ) );
    }

    neutral_parameter( const self_type & other ) :
        m_dist( other.m_dist.param() )
    {}

    void write_parameter( boost::property_tree::ptree & l ) {
        boost::property_tree::ptree c;
        c.put( P_K, m_dist.p() );
        l.put_child( NEUTRAL_BLOCK_K, c );
    }

    virtual ~neutral_parameter() {}
};

#undef DEFAULT_NEUTRALITY

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_NEUTRAL_PARAMETER_HPP_


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/*
 * mfsample.cc
 *
 *  Created on: Jul 13, 2011
 *      Author: chteflio
 */

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace boost;
using namespace boost::program_options;
using namespace rg;

/*
 * This tool samples a SPARSE matrix stored in file 'input-file'
 * It decides the size of the projected matrix according to the values:
 * i) size1, size2
 * ii) fsize1, fsize2 (if i) is not present)
 * iii) nnz
 *
 * If the output file has extension .mfp, it also creates a file descriptor and stores
 * the mappings for rows and columns between the original and sampled matrix
 *
 * If the output has any other extension of matrix, it only stores the matrix' data
 *
 * This tool does NOT eliminate empty rows and columns from the resulting projected matrix
 */

int main(int argc, char *argv[]){
	// parse the command line
	mf_size_type size1, size2, nnz;
	double fsize1;
	double fsize2;
	unsigned seed;
	std::string inFilename,outFilename,extension, dataFile;

	// read command line
	options_description desc("Options");
	desc.add_options()
		("help", "produce help message")
		("input-file", value<string>(&inFilename), "input file")
		("size1", value<mf_size_type>(&size1), "number of rows to generate")
		("size2", value<mf_size_type>(&size2), "number of columns to generate")
		("fsize1", value<double>(&fsize1), "fraction of the rows of the initial matrix to generate e.g. 0.6")
		("fsize2", value<double>(&fsize2), "fraction of the columns of the initial matrix to generate e.g. 0.6")
		("nnz", value<mf_size_type>(&nnz), "number of non-zero entries to generate")
		("seed", value<unsigned>(&seed), "seed for random number generator (if not set, system time is used)")
//		("format", value<string>(&extension)->default_value(""), "file format of blocks (default: one of the matrix market formats)")
		("output-file", value<string>(&outFilename), "output file (if .mfp write also mappings)");
	;

	positional_options_description pdesc;
	pdesc.add("input-file", 1);
	pdesc.add("output-file", 2);

	variables_map vm;
	store(command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
	notify(vm);

	// check for exceptions
	if (vm.count("help")  || vm.count("input-file")==0 || vm.count("output-file")==0) {
		cerr << "Error: Options input-file, output-file have to be present" << endl;
		cout << "mfsample [options]" << endl;
	    cout << desc << endl;
	    return 1;
	}

	boost::filesystem::path path(outFilename);
	std::string outFile = path.filename().string();
	std::string outDir = path.parent_path().string();
	if (outDir.length()!=0)	outDir+="/";

	//std::cout<<"outFile: "<<outFile<<endl;
	//std::cout<<"outDir: "<<outDir<<endl;

	SparseMatrix v;
	std::cout<<"reading input matrix ... "<<endl;
	readMatrix(inFilename, v);
	std::cout<<"Data matrix: "<< v.size1() << " x " << v.size2()
			<< ", " << v.nnz() << " nonzeros"<<endl;

	// find the sizes for the sample matrix
	if (vm.count("size1") == 0)	{
		if (vm.count("fsize1") != 0) {
			size1=fsize1*v.size1();
		}
	}
	if (vm.count("size2") == 0){
		if (vm.count("fsize2") != 0){
			size2=fsize2*v.size2();
		}
	}

	if ((vm.count("size1") == 0 && vm.count("fsize1") == 0)||
			(vm.count("size2") == 0 && vm.count("fsize2") == 0)){

		// check for exceptions
		if (vm.count("nnz")==0){
			cerr << "Error: You should define ((size1 or fsize1) AND (size2 or fsize2)) OR nnz " << endl;
			return 1;
		}
		if (vm.count("nnz")!=0 && nnz>v.nnz()){
			cerr << "Error: The sample should be smaller than the initial matrix. NNZ<"<< v.nnz()<< endl;
			return 1;
		}
		int ratio=v.nnz()/nnz;
		size1=v.size1()/sqrt(ratio);
		size2=v.size2()/sqrt(ratio);
	}

	// find and print the seed
	if (vm.count("seed") == 0) seed = time(NULL);
	std::cout<<"seed: "<<seed<<endl;
	Random32 random(seed);

	ProjectedSparseMatrix Vsample;
	std::cout<<"Sampling..."<<endl;
	projectRandomSubmatrix(random, v, Vsample, size1, size2);
	std::cout<<"Sample matrix: "<< Vsample.data.size1()
			<< " x " << Vsample.data.size2()<< ", " << Vsample.data.nnz() << " nonzeros"<<endl;

	if (getMatrixFormat(outFile)==MF_PROJECTED_SPARSE_MATRIX){

		string base=outFile.substr(0,outFile.rfind("."));
		string descriptorFile=outFile;

		//extension=(extension==""?"mmc":getExtension(getMatrixFormat(extension)));
		descriptorFile=outDir+descriptorFile;

		IndexMapFileDescriptor fileDescriptor(descriptorFile,  inFilename);

		// write the files in disk
		writeProjectedMatrix(Vsample, fileDescriptor);
		fileDescriptor.save(descriptorFile);

	}
	else if (isSparse(getMatrixFormat(outFile))){

		dataFile=outDir+outFile;
		// write only the data in disk
		std::cout<<"Writing only the data..."<<endl;
		writeMatrix(dataFile, Vsample.data);

	}
	else{
		cerr << "Error: This not a matrix extension."<< endl;
		return 1;
	}

	return 0;
}


/***************************************************************************
 * Copyright 1998-2017 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxRender.                                       *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <iostream>
#include <fstream>
#include <cstring>

#include <boost/format.hpp>

#include "luxrays/core/exttrianglemesh.h"
#include "luxrays/utils/ply/rply.h"

using namespace std;
using namespace luxrays;

//------------------------------------------------------------------------------
// ExtMesh
//------------------------------------------------------------------------------

void ExtMesh::GetDifferentials(const float time, const u_int triIndex, const Normal &shadeNormal,
        Vector *dpdu, Vector *dpdv,
        Normal *dndu, Normal *dndv) const {
    // Compute triangle partial derivatives
    const Triangle &tri = GetTriangles()[triIndex];
    UV uv0, uv1, uv2;
    if (HasUVs()) {
        uv0 = GetUV(tri.v[0]);
        uv1 = GetUV(tri.v[1]);
        uv2 = GetUV(tri.v[2]);
    } else {
		uv0 = UV(.5f, .5f);
		uv1 = UV(.5f, .5f);
		uv2 = UV(.5f, .5f);
	}

    // Compute deltas for triangle partial derivatives
	const float du1 = uv0.u - uv2.u;
	const float du2 = uv1.u - uv2.u;
	const float dv1 = uv0.v - uv2.v;
	const float dv2 = uv1.v - uv2.v;
	const float determinant = du1 * dv2 - dv1 * du2;

	if (determinant == 0.f) {
		// Handle 0 determinant for triangle partial derivative matrix
		CoordinateSystem(Vector(shadeNormal), dpdu, dpdv);
		*dndu = Normal();
		*dndv = Normal();
	} else {
		const float invdet = 1.f / determinant;

		// Using GetVertex() in order to do all computation relative to
		// the global coordinate system.
		const Point p0 = GetVertex(time, tri.v[0]);
		const Point p1 = GetVertex(time, tri.v[1]);
		const Point p2 = GetVertex(time, tri.v[2]);

		const Vector dp1 = p0 - p2;
		const Vector dp2 = p1 - p2;

		const Vector geometryDpDu = ( dv2 * dp1 - dv1 * dp2) * invdet;
		const Vector geometryDpDv = (-du2 * dp1 + du1 * dp2) * invdet;

		*dpdu = Cross(shadeNormal, Cross(geometryDpDu, shadeNormal));
		*dpdv = Cross(shadeNormal, Cross(geometryDpDv, shadeNormal));

		if (HasNormals()) {
			// Using GetShadeNormal() in order to do all computation relative to
			// the global coordinate system.
			const Normal n0 = GetShadeNormal(time, tri.v[0]);
			const Normal n1 = GetShadeNormal(time, tri.v[1]);
			const Normal n2 = GetShadeNormal(time, tri.v[2]);

			const Normal dn1 = n0 - n2;
			const Normal dn2 = n1 - n2;
			*dndu = ( dv2 * dn1 - dv1 * dn2) * invdet;
			*dndv = (-du2 * dn1 + du1 * dn2) * invdet;
		} else {
			*dndu = Normal();
			*dndv = Normal();
		}
	}
}

// rply vertex callback
static int VertexCB(p_ply_argument argument) {
	long userIndex = 0;
	void *userData = NULL;
	ply_get_argument_user_data(argument, &userData, &userIndex);

	Point *p = *static_cast<Point **> (userData);

	long vertIndex;
	ply_get_argument_element(argument, NULL, &vertIndex);

	if (userIndex == 0)
		p[vertIndex].x =
			static_cast<float>(ply_get_argument_value(argument));
	else if (userIndex == 1)
		p[vertIndex].y =
			static_cast<float>(ply_get_argument_value(argument));
	else if (userIndex == 2)
		p[vertIndex].z =
			static_cast<float>(ply_get_argument_value(argument));

	return 1;
}

// rply normal callback
static int NormalCB(p_ply_argument argument) {
	long userIndex = 0;
	void *userData = NULL;

	ply_get_argument_user_data(argument, &userData, &userIndex);

	Normal *n = *static_cast<Normal **> (userData);

	long normIndex;
	ply_get_argument_element(argument, NULL, &normIndex);

	if (userIndex == 0)
		n[normIndex].x =
			static_cast<float>(ply_get_argument_value(argument));
	else if (userIndex == 1)
		n[normIndex].y =
			static_cast<float>(ply_get_argument_value(argument));
	else if (userIndex == 2)
		n[normIndex].z =
			static_cast<float>(ply_get_argument_value(argument));

	return 1;
}

// rply uv callback
static int UVCB(p_ply_argument argument) {
	long userIndex = 0;
	void *userData = NULL;
	ply_get_argument_user_data(argument, &userData, &userIndex);

	UV *uv = *static_cast<UV **> (userData);

	long uvIndex;
	ply_get_argument_element(argument, NULL, &uvIndex);

	if (userIndex == 0)
		uv[uvIndex].u =
			static_cast<float>(ply_get_argument_value(argument));
	else if (userIndex == 1)
		uv[uvIndex].v =
			static_cast<float>(ply_get_argument_value(argument));

	return 1;
}

// rply color callback
static int ColorCB(p_ply_argument argument) {
	long userIndex = 0;
	void *userData = NULL;
	ply_get_argument_user_data(argument, &userData, &userIndex);

	float *c = *static_cast<float **> (userData);

	long colIndex;
	ply_get_argument_element(argument, NULL, &colIndex);

	// Check the type of value used
	p_ply_property property = NULL;
	ply_get_argument_property(argument, &property, NULL, NULL);
	e_ply_type dataType;
	ply_get_property_info(property, NULL, &dataType, NULL, NULL);
	if (dataType == PLY_UCHAR) {
		if (userIndex == 0)
			c[colIndex * 3] =
				static_cast<float>(ply_get_argument_value(argument) / 255.0);
		else if (userIndex == 1)
			c[colIndex * 3 + 1] =
				static_cast<float>(ply_get_argument_value(argument) / 255.0);
		else if (userIndex == 2)
			c[colIndex * 3 + 2] =
				static_cast<float>(ply_get_argument_value(argument) / 255.0);
	} else {
		if (userIndex == 0)
			c[colIndex * 3] =
				static_cast<float>(ply_get_argument_value(argument));
		else if (userIndex == 1)
			c[colIndex * 3 + 1] =
				static_cast<float>(ply_get_argument_value(argument));
		else if (userIndex == 2)
			c[colIndex * 3 + 2] =
				static_cast<float>(ply_get_argument_value(argument));
	}

	return 1;
}

// rply vertex callback
static int AlphaCB(p_ply_argument argument) {
	long userIndex = 0;
	void *userData = NULL;
	ply_get_argument_user_data(argument, &userData, &userIndex);

	float *c = *static_cast<float **> (userData);

	long alphaIndex;
	ply_get_argument_element(argument, NULL, &alphaIndex);

	// Check the type of value used
	p_ply_property property = NULL;
	ply_get_argument_property(argument, &property, NULL, NULL);
	e_ply_type dataType;
	ply_get_property_info(property, NULL, &dataType, NULL, NULL);
	if (dataType == PLY_UCHAR) {
		if (userIndex == 0)
			c[alphaIndex] =
				static_cast<float>(ply_get_argument_value(argument) / 255.0);
	} else {
		if (userIndex == 0)
			c[alphaIndex] =
				static_cast<float>(ply_get_argument_value(argument));
	}

	return 1;
}

// rply face callback
static int FaceCB(p_ply_argument argument) {
	void *userData = NULL;
	ply_get_argument_user_data(argument, &userData, NULL);

	vector<Triangle> *tris = static_cast<vector<Triangle> *> (userData);

	long length, valueIndex;
	ply_get_argument_property(argument, NULL, &length, &valueIndex);

	if (length == 3) {
		if (valueIndex < 0)
			tris->push_back(Triangle());
		else if (valueIndex < 3)
			tris->back().v[valueIndex] =
					static_cast<u_int> (ply_get_argument_value(argument));
	} else if (length == 4) {
		// I have to split the quad in 2x triangles
		if (valueIndex < 0) {
			tris->push_back(Triangle());
		} else if (valueIndex < 3)
			tris->back().v[valueIndex] =
					static_cast<u_int> (ply_get_argument_value(argument));
		else if (valueIndex == 3) {
			const u_int i0 = tris->back().v[0];
			const u_int i1 = tris->back().v[2];
			const u_int i2 = static_cast<u_int> (ply_get_argument_value(argument));

			tris->push_back(Triangle(i0, i1, i2));
		}
	}

	return 1;
}

ExtTriangleMesh *ExtTriangleMesh::LoadExtTriangleMesh(const string &fileName) {
	p_ply plyfile = ply_open(fileName.c_str(), NULL);
	if (!plyfile) {
		stringstream ss;
		ss << "Unable to read PLY mesh file '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	if (!ply_read_header(plyfile)) {
		stringstream ss;
		ss << "Unable to read PLY header from '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	Point *p;
	const long plyNbVerts = ply_set_read_cb(plyfile, "vertex", "x", VertexCB, &p, 0);
	ply_set_read_cb(plyfile, "vertex", "y", VertexCB, &p, 1);
	ply_set_read_cb(plyfile, "vertex", "z", VertexCB, &p, 2);
	if (plyNbVerts <= 0) {
		stringstream ss;
		ss << "No vertices found in '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	vector<Triangle> vi;
	const long plyNbFaces = ply_set_read_cb(plyfile, "face", "vertex_indices", FaceCB, &vi, 0);
	if (plyNbFaces <= 0) {
		stringstream ss;
		ss << "No faces found in '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	// Check if the file includes normal informations
	Normal *n;
	const long plyNbNormals = ply_set_read_cb(plyfile, "vertex", "nx", NormalCB, &n, 0);
	ply_set_read_cb(plyfile, "vertex", "ny", NormalCB, &n, 1);
	ply_set_read_cb(plyfile, "vertex", "nz", NormalCB, &n, 2);
	if ((plyNbNormals > 0) && (plyNbNormals != plyNbVerts)) {
		stringstream ss;
		ss << "Wrong count of normals in '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	// Check if the file includes uv informations
	UV *uv;
	const long plyNbUVs = ply_set_read_cb(plyfile, "vertex", "s", UVCB, &uv, 0);
	ply_set_read_cb(plyfile, "vertex", "t", UVCB, &uv, 1);
	if ((plyNbUVs > 0) && (plyNbUVs != plyNbVerts)) {
		stringstream ss;
		ss << "Wrong count of uvs in '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	// Check if the file includes color informations
	Spectrum *cols;
	const long plyNbColors = ply_set_read_cb(plyfile, "vertex", "red", ColorCB, &cols, 0);
	ply_set_read_cb(plyfile, "vertex", "green", ColorCB, &cols, 1);
	ply_set_read_cb(plyfile, "vertex", "blue", ColorCB, &cols, 2);
	if ((plyNbColors > 0) && (plyNbColors != plyNbVerts)) {
		stringstream ss;
		ss << "Wrong count of colors in '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	// Check if the file includes alpha informations
	float *alphas;
	const long plyNbAlphas = ply_set_read_cb(plyfile, "vertex", "alpha", AlphaCB, &alphas, 0);
	if ((plyNbAlphas > 0) && (plyNbAlphas != plyNbVerts)) {
		stringstream ss;
		ss << "Wrong count of alphas in '" << fileName << "'";
		throw runtime_error(ss.str());
	}

	p = TriangleMesh::AllocVerticesBuffer(plyNbVerts);
	if (plyNbNormals == 0)
		n = NULL;
	else
		n = new Normal[plyNbNormals];
	if (plyNbUVs == 0)
		uv = NULL;
	else
		uv = new UV[plyNbUVs];
	if (plyNbColors == 0)
		cols = NULL;
	else
		cols = new Spectrum[plyNbColors];
	if (plyNbAlphas == 0)
		alphas = NULL;
	else
		alphas = new float[plyNbAlphas];

	if (!ply_read(plyfile)) {
		stringstream ss;
		ss << "Unable to parse PLY file '" << fileName << "'";

		delete[] p;
		delete[] n;
		delete[] uv;
		delete[] cols;
		delete[] alphas;

		throw runtime_error(ss.str());
	}

	ply_close(plyfile);

	// Copy triangle indices vector
	Triangle *tris = TriangleMesh::AllocTrianglesBuffer(vi.size());
	copy(vi.begin(), vi.end(), tris);

	return new ExtTriangleMesh(plyNbVerts, vi.size(), p, tris, n, uv, cols, alphas);
}

//------------------------------------------------------------------------------
// ExtTriangleMesh
//------------------------------------------------------------------------------

ExtTriangleMesh::ExtTriangleMesh(const u_int meshVertCount, const u_int meshTriCount,
		Point *meshVertices, Triangle *meshTris, Normal *meshNormals, UV *meshUV,
			Spectrum *meshCols, float *meshAlpha) :
		TriangleMesh(meshVertCount, meshTriCount, meshVertices, meshTris) {
	normals = meshNormals;
	uvs = meshUV;
	cols = meshCols;
	alphas = meshAlpha;

	triNormals = new Normal[triCount];
	Preprocess();
}

void ExtTriangleMesh::Preprocess() {
	// Compute all triangle normals and mesh area
	area = 0.f;
	for (u_int i = 0; i < triCount; ++i) {
		triNormals[i] = tris[i].GetGeometryNormal(vertices);
		area += tris[i].Area(vertices);
	}
}

Normal *ExtTriangleMesh::ComputeNormals() {
	bool allocated;
	if (!normals) {
		allocated = true;
		normals = new Normal[vertCount];
	} else
		allocated = false;

	for (u_int i = 0; i < vertCount; ++i)
		normals[i] = Normal(0.f, 0.f, 0.f);
	for (u_int i = 0; i < triCount; ++i) {
		const Vector e1 = vertices[tris[i].v[1]] - vertices[tris[i].v[0]];
		const Vector e2 = vertices[tris[i].v[2]] - vertices[tris[i].v[0]];
		const Normal N = Normal(Normalize(Cross(e1, e2)));
		normals[tris[i].v[0]] += N;
		normals[tris[i].v[1]] += N;
		normals[tris[i].v[2]] += N;
	}
	//int printedWarning = 0;
	for (u_int i = 0; i < vertCount; ++i) {
		normals[i] = Normalize(normals[i]);
		// Check for degenerate triangles/normals, they can freeze the GPU
		if (isnan(normals[i].x) || isnan(normals[i].y) || isnan(normals[i].z)) {
			/*if (printedWarning < 15) {
				SDL_LOG("The model contains a degenerate normal (index " << i << ")");
				++printedWarning;
			} else if (printedWarning == 15) {
				SDL_LOG("The model contains more degenerate normals");
				++printedWarning;
			}*/
			normals[i] = Normal(0.f, 0.f, 1.f);
		}
	}

	return allocated ? normals : NULL;
}

void ExtTriangleMesh::ApplyTransform(const Transform &trans) {
	TriangleMesh::ApplyTransform(trans);

	if (normals) {
		for (u_int i = 0; i < vertCount; ++i) {
			normals[i] *= trans;
			normals[i] = Normalize(normals[i]);
		}
	}

	Preprocess();
}

void ExtTriangleMesh::WritePly(const string &fileName) const {
	BOOST_OFSTREAM plyFile(fileName.c_str(), ofstream::out | ofstream::binary | ofstream::trunc);
	if(!plyFile.is_open())
		throw runtime_error("Unable to open: " + fileName);

	// Write the PLY header
	plyFile << "ply\n"
			"format " + string(ply_storage_mode_list[ply_arch_endian()]) + " 1.0\n"
			"comment Created by LuxRays v" LUXRAYS_VERSION_MAJOR "." LUXRAYS_VERSION_MINOR "\n"
			"element vertex " + boost::lexical_cast<string>(vertCount) + "\n"
			"property float x\n"
			"property float y\n"
			"property float z\n";

	if (HasNormals())
		plyFile << "property float nx\n"
				"property float ny\n"
				"property float nz\n";

	if (HasUVs())
		plyFile << "property float s\n"
				"property float t\n";

	if (HasColors())
		plyFile << "property float red\n"
				"property float green\n"
				"property float blue\n";

	if (HasAlphas())
		plyFile << "property float alpha\n";

	plyFile << "element face " + boost::lexical_cast<string>(triCount) + "\n"
				"property list uchar uint vertex_indices\n"
				"end_header\n";

	if (!plyFile.good())
		throw runtime_error("Unable to write PLY header to: " + fileName);

	// Write all vertex data
	for (u_int i = 0; i < vertCount; ++i) {
		plyFile.write((char *)&vertices[i], sizeof(Point));
		if (HasNormals())
			plyFile.write((char *)&normals[i], sizeof(Normal));
		if (HasUVs())
			plyFile.write((char *)&uvs[i], sizeof(UV));
		if (HasColors())
			plyFile.write((char *)&cols[i], sizeof(Spectrum));
		if (HasAlphas())
			plyFile.write((char *)&alphas[i], sizeof(float));
	}
	if (!plyFile.good())
		throw runtime_error("Unable to write PLY vertex data to: " + fileName);

	// Write all face data
	const u_char len = 3;
	for (u_int i = 0; i < triCount; ++i) {
		plyFile.write((char *)&len, 1);
		plyFile.write((char *)&tris[i], sizeof(Triangle));
	}
	if (!plyFile.good())
		throw runtime_error("Unable to write PLY face data to: " + fileName);

	plyFile.close();
}

ExtTriangleMesh *ExtTriangleMesh::Copy(Point *meshVertices, Triangle *meshTris, Normal *meshNormals, UV *meshUV,
			Spectrum *meshCols, float *meshAlpha) const {
	Point *vs = meshVertices;
	if (!vs) {
		vs = AllocVerticesBuffer(vertCount);
		copy(vertices, vertices + vertCount, vs);
	}

	Triangle *ts = meshTris;
	if (!ts) {
		ts = AllocTrianglesBuffer(triCount);
		copy(tris, tris + triCount, ts);
	}

	Normal *ns = meshNormals;
	if (!ns && HasNormals()) {
		ns = new Normal[vertCount];
		copy(normals, normals + vertCount, ns);
	}

	UV *us = meshUV;
	if (!us && HasUVs()) {
		us = new UV[vertCount];
		copy(uvs, uvs + vertCount, us);
	}

	Spectrum *cs = meshCols;
	if (!cs && HasColors()) {
		cs = new Spectrum[vertCount];
		copy(cols, cols + vertCount, cs);
	}

	float *as = meshAlpha;
	if (!as && HasAlphas()) {
		as = new float[vertCount];
		copy(alphas, alphas + vertCount, as);
	}

	return new ExtTriangleMesh(vertCount, triCount, vs, ts, ns, us, cs, as);
}