//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_BITWISE_FUNCTIONS_GENERIC_HI_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_GENERIC_HI_HPP_INCLUDED

#include <boost/simd/bitwise/functions/hi.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/lo.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( hi_, tag::cpu_
                                    , (A0)
                                    , (generic_< arithmetic_<A0> >)
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0,unsigned>::type  result_type;
    typedef typename dispatch::meta::scalar_of<result_type>::type   s_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      const s_t half_bits = sizeof(s_t)*(CHAR_BIT/2);
      return lo( shri(a0, half_bits) );
    }
  };
} } }

#endif


// ================================================================================================
//
// This file is part of the CAMPVis Software Framework.
//
// If not explicitly stated otherwise: Copyright (C) 2012-2015, all rights reserved,
//      Christian Schulte zu Berge <christian.szb@in.tum.de>
//      Chair for Computer Aided Medical Procedures
//      Technische Universitaet Muenchen
//      Boltzmannstr. 3, 85748 Garching b. Muenchen, Germany
//
// For a full list of authors and contributors, please refer to the file "AUTHORS.txt".
//
// 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 "tensoranalyzer.h"

#include "cgt/cgt_math.h"
#include "cgt/logmanager.h"

#include <Eigen/Eigenvalues>
#include <tbb/tbb.h>
#include <tbb/atomic.h>

#include "core/datastructures/imagedata.h"
#include "core/tools/stringutils.h"

namespace campvis {

    static const GenericOption<TensorAnalyzer::DegeneratedEvHandling> handlingModes[4] = {
        GenericOption<TensorAnalyzer::DegeneratedEvHandling>("none", "None", TensorAnalyzer::NONE),
        GenericOption<TensorAnalyzer::DegeneratedEvHandling>("mask", "Mask", TensorAnalyzer::MASK),
        GenericOption<TensorAnalyzer::DegeneratedEvHandling>("invert", "Invert", TensorAnalyzer::INVERT),
        GenericOption<TensorAnalyzer::DegeneratedEvHandling>("shift", "Shift", TensorAnalyzer::SHIFT)
    };

    static const GenericOption<std::string> measurementOptions[15] = {
        GenericOption<std::string>("Disabled", "Disabled"),
        GenericOption<std::string>("EigenValue1", "Eigenvalue 1"),
        GenericOption<std::string>("EigenValue2", "Eigenvalue 2"),
        GenericOption<std::string>("EigenValue3", "Eigenvalue 3"),
        GenericOption<std::string>("MainEigenvector", "Main Eigenvector"),
        GenericOption<std::string>("VolumeRatio", "Volume Ratio"),
        GenericOption<std::string>("FractionalAnisotropy", "Fractional Anisotropy"),
        GenericOption<std::string>("RelativeAnisotropy", "Relative Anisotropy"),
        GenericOption<std::string>("MeanDiffusivity", "Mean Diffusivity"),
        GenericOption<std::string>("Trace", "Trace"),
        GenericOption<std::string>("AxialDiffusivity", "Axial Diffusivity"),
        GenericOption<std::string>("RadialDiffusivity", "Radial Diffusivity"),
        GenericOption<std::string>("LinearAnisotropy", "Linear Anisotropy"),
        GenericOption<std::string>("PlanarAnisotropy", "Planar Anisotropy"),
        GenericOption<std::string>("Isotropy", "Isotropy")
    };

    const std::string TensorAnalyzer::loggerCat_ = "CAMPVis.modules.classification.TensorAnalyzer";

    TensorAnalyzer::OutputPropertyPair::OutputPropertyPair(size_t index)
        : _imageId("OutputId" + StringUtils::toString(index), "Output " + StringUtils::toString(index) + " Image", "TensorAnalyzer.output" + StringUtils::toString(index), DataNameProperty::WRITE)
        , _imageType("OutputType" + StringUtils::toString(index), "Output " + StringUtils::toString(index) + " Image Type", measurementOptions, 15)
    {}

    TensorAnalyzer::TensorAnalyzer()
        : AbstractProcessor()
        , p_inputImage("InputImage", "Input Tensor Image", "tensors", DataNameProperty::READ)
        , p_evalsImage("EvalsImage", "Output Eigenvalues Image", "TensorAnalyzer.eigenvalues", DataNameProperty::WRITE)
        , p_evecsImage("EvecsImage", "Output Eigenvectors Image", "TensorAnalyzer.eigenvectors", DataNameProperty::WRITE)
        , p_degeneratedHandling("DegeneratedHandling", "Handling of Degenerated Tensors", handlingModes, 4)
        , p_maskMixedTensors("MaskMixedTensors", "Mask Mixed Tensors", true)
        , p_addOutputButton("AddOutputButton", "Add Output")
        , _eigenvalues(0)
        , _eigenvectors(0)
    {
        addProperty(p_inputImage, INVALID_RESULT | EIGENSYSTEM_INVALID);
        addProperty(p_evalsImage);
        addProperty(p_evecsImage);
        addProperty(p_degeneratedHandling);
        addProperty(p_maskMixedTensors);
        addProperty(p_addOutputButton, VALID);
        addOutput();

        p_addOutputButton.s_clicked.connect(this, &TensorAnalyzer::addOutput);
    }

    TensorAnalyzer::~TensorAnalyzer() {

    }

    void TensorAnalyzer::updateResult(DataContainer& data) {
        if (getInvalidationLevel() & EIGENSYSTEM_INVALID) {
            computeEigensystem(data);
        }

        if (_eigenvalues.getData() != 0 && _eigenvectors.getData() != 0) {
            for (size_t i = 0; i < p_outputProperties.size(); ++i) {
                computeOutput(data, i);
            }
        }
        else {
            LERROR("Could not compute Eigensystem");
        }
    }

    void TensorAnalyzer::computeEigensystem(DataContainer& data) {
        // get input tensor image
        GenericImageRepresentationLocal<float, 6>::ScopedRepresentation input(data, p_inputImage.getValue());

        if (input != 0) {
            // create output images for eigenvalues (stored as vec3) and eigenvectors (stored as mat3)
            ImageData* evals = new ImageData(input->getDimensionality(), input->getSize(), 3);
            GenericImageRepresentationLocal<float, 3>* evalRep = GenericImageRepresentationLocal<float, 3>::create(evals, 0);

            ImageData* evecs = new ImageData(input->getDimensionality(), input->getSize(), 9);
            GenericImageRepresentationLocal<float, 9>* evecRep = GenericImageRepresentationLocal<float, 9>::create(evecs, 0);

            tbb::atomic<size_t> countDiscarded;
            countDiscarded = 0;
            DegeneratedEvHandling evh = p_degeneratedHandling.getOptionValue();

            // perform eigen decomposition in parallel
            tbb::parallel_for(tbb::blocked_range<size_t>(0, input->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const Tensor2<float>& t = input->getElement(i);

                    if (t.Dxx == 0 && t.Dxy == 0 && t.Dxz == 0 && t.Dyy == 0 && t.Dyz == 0 && t.Dzz == 0) {
                        evalRep->setElement(i, cgt::vec3::zero);
                        evecRep->setElement(i, cgt::mat3::zero);
                        continue;
                    }

                    // use Eigen library for eigensystem calculation
                    Eigen::Matrix3f tensorMatrix;
                    tensorMatrix << t.Dxx, t.Dxy, t.Dxz, t.Dxy, t.Dyy, t.Dyz, t.Dxz, t.Dyz, t.Dzz;

                    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(tensorMatrix);

                    // gather data from eigen library
                    const Eigen::Matrix<float, 3, 1>& values = solver.eigenvalues();
                    const Eigen::Matrix3f& vectors = solver.eigenvectors();

                    // kill NaN values
                    if (cgt::isNaN(values(0)) || cgt::isNaN(values(1)) || cgt::isNaN(values(2))) {
                        evalRep->setElement(i, cgt::vec3::zero);
                        evecRep->setElement(i, cgt::mat3::zero);
                        continue;
                    }

                    // determine order of eigenvalues (i.e. sort'em)
                    int high, mid, low;
                    bool negative = false;
                    if (values(0) > values(1)) {
                        if (values(1) > values(2)) {
                            high = 0; mid = 1; low = 2;
                        }
                        else if (values(0) > values(2)) {
                            high = 0; mid = 2; low = 1;
                        }
                        else {
                            high = 2; mid = 0; low = 1;
                        }
                    }
                    else {
                        if (values(0) > values(2)) {
                            high = 1; mid = 0; low = 2;
                        }
                        else if (values(1) > values(2)) {
                            high = 1; mid = 2; low = 0;
                        }
                        else {
                            high = 2; mid = 1; low = 0;
                        }
                    }

                    // perform handling of degenerated tensors (i.e. negative/mixed eigenvalues):
                    if (values(0) < 0 && values(1) < 0 && values(2) < 0) {
                        negative = true;
                        if (evh == INVERT)
                            std::swap(high, low);
                    }
                    else if (p_maskMixedTensors.getValue() && (values(0) < 0 || values(1) < 0 || values(2) < 0)) {
                        // We assume that either all eigenvalues are positive or all eigenvalues are negative.
                        // If we encounter both positive and negative eigenvalues this must be due to severe noise
                        // (e.g. area outside brain) so it is reasonable to discard these voxels.
                        ++countDiscarded;
                        evalRep->setElement(i, cgt::vec3::zero);
                        evecRep->setElement(i, cgt::mat3::zero);
                        continue;
                    }

                    if (negative && evh == MASK) {
                        evalRep->setElement(i, cgt::vec3::zero);
                        evecRep->setElement(i, cgt::mat3::zero);
                    }
                    else {
                        evalRep->setElement(i, cgt::vec3(values(high), values(mid), values(low)));
                        evecRep->setElement(i, cgt::mat3(vectors(0, high), vectors(1, high), vectors(2, high),
                            vectors(0, mid),  vectors(1, mid),  vectors(2, mid),
                            vectors(0, low),  vectors(1, low),  vectors(2, low)));

                        if (negative && evh == SHIFT) {
                            evalRep->getElement(i) -= (evalRep->getElement(i).x + evalRep->getElement(i).z);
                        }
                    }
                }
            });

            // write results to DataContainer and also cache them in local members
            _eigenvalues = data.addData(p_evalsImage.getValue(), evals);
            _eigenvectors = data.addData(p_evecsImage.getValue(), evecs);
        }
        else {
            LDEBUG("No suitable input image found.");
        }

        validate(EIGENSYSTEM_INVALID);
    }

    void TensorAnalyzer::computeOutput(DataContainer& data, size_t index) {
        // sanity check
        if (index >= p_outputProperties.size()) {
            LERROR("Index out of bounds while computing output #" << index);
            return;
        }

        // gather eigensystem
        const GenericImageRepresentationLocal<float, 3>* evalRep = 0;
        const GenericImageRepresentationLocal<float, 9>* evecRep = 0;
        if (_eigenvalues.getData() != 0 && _eigenvectors.getData() != 0) {
            evalRep = static_cast<const ImageData*>(_eigenvalues.getData())->getRepresentation< GenericImageRepresentationLocal<float, 3> >(false);
            evecRep = static_cast<const ImageData*>(_eigenvectors.getData())->getRepresentation< GenericImageRepresentationLocal<float, 9> >(false);
        }
        if (evalRep == 0 || evecRep == 0) {
            LERROR("Could not compute output, no eigensystem present.");
            return;
        }

        OutputPropertyPair* opp = p_outputProperties[index];
        const std::string& type = opp->_imageType.getOptionValue();

        if (type == "Disabled") {
            return;
        }
        else if (type == "EigenValue1") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    float ev = vals.x;
                    if (ev == 0.0f || cgt::isNaN(ev))
                        output->setElement(i, 0.0f);
                    else
                        output->setElement(i, ev);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "EigenValue2") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    float ev = vals.y;
                    if (ev == 0.0f || cgt::isNaN(ev))
                        output->setElement(i, 0.0f);
                    else
                        output->setElement(i, ev);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "EigenValue3") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    float ev = vals.z;
                    if (ev == 0.0f || cgt::isNaN(ev))
                        output->setElement(i, 0.0f);
                    else
                        output->setElement(i, ev);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }

        else if (type == "MainEigenvector") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 3);
            GenericImageRepresentationLocal<float, 3>* output = GenericImageRepresentationLocal<float, 3>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    output->setElement(i, evecRep->getElement(i)[0]);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }

        // = Anisotropy Measures: =========================================================================
        else if (type == "VolumeRatio") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = (vals.x * vals.y * vals.z) / pow((vals.x + vals.y + vals.z)/3.f, 3);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "FractionalAnisotropy") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);
                    const float root = sqrt(.5f);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = root * sqrt((vals.x-vals.y)*(vals.x-vals.y) + (vals.y-vals.z)*(vals.y-vals.z) + (vals.z-vals.x)*(vals.z-vals.x)) / sqrt(vals.x*vals.x + vals.y*vals.y + vals.z*vals.z);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "RelativeAnisotropy") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);
                    const float root = sqrt(.5f);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = root * sqrt((vals.x-vals.y)*(vals.x-vals.y) + (vals.y-vals.z)*(vals.y-vals.z) + (vals.z-vals.x)*(vals.z-vals.x)) / (vals.x + vals.y + vals.z);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "MeanDiffusivity") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = (vals.x + vals.y + vals.z) / 3;
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "Trace") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = vals.x + vals.y + vals.z;
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "AxialDiffusivity") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = vals.x;
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "RadialDiffusivity") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = (vals.y + vals.z) / 2;
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "LinearAnisotropy") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = (vals.x - vals.y) / (vals.x + vals.y + vals.z);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "PlanarAnisotropy") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = 2.f*(vals.y - vals.z) / (vals.x + vals.y + vals.z);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }
        else if (type == "Isotropy") {
            ImageData* id = new ImageData(evalRep->getDimensionality(), evalRep->getSize(), 1);
            GenericImageRepresentationLocal<float, 1>* output = GenericImageRepresentationLocal<float, 1>::create(id, 0);
            tbb::parallel_for(tbb::blocked_range<size_t>(0, id->getNumElements()), [&] (const tbb::blocked_range<size_t>& range) {
                for (size_t i = range.begin(); i != range.end(); ++i) {
                    const cgt::vec3& vals = evalRep->getElement(i);

                    if (vals == cgt::vec3::zero || cgt::isNaN(vals))
                        output->getElement(i) = 0;
                    else
                        output->getElement(i) = (3.f*vals.z) / (vals.x + vals.y + vals.z);
                }
            });
            data.addData(opp->_imageId.getValue(), id);
        }

    }

    void TensorAnalyzer::addOutput() {
        OutputPropertyPair* opp = new OutputPropertyPair(p_outputProperties.size() + 1);
        p_outputProperties.push_back(opp);
        addProperty(opp->_imageId);
        addProperty(opp->_imageType);
    }

    void TensorAnalyzer::deinit() {
        _eigenvectors = DataHandle(0);
        _eigenvalues = DataHandle(0);
        AbstractProcessor::deinit();
    }

}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          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_SDK_NEXT_POWER_OF_2_HPP_INCLUDED
#define BOOST_SIMD_SDK_NEXT_POWER_OF_2_HPP_INCLUDED

#include <boost/config.hpp>
#include <type_traits>

namespace boost { namespace simd
{
  /*!
    @brief Evaluates next power of 2

    Computes the power of two greater or equal to any given integral value @c n.

    @par Semantic:
    For any given integral value @c n:

    @code
    auto r = next_power_of_2(n);
    @endcode

    where @c r verifies:

    @code
    is_power_of_2(r) == true && r >= n
    @endcode

    @param n Integral value.

    @return Integral value of same type as n.
  **/

  namespace detail
  {

    template < typename Int , int s >
    struct next_power_of_2_impl
    {
      static BOOST_CXX14_CONSTEXPR BOOST_FORCEINLINE
      Int apply( Int n )
      {
        n = next_power_of_2_impl<Int,s/2>::apply(n);
        return n | (n >> s/2);
      }
    };

    template < typename Int >
    struct next_power_of_2_impl<Int,1>
    {
      static BOOST_CXX14_CONSTEXPR BOOST_FORCEINLINE
      Int apply( Int n )
      {
        return n;
      }
    };

  }

  template < typename Int >
  BOOST_CXX14_CONSTEXPR inline Int next_power_of_2( Int n )
  {
    static_assert( std::is_integral<Int>::value , "Int must be an integral type." );
    using impl = detail::next_power_of_2_impl< Int, sizeof(Int)*8 >;
    return  impl::apply(--n) + Int{1};
  }
} }

#endif


/**
 *  This file is part of dvo.
 *
 *  Copyright 2013 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  dvo is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <Eigen/Core>
#include <dvo/core/math_sse.h>
#include <dvo/util/stopwatch.h>

#include <iostream>

typedef Eigen::Matrix<float, Eigen::Dynamic, 6> JacobianWorkspace;
typedef Eigen::Matrix<float, 6, 6> Hessian;

static const int Dimension = 2;

void do_reduction(const JacobianWorkspace& j, const Eigen::Matrix<float, Dimension, Dimension>& alpha, Hessian& A)
{
  A.setZero();

  for(int idx = 0; idx < j.rows(); idx += Dimension)
  {
    if(Dimension == 1)
      A += j.block<Dimension,6>(idx, 0).transpose() * alpha * j.block<Dimension,6>(idx, 0);
    else
      A += j.block<Dimension,6>(idx, 0).transpose() * alpha * j.block<Dimension,6>(idx, 0);

  }
}

void do_optimized_reduction(const JacobianWorkspace& j, const Eigen::Matrix<float, Dimension, Dimension>& alpha, Hessian& A)
{
  dvo::core::OptimizedSelfAdjointMatrix6x6f A_opt;
  A_opt.setZero();

  Eigen::Matrix2f my_alpha;
  my_alpha.block<Dimension, Dimension>(0, 0) = alpha;

  for(int idx = 0; idx < j.rows(); idx += Dimension)
  {
    if(Dimension == 1)
      A_opt.rankUpdate(j.block<1,6>(idx, 0).transpose(), my_alpha(0, 0));
    else
      A_opt.rankUpdate(j.block<2,6>(idx, 0), my_alpha);
  }

  A_opt.toEigen(A);
}

int main(int argc, char **argv)
{
  Hessian A1, A2;

  Eigen::Matrix<float, Dimension, Dimension> alpha;
  alpha.setRandom();

  if(Dimension > 1)
    alpha(0, 1) = alpha(1, 0); // make symmetric

  JacobianWorkspace J;
  J.resize(640 * 480 * Dimension, Eigen::NoChange);
  J.setRandom();

  dvo::util::stopwatch w1("unoptimized", int(1e3 * 0.5)),  w2("optimized", int(1e3 * 0.5));

  for(int idx = 0; idx < 1e2; idx++)
    do_reduction(J, alpha, A1);

  for(int idx = 0; idx < 1e3; idx++)
  {
    w1.start();
    do_reduction(J, alpha, A1);
    w1.stopAndPrint();
  }

  for(int idx = 0; idx < 1e2; idx++)
    do_optimized_reduction(J, alpha, A2);

  for(int idx = 0; idx < 1e3; idx++)
  {
    w2.start();
    do_optimized_reduction(J, alpha, A2);
    w2.stopAndPrint();
  }
}


/*
 * sdf_2_sdf_optimizer.cpp
 *
 *  Created on: Mar 21, 2019
 *      Author: Fei Shan
 */
//libraries
#include <Eigen/Eigen>
#include <boost/python.hpp>
#include <boost/shared_ptr.hpp>
//local
#include "../rigid_optimization/sdf_2_sdf_optimizer2d.hpp"
#include "../tsdf/parameters.hpp"
#include "eigen_numpy.hpp"

namespace bp = boost::python;
namespace ro = rigid_optimization;

namespace python_export {
    namespace sdf_2_sdf_optimizer {
        void export_algorithms() {
            {
                bp::scope outer =
                        bp::class_<ro::Sdf2SdfOptimizer2d>("Sdf2SdfOptimizer2d",
                                bp::init<bp::optional<
                                        float, int, tsdf::Parameters2d,
                                        ro::Sdf2SdfOptimizer2d::VerbosityParameters>>(
                                        bp::args("rate",
                                                 "maximum_iteration_count",
												 "tsdf_generation_parameters",
                                                 "verbosity_parameters")))
                                .def("optimize", &ro::Sdf2SdfOptimizer2d::optimize,
                                     "Find optimal twist to map given live SDF to given canonical SDF",
                                     bp::args("image_y_coordinate",
                                              "canonical_field",
                                              "live_depth_image",
                                              "eta",
											  "initial_camera_pose"));
                bp::class_<ro::Sdf2SdfOptimizer2d::VerbosityParameters>("VerbosityParameters",
                        "Parameters that control verbosity to stdout. "
                        "Assumes being used in an \"immutable\" manner, i.e. just a structure that holds values",
                        bp::init<bp::optional<bool, bool>>(
                                bp::args(/*"self",*/
                                "print_iteration_max_warp_update",
                                "print_iteration_energy")))
                        .add_property("print_iteration_max_warp_update",
                                      &ro::Sdf2SdfOptimizer2d::VerbosityParameters
                                      ::print_iteration_max_warp_update)
                        .add_property("print_iteration_energy",
                                      &ro::Sdf2SdfOptimizer2d::VerbosityParameters
                                      ::print_iteration_energy)
                                //============================================
                        .add_property("print_per_iteration_info",
                                      &ro::Sdf2SdfOptimizer2d::VerbosityParameters
                                      ::print_per_iteration_info);
            }
        }
    }
}


/*
 *  heuristics.hpp
 *
 *
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright (c) 2011-2014, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#ifndef HEURISTICS_HPP
#define HEURISTICS_HPP

#include <boost/range/algorithm.hpp>

namespace heuristics {
  using namespace boost;

  template<class Vertex, class Graph>
  void eliminate_vertex(Vertex v, Graph& g) {
    for (auto a : as_range(adjacent_vertices(v, g))) {
      for (auto b : as_range(adjacent_vertices(v, g))) {
        if (a != b and not edge(a, b, g).second) {
          add_edge(a, b, g);
        }
      }
    }
    clear_vertex(v, g);
    remove_vertex(v, g);
  }

  template<class Graph, class Vertex>
  unsigned int num_non_adjacent_neighbors(Vertex v, Graph const& g)
  {
    unsigned int n = 0;
    for (auto u : as_range(adjacent_vertices(v, g))) {
      auto adj = adjacent_vertices(u, g);
      for (auto z : as_range(adjacent_vertices(v, g))) {
        if (boost::find(adj, z) != end(adj))
          n ++;
      }
    }
    return n;
  }

  template<class Graph, class OutputIterator>
  void greedy_degree_order(Graph g, OutputIterator out) {
    using vertex = typename Graph::vertex_descriptor;
    while (num_vertices(g) > 0) {
      auto v = *min_element(vertices(g), [&](vertex v, vertex u) {
        return degree(v, g) < degree(u, g);
      });
      *out++ = get(vertex_index, g, v);
      eliminate_vertex(v, g);
    }
  }

  //////////////////////////////////////////////////////////////////////

  template<class Graph, class OutputIterator>
  void greedy_fillin_order(Graph g, OutputIterator out) {
    using vertex = typename Graph::vertex_descriptor;
    while (num_vertices(g) > 0) {
      auto v = *min_element(vertices(g), [&](vertex v, vertex u) {
        return num_non_adjacent_neighbors(v, g) < num_non_adjacent_neighbors(u, g);
      });
      *out++ = get(vertex_index, g, v);
      eliminate_vertex(v, g);
    }
  }

  //////////////////////////////////////////////////////////////////////

  template<class Graph, class OutputIterator>
  void greedy_local_degree_order(Graph g, OutputIterator out) {
    using vertex = typename Graph::vertex_descriptor;
    auto compare = [&](vertex v, vertex u) {
      return degree(v, g) < degree(u, g);
    };
    vertex current = *min_element(vertices(g), compare);
    while (num_vertices(g) > 0) {
      *out++ = get(vertex_index, g, current);
      vertex next = *min_element(vertices(g), compare);
      eliminate_vertex(current, g);
      current = next;
    }
  }

  //////////////////////////////////////////////////////////////////////

  template<class Graph, class OutputIterator>
  void greedy_local_fillin_order(Graph g, OutputIterator out) {
    using vertex = typename Graph::vertex_descriptor;
    auto compare =  [&](vertex v, vertex u) {
      return num_non_adjacent_neighbors(v, g) < num_non_adjacent_neighbors(u, g);
    };
    vertex current = *min_element(vertices(g), compare);
    while (num_vertices(g) > 0) {
      *out++ = get(vertex_index, g, current);
      vertex next = *min_element(adjacent_vertices(current, g), compare);
      eliminate_vertex(current, g);
      current = next;
    }
  }
}

#endif


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <stdlib.h>
#include <vector>
#include <string>
#include <functional>

#include <random>

// ML-Data Utils
#include <toolkits/ml_data_1/ml_data.hpp>
#include <toolkits/ml_data_1/metadata.hpp>
#include <toolkits/ml_data_1/sframe_index_mapping.hpp>

// Optimization Interface
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/utils.hpp>

// Models
#include <toolkits/supervised_learning/linear_regression.hpp>
#include <core/storage/sframe_data/testing_utils.hpp>

#include <cfenv>

using namespace turi;
using namespace turi::supervised;

void run_linear_regression_test(std::map<std::string, flexible_type> opts) {

  size_t examples = opts.at("examples");
  size_t features = opts.at("features");
  std::string target_column_name = "target";
  std::vector<std::string> column_names = {"user", "item"};

  // Answers
  // -----------------------------------------------------------------------
  DenseVector coefs(features+1);
  coefs.setRandom();

  // Feature names
  std::vector<std::string> feature_names;
  std::vector<flex_type_enum> feature_types;
  for(size_t i=0; i < features; i++){
    feature_names.push_back(std::to_string(i));
    feature_types.push_back(flex_type_enum::FLOAT);
  }

  // Data
  std::vector<std::vector<flexible_type>> X_data;
  std::vector<std::vector<flexible_type>> y_data;
  X_data.resize(examples);
  y_data.resize(examples);
  for(size_t i=0; i < examples; i++){
    X_data[i].resize(features);
    for(size_t k=0; k < features; k++){
      X_data[i][k] = turi::random::normal();
    }
    y_data[i] = flexible_type(turi::random::bernoulli(0.5));
  }

  // Options
  std::map<std::string, flexible_type> options = {
    {"convergence_threshold", 1e-2},
    {"solver", "auto"},
    {"max_iterations", 10},
  };

  // Make the data
  sframe X = make_testing_sframe(feature_names, feature_types, X_data);
  sframe y = make_testing_sframe({"target"}, {flex_type_enum::INTEGER}, y_data);

  std::shared_ptr<linear_regression> model;
  model.reset(new linear_regression);
  model->init(X,y);
  model->init_options(options);
  model->train();
  model.reset();

}

/**
 *  Check linear regression
*/
struct linear_regression_test  {
  public:

  void test_linear_regression_tiny() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 100},
      {"features", 10}};
    run_linear_regression_test(opts);
  }

  void test_linear_regression_small() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 1000000},
      {"features", 10}};
    run_linear_regression_test(opts);
  }

  //void test_linear_regression_medium() {
  //  std::map<std::string, flexible_type> opts = {
  //    {"examples", 1000000},
  //    {"features", 100}};
  //  run_linear_regression_test(opts);
  //}

  //void test_linear_regression_large() {
  //  std::map<std::string, flexible_type> opts = {
  //    {"examples", 10000000},
  //    {"features", 100}};
  //  run_linear_regression_test(opts);
  //}
};

BOOST_FIXTURE_TEST_SUITE(_linear_regression_test, linear_regression_test)
BOOST_AUTO_TEST_CASE(test_linear_regression_tiny) {
  linear_regression_test::test_linear_regression_tiny();
}
BOOST_AUTO_TEST_CASE(test_linear_regression_small) {
  linear_regression_test::test_linear_regression_small();
}
BOOST_AUTO_TEST_SUITE_END()


#include "block_tridiagonal_matrice.hpp"

#include <Eigen/Dense>

using namespace Eigen;

BlockTridiagonalMatrice::BlockTridiagonalMatrice(int size_block, int num_block)
    : sb_(size_block), nb_(num_block), ndim_(sb_ * nb_), tA_(sb_, sb_, nb_),
      tB_(sb_, sb_, nb_ - 1) {}

MatrixXd BlockTridiagonalMatrice::inverse() {}

void BlockTridiagonalMatrice::setA(
    const Eigen::Ref<const Eigen::MatrixXd> &block, int index) {
  ;
}
void BlockTridiagonalMatrice::setB(
    const Eigen::Ref<const Eigen::MatrixXd> &block, int index) {
  ;
}


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/kdtree.hpp>

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <math.h>
#include <random>
#include "fmt/color.h"
#include "bvp/obvp_solver.hpp"

using namespace fmt;
using namespace std;
class RandomMap {
private:
    ros::Subscriber click_sub_;
    ros::NodeHandle nh_;

    ros::Timer pub_timer_;
    ros::Publisher pc_pub_, pc_no_ground_pub_;
    pcl::PointCloud<pcl::PointXYZ> pcl_map_, pcl_no_ground_map_;
    pcl::search::KdTree<pcl::PointXYZ> kdtreeMap;
    sensor_msgs::PointCloud2 pc2_map_, pc2_no_ground_map_;
    vector<int> pointIdxSearch;
    vector<float> pointSquaredDistance;
    bool map_is_ready{false};

    bvp::ObvpSolver::Ptr bvp_;

    struct RandomUtils {
        default_random_engine eng;
        uniform_real_distribution<double> unif_pm_1;
        uniform_real_distribution<double> unif_0_1;
    } rand_;

    struct ConfigParams {
        int cylinder_num, circle_num, spline_num, pillar_num;
        double map_size_x, map_size_y, map_size_z, resolution;
        double init_x, init_y, init_z;
        string frame_id;
        double ground_height;

        vector<Eigen::Vector3d> clear_area;
        vector<double> clear_radius;
        // ground params
        double ground_size_x,ground_size_y,ground_size_z;
        double ground_resolution;
        bool use_ground{false};
        bool visual_no_groud_map{false};

        // all params has [0] as min and [1] as max
        // cylinder params
        double cylinder_radius[2];
        double cylinder_height[2];
        double cylinder_angle_resolution;
        vector<Eigen::Vector3d> fix_cylinder;

        // pillar params
        double pillar_width[2];
        double pillar_height[2];
        double pillar_length[2];

        // circle parames
        double circle_height[2];
        double circle_radius[2];
        double circle_thickness[2];

        // spline params
        double bending_rate;
        double height_max, height_min;
        double radius_max, radius_min;
        double sample_theta_reso;
        double sample_t_reso;
        int max_branch_num;
        int total_num;
        double tree_width;

    } cfg_;

    template<class T>
    void LoadParam(string param_name, T &param_value, T default_value) {
        if (nh_.getParam(param_name, param_value)) {
            print("\033[0;32m Load param {} succes: \033[0;0m", param_name.c_str());
            cout << param_value << endl;
        } else {
            print("\033[0;33m Load param {} failed, use default value: \033[0;0m", param_name);
            param_value = default_value;
            cout << param_value << endl;
        }
    }

    template<class T>
        void LoadParam(string param_name, vector<T> &param_value, vector<T> default_value) {
        if (nh_.getParam(param_name, param_value)) {
            print("\033[0;32m Load param {} succes: \033[0;0m", param_name.c_str());
            for(int i = 0 ; i< param_value.size();i++){
                cout << param_value[i] <<" ";
            }cout<<endl;
        } else {
            print("\033[0;33m Load param {} failed, use default value: \033[0;0m", param_name);
            param_value = default_value;
            for(int i = 0 ; i< param_value.size();i++){
                cout << param_value[i] <<" ";
            }cout<<endl;
        }
    }

public:
    RandomMap(ros::NodeHandle &nh) {
        nh_ = nh;

        rand_.unif_pm_1 = uniform_real_distribution<double>(-1, 1);
        rand_.unif_0_1 = uniform_real_distribution<double>(0, 1);

        LoadParam("map/x_size", cfg_.map_size_x, 20.0);
        LoadParam("map/y_size", cfg_.map_size_y, 20.0);
        LoadParam("map/z_size", cfg_.map_size_z, 5.0);
        LoadParam("map/ground_height", cfg_.ground_height, -1.0);
        LoadParam("map/init_x", cfg_.init_x, 0.0);
        LoadParam("map/init_y", cfg_.init_y, 0.0);
        LoadParam("map/init_z", cfg_.init_z, 0.0);
        LoadParam("map/resolution", cfg_.resolution, 0.1);
        double hz;
        LoadParam("map/pub_freq", hz, 1.0);
        LoadParam<string>("map/frame_id", cfg_.frame_id, "world");
        int seed;
        LoadParam("map/random_seed", seed, -1);
        rand_.eng = default_random_engine (seed);
        // Load ground parameters
        LoadParam("ground/enable", cfg_.use_ground, true);
        LoadParam("ground/visual_no_ground", cfg_.visual_no_groud_map, true);
        LoadParam("ground/x_size", cfg_.ground_size_x, cfg_.map_size_x);
        LoadParam("ground/y_size", cfg_.ground_size_y, cfg_.map_size_y);
        LoadParam("ground/z_size", cfg_.ground_size_z, cfg_.map_size_z);
        LoadParam("ground/resolution", cfg_.ground_resolution, cfg_.resolution);

        // Load cylinder parameters
        LoadParam("cylinder/num", cfg_.cylinder_num, 20);
        LoadParam("cylinder/angle_resolution", cfg_.cylinder_angle_resolution, 0.1);
        LoadParam("cylinder/height_min", cfg_.cylinder_height[0], 1.0);
        LoadParam("cylinder/height_max", cfg_.cylinder_height[1], 3.0);
        LoadParam("cylinder/radius_min", cfg_.cylinder_radius[0], 0.3);
        LoadParam("cylinder/radius_max", cfg_.cylinder_radius[1], 0.5);

        // Load circle parameters
        LoadParam("circle/num", cfg_.circle_num, 20);
        LoadParam("circle/height_min", cfg_.circle_height[0], 1.0);
        LoadParam("circle/height_max", cfg_.circle_height[1], 3.0);
        LoadParam("circle/radius_min", cfg_.circle_radius[0], 0.3);
        LoadParam("circle/radius_max", cfg_.circle_radius[1], 0.5);
        LoadParam("circle/thickness_min", cfg_.circle_thickness[0], 0.1);
        LoadParam("circle/thickness_max", cfg_.circle_thickness[1], 0.1);

        // Load pillar parameters
        LoadParam("pillar/num", cfg_.pillar_num, 20);
        LoadParam("pillar/height_min", cfg_.pillar_height[0], 1.0);
        LoadParam("pillar/height_max", cfg_.pillar_height[1], 3.0);
        LoadParam("pillar/width_min", cfg_.pillar_width[0], 0.3);
        LoadParam("pillar/width_max", cfg_.pillar_width[1], 0.5);
        LoadParam("pillar/length_min", cfg_.pillar_length[0], 0.3);
        LoadParam("pillar/length_max", cfg_.pillar_length[1], 0.5);

        //Load spline parameters
//        LoadParam("spline/num", cfg_.spline_num, 5);

        LoadParam("spline/num", cfg_.spline_num, 5);
        LoadParam("spline/radius_min", cfg_.radius_min, 0.1);
        LoadParam("spline/radius_max", cfg_.radius_max, 0.2);
        LoadParam("spline/bending_rate", cfg_.bending_rate, 0.5);
        LoadParam("spline/height_max", cfg_.height_max, 4.0);
        LoadParam("spline/height_min", cfg_.height_min, 3.0);
        LoadParam("spline/width", cfg_.tree_width, 3.0);
        LoadParam("spline/sample_t_reso", cfg_.sample_t_reso, 0.05);
        LoadParam("spline/sample_theta_reso", cfg_.sample_theta_reso, 0.5);
        LoadParam("spline/max_branch_num", cfg_.max_branch_num, 3);

        vector<double> clear_x, clear_y;
        LoadParam("map/clear_radius",cfg_.clear_radius,cfg_.clear_radius);
        LoadParam("map/clear_area/x",clear_x,clear_x);
        LoadParam("map/clear_area/y",clear_y,clear_y);
        for(size_t i = 0; i < clear_x.size();i++){
            cfg_.clear_area.push_back(Eigen::Vector3d(clear_x[i], clear_y[i],0));
        }

        vector<double> fix_x, fix_y;
        LoadParam("cylinder/fix/x",fix_x,fix_x);
        LoadParam("cylinder/fix/y",fix_y,fix_y);
        for(size_t i = 0; i < fix_x.size();i++){
            cfg_.fix_cylinder.push_back(Eigen::Vector3d(fix_x[i], fix_y[i],0));
        }

        pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/map_generator/global_cloud", 1);
        pc_no_ground_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/map_generator/visual_cloud", 1);
        pub_timer_ = nh_.createTimer(ros::Duration(1.0 / hz), &RandomMap::PubTimerCallback, this);
        GenerateRandomMap();
    }

    ~RandomMap() {}

private:

    void PubTimerCallback(const ros::TimerEvent &e) {
        if (!map_is_ready) {
            return;
        }
        pc2_map_.header.stamp = ros::Time::now();
        pc_pub_.publish(pc2_map_);

        if(cfg_.visual_no_groud_map){
            pc2_no_ground_map_.header.stamp = ros::Time::now();
            pc_no_ground_pub_.publish(pc2_no_ground_map_);
        }
    }

    void GenerateRandomMap() {
        GeneratePillars();
        GenerateCylinders();
        GenerateCircles();
        GenerateSpline();
        if(cfg_.visual_no_groud_map){
            pcl_no_ground_map_ = pcl_map_;
            pcl_no_ground_map_.width = pcl_no_ground_map_.points.size();
            pcl_no_ground_map_.height = 1;
            pcl_no_ground_map_.is_dense = true;
            pcl::toROSMsg(pcl_no_ground_map_, pc2_no_ground_map_);
            pc2_no_ground_map_.header.frame_id = cfg_.frame_id;
        }

        GenerateGround();
        pcl_map_.width = pcl_map_.points.size();
        pcl_map_.height = 1;
        pcl_map_.is_dense = true;
        pcl::toROSMsg(pcl_map_, pc2_map_);
        pc2_map_.header.frame_id = cfg_.frame_id;
        map_is_ready = true;
    }

    void GenerateCircles() {
        pcl::PointXYZ pt_random;
        uniform_real_distribution<double> rand_roll = uniform_real_distribution<double>(-M_PI, +M_PI);
        uniform_real_distribution<double> rand_pitch = uniform_real_distribution<double>(+M_PI / 4.0, +M_PI / 2.0);
        uniform_real_distribution<double> rand_yaw = uniform_real_distribution<double>(+M_PI / 4.0, +M_PI / 2.0);
        uniform_real_distribution<double> rand_ellipse_c = uniform_real_distribution<double>(0.5, 2.0);
        for (int i = 0; i < cfg_.circle_num; i++) {
            double x0, y0, z0, rmin, rmax;
            std::vector<Eigen::Vector3d> circle_set;

            x0 = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_x / 2;
            y0 = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_y / 2;
            bool conti{false};
            for(size_t i = 0 ; i< cfg_.clear_area.size();i++){
                Eigen::Vector3d cur(x0,y0,0);
                if((cur-cfg_.clear_area[i]).norm() < cfg_.clear_radius[i]){
                    conti = true;
                    break;
                }
            }
            if(conti){
                i--;
                continue;
            }

            z0 = rand_.unif_0_1(rand_.eng) * (cfg_.circle_height[1] - cfg_.circle_height[0]) + cfg_.circle_height[0];
            rmin = rand_.unif_0_1(rand_.eng) * (cfg_.circle_radius[1] - cfg_.circle_radius[0]) +
                    cfg_.circle_radius[0];
            rmax = rmin + rand_.unif_0_1(rand_.eng) * (cfg_.circle_thickness[1] - cfg_.circle_thickness[0]) +
                    cfg_.circle_thickness[0];

            double a, b;
            a = rand_ellipse_c(rand_.eng);
            b = rand_ellipse_c(rand_.eng);

            double x, y, z;
            Eigen::Vector3d pt3, pt3_rot;
            for (double theta = -M_PI; theta < M_PI; theta += 0.025) {
                for(double R = rmin; R < rmax; R += cfg_.resolution){
                    x = a * cos(theta) * R;
                    y = b * sin(theta) * R;
                    z = 0;
                    pt3 << x, y, z;
                    circle_set.push_back(pt3);
                }
            }
            // Define a random 3d rotation matrix
            Eigen::Matrix3d Rot;
            double roll, pitch, yaw;
            double alpha, beta, gama;
            roll = rand_roll(rand_.eng); // alpha
            pitch = rand_pitch(rand_.eng); // beta
            yaw = rand_yaw(rand_.eng); // gama

            alpha = roll;
            beta = pitch;
            gama = yaw;

            double p = rand_.unif_0_1(rand_.eng);
            if (p < 0.5) {
                beta = M_PI / 2.0;
                gama = M_PI / 2.0;
            }

            Rot << cos(alpha) * cos(gama) - cos(beta) * sin(alpha) * sin(gama), -cos(beta) * cos(gama) * sin(alpha) -
                                                                                cos(alpha) * sin(gama), sin(alpha) *
                                                                                sin(beta),
                    cos(gama) * sin(alpha) + cos(alpha) * cos(beta) * sin(gama), cos(alpha) * cos(beta) * cos(gama) -
                                                                                sin(alpha) * sin(gama), -cos(alpha) *
                                                                                sin(beta),
                    sin(beta) * sin(gama), cos(gama) * sin(beta), cos(beta);

            for (auto pt: circle_set) {
                pt3_rot = Rot * pt;
                pt_random.x = pt3_rot(0) + x0 + 0.001;
                pt_random.y = pt3_rot(1) + y0 + 0.001;
                pt_random.z = pt3_rot(2) + z0 + 0.001;

                if (pt_random.z >= 0.0){
                    pt_random.z+=  cfg_.ground_height;
                    pcl_map_.points.push_back(pt_random);
                }

            }
        }
        print(fg(color::light_sea_green), " [Rings] Generate success.\n");
    }

    void GenerateCylinders() {
        pcl::PointXYZ pt_random;
        bool is_kdtree_empty = false;
        if (pcl_map_.points.size() > 0)
            kdtreeMap.setInputCloud(pcl_map_.makeShared());
        else
            is_kdtree_empty = true;

        for (size_t i = 0; i < cfg_.cylinder_num; i++) {
            double x, y, r, h;
            x = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_x / 2;
            y = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_y / 2;
            bool conti{false};
            for(size_t i = 0 ; i< cfg_.clear_area.size();i++){
                Eigen::Vector3d cur(x,y,0);
                if((cur-cfg_.clear_area[i]).norm() < cfg_.clear_radius[i]){
                    conti = true;
                    break;
                }
            }
            if(conti){
                i--;
                continue;
            }

            r = rand_.unif_0_1(rand_.eng) * (cfg_.cylinder_radius[1] - cfg_.cylinder_radius[0]) +
                cfg_.cylinder_radius[0];
            h = rand_.unif_0_1(rand_.eng) * (cfg_.cylinder_height[1] - cfg_.cylinder_height[0]) +
                cfg_.cylinder_height[0];

            pcl::PointXYZ searchPoint(x, y, (cfg_.pillar_height[0] + cfg_.pillar_height[1]) / 2.0);
            pointIdxSearch.clear();
            pointSquaredDistance.clear();

            if (is_kdtree_empty == false) {
                if (kdtreeMap.nearestKSearch(searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0) {
                    if (sqrt(pointSquaredDistance[0]) < 1.0) {
                        i--;
                        continue;
                    }
                }
            }

            x = floor(x / cfg_.resolution) * cfg_.resolution + cfg_.resolution / 2.0;
            y = floor(y / cfg_.resolution) * cfg_.resolution + cfg_.resolution / 2.0;

            for (double theta = 0; theta < M_PI * 2; theta += cfg_.cylinder_angle_resolution) {
                for (double radius = 0.0; radius < r; radius += cfg_.resolution) {
                    for (double hei = 0.0; hei < h; hei += cfg_.resolution) {
                        pt_random.x = x + cos(theta) * radius;
                        pt_random.y = y + sin(theta) * radius;
                        pt_random.z = hei + cfg_.ground_height;
                        pcl_map_.points.push_back(pt_random);
                    }
                }
            }
        }

        for(size_t i = 0 ; i < cfg_.fix_cylinder.size(); i++){
            double x, y, r, h;
            x = cfg_.fix_cylinder[i].x();
            y = cfg_.fix_cylinder[i].y();

            r = rand_.unif_0_1(rand_.eng) * (cfg_.cylinder_radius[1] - cfg_.cylinder_radius[0]) +
                    cfg_.cylinder_radius[0];
            h = rand_.unif_0_1(rand_.eng) * (cfg_.cylinder_height[1] - cfg_.cylinder_height[0]) +
                    cfg_.cylinder_height[0];

            x = floor(x / cfg_.resolution) * cfg_.resolution + cfg_.resolution / 2.0;
            y = floor(y / cfg_.resolution) * cfg_.resolution + cfg_.resolution / 2.0;

            for (double theta = 0; theta < M_PI * 2; theta += cfg_.cylinder_angle_resolution) {
                for (double radius = 0.0; radius < r; radius += cfg_.resolution) {
                    for (double hei = 0.0; hei < h; hei += cfg_.resolution) {
                        pt_random.x = x + cos(theta) * radius;
                        pt_random.y = y + sin(theta) * radius;
                        pt_random.z = hei + cfg_.ground_height;
                        pcl_map_.points.push_back(pt_random);
                    }
                }
            }
        }

        print(fg(color::light_sea_green), " [Cylinder] Generate success.\n");
    }

    void GeneratePillars() {
        pcl::PointXYZ pt_random;
        bool is_kdtree_empty = false;
        if (pcl_map_.points.size() > 0)
            kdtreeMap.setInputCloud(pcl_map_.makeShared());
        else
            is_kdtree_empty = true;

        for (size_t i = 0; i < cfg_.pillar_num; i++) {
            double x, y, w, h, l;
            x = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_x / 2;
            y = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_y / 2;
            bool conti{false};
            for(size_t i = 0 ; i< cfg_.clear_area.size();i++){
                Eigen::Vector3d cur(x,y,0);
                if((cur-cfg_.clear_area[i]).norm() < cfg_.clear_radius[i]){
                    conti = true;
                    break;
                }
            }
            if(conti){
                i--;
                continue;
            }

            w = rand_.unif_0_1(rand_.eng) * (cfg_.pillar_width[1] - cfg_.pillar_width[0]) + cfg_.pillar_width[0];
            l = rand_.unif_0_1(rand_.eng) * (cfg_.pillar_length[1] - cfg_.pillar_length[0]) + cfg_.pillar_length[0];

            pcl::PointXYZ searchPoint(x, y, (cfg_.pillar_height[0] + cfg_.pillar_height[1]) / 2.0);
            pointIdxSearch.clear();
            pointSquaredDistance.clear();

            if (is_kdtree_empty == false) {
                if (kdtreeMap.nearestKSearch(searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0) {
                    if (sqrt(pointSquaredDistance[0]) < 1.0) {
                        i--;
                        continue;
                    }

                }
            }

            x = floor(x / cfg_.resolution) * cfg_.resolution + cfg_.resolution / 2.0;
            y = floor(y / cfg_.resolution) * cfg_.resolution + cfg_.resolution / 2.0;

            int widNum = ceil(w / cfg_.resolution);
            int lenNum = ceil(l / cfg_.resolution);

            for (int r = -widNum / 2.0; r < widNum / 2.0; r++) {
                for (int s = -lenNum / 2.0; s < lenNum / 2.0; s++) {
                    h = rand_.unif_0_1(rand_.eng) * (cfg_.pillar_height[1] - cfg_.pillar_height[0]) +
                        cfg_.cylinder_height[0];
                    int heiNum = 2.0 * ceil(h / cfg_.resolution);
                    for (int t = 0; t < heiNum; t++) {
                        pt_random.x = x + (r + 0.0) * cfg_.resolution + 0.001;
                        pt_random.y = y + (s + 0.0) * cfg_.resolution + 0.001;
                        pt_random.z = (t + 0.0) * cfg_.resolution * 0.5 + 0.001 + cfg_.ground_height;
                        pcl_map_.points.push_back(pt_random);
                    }
                }
            }
        }

        print(fg(color::light_sea_green), " [Pillar] Generate success.\n");
    }

    void GenerateGround(){
        if(!cfg_.use_ground){return;}

        pcl::PointXYZ pt_random;
        for (double i = -cfg_.ground_size_x / 2.0; i <cfg_.ground_size_x / 2.0; i += cfg_.ground_resolution)
            for (double j = -cfg_.ground_size_y / 2.0; j <cfg_.ground_size_y / 2.0; j += cfg_.ground_resolution) {
                pt_random.x = i;
                pt_random.y = j;
                pt_random.z = cfg_.ground_height;
                pcl_map_.points.push_back(pt_random);
                if(cfg_.ground_size_z == 0){
                    continue;
                }
                pt_random.z = cfg_.ground_size_z;
                pcl_map_.points.push_back(pt_random);
            }
        print(fg(color::light_sea_green), " [Groune] Generate success.\n");
    }

    void GenerateTree(pcl::PointCloud<pcl::PointXYZ> &input_map, bvp::Vec3 &center,
                      bvp::Vec3 &start_dir, bvp::Vec3 &end_dir, bvp::Vec3 &end_pos, int &b_num) {
        bvp_.reset(new bvp::ObvpSolver(1000));
        uniform_real_distribution<double> rand_b_xy = uniform_real_distribution<double>(-cfg_.tree_width, cfg_.tree_width);
        uniform_real_distribution<double> rand_bt = uniform_real_distribution<double>(0, 0.3);

        bvp::StatePVAJ start_state;
        bvp::StatePVAJ end_state;
        bvp::Vec3 zero3(0,0,0);
        start_state << zero3, start_dir * cfg_.bending_rate, zero3, zero3;
        end_state << end_pos, end_dir * cfg_.bending_rate, zero3, zero3;

        /* Generate Main Branch */
        bvp::Piece main_branch = bvp_->GenFixStateMinJerkOptT(start_state, end_state);
        double main_dur = main_branch.getDuration();
        double max_vel = main_branch.getMaxVelRate();
        double step = (cfg_.radius_max - cfg_.radius_min) / max_vel;
        for (double eval_t = 0.0; eval_t < main_dur; eval_t += cfg_.sample_t_reso) {
            double radius = step * (max_vel - main_branch.getVel(eval_t).norm()) + cfg_.radius_min;
            bvp::Vec3 pos = main_branch.getPos(eval_t);
            for (double theta = 0.0; theta < 2 * M_PI; theta += cfg_.sample_theta_reso) {
                pcl::PointXYZ p;
                p.x = center.x() + pos.x() + sin(theta) * radius;
                p.y = center.y() + pos.y() + cos(theta) * radius;
                p.z = pos.z() + cfg_.ground_height;
                input_map.points.push_back(p);
            }
        }
        bvp::StatePVAJ branch_start, branch_end;

        for (int i = 0; i < b_num; i++) {
            double split_t = rand_bt(rand_.eng) * main_dur;
            bvp::Vec3 endp(rand_b_xy(rand_.eng), rand_b_xy(rand_.eng), rand_b_xy(rand_.eng) / cfg_.tree_width + end_pos.z());
            branch_start << main_branch.getPos(split_t), main_branch.getVel(split_t), main_branch.getAcc(split_t), zero3;
            branch_end << endp, zero3, zero3, zero3;

            bvp::Piece branch = bvp_->GenFixStateMinJerkOptT(branch_start, branch_end, FIXP);
            double main_dur = branch.getDuration();
            //        double max_vel = branch.getMaxVelRate();
            double step = (cfg_.radius_max - cfg_.radius_min) / max_vel;
            for (double eval_t = 0.0; eval_t < main_dur; eval_t += cfg_.sample_t_reso) {
                double radius = step * max(0.0, max_vel - branch.getVel(eval_t).norm()) + cfg_.radius_min;
                bvp::Vec3 pos = branch.getPos(eval_t);
                for (double theta = 0.0; theta < 2 * M_PI; theta += cfg_.sample_theta_reso) {
                    pcl::PointXYZ p;
                    p.x = center.x() + pos.x() + sin(theta) * radius;
                    p.y = center.y() + pos.y() + cos(theta) * radius;
                    p.z = pos.z() + cfg_.ground_height;
                    input_map.points.push_back(p);
                }
            }
        }
    }

    void GenerateSpline(){
        pcl::PointXYZ pt_random;
        bool is_kdtree_empty = false;
        if (pcl_map_.points.size() > 0)
            kdtreeMap.setInputCloud(pcl_map_.makeShared());
        else
            is_kdtree_empty = true;
        uniform_real_distribution<double> rand_h = uniform_real_distribution<double>(cfg_.height_min, cfg_.height_max);
        uniform_int_distribution<int> rand_b_num = uniform_int_distribution<int>(0, cfg_.max_branch_num);
        for (size_t i = 0; i < cfg_.spline_num; i++) {
            double x, y, w, h, l;
            x = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_x / 2;
            y = rand_.unif_pm_1(rand_.eng) * cfg_.map_size_y / 2;
            bool conti{false};
            for(size_t i = 0 ; i< cfg_.clear_area.size();i++){
                Eigen::Vector3d cur(x,y,0);
                if((cur-cfg_.clear_area[i]).norm() < cfg_.clear_radius[i]){
                    conti = true;
                    break;
                }
            }
            if(conti){
                i--;
                continue;
            }

            if (sqrt(pow(x - cfg_.init_x, 2) + pow(y - cfg_.init_y, 2)) < 0.8) {
                i--;
                continue;
            }

            pcl::PointXYZ searchPoint(x, y, (cfg_.pillar_height[0] + cfg_.pillar_height[1]) / 2.0);
            pointIdxSearch.clear();
            pointSquaredDistance.clear();

            if (is_kdtree_empty == false) {
                if (kdtreeMap.nearestKSearch(searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0) {
                    if (sqrt(pointSquaredDistance[0]) < 1.0) {
                        i--;
                        continue;
                    }

                }
            }

            bvp::Vec3 rand_pos(x,y,0);

            bvp::Vec3 start_dir(rand_.unif_pm_1(rand_.eng), rand_.unif_pm_1(rand_.eng), abs(rand_.unif_pm_1(rand_.eng)) + 1);
            bvp::Vec3 end_dir(rand_.unif_pm_1(rand_.eng), rand_.unif_pm_1(rand_.eng), abs(rand_.unif_pm_1(rand_.eng)) + 1);
            bvp::Vec3 end_pos(rand_.unif_pm_1(rand_.eng), rand_.unif_pm_1(rand_.eng), rand_h(rand_.eng));
            start_dir.normalized();
            end_dir.normalized();
            int b_num = rand_b_num(rand_.eng);
            GenerateTree(pcl_map_, rand_pos, start_dir, end_dir, end_pos, b_num);
        }

        print(fg(color::light_sea_green), " [Spline] Generate success.\n");
    }

};


//==============================================================================
//         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_SWAR_FUNCTIONS_SIMD_SSE_SSE2_SPLIT_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_SPLIT_MULTIPLIES_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/swar/functions/split_multiplies.hpp>
#include <boost/simd/include/functions/simd/interleave_first.hpp>
#include <boost/simd/include/functions/simd/interleave_second.hpp>
#include <boost/simd/include/functions/simd/deinterleave_first.hpp>
#include <boost/simd/include/functions/simd/deinterleave_second.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( split_multiplies_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<int32_<A1>,boost::simd::tag::sse_>))
                                     ((simd_<int32_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1, A1& a2, A1& a3) const
    {
      A0 lo = _mm_mullo_epi16(a0, a1);
      A0 hi = _mm_mulhi_epi16(a0, a1);

      a2 = interleave_first(lo, hi);
      a3 = interleave_second(lo, hi);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( split_multiplies_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<uint16_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<uint16_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<uint32_<A1>,boost::simd::tag::sse_>))
                                     ((simd_<uint32_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1, A1& a2, A1& a3) const
    {
      A0 lo = _mm_mullo_epi16(a0, a1);
      A0 hi = _mm_mulhi_epu16(a0, a1);

      a2 = interleave_first(lo, hi);
      a3 = interleave_second(lo, hi);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( split_multiplies_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<uint64_<A1>,boost::simd::tag::sse_>))
                                     ((simd_<uint64_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1, A1& a2, A1& a3) const
    {
      A1 lo = _mm_mul_epu32(a0, a1);
      A1 hi = _mm_mul_epu32(_mm_srli_si128(a0, 4), _mm_srli_si128(a1, 4));

      a2 = deinterleave_first(lo, hi);
      a3 = deinterleave_second(lo, hi);
    }
  };
} } }

#endif
#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the sine of the input in radians.

    @par Header <boost/simd/function/sin.hpp>

    @par Note

      As most other trigonometric function @c sin may be called with a second
      optional parameter  which is a tag on speed and accuracy
      (see @ref cos for further details)

    @see sincos, sind, sinpi

    @par Example:

      @snippet sin.cpp sin

    @par Possible output:

      @snippet sin.txt sin

  **/
  IEEEValue sin(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sin.hpp>
#include <boost/simd/function/simd/sin.hpp>

#endif