/*
 * BSD 2-Clause License
 *
 * Copyright (c) 2020, Christoph Neuhauser
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <cstdio>
#include <iostream>

#include <boost/algorithm/string/predicate.hpp>
#include <boost/algorithm/string/case_conv.hpp>

#include <Utils/File/Logfile.hpp>
#include <Utils/File/LineReader.hpp>

#include "Utils/TriangleNormals.hpp"
#include "StressTrajectoriesDatLoader.hpp"

#ifdef USE_EIGEN
#include <Eigen/Eigenvalues>

void computePrincipalStresses(
        float xx, float yy, float zz, float xy, float yz, float zx,
        float& majorStress, float& mediumStress, float& minorStress/*,
        glm::vec3& v0, glm::vec3& v1, glm::vec3& v2*/) {
    Eigen::Matrix3f stressTensor;
    stressTensor.row(0) << xx, xy, zx;
    stressTensor.row(1) << xy, yy, yz;
    stressTensor.row(2) << zx, yz, zz;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> selfAdjointEigenSolver;
    selfAdjointEigenSolver.compute(stressTensor);
    Eigen::Vector3f eigenvalues = selfAdjointEigenSolver.eigenvalues();
    /*Eigen::Matrix3f eigenvectors = selfAdjointEigenSolver.eigenvectors();
    v0 = glm::vec3(eigenvectors(0, 0), eigenvectors(1, 0), eigenvectors(2, 0));
    v1 = glm::vec3(eigenvectors(0, 1), eigenvectors(1, 1), eigenvectors(2, 1));
    v2 = glm::vec3(eigenvectors(0, 2), eigenvectors(1, 2), eigenvectors(2, 2));*/

    minorStress = eigenvalues(0);
    mediumStress = eigenvalues(1);
    majorStress = eigenvalues(2);
}

float computeDegeneracyMeasure(float sigma1, float sigma2, float sigma3) {
    float degeneracyMeasure = 0.5f * std::abs((sigma1 - sigma2) / (sigma1 + sigma2));
    degeneracyMeasure = std::min(degeneracyMeasure, 0.5f * std::abs((sigma3 - sigma2) / (sigma3 + sigma2)));
    return degeneracyMeasure;
}
#endif

void loadStressLineHierarchyFromDat(
        const std::vector<std::string>& filenamesHierarchy,
        std::vector<StressTrajectoriesData>& stressTrajectoriesDataPs) {
    size_t psIdx = 0;
    for (size_t fileIdx = 0; fileIdx < filenamesHierarchy.size(); fileIdx++) {
        const std::string& filename = filenamesHierarchy.at(fileIdx);

        sgl::LineReader lineReader(filename);
        while (lineReader.isLineLeft()) {
            assert(psIdx < stressTrajectoriesDataPs.size());
            StressTrajectoriesData& stressTrajectoriesData = stressTrajectoriesDataPs.at(psIdx);
            std::vector<std::string> linesInfo = lineReader.readVectorLine<std::string>();
            // Line metadata saved?
            uint32_t numLines = 0;
            if (linesInfo.size() == 1) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(0));
            } else if (linesInfo.size() == 2) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(1));
            } else {
                sgl::Logfile::get()->writeError(
                        std::string() + "ERROR in loadStressLineHierarchyFromDat: Invalid line metadata in file \""
                        + filename + "\".");
            }
            assert(stressTrajectoriesData.size() == numLines);
            for (uint32_t lineIdx = 0; lineIdx < numLines; lineIdx++) {
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(lineIdx);
                float lineHierarchyLevel = lineReader.readScalarLine<float>();
                stressTrajectoryData.hierarchyLevels.push_back(lineHierarchyLevel);
            }

            psIdx++;
        }
    }
}

void loadStressTrajectoriesFromDat_v1(
        const std::vector<std::string>& filenamesTrajectories,
        const std::vector<std::string>& filenamesHierarchy,
        std::vector<int>& loadedPsIndices,
        std::vector<Trajectories>& trajectoriesPs,
        std::vector<StressTrajectoriesData>& stressTrajectoriesDataPs) {
    trajectoriesPs.reserve(filenamesTrajectories.size());
    stressTrajectoriesDataPs.reserve(filenamesTrajectories.size());
    size_t geometryByteSize = 0;

    size_t psIdx = 0;
    for (size_t fileIdx = 0; fileIdx < filenamesTrajectories.size(); fileIdx++) {
        const std::string& filename = filenamesTrajectories.at(fileIdx);

        sgl::LineReader lineReader(filename);
        while (lineReader.isLineLeft()) {
            Trajectories trajectories;
            StressTrajectoriesData stressTrajectoriesData;
            std::vector<std::string> linesInfo = lineReader.readVectorLine<std::string>();
            // Line metadata saved?
            uint32_t numLines = 0;
            if (linesInfo.size() == 1) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(0));
            } else if (linesInfo.size() == 2) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(1));
                boost::algorithm::to_lower(linesInfo.at(0));
                if (boost::ends_with(linesInfo.at(0), "major")) {
                    loadedPsIndices.push_back(0);
                } else if (boost::ends_with(linesInfo.at(0), "medium")) {
                    loadedPsIndices.push_back(1);
                } else if (boost::ends_with(linesInfo.at(0), "minor")) {
                    loadedPsIndices.push_back(2);
                } else {
                    sgl::Logfile::get()->writeError(
                            std::string() + "ERROR in loadStressTrajectoriesFromDat_: "
                            + "Invalid principal stress identifier \"" + linesInfo.at(0) + "\".");
                }
            } else {
                sgl::Logfile::get()->writeError(
                        std::string() + "ERROR in loadStressTrajectoriesFromDat_v1: Invalid line metadata in file \""
                        + filename + "\".");
            }
            trajectories.resize(numLines);
            stressTrajectoriesData.resize(numLines);
            for (uint32_t lineIdx = 0; lineIdx < numLines; lineIdx++) {
                Trajectory& trajectory = trajectories.at(lineIdx);
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(lineIdx);

                uint32_t lineLength = lineReader.readScalarLine<uint32_t>();
                trajectory.positions.reserve(lineLength);
                stressTrajectoryData.majorPs.reserve(lineLength);
                stressTrajectoryData.mediumPs.reserve(lineLength);
                stressTrajectoryData.minorPs.reserve(lineLength);
                stressTrajectoryData.majorPsDir.reserve(lineLength);
                stressTrajectoryData.mediumPsDir.reserve(lineLength);
                stressTrajectoryData.minorPsDir.reserve(lineLength);
                trajectory.attributes.resize(2);
                trajectory.attributes.front().reserve(lineLength);
                std::vector<float> positionData = lineReader.readVectorLine<float>(lineLength * 3);
                std::vector<float> psData = lineReader.readVectorLine<float>(lineLength * 12);
                std::vector<float> vonMisesData = lineReader.readVectorLine<float>(lineLength);

                for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                    trajectory.positions.push_back(glm::vec3(
                            positionData.at(pointIdx * 3),
                            positionData.at(pointIdx * 3 + 1),
                            positionData.at(pointIdx * 3 + 2)));
                    stressTrajectoryData.majorPs.push_back(psData.at(pointIdx * 12));
                    stressTrajectoryData.majorPsDir.push_back(glm::vec3(
                            psData.at(pointIdx * 12 + 1),
                            psData.at(pointIdx * 12 + 2),
                            psData.at(pointIdx * 12 + 3)));
                    stressTrajectoryData.mediumPs.push_back(psData.at(pointIdx * 12 + 4));
                    stressTrajectoryData.mediumPsDir.push_back(glm::vec3(
                            psData.at(pointIdx * 12 + 5),
                            psData.at(pointIdx * 12 + 6),
                            psData.at(pointIdx * 12 + 7)));
                    stressTrajectoryData.minorPs.push_back(psData.at(pointIdx * 12 + 8));
                    stressTrajectoryData.minorPsDir.push_back(glm::vec3(
                            psData.at(pointIdx * 12 + 9),
                            psData.at(pointIdx * 12 + 10),
                            psData.at(pointIdx * 12 + 11)));
                    trajectory.attributes.at(0).push_back(vonMisesData.at(pointIdx));
                    if (psIdx == 0) {
                        trajectory.attributes.at(1).push_back(std::abs(stressTrajectoryData.majorPs.back()));
                    } else if (psIdx == 1) {
                        trajectory.attributes.at(1).push_back(std::abs(stressTrajectoryData.mediumPs.back()));
                    } else {
                        trajectory.attributes.at(1).push_back(std::abs(stressTrajectoryData.minorPs.back()));
                    }
                }
            }

            for (size_t trajectoryIdx = 0; trajectoryIdx < trajectories.size(); trajectoryIdx++) {
                Trajectory& trajectory = trajectories.at(trajectoryIdx);
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(trajectoryIdx);
                geometryByteSize += trajectory.positions.size() * sizeof(float) * 3;
                geometryByteSize += sizeof(float); // hierarchy level
                geometryByteSize += stressTrajectoryData.majorPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.mediumPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.minorPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.majorPsDir.size() * sizeof(float) * 3;
                geometryByteSize += stressTrajectoryData.mediumPsDir.size() * sizeof(float) * 3;
                geometryByteSize += stressTrajectoryData.minorPsDir.size() * sizeof(float) * 3;
                for (const std::vector<float>& attributes : trajectory.attributes) {
                    geometryByteSize += attributes.size() * sizeof(float);
                }
            }

            trajectoriesPs.emplace_back(trajectories);
            stressTrajectoriesDataPs.emplace_back(stressTrajectoriesData);
            psIdx++;
        }
    }

    // Check if there's additional line hierarchy data.
    if (filenamesHierarchy.size() > 0) {
        loadStressLineHierarchyFromDat(filenamesHierarchy, stressTrajectoriesDataPs);
    }

    // Assume that all three PS directions are provided for the v1 .dat format.
    if (loadedPsIndices.size() == 0 && trajectoriesPs.size() == 3) {
        loadedPsIndices = {0, 1, 2};
    }

    std::cout << "Size of line geometry data (MiB): " << (geometryByteSize / (1024.0 * 1024.0)) << std::endl;
}

void loadStressTrajectoriesFromDat_v2(
        const std::vector<std::string>& filenamesTrajectories,
        std::vector<int>& loadedPsIndices,
        std::vector<Trajectories>& trajectoriesPs,
        std::vector<StressTrajectoriesData>& stressTrajectoriesDataPs,
        std::vector<std::vector<std::vector<glm::vec3>>>& bandPointsListLeftPs,
        std::vector<std::vector<std::vector<glm::vec3>>>& bandPointsListRightPs) {
    trajectoriesPs.reserve(filenamesTrajectories.size());
    stressTrajectoriesDataPs.reserve(filenamesTrajectories.size());
    bandPointsListLeftPs.reserve(filenamesTrajectories.size());
    bandPointsListRightPs.reserve(filenamesTrajectories.size());
    size_t geometryByteSize = 0;

    size_t psIdx = 0;
    for (size_t fileIdx = 0; fileIdx < filenamesTrajectories.size(); fileIdx++) {
        const std::string& filename = filenamesTrajectories.at(fileIdx);

        sgl::LineReader lineReader(filename);
        while (lineReader.isLineLeft()) {
            Trajectories trajectories;
            StressTrajectoriesData stressTrajectoriesData;
            std::vector<std::vector<glm::vec3>> bandPointsListLeft;
            std::vector<std::vector<glm::vec3>> bandPointsListRight;
            std::vector<std::string> linesInfo = lineReader.readVectorLine<std::string>();
            // Line metadata saved?
            uint32_t numLines = 0;
            if (linesInfo.size() == 1) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(0));
            } else if (linesInfo.size() == 2) {
                boost::algorithm::to_lower(linesInfo.at(0));
                if (boost::ends_with(linesInfo.at(0), "major")) {
                    loadedPsIndices.push_back(0);
                } else if (boost::ends_with(linesInfo.at(0), "medium")) {
                    loadedPsIndices.push_back(1);
                } else if (boost::ends_with(linesInfo.at(0), "minor")) {
                    loadedPsIndices.push_back(2);
                } else {
                    sgl::Logfile::get()->writeError(
                            std::string() + "ERROR in loadStressTrajectoriesFromDat_v2: "
                            + "Invalid principal stress identifier \"" + linesInfo.at(0) + "\".");
                }
                numLines = sgl::fromString<uint32_t>(linesInfo.at(1));
            } else {
                sgl::Logfile::get()->writeError(
                        std::string() + "ERROR in loadStressTrajectoriesFromDat_v2: "
                        + "Invalid line metadata in file \"" + filename + "\".");
            }
            trajectories.resize(numLines);
            stressTrajectoriesData.resize(numLines);
            bandPointsListLeft.resize(numLines);
            bandPointsListRight.resize(numLines);
            for (uint32_t lineIdx = 0; lineIdx < numLines; lineIdx++) {
                Trajectory& trajectory = trajectories.at(lineIdx);
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(lineIdx);
                std::vector<glm::vec3>& bandPointsLeft = bandPointsListLeft.at(lineIdx);
                std::vector<glm::vec3>& bandPointsRight = bandPointsListRight.at(lineIdx);

                std::vector<std::string> firstLineVector = lineReader.readVectorLine<std::string>();
                if (firstLineVector.size() != 2) {
                    sgl::Logfile::get()->writeError(
                            std::string() + "ERROR in loadStressTrajectoriesFromDat_v2: "
                            + "Invalid per line metadata in file \"" + filename + "\".");
                }
                uint32_t lineLength = sgl::fromString<uint32_t>(firstLineVector.at(0));
                float hierarchyLevel = sgl::fromString<float>(firstLineVector.at(1));
                stressTrajectoryData.hierarchyLevels.push_back(hierarchyLevel);
                trajectory.positions.reserve(lineLength);
                trajectory.attributes.resize(1);
                trajectory.attributes.front().reserve(lineLength);
                bandPointsLeft.reserve(lineLength);
                bandPointsRight.reserve(lineLength);
                std::vector<float> positionData = lineReader.readVectorLine<float>(lineLength * 3);
                std::vector<float> bandVertexData = lineReader.readVectorLine<float>(lineLength * 6);
                std::vector<float> scalarFieldData = lineReader.readVectorLine<float>(lineLength);

                for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                    trajectory.positions.push_back(glm::vec3(
                            positionData.at(pointIdx * 3),
                            positionData.at(pointIdx * 3 + 1),
                            positionData.at(pointIdx * 3 + 2)));
                    bandPointsLeft.push_back(glm::vec3(
                            bandVertexData.at(pointIdx * 6 + 0),
                            bandVertexData.at(pointIdx * 6 + 1),
                            bandVertexData.at(pointIdx * 6 + 2)));
                    bandPointsRight.push_back(glm::vec3(
                            bandVertexData.at(pointIdx * 6 + 3),
                            bandVertexData.at(pointIdx * 6 + 4),
                            bandVertexData.at(pointIdx * 6 + 5)));
                    trajectory.attributes.at(0).push_back(scalarFieldData.at(pointIdx));
                }
            }

            for (size_t trajectoryIdx = 0; trajectoryIdx < trajectories.size(); trajectoryIdx++) {
                Trajectory& trajectory = trajectories.at(trajectoryIdx);
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(trajectoryIdx);
                geometryByteSize += trajectory.positions.size() * sizeof(float) * 3;
                geometryByteSize += sizeof(float); // hierarchy level
                geometryByteSize += stressTrajectoryData.majorPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.mediumPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.minorPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.majorPsDir.size() * sizeof(float) * 3;
                geometryByteSize += stressTrajectoryData.mediumPsDir.size() * sizeof(float) * 3;
                geometryByteSize += stressTrajectoryData.minorPsDir.size() * sizeof(float) * 3;
                for (const std::vector<float>& attributes : trajectory.attributes) {
                    geometryByteSize += attributes.size() * sizeof(float);
                }
                geometryByteSize += bandPointsListLeft.at(trajectoryIdx).size() * sizeof(float) * 3;
                geometryByteSize += bandPointsListRight.at(trajectoryIdx).size() * sizeof(float) * 3;
            }

            trajectoriesPs.emplace_back(trajectories);
            stressTrajectoriesDataPs.emplace_back(stressTrajectoriesData);
            bandPointsListLeftPs.emplace_back(bandPointsListLeft);
            bandPointsListRightPs.emplace_back(bandPointsListRight);
            psIdx++;
        }
    }

    std::cout << "Size of line geometry data (MiB): " << (geometryByteSize / (1024.0 * 1024.0)) << std::endl;
}

void parseOutlineMeshHull(
        sgl::LineReader& lineReader,
        std::vector<uint32_t>& simulationMeshOutlineTriangleIndices,
        std::vector<glm::vec3>& simulationMeshOutlineVertexPositions) {
    std::vector<std::string> numVerticesLine = lineReader.readVectorLine<std::string>();
    if (numVerticesLine.size() != 2 || numVerticesLine.front() != "#Vertices") {
        sgl::Logfile::get()->writeError("Error in parseOutlineMeshHull: Invalid vertex information.");
    }
    uint32_t numVertices = sgl::fromString<uint32_t>(numVerticesLine.at(1));
    for (uint32_t vertexIdx = 0; vertexIdx < numVertices; vertexIdx++) {
        std::vector<float> vertexPosition = lineReader.readVectorLine<float>(3);
        assert(vertexPosition.size() == 3);
        simulationMeshOutlineVertexPositions.push_back(
                glm::vec3(vertexPosition.at(0), vertexPosition.at(1), vertexPosition.at(2)));
    }

    std::vector<std::string> numFacesLine = lineReader.readVectorLine<std::string>();
    if (numFacesLine.size() != 2 || numFacesLine.front() != "#Faces") {
        sgl::Logfile::get()->writeError("Error in parseOutlineMeshHull: Invalid face information.");
    }
    uint32_t numFaces = sgl::fromString<uint32_t>(numFacesLine.at(1));
    for (uint32_t faceIdx = 0; faceIdx < numFaces; faceIdx++) {
        std::vector<uint32_t> faceIndices = lineReader.readVectorLine<uint32_t>(4);
        assert(faceIndices.size() == 4);

        simulationMeshOutlineTriangleIndices.push_back(faceIndices.at(0));
        simulationMeshOutlineTriangleIndices.push_back(faceIndices.at(1));
        simulationMeshOutlineTriangleIndices.push_back(faceIndices.at(2));

        simulationMeshOutlineTriangleIndices.push_back(faceIndices.at(0));
        simulationMeshOutlineTriangleIndices.push_back(faceIndices.at(2));
        simulationMeshOutlineTriangleIndices.push_back(faceIndices.at(3));
    }
}

void loadStressTrajectoriesFromDat_v3(
        const std::vector<std::string>& filenamesTrajectories,
        std::vector<int>& loadedPsIndices, MeshType& meshType,
        std::vector<Trajectories>& trajectoriesPs,
        std::vector<StressTrajectoriesData>& stressTrajectoriesDataPs,
        std::vector<std::vector<std::vector<glm::vec3>>>& bandPointsUnsmoothedListLeftPs,
        std::vector<std::vector<std::vector<glm::vec3>>>& bandPointsUnsmoothedListRightPs,
        std::vector<std::vector<std::vector<glm::vec3>>>& bandPointsSmoothedListLeftPs,
        std::vector<std::vector<std::vector<glm::vec3>>>& bandPointsSmoothedListRightPs,
        std::vector<uint32_t>& simulationMeshOutlineTriangleIndices,
        std::vector<glm::vec3>& simulationMeshOutlineVertexPositions) {
    trajectoriesPs.reserve(filenamesTrajectories.size());
    stressTrajectoriesDataPs.reserve(filenamesTrajectories.size());
    bandPointsUnsmoothedListLeftPs.reserve(filenamesTrajectories.size());
    bandPointsUnsmoothedListRightPs.reserve(filenamesTrajectories.size());
    bandPointsSmoothedListLeftPs.reserve(filenamesTrajectories.size());
    bandPointsSmoothedListRightPs.reserve(filenamesTrajectories.size());
    size_t geometryByteSize = 0;

    size_t psIdx = 0;
    for (size_t fileIdx = 0; fileIdx < filenamesTrajectories.size(); fileIdx++) {
        const std::string& filename = filenamesTrajectories.at(fileIdx);

        sgl::LineReader lineReader(filename);
        while (lineReader.isLineLeft()) {
            Trajectories trajectories;
            StressTrajectoriesData stressTrajectoriesData;
            std::vector<std::vector<glm::vec3>> bandPointsUnsmoothedListLeft;
            std::vector<std::vector<glm::vec3>> bandPointsUnsmoothedListRight;
            std::vector<std::vector<glm::vec3>> bandPointsSmoothedListLeft;
            std::vector<std::vector<glm::vec3>> bandPointsSmoothedListRight;
            std::vector<std::string> linesInfo = lineReader.readVectorLine<std::string>();

            if (linesInfo.front() == "#Outline") {
                if (linesInfo.size() == 1) {
                    meshType = MeshType::CARTESIAN;
                } else {
                    if (linesInfo.at(1) == "Cartesian") {
                        meshType = MeshType::CARTESIAN;
                    } else {
                        meshType = MeshType::UNSTRUCTURED;
                    }
                }
                parseOutlineMeshHull(
                        lineReader, simulationMeshOutlineTriangleIndices, simulationMeshOutlineVertexPositions);
                continue;
            }

            // Line metadata saved?
            uint32_t numLines = 0;
            if (linesInfo.size() == 1) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(0));
                if (numLines == 0) {
                    continue;
                }
            } else if (linesInfo.size() == 2) {
                numLines = sgl::fromString<uint32_t>(linesInfo.at(1));
                if (numLines == 0) {
                    continue;
                }
                boost::algorithm::to_lower(linesInfo.at(0));
                if (boost::ends_with(linesInfo.at(0), "major")) {
                    loadedPsIndices.push_back(0);
                } else if (boost::ends_with(linesInfo.at(0), "medium")) {
                    loadedPsIndices.push_back(1);
                } else if (boost::ends_with(linesInfo.at(0), "minor")) {
                    loadedPsIndices.push_back(2);
                } else {
                    sgl::Logfile::get()->writeError(
                            std::string() + "ERROR in loadStressTrajectoriesFromDat_v2: "
                            + "Invalid principal stress identifier \"" + linesInfo.at(0) + "\".");
                }
            } else {
                sgl::Logfile::get()->writeError(
                        std::string() + "ERROR in loadStressTrajectoriesFromDat_v2: "
                        + "Invalid line metadata in file \"" + filename + "\".");
            }
            trajectories.resize(numLines);
            stressTrajectoriesData.resize(numLines);
            bandPointsUnsmoothedListLeft.resize(numLines);
            bandPointsUnsmoothedListRight.resize(numLines);
            bandPointsSmoothedListLeft.resize(numLines);
            bandPointsSmoothedListRight.resize(numLines);
            for (uint32_t lineIdx = 0; lineIdx < numLines; lineIdx++) {
                Trajectory& trajectory = trajectories.at(lineIdx);
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(lineIdx);
                std::vector<glm::vec3>& bandPointsUnsmoothedLeft = bandPointsUnsmoothedListLeft.at(lineIdx);
                std::vector<glm::vec3>& bandPointsUnsmoothedRight = bandPointsUnsmoothedListRight.at(lineIdx);
                std::vector<glm::vec3>& bandPointsSmoothedLeft = bandPointsSmoothedListLeft.at(lineIdx);
                std::vector<glm::vec3>& bandPointsSmoothedRight = bandPointsSmoothedListRight.at(lineIdx);

                std::vector<std::string> firstLineVector = lineReader.readVectorLine<std::string>();
                if (firstLineVector.size() == 0) {
                    sgl::Logfile::get()->writeError(
                            std::string() + "ERROR in loadStressTrajectoriesFromDat_v2: "
                            + "Invalid per line metadata in file \"" + filename + "\".");
                }
                uint32_t lineLength = sgl::fromString<uint32_t>(firstLineVector.at(0));

                // Add the hierarchy levels.
                for (int hierarchyIdx = 1; hierarchyIdx < std::max(int(firstLineVector.size()), 5); hierarchyIdx++) {
                    float hierarchyLevel = sgl::fromString<float>(firstLineVector.at(hierarchyIdx));
                    stressTrajectoryData.hierarchyLevels.push_back(hierarchyLevel);
                }
                if (firstLineVector.size() == 9) {
                    stressTrajectoryData.appearanceOrder = sgl::fromString<int>(firstLineVector.at(5)) - 1;
                    stressTrajectoryData.seedPosition = glm::vec3(
                            sgl::fromString<float>(firstLineVector.at(6)),
                            sgl::fromString<float>(firstLineVector.at(7)),
                            sgl::fromString<float>(firstLineVector.at(8)));
                }

                trajectory.positions.reserve(lineLength);
                bandPointsUnsmoothedLeft.reserve(lineLength);
                bandPointsUnsmoothedRight.reserve(lineLength);
                bandPointsSmoothedLeft.reserve(lineLength);
                bandPointsSmoothedRight.reserve(lineLength);
                std::vector<float> positionData = lineReader.readVectorLine<float>(
                        lineLength * 3);
                std::vector<float> bandVertexDataUnsmoothed = lineReader.readVectorLine<float>(
                        lineLength * 6);
                std::vector<float> bandVertexDataSmoothed = lineReader.readVectorLine<float>(
                        lineLength * 6);

                for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                    trajectory.positions.emplace_back(
                            positionData.at(pointIdx * 3),
                            positionData.at(pointIdx * 3 + 1),
                            positionData.at(pointIdx * 3 + 2));
                    bandPointsUnsmoothedLeft.emplace_back(
                            bandVertexDataUnsmoothed.at(pointIdx * 6 + 0),
                            bandVertexDataUnsmoothed.at(pointIdx * 6 + 1),
                            bandVertexDataUnsmoothed.at(pointIdx * 6 + 2));
                    bandPointsUnsmoothedRight.emplace_back(
                            bandVertexDataUnsmoothed.at(pointIdx * 6 + 3),
                            bandVertexDataUnsmoothed.at(pointIdx * 6 + 4),
                            bandVertexDataUnsmoothed.at(pointIdx * 6 + 5));
                    bandPointsSmoothedLeft.emplace_back(
                            bandVertexDataSmoothed.at(pointIdx * 6 + 0),
                            bandVertexDataSmoothed.at(pointIdx * 6 + 1),
                            bandVertexDataSmoothed.at(pointIdx * 6 + 2));
                    bandPointsSmoothedRight.emplace_back(
                            bandVertexDataSmoothed.at(pointIdx * 6 + 3),
                            bandVertexDataSmoothed.at(pointIdx * 6 + 4),
                            bandVertexDataSmoothed.at(pointIdx * 6 + 5));
                }

#ifdef USE_EIGEN
                trajectory.attributes.resize(13);
#else
                trajectory.attributes.resize(9);
#endif

                // Principal stress.
                trajectory.attributes.at(0).reserve(lineLength);
                std::vector<float> scalarFieldData = lineReader.readVectorLine<float>(lineLength);
                for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                    trajectory.attributes.at(0).push_back(scalarFieldData.at(pointIdx));
                }

                // Principal stress magnitude.
                for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                    trajectory.attributes.at(1).push_back(std::abs(scalarFieldData.at(pointIdx)));
                }

                // Von Mises stress, normal stress (xx), normal stress (yy), normal stress (zz), shear stress (yz),
                // shear stress (zx), shear stress (xy).
                for (int varIdx = 2; varIdx < 9; varIdx++) {
                    trajectory.attributes.at(varIdx).reserve(lineLength);
                    std::vector<float> scalarFieldData = lineReader.readVectorLine<float>(lineLength);
                    for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                        trajectory.attributes.at(varIdx).push_back(scalarFieldData.at(pointIdx));
                    }
                }

#ifdef USE_EIGEN
                int xxIdx = 3;
                int yyIdx = 4;
                int zzIdx = 5;
                int xyIdx = 8;
                int yzIdx = 6;
                int zxIdx = 7;

                float majorStress, mediumStress, minorStress;
                //glm::vec3 v0, v1, v2;
                for (uint32_t pointIdx = 0; pointIdx < lineLength; pointIdx++) {
                    computePrincipalStresses(
                            trajectory.attributes.at(xxIdx).at(pointIdx),
                            trajectory.attributes.at(yyIdx).at(pointIdx),
                            trajectory.attributes.at(zzIdx).at(pointIdx),
                            trajectory.attributes.at(xyIdx).at(pointIdx),
                            trajectory.attributes.at(yzIdx).at(pointIdx),
                            trajectory.attributes.at(zxIdx).at(pointIdx),
                            majorStress, mediumStress, minorStress/*, v0, v1, v2*/);
                    float degeneracyMeasure = computeDegeneracyMeasure(
                            minorStress, mediumStress, majorStress);
                    trajectory.attributes.at(9).push_back(majorStress);
                    trajectory.attributes.at(10).push_back(mediumStress);
                    trajectory.attributes.at(11).push_back(minorStress);
                    trajectory.attributes.at(12).push_back(degeneracyMeasure);
                }
#endif
            }

            for (size_t trajectoryIdx = 0; trajectoryIdx < trajectories.size(); trajectoryIdx++) {
                Trajectory& trajectory = trajectories.at(trajectoryIdx);
                StressTrajectoryData& stressTrajectoryData = stressTrajectoriesData.at(trajectoryIdx);
                geometryByteSize += trajectory.positions.size() * sizeof(float) * 3;
                geometryByteSize += sizeof(float); // hierarchy level
                geometryByteSize += stressTrajectoryData.majorPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.mediumPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.minorPs.size() * sizeof(float);
                geometryByteSize += stressTrajectoryData.majorPsDir.size() * sizeof(float) * 3;
                geometryByteSize += stressTrajectoryData.mediumPsDir.size() * sizeof(float) * 3;
                geometryByteSize += stressTrajectoryData.minorPsDir.size() * sizeof(float) * 3;
                for (const std::vector<float>& attributes : trajectory.attributes) {
                    geometryByteSize += attributes.size() * sizeof(float);
                }
                geometryByteSize += bandPointsUnsmoothedListLeft.at(trajectoryIdx).size() * sizeof(float) * 3;
                geometryByteSize += bandPointsUnsmoothedListRight.at(trajectoryIdx).size() * sizeof(float) * 3;
                geometryByteSize += bandPointsSmoothedListLeft.at(trajectoryIdx).size() * sizeof(float) * 3;
                geometryByteSize += bandPointsSmoothedListRight.at(trajectoryIdx).size() * sizeof(float) * 3;
            }

            trajectoriesPs.emplace_back(trajectories);
            stressTrajectoriesDataPs.emplace_back(stressTrajectoriesData);
            bandPointsUnsmoothedListLeftPs.emplace_back(bandPointsUnsmoothedListLeft);
            bandPointsUnsmoothedListRightPs.emplace_back(bandPointsUnsmoothedListRight);
            bandPointsSmoothedListLeftPs.emplace_back(bandPointsSmoothedListLeft);
            bandPointsSmoothedListRightPs.emplace_back(bandPointsSmoothedListRight);
            psIdx++;
        }
    }

    std::cout << "Size of line geometry data (MiB): " << (geometryByteSize / (1024.0 * 1024.0)) << std::endl;
}


#include <iostream>
#include <alglib/containers/disjoint_sets.h>

int main() {
	disjoint_sets<int> sets;
	sets.make_set(32);
	sets.make_set(221);
	sets.make_set(311);
	sets.unite(32, 311);
	cout << sets.find_set(32) << endl;
	cout << sets.find_set(221) << endl;
}


<% render(instance) %>
<%= includes((0..x/2).size * 2) %>

#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/iterable/iterable.hpp>

BOOST_HANA_CONSTEXPR_LAMBDA auto is_odd = [](auto x) {
    return x % boost::hana::int_<2> != boost::hana::int_<0>;
};

int main() {
    auto go = boost::hana::drop_while(is_odd,
        <%= list(
            (0..x/2).map { |i| "decltype(boost::hana::int_<#{2*i+1}>)" } +
            (0..x/2).map { |i| "decltype(boost::hana::int_<#{2*i}>)" },

            (0..x/2).map { |i| "boost::hana::int_<#{2*i+1}>" } +
            (0..x/2).map { |i| "boost::hana::int_<#{2*i}>" }
        ) %>
    );
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IDIVFIX_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IDIVFIX_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/idivfix.hpp>
#include <boost/simd/include/functions/simd/trunc.hpp>
#include <boost/simd/include/functions/simd/divs.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivfix_, tag::cpu_, (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return boost::simd::rdivide(a0, a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivfix_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer < A0 > ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return boost::simd::toint(boost::simd::trunc(a0/a1)); }
  };
} } }

#endif


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// MetaData.cpp
// This file is part of the Garamon Generator.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

#include <math.h>
#include <set>

#include <Eigen/Eigenvalues>

#include "MetaData.hpp"
#include "ConfigParser.hpp"

MetaData::MetaData() : dimension(0), inputMetricDiagonal(false), identityMetric(false), inputMetricPermutationOfDiagonal(false), maxDimPrecomputedProducts(256) {}

MetaData::~MetaData() {}

void MetaData::display() const {
    std::cout << "MetaData" << std::endl;
    std::cout << "dimension         : " << dimension << std::endl;
    std::cout << "namespace         : " << namespaceName << std::endl;

    std::cout << "refinement        : " << (useEigenRefinement==true?"true":"false") << std::endl;
    std::cout << "cleanup           : " << (useNumericalCleanUp==true?"true":"false") << std::endl;
    std::cout << "maxDim prec func  : " << maxDimPrecomputedProducts << std::endl;
    std::cout << "maxDim accessors  : " << maxDimBasisAccessor << std::endl;
    std::cout << "epsilon           : " << epsilon << std::endl;
    std::cout << "basis vector name : ";
    for(unsigned int i=0; i<basisVectorName.size(); ++i)
        std::cout << basisVectorName[i] << " ";
    std::cout << std::endl;
    std::cout << "metric            : \n" << metric << std::endl;
    std::cout << "is initialy diag  : " << (inputMetricDiagonal==true?"true":"false") << std::endl;
    std::cout << "is identity       : " << (identityMetric==true?"true":"false") << std::endl;
    std::cout << "is full rank      : " << (fullRankMetric==true?"true":"false") << std::endl;
    std::cout << "is permutation of diagonal matrix : " << (inputMetricPermutationOfDiagonal==true?"true":"false") << std::endl;
    std::cout << "diag metric       : " << diagonalMetric.transpose() << std::endl;
    if(!inputMetricDiagonal){
        std::cout << "Vector transformation matrix : \n" << transformationMatrix << std::endl;
        std::cout << "Vector inverse Transformation  : \n" << inverseTransformationMatrix<< std::endl;
    }
}

bool MetaData::checkConsistency() const {

    bool consistencyCheck = true;

    // maxDimBasisAccessor consistency: at least for vectors
    if(maxDimBasisAccessor == 0){
        std::cout << "error: at least vector accessors are required, see 'max dimension basis accessor' in the conf file." << std::endl;
        consistencyCheck = false;
    }

    // dimension consistency
    if(dimension == 0){
        std::cout << "error: dimension should not be 0." << std::endl;
        consistencyCheck = false;
    }

    // basis vector name dimension consistency
    if(basisVectorName.size() != dimension){
        std::cout << "error: 'basis vector name' size is not consistent with 'dimension'." << std::endl;
        consistencyCheck = false;
    }

    // metric empty
    bool metricDefined = true;
    if( (metric.rows()==0) || (metric.cols()==0) ) {
        metricDefined = false;
        std::cout << "error: the metric matrix is not defined." << std::endl;
        consistencyCheck = false;
    }

    // metric matrix ?
    if(metricDefined) {
        // metric : square matrix ?
        bool squareMatrix = true;
        if( metric.rows() != metric.cols() ){
            std::cout << "error: the metric is not a square matrix." << std::endl;
            consistencyCheck = false;
            squareMatrix = false;
        }

        // metric : dimension consistency
        if( ((unsigned int)metric.cols() != dimension) || ((unsigned int)metric.rows() != dimension) ) {
            std::cout << "error: the metric dimension is not consistent with 'dimension'." << std::endl;
            consistencyCheck = false;
        }

        if(squareMatrix) {
            // check if metric is symetric
            bool symetric = true;
            for(unsigned int i=0; i<(unsigned int)metric.rows(); ++i)
                for(unsigned int j=i; j<(unsigned int)metric.cols(); ++j)
                    if(fabs(metric(i,j) - metric(j,i)) > epsilon)
                        symetric = false;
            if (!symetric) {
                std::cout << "error: the metric is not a symmetric matrix." << std::endl;
                consistencyCheck = false;
            }
        }
    }

    // check if the name of the vector basis are not ambiguous for high dimensions
    // i.e. in dimension 15: e12 is for twelve or one-two ?
    std::set<std::string> basis;

    // represent a k-vector with a binary number (k-st bit to 1 means the k-st basis is used)
    for(unsigned int i=1; i<=pow(2,dimension); ++i){

        std::string kvector;
        for(unsigned int k=0; k<dimension; ++k)
            if(i & (1 << k))
                kvector = kvector + basisVectorName[k];

        if(basis.count(kvector) != 0){
            std::cout << "error in the basis vector name: " << kvector << " is ambiguous." << std::endl;
            consistencyCheck = false;
        }else{
            basis.insert(kvector);
        }
    }

    // namespace name compatible with C++
    if(isalpha(namespaceName[0]) == 0){
        std::cout << "error in the namespace name: the first character of '" << namespaceName << "' should be an alphabetic letter for C++ compliance." << std::endl;
        consistencyCheck = false;
    }

    return consistencyCheck;
}

MetaData::MetaData(const std::string &filename):inputMetricDiagonal(false), identityMetric(false) {

    // open the parser
    ConfigParser parser(filename);

    // load all components of the meta data
    if(!parser.readString("namespace", namespaceName)) {
        std::cerr << "error: failed to find " << "namespace" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readUInt("dimension", dimension)){
        std::cerr << "error: failed to find " << "dimension" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readUInt("max dimension precomputed products", maxDimPrecomputedProducts)){
        std::cerr << "error: failed to find " << "max dimension precomputed products" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readUInt("max dimension basis accessor", maxDimBasisAccessor)){
        std::cerr << "error: failed to find " << "max dimension basis accessor" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readStringList("basis vector name", basisVectorName)){
        std::cerr << "error: failed to find " << "basis vector name" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readMatrix("metric", metric)){
        std::cerr << "error: failed to find " << "metric" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readBool("metric decomposition refinement", useEigenRefinement)){
        std::cerr << "error: failed to find " << "metric decomposition refinement" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readBool("metric decomposition numerical cleanup", useNumericalCleanUp)){
        std::cerr << "error: failed to find " << "metric decomposition numerical cleanup" << std::endl;
        exit(EXIT_FAILURE);
    }

    if(!parser.readDouble("metric decomposition numerical cleanup espilon", epsilon)){
        std::cerr << "error: failed to find " << "metric decomposition numerical cleanup espilon" << std::endl;
        exit(EXIT_FAILURE);
    }

    // check the data consistency
    if(!checkConsistency()){
        std::cerr << "configuration inconsistent ... abord" << std::endl;
        exit(EXIT_FAILURE);
    }

    // metric diagonalization
    if(!metricDiagonalization()){
        std::cerr << "metric diagonalization ... failed" << std::endl;
        std::cerr << "Please try again without the metric decomposition refinement or without the numerical cleanup." << std::endl;
        exit(EXIT_FAILURE);
    }
}

bool MetaData::metricDiagonalization() {

	// compute the metric rank
	if (getRank(metric) == dimension)
		fullRankMetric = true;
	else fullRankMetric = false;

    // check if the metric is already diagonal
    if(isMatrixDiagonal(metric, epsilon)){
        diagonalMetric = metric.diagonal();
        inputMetricDiagonal = true;
        if(isMatrixIdentity(metric,epsilon))
            identityMetric = true;
        return true;
    }

    // ckeck if the metric is a permutation of a diagonal matrix (for fast dual)
    inputMetricPermutationOfDiagonal = isMatrixPermutationOfDiagonal(metric, epsilon);

    // compute the diagonalization
    Eigen::MatrixXd diagonalMatrix;
    eigenDecomposition(metric, transformationMatrix, diagonalMatrix);
//    std::cout << "metric\n" << metric << std::endl;
//    std::cout << "P\n" << transformationMatrix << std::endl;
//    std::cout << "D\n" << diagonalMatrix << std::endl;
//    std::cout << "metric ??\n" << transformationMatrix * diagonalMatrix * transformationMatrix.transpose() << std::endl;

    // invert the transformation matrix
    inverseTransformationMatrix = Eigen::MatrixXd(transformationMatrix.transpose());

    // convert floating points to nearest integers (when possible)
    Eigen::MatrixXd scaleMatrix = Eigen::MatrixXd::Identity(metric.rows(),metric.cols()); // refer to issue #9
    if(useEigenRefinement)
        scaleMatrix = eigenRefinement(transformationMatrix, diagonalMatrix, inverseTransformationMatrix);
//    std::cout << "metric\n" << metric << std::endl;
//    std::cout << "P\n" << transformationMatrix << std::endl;
//    std::cout << "D\n" << diagonalMatrix << std::endl;
//    std::cout << "Pinv\n" << inverseTransformationMatrix << std::endl;
//    std::cout << "metric ??\n" << transformationMatrix * diagonalMatrix * inverseTransformationMatrix << std::endl;

    //numerical clean up
    if(useNumericalCleanUp) {
        transformationMatrix = numericalCleanUp(transformationMatrix,epsilon);
        diagonalMatrix = numericalCleanUp(diagonalMatrix, epsilon);
        inverseTransformationMatrix = numericalCleanUp(inverseTransformationMatrix,epsilon);
    }

    // check if the new metric is identity
    if(isMatrixIdentity(diagonalMatrix,epsilon))
        identityMetric = true;

    // check decomposition
    if(!checkNumericalCleanUp(metric,transformationMatrix,diagonalMatrix,inverseTransformationMatrix, epsilon))
        return false;

    // diagonal metric must be changed if we consider only inverse
    diagonalMatrix = (scaleMatrix*scaleMatrix)*diagonalMatrix;
    if(useNumericalCleanUp)
        diagonalMatrix = numericalCleanUp(diagonalMatrix, epsilon);

    // compact form of the diagonal matrix
    diagonalMetric = diagonalMatrix.diagonal();

    return true;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_PC_IC_0_INCLUDE
#define ITL_PC_IC_0_INCLUDE

#include <boost/mpl/bool.hpp>

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/operation/lower_trisolve.hpp>
#include <boost/numeric/mtl/operation/upper_trisolve.hpp>
#include <boost/numeric/mtl/matrix/upper.hpp>
#include <boost/numeric/mtl/matrix/strict_lower.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/itl/pc/solver.hpp>

namespace itl { namespace pc {

template <typename Matrix, typename Value= typename mtl::Collection<Matrix>::value_type>
class ic_0
{
  public:
    typedef Value                                                    value_type;
    typedef typename mtl::Collection<Matrix>::size_type              size_type;
    typedef ic_0                                                     self;

    typedef mtl::matrix::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
    typedef mtl::matrix::compressed2D<value_type, para>                      U_type;
#ifndef ITL_IC_0_ONE_MATRIX
    typedef U_type                                                   L_type;
#else
    typedef typename mtl::matrix::transposed_view<U_type>            L_type;
#endif
    typedef mtl::matrix::detail::lower_trisolve_t<L_type, mtl::tag::inverse_diagonal, true> lower_solver_t;
    typedef mtl::matrix::detail::upper_trisolve_t<U_type, mtl::tag::inverse_diagonal, true> upper_solver_t;

    ic_0(const Matrix& A) : f(A, U), L(trans(U)), lower_solver(L), upper_solver(U) {}

    // solve x = U^* U y --> y= U^{-1} U^{-*} x
    template <typename Vector>
    Vector solve(const Vector& x) const
    {
	mtl::vampir_trace<5036> tracer;
	return inverse_upper_trisolve(U, inverse_lower_trisolve(adjoint(U), x));
    }

    // solve x = U^* y --> y0= U^{-*} x
    template <typename VectorIn, typename VectorOut>
    const VectorOut& solve_lower(const VectorIn& x, VectorOut&) const
    {
	static VectorOut y0;
	y0.change_resource(resource(x));
	lower_solver(x, y0);
	return y0;
    }

    // solve x = U^* U y --> y= U^{-1} U^{-*} x
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5037> tracer;
	const VectorOut& y0= solve_lower(x, y);

	y.checked_change_resource(x);
	upper_solver(y0, y);
    }

    // solve x = (LU)^* y --> y= L^{-*} U^{-*} x
    template <typename Vector>
    Vector adjoint_solve(const Vector& x) const
    {
	mtl::vampir_trace<5044> tracer;
	return solve(x);
    }

    // solve x = (LU)^* y --> y= L^{-*} U^{-*} x
    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5044> tracer;
	solve(x, y);
    }

    L_type get_L() { return L_type(L); }
    U_type get_U() { return U; }

  protected:
    template <typename VectorOut, typename Solver> friend struct ic_0_evaluator;

    // Dummy type to perform factorization in initializer list not in
    struct factorizer
    {
	factorizer(const Matrix &A, U_type& U)
	{   factorize(A, U, mtl::traits::is_sparse<Matrix>(), boost::is_same<Value, typename mtl::Collection<Matrix>::value_type>());  }

	template <typename T>
	void factorize(const Matrix&, U_type&, boost::mpl::false_, T)
	{   MTL_THROW_IF(true, mtl::logic_error("IC(0) is not suited for dense matrices"));	}

	// When we change the value_type then the factorization is still performed with that of A
	template <typename UF>
	void factorize(const Matrix& A, UF& U, boost::mpl::true_, boost::mpl::false_)
	{
	    typedef mtl::matrix::compressed2D<typename mtl::Collection<Matrix>::value_type, para> tmp_type;
	    tmp_type U_tmp;
	    factorize(A, U_tmp, boost::mpl::true_(), boost::mpl::true_());
	    U= U_tmp;
	}

	// Factorization adapted from Saad
	// Undefined (runtime) behavior if matrix is not symmetric
	// UF is type for the factorization
	template <typename UF>
	void factorize(const Matrix& A, UF& U, boost::mpl::true_, boost::mpl::true_)
	{
	    using namespace mtl; using namespace mtl::tag;  using mtl::traits::range_generator;
	    using math::reciprocal; using mtl::matrix::upper;
	    mtl::vampir_trace<5035> tracer;

	    // For the factorization we take still the value_type of A and later we copy it maybe to another value_type
	    typedef typename mtl::Collection<Matrix>::value_type      value_type;
	    typedef typename range_generator<row, UF>::type       cur_type;
	    typedef typename range_generator<nz, cur_type>::type      icur_type;

	    MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
	    U= upper(A);

	    typename mtl::traits::col<UF>::type                   col(U);
	    typename mtl::traits::value<UF>::type                 value(U);

	    cur_type kc= begin<row>(U), kend= end<row>(U);
	    for (size_type k= 0; kc != kend; ++kc, ++k) {

		icur_type ic= begin<nz>(kc), iend= end<nz>(kc);
		MTL_DEBUG_THROW_IF(col(*ic) != k, mtl::missing_diagonal());

		// U[k][k]= 1.0 / sqrt(U[k][k]);
		value_type inv_dia= reciprocal(sqrt(value(*ic)));
		value(*ic, inv_dia);
		// icur_type jbegin=
		++ic;
		for (; ic != iend; ++ic) {
		    // U[k][i] *= U[k][k]
		    value_type d= value(*ic) * inv_dia;
		    value(*ic, d);
		    size_type i= col(*ic);

		    // find non-zeros U[j][i] below U[k][i] for j in (k, i]
		    // 1. Go to ith row in U (== ith column in U)
		    cur_type irow(i, U); // = begin<row>(U); irow+= i;
		    // 2. Find nonzeros with col() in (k, i]
		    icur_type jc= begin<nz>(irow), jend= end<nz>(irow);
		    while (col(*jc) <= k)  ++jc;
		    while (col(*--jend) > i) ;
		    ++jend;

		    for (; jc != jend; ++jc) {
			size_type j= col(*jc);
			U.lvalue(j, i)-= d * U[k][j];
		    }
		    // std::cout << "U after eliminating U[" << i << "][" << k << "] =\n" << U;
		}
	    }
	}
    };

    U_type                       U;
    factorizer                   f;
    L_type                       L;
    lower_solver_t               lower_solver;
    upper_solver_t               upper_solver;
};

#if 0
template <typename Matrix, typename Value, typename Vector>
struct ic_0_solver
  : mtl::vector::assigner<ic_0_solver<Matrix, Value, Vector> >
{
    typedef ic_0<Matrix, Value> pc_type;

    ic_0_solver(const ic_0<Matrix, Value>& P, const Vector& x) : P(P), x(x) {}

    template <typename VectorOut>
    void assign_to(VectorOut& y) const
    {	P.solve(x, y);    }

    const ic_0<Matrix, Value>& P;
    const Vector&              x;
};
#endif

template <typename VectorOut, typename Solver>
struct ic_0_evaluator
{
    typedef typename Solver::pc_type                        pc_type;
    typedef typename pc_type::size_type                     size_type;
    typedef typename mtl::Collection<VectorOut>::value_type out_value_type;

    ic_0_evaluator(VectorOut& y, const Solver& s)
      : y(y), s(s), U(s.P.U), y0(s.P.solve_lower(s.x, y)) { MTL_DEBUG_ARG(lr= 99999999); }

    void operator()(size_type i) { at<0>(i); }
    void operator[](size_type i) { at<0>(i); }

    template <unsigned Offset>
    void at(size_type r)
    {
#ifndef NDEBUG
	MTL_THROW_IF(r+Offset >= lr, mtl::logic_error("Traversal must be backward")); lr= r+Offset;
#endif
	size_type j0= U.ref_major()[r+Offset];
	const size_type cj1= U.ref_major()[r+Offset+1];
	MTL_DEBUG_THROW_IF(j0 == cj1 || U.ref_minor()[j0] != r+Offset, mtl::missing_diagonal());
	out_value_type rr= y0[r+Offset], dia= U.data[j0++];
	for (; j0 != cj1; ++j0) {
	    MTL_DEBUG_THROW_IF(U.ref_minor()[j0] <= r+Offset, mtl::logic_error("Matrix entries must be sorted for this."));
	    rr-= U.data[j0] * y[U.ref_minor()[j0]];
	}
	y[r+Offset]= rr * dia;
    }

    VectorOut&                               y;
    const Solver&                            s;
    const typename pc_type::U_type&          U;
    const VectorOut&                         y0;
    MTL_DEBUG_ARG(size_type                  lr;)
};

template <typename VectorOut, typename Solver>
inline std::size_t size(const ic_0_evaluator<VectorOut, Solver>& eval)
{   return size(eval.y); }

template <typename Matrix, typename Value, typename Vector>
solver<ic_0<Matrix, Value>, Vector, false>
inline solve(const ic_0<Matrix, Value>& P, const Vector& x)
{
    return solver<ic_0<Matrix, Value>, Vector, false>(P, x);
}

template <typename Matrix, typename Value, typename Vector>
solver<ic_0<Matrix, Value>, Vector, true>
inline adjoint_solve(const ic_0<Matrix, Value>& P, const Vector& x)
{
    return solver<ic_0<Matrix, Value>, Vector, true>(P, x);
}

}} // namespace itl::pc

namespace mtl { namespace vector {
    using itl::pc::size;
}} // namespace mtl::vector

#endif // ITL_PC_IC_0_INCLUDE


/* =========================================================================
   Copyright (c) 2012-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
                             -----------------
               ViennaFEM - The Vienna Finite Element Method Library
                             -----------------

   Author:     Karl Rupp                          rupp@iue.tuwien.ac.at

   License:    MIT (X11), see file LICENSE in the ViennaFEM base directory
============================================================================ */

// remove assert() statements and the like in order to get reasonable performance
#ifndef NDEBUG
  #define NDEBUG
#endif

// include necessary system headers
#include <iostream>

// ViennaFEM includes:
#include "viennafem/fem.hpp"
#include "viennafem/io/vtk_writer.hpp"

// ViennaGrid includes:
#include "viennagrid/forwards.hpp"
#include "viennagrid/config/default_configs.hpp"
#include "viennagrid/io/netgen_reader.hpp"

// ViennaData includes:
#include "viennadata/api.hpp"

// ViennaMath includes:
#include "viennamath/expression.hpp"

// Boost.uBLAS includes:
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//ViennaCL includes:
#ifndef VIENNACL_HAVE_UBLAS
 #define VIENNACL_HAVE_UBLAS
#endif

#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"

int main()
{
  typedef viennagrid::hexahedral_3d_mesh                                                  DomainType;
  typedef viennagrid::result_of::segmentation<DomainType>::type                           SegmentationType;

  typedef viennagrid::result_of::element<DomainType, viennagrid::vertex_tag>::type        VertexType;
  typedef viennagrid::result_of::element_range<DomainType, viennagrid::vertex_tag>::type  VertexContainer;
  typedef viennagrid::result_of::iterator<VertexContainer>::type                          VertexIterator;

  typedef boost::numeric::ublas::compressed_matrix<viennafem::numeric_type>  MatrixType;
  typedef boost::numeric::ublas::vector<viennafem::numeric_type>             VectorType;

  typedef viennamath::function_symbol   FunctionSymbol;
  typedef viennamath::equation          Equation;

  //
  // Create a domain from file
  //
  DomainType my_domain;
  SegmentationType segments(my_domain);

  //
  // Create a storage object
  //
  typedef viennadata::storage<> StorageType;
  StorageType   storage;

  try
  {
    viennagrid::io::netgen_reader my_reader;
    my_reader(my_domain, segments, "../examples/data/cube343_hex.mesh");
  }
  catch (...)
  {
    std::cerr << "File-Reader failed. Aborting program..." << std::endl;
    exit(EXIT_FAILURE);
  }

  //
  // Specify two PDEs:
  //
  FunctionSymbol u(0, viennamath::unknown_tag<>());   //an unknown function used for PDE specification
  Equation poisson_equ_1 = viennamath::make_equation( viennamath::laplace(u), -1);
  Equation poisson_equ_2 = viennamath::make_equation( viennamath::laplace(u), -1);

  MatrixType system_matrix_1, system_matrix_2;
  VectorType load_vector_1, load_vector_2;

  //
  // Setting boundary information on domain (this should come from device specification)
  //
  //setting some boundary flags:
  VertexContainer vertices = viennagrid::elements<VertexType>(my_domain);
  for (VertexIterator vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    // First equation: Homogeneous boundary conditions at x=0, x=1, y=0, or y=1
    if ( viennagrid::point(my_domain, *vit)[0] == 0.0 || viennagrid::point(my_domain, *vit)[0] == 1.0
         || viennagrid::point(my_domain, *vit)[1] == 0.0 || viennagrid::point(my_domain, *vit)[1] == 1.0 )
      viennafem::set_dirichlet_boundary(storage, *vit, 0.0, 0);  //simulation with ID 0 uses homogeneous boundary data

    // Boundary for second equation (ID 1): 0 at left boundary, 1 at right boundary
    if ( viennagrid::point(my_domain, *vit)[0] == 0.0)
      viennafem::set_dirichlet_boundary(storage, *vit, 0.0, 1);
    else if ( viennagrid::point(my_domain, *vit)[0] == 1.0)
      viennafem::set_dirichlet_boundary(storage, *vit, 1.0, 1);
  }

  //
  // Create PDE solver functors: (discussion about proper interface required)
  //
  viennafem::pde_assembler<StorageType> fem_assembler(storage);

  //
  // Solve system and write solution vector to pde_result:
  // (discussion about proper interface required. Introduce a pde_result class?)
  //
  fem_assembler(viennafem::make_linear_pde_system(poisson_equ_1,
                                                  u,
                                                  viennafem::make_linear_pde_options(0,
                                                                                viennafem::lagrange_tag<1>(),
                                                                                viennafem::lagrange_tag<1>())
                                                 ),
                my_domain,
                system_matrix_1,
                load_vector_1
               );

  fem_assembler(viennafem::make_linear_pde_system(poisson_equ_2,
                                                  u,
                                                  viennafem::make_linear_pde_options(1,
                                                                                viennafem::lagrange_tag<1>(),
                                                                                viennafem::lagrange_tag<1>())
                                                 ),
                my_domain,
                system_matrix_2,
                load_vector_2
               );

  VectorType pde_result_1 = viennacl::linalg::solve(system_matrix_1, load_vector_1, viennacl::linalg::cg_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix_1, pde_result_1) - load_vector_1) << std::endl;

  VectorType pde_result_2 = viennacl::linalg::solve(system_matrix_2, load_vector_2, viennacl::linalg::cg_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix_2, pde_result_2) - load_vector_2) << std::endl;

  //
  // Writing solution back to domain (discussion about proper way of returning a solution required...)
  //
  viennafem::io::write_solution_to_VTK_file(pde_result_1, "poisson_3d_hex_1", my_domain, segments, storage, 0);
  viennafem::io::write_solution_to_VTK_file(pde_result_2, "poisson_3d_hex_2", my_domain, segments, storage, 1);

  std::cout << "*****************************************" << std::endl;
  std::cout << "* Poisson solver finished successfully! *" << std::endl;
  std::cout << "*****************************************" << std::endl;
  return EXIT_SUCCESS;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_ACOSH_HPP
#define STAN_MATH_PRIM_SCAL_FUN_ACOSH_HPP

#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <stan/math/prim/scal/meta/likely.hpp>
#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <boost/math/special_functions/acosh.hpp>

namespace stan {
  namespace math {

    /**
     * Return the inverse hyperbolic cosine of the specified value.
     * Returns nan for nan argument.
     *
     * @param[in] x Argument.
     * @return Inverse hyperbolic cosine of the argument.
     * @throw std::domain_error If argument is less than 1.
     */
    inline double acosh(double x) {
      if (unlikely(is_nan(x)))
        return x;
      else
        return boost::math::acosh(x, boost_policy_t());
    }

    /**
     * Integer version of acosh.
     *
     * @param[in] x Argument.
     * @return Inverse hyperbolic cosine of the argument.
     * @throw std::domain_error If argument is less than 1.
     */
    inline double acosh(int x) {
      return acosh(static_cast<double>(x));
    }

  }
}
#endif


//habitus.cpp
/*
* habitus detection from 3D density map of some particle
* obtained by tomography reconstruction
* (C) 2005 olegabr. All rights reserved.
*/

#include <Magick++.h>

#include <iostream>
#include <sstream>
#include <iomanip>
#include <fstream>
using std::cout; using std::endl;

#include <cmath>
using std::sqrt;

#include <boost/lexical_cast.hpp>
using boost::lexical_cast;

#include <object3d.h>
#include "transforms_ex.h"
#include "grid2d.h"
#include "angle.h"

namespace {
	void usage()
	{
		std::cout << "habitus images_directory\n";
	}
}

int main(int argc, char* argv[])
{
	if (argc < 2)
	{
		usage();
		return 0;
	}
	try {
		Magick::InitializeMagick(argv[0]);

		// load the 3D object
		object3d particle(argv[1]);

		// calculate or get from user the threshold value
		// points with values less then the threshold value
		// are not considered
		object3d::value_type threshold = calc_mean(particle);

		// get from user, or set by default the lambda value
		object3d::value_type lambda = 1;

		// calculate the particle's center
		// for example, the mass center
		point3d center = calc_gravity_center(particle);

		// calculate Rmin and Rmax values
		// Rmin - is the maximum radius of continuos density
		// without holes
		// Rmax - is the minimum radius of sphere that contains all
		// points with non-zero (note threshold) values
		point3d::value_type Rmin = calc_min_radius(particle, center, threshold);
		point3d::value_type Rmax = calc_max_radius(particle, center, Rmin, threshold);

		// construct a 2D grid in theta-phi space with d(theta), d(phi) steps
		typedef grid2d<angle_t, angle_t> grid_t;

		const angle_t theta_min(0);
		const angle_t theta_max(90);
		const angle_t d_theta(1);
		grid_t::xgrid_type grid_theta(theta_min, theta_max, d_theta);

		const angle_t phi_min(0);
		const angle_t phi_max(360);
		const angle_t d_phi(1);
		grid_t::ygrid_type grid_phi(phi_min, phi_max, d_phi);

		grid_t grid(grid_theta, grid_phi);

		// for each node of this grid
		std::vector<std::pair<grid_t::value_type, double> > values(grid.size());
		for (grid_t::const_iterator gi = grid.begin(); gi != grid.end(); ++gi)
		{
			// find the L: the sum between Lmax and L planes is
			// equal to the sum between Lmin and L planes.
			// Lmin and Lmax are found during this process.
			// as an additional result we have a M, Sfull and h values here.
			// M is a sum of masses between Lmin and Lmax
			// h is a distance between them
			// Sfull is an area of a figure given by intersection of
			// the Lmin plane with the particle's shape

			// S is an area of a figure given by intersection of
			// the L plane with the particle's shape

			// calc the M' = M / (h * Sfull)

			// calc the S' = S / Sfull

			// calc the functional:
			// G = (M' - lambda * (S' - 1)) / (1 + lambda)

			typedef grid1d<point3d::value_type> lgrid_t;
			lgrid_t lgrid(Rmin, Rmax, particle.get_point_size());

			std::vector<std::pair<lgrid_t::value_type, double> > lvalues(lgrid.size());
			for (lgrid_t::const_iterator lgi = lgrid.begin(); lgi != lgrid.end(); ++lgi)
			{
			}

			double G, S;
			calc_G_S(particle, Rmin, Rmax, *gi, G, S);
			values.push_back(std::make_pair(*gi, G));
			//const point3d::value_type dL = particle.get_point_size();
			//for (point3d::value_type L(Rmin); L <= Rmax; L += dL)
			//{
			//}
		}

		// select points where a local minimum is reached

		// for each local minimum:
			// repeat the described procedure for new grid with
			// fine-tuned d(theta), d(phi) in the area of this local minimum
	}
	catch (boost::bad_lexical_cast &ex) {
		cout << ex.what() << endl;
	}
	catch (Magick::Exception &ex) {
		cout << ex.what() << endl;
	}
	catch (std::exception &e) {
		cout << e.what() << endl;
	}
	catch (...) {
		cout << "Unknown exception occured." << endl;
	}

	return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/SimulationSetup/tudatSimulationHeader.h"
namespace tudat
{
namespace unit_tests
{

using namespace mathematical_constants;

BOOST_AUTO_TEST_SUITE( test_cr3bp_propagation )

//! Test if CR3BP propagation is working correctly
BOOST_AUTO_TEST_CASE( testCR3BPPropagation )
{
    using namespace tudat::propagators;
    using namespace tudat::numerical_integrators;
    using namespace tudat::basic_mathematics;
    using namespace tudat::numerical_integrators;
    using namespace tudat::orbital_element_conversions;

    // Test three different cases of CR3BP propagation. Reference values independently generated bvy Y. Liu
    for( int testCase = 0; testCase < 3; testCase++ )
    {
        // Define initial normalized state for each case
        Eigen::Vector6d massLessBodyInitialCartesianState;
        if( testCase == 0 )
        {
            massLessBodyInitialCartesianState[ xCartesianPositionIndex ] = 0.994;
            massLessBodyInitialCartesianState[ yCartesianPositionIndex ] = 0.853;
            massLessBodyInitialCartesianState[ zCartesianPositionIndex ] = 0.312;
            massLessBodyInitialCartesianState[ xCartesianVelocityIndex ] = 0.195;
            massLessBodyInitialCartesianState[ yCartesianVelocityIndex ] = -0.211;
            massLessBodyInitialCartesianState[ zCartesianVelocityIndex ] = 0.15;
        }
        else if( testCase == 1 )
        {
            massLessBodyInitialCartesianState[ xCartesianPositionIndex ] = 1.05;
            massLessBodyInitialCartesianState[ yCartesianPositionIndex ] = 0.13;
            massLessBodyInitialCartesianState[ zCartesianPositionIndex ] = 0.0;
            massLessBodyInitialCartesianState[ xCartesianVelocityIndex ] = 0.2;
            massLessBodyInitialCartesianState[ yCartesianVelocityIndex ] = 0.2;
            massLessBodyInitialCartesianState[ zCartesianVelocityIndex ] = 0.0;
        }
        else if( testCase == 2 )
        {
            massLessBodyInitialCartesianState[ xCartesianPositionIndex ] = 0.988;
            massLessBodyInitialCartesianState[ yCartesianPositionIndex ] = 0.5;
            massLessBodyInitialCartesianState[ zCartesianPositionIndex ] = -0.1;
            massLessBodyInitialCartesianState[ xCartesianVelocityIndex ] = 0.4;
            massLessBodyInitialCartesianState[ yCartesianVelocityIndex ] = 0.1;
            massLessBodyInitialCartesianState[ zCartesianVelocityIndex ] = -0.1;
        }

        // Set normalized-mass parameter and propagation intervals
        double massParameter = 0.0;
        double simulationStartEpoch = 0.0;
        double simulationEndEpoch = 0.0;
        double timeStep = 0.0;
        if( testCase == 0 )
        {
            massParameter = 2.528e-5;
            simulationEndEpoch = 20.0;
            timeStep = 0.0001;
        }
        else if( testCase == 1 )
        {
            massParameter = 1.21506683e-2;
            simulationEndEpoch = 10.0;
            timeStep = 0.0001;
        }
        else if( testCase == 2 )
        {
            massParameter = 9.537e-4;
            simulationEndEpoch = 15.0;
            timeStep = 0.001;
        }

        // Set integrator settings
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< IntegratorSettings< > >
                ( rungeKutta4, simulationStartEpoch, timeStep );

        // Propagate dynamics
        std::map< double, Eigen::Vector6d > stateHistory = performCR3BPIntegration(
                    integratorSettings, massParameter,
                    massLessBodyInitialCartesianState,
                    simulationEndEpoch );

        // Retrieve dynamics and set iterator at final time
        std::map< double, Eigen::Vector6d >::reverse_iterator stateIterator =
                stateHistory.rbegin( );

        // Check for overshoot, and increment iterator if overshoot found.
        if( stateIterator->first - simulationEndEpoch > timeStep / 2.0 )
        {
            stateIterator++;
        }

        // Set stets computed independently by Y. Loi
        Eigen::Vector6d expectedFinalState;
        if( testCase == 0 )
        {
            expectedFinalState << -1.34313636385140, -1.54200249942130, -0.416194453794142,  -0.863033291171519,
                    1.12530842202949, 0.181821699265344;

        }
        else if( testCase == 1 )
        {
            expectedFinalState << -0.636895857906707, 0.977767324698871, 0, 0.133701380037512, 0.0704414501083985, 0;

        }
        else if( testCase == 2 )
        {
            expectedFinalState << 0.770391451297307, -1.53581492635515, -0.181256821037128,  -0.783687153930728,
                    -0.805869201715479, -0.0200378316314463;

        }

        // Check Tudat output with reference values. Remaining differences can be attributed to different integrators
        for( unsigned int i = 0; i < 6; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( stateIterator->second( i ) - expectedFinalState( i ) ), 1.0E-10 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


// Copyright 2018 Tier IV, Inc. All rights reserved.
//
// 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 VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_UTILS_HPP_
#define VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_UTILS_HPP_

#include <autoware_utils/autoware_utils.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <boost/optional.hpp>

#include <tf2/utils.h>

#include <cmath>
#include <limits>

namespace velocity_controller_utils
{
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Quaternion;

/**
 * @brief check if trajectory is invalid or not
 */
bool isValidTrajectory(const Trajectory & traj);

/**
 * @brief calculate distance to stopline from current vehicle position where velocity is 0
 */
double calcStopDistance(const Point & current_pos, const Trajectory & traj);

/**
 * @brief calculate pitch angle from estimated current pose
 */
double getPitchByPose(const Quaternion & quaternion);

/**
 * @brief calculate pitch angle from trajectory on map
 * @param [in] trajectory input trajectory
 * @param [in] closest_idx nearest index to current vehicle position
 * @param [in] wheel_base length of wheel base
 */
double getPitchByTraj(
  const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);

/**
 * @brief calculate elevation angle
 */
double calcElevationAngle(const Point & p_from, const Point & p_to);

/**
 * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for
 * delayed time
 */
Pose calcPoseAfterTimeDelay(
  const Pose & current_pose, const double delay_time, const double current_vel);

/**
 * @brief apply linear interpolation
 * @param [in] v_from first value
 * @param [in] v_to second value
 * @param [in] ratio ratio between o_from and o_to for interpolation
 */
double lerp(const double v_from, const double v_to, const double ratio);

/**
 * @brief apply linear interpolation to position
 * @param [in] p_from first position
 * @param [in] p_to second position
 * @param [in] ratio ratio between o_from and o_to for interpolation
 */
template <class T>
T lerpXYZ(const T & p_from, const T & p_to, const double ratio)
{
  T point;
  point.x = lerp(p_from.x, p_to.x, ratio);
  point.y = lerp(p_from.y, p_to.y, ratio);
  point.z = lerp(p_from.z, p_to.z, ratio);
  return point;
}

/**
 * @brief apply linear interpolation to orientation
 * @param [in] o_from first orientation
 * @param [in] o_to second orientation
 * @param [in] ratio ratio between o_from and o_to for interpolation
 */
Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);

/**
 * @brief apply linear interpolation to trajectory point that is nearest to a certain point
 * @param [in] points trajectory points
 * @param [in] point Interpolated point is nearest to this point.
 */
template <class T>
TrajectoryPoint lerpTrajectoryPoint(const T & points, const Point & point)
{
  TrajectoryPoint interpolated_point;

  const size_t nearest_seg_idx = autoware_utils::findNearestSegmentIndex(points, point);

  const double len_to_interpolated =
    autoware_utils::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point);
  const double len_segment =
    autoware_utils::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1);
  const double interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0);

  {
    const size_t i = nearest_seg_idx;

    interpolated_point.pose.position =
      lerpXYZ(points.at(i).pose.position, points.at(i + 1).pose.position, interpolate_ratio);
    interpolated_point.pose.orientation = lerpOrientation(
      points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
    interpolated_point.twist.linear =
      lerpXYZ(points.at(i).twist.linear, points.at(i + 1).twist.linear, interpolate_ratio);
    interpolated_point.twist.angular =
      lerpXYZ(points.at(i).twist.angular, points.at(i + 1).twist.angular, interpolate_ratio);
    interpolated_point.accel.linear =
      lerpXYZ(points.at(i).accel.linear, points.at(i + 1).accel.linear, interpolate_ratio);
    interpolated_point.accel.angular =
      lerpXYZ(points.at(i).accel.angular, points.at(i + 1).accel.angular, interpolate_ratio);
  }

  return interpolated_point;
}

/**
 * @brief limit variable whose differential is within a certain value
 * @param [in] input_val current value
 * @param [in] prev_val previous value
 * @param [in] dt time between current and previous one
 * @param [in] lim_val limitation value for differential
 */
double applyDiffLimitFilter(
  const double input_val, const double prev_val, const double dt, const double lim_val);

/**
 * @brief limit variable whose differential is within a certain value
 * @param [in] input_val current value
 * @param [in] prev_val previous value
 * @param [in] dt time between current and previous one
 * @param [in] max_val maximum value for differential
 * @param [in] min_val minimum value for differential
 */
double applyDiffLimitFilter(
  const double input_val, const double prev_val, const double dt, const double max_val,
  const double min_val);
}  // namespace velocity_controller_utils

#endif  // VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_UTILS_HPP_


/**
 * @Copyright 2020, Wuhan University of Technology
 * @Author: Pengwei Zhou
 * @Date: 2021/3/12 下午11:19
 * @FileName: feature_extract.hpp.h
 * @Description: ${DESCRIPTION}
 * @License: See LICENSE for the license information
 */

#ifndef TLOAM_FEATURE_EXTRACT_HPP
#define TLOAM_FEATURE_EXTRACT_HPP

#include <iostream>
#include <vector>
#include <cmath>
#include <thread>
#include <mutex>
#include <future>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <yaml-cpp/yaml.h>

#include "tloam/models/utils/sensor_data.hpp"
#include "tloam/open3d/open3d_to_ros.hpp"
#include "tloam/models/utils/utils.hpp"
#include "tloam/models/utils/work_space_path.h"

namespace tloam{
// feature extract based on Principe Component Analysis
class featureExtract{
public:
  struct PCAInfo{
    Eigen::Vector3d normal_dir;
    double cvr, flatness, sphericity;
    int cur_index, num_sum;
    std::vector<int> neigh_index;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  };
  explicit featureExtract();
  ~featureExtract() = default;

  void initConfig(const YAML::Node& node);

  /**
   * @brief PCA information calculation by SVD
   * @param cloud_in_, input cloud
   * @param pca_info_, output info
   * @param r_, KNN radius
   * @param K_, domain number
   * @return true if success otherwise false
   */
  bool calculatePCAInfo(
    CloudData& cloud_in_,
    std::vector<PCAInfo>& pca_info_,
    double r_,
    int K_
  );

  /**
   * @brief extract planar features and sphere features
   * @param cloud_in_, input cloud
   * @param planar_scan_index, current scan planar features
   * @param planar_submap_index, submap planar features
   * @param sphere_scan_index, current scan planar features
   * @param sphere_submap_index, submap planar features
   * @return true if success otherwise false
   */
  bool extractPlanarSphere(
    CloudData& cloud_in_,
    std::vector<size_t>& planar_scan_index,
    std::vector<size_t>& planar_submap_index,
    std::vector<size_t>& sphere_scan_index,
    std::vector<size_t>& sphere_submap_index
  );

private:

  int num_threads_{};
  double radius;
  int K;

  double planar_submap_thres;
  double planar_vertic_thres;
  double planar_scan_thres;
  double cvr_submap;
  double cvr_scan;

  int planar_num;
  int sphere_num;
  int min_neigh;
};
}

#endif //TLOAM_FEATURE_EXTRACT_HPP


#include <vector>
#include <random>
#include <benchmark/benchmark.h>
#include <boost/math/special_functions/chebyshev.hpp>

template<class Real>
void ChebyshevClenshaw(benchmark::State& state)
{
    std::vector<Real> v(state.range(0));
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<Real> unif(-1,1);
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = unif(mt);
    }

    using boost::math::chebyshev_clenshaw_recurrence;
    Real x = unif(mt);
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(chebyshev_clenshaw_recurrence(v.data(), v.size(), x));
    }
    state.SetComplexityN(state.range(0));
}

template<class Real>
void TranslatedChebyshevClenshaw(benchmark::State& state)
{
    std::vector<Real> v(state.range(0));
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<Real> unif(-1,1);
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = unif(mt);
    }

    using boost::math::detail::unchecked_chebyshev_clenshaw_recurrence;
    Real x = unif(mt);
    Real a = -2;
    Real b = 5;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(unchecked_chebyshev_clenshaw_recurrence(v.data(), v.size(), a, b, x));
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(TranslatedChebyshevClenshaw, double)->RangeMultiplier(2)->Range(1<<1, 1<<22)->Complexity(benchmark::oN);
BENCHMARK_TEMPLATE(ChebyshevClenshaw, double)->RangeMultiplier(2)->Range(1<<1, 1<<22)->Complexity(benchmark::oN);

BENCHMARK_MAIN();