// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/greater_equal.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/less_equal.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/optional.hpp>

#include <laws/base.hpp>
namespace hana = boost::hana;
using hana::test::ct_ord;

struct undefined { };

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::nothing < hana::just(undefined{}));
    BOOST_HANA_CONSTANT_CHECK(hana::just(ct_ord<3>{}) < hana::just(ct_ord<4>{}));
    BOOST_HANA_CONSTANT_CHECK(hana::just(ct_ord<3>{}) <= hana::just(ct_ord<4>{}));
    BOOST_HANA_CONSTANT_CHECK(hana::just(ct_ord<4>{}) > hana::just(ct_ord<3>{}));
    BOOST_HANA_CONSTANT_CHECK(hana::just(ct_ord<4>{}) >= hana::just(ct_ord<3>{}));

    BOOST_HANA_CONSTANT_CHECK(hana::less(
        hana::nothing,
        hana::just(undefined{})
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::less(
        hana::just(undefined{}),
        hana::nothing
    )));
    BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::less(
        hana::nothing,
        hana::nothing
    )));

    BOOST_HANA_CONSTANT_CHECK(hana::less(
        hana::just(ct_ord<3>{}),
        hana::just(ct_ord<4>{})
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::less(
        hana::just(ct_ord<3>{}),
        hana::just(ct_ord<3>{})
    )));
    BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::less(
        hana::just(ct_ord<4>{}),
        hana::just(ct_ord<3>{})
    )));
}


// Copyright 2014 BVLC and contributors.

#include <algorithm>
#include <vector>
#include <cmath>
#include <stdio.h>

#include "google/protobuf/descriptor.h"
#include "google/protobuf/descriptor.h"
#include "caffe/layer.hpp"
#include "caffe/layers/flow_warp_layer.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/util/math_functions.hpp"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

#include <iostream>
#include <fstream>
#include <omp.h>

using std::max;

namespace caffe {

template <typename Dtype>
void FlowWarpLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
}

template <typename Dtype>
void FlowWarpLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
  CHECK_EQ(bottom.size(), 2) << "FlowWarpLayer takes two input blobs: image and flow.";
  CHECK_EQ(top.size(), 1) << "FlowWarpLayer outputs one blob.";

  const int num = bottom[0]->num();
  const int channels = bottom[0]->channels();
  const int height = bottom[0]->height();
  const int width = bottom[0]->width();

  CHECK_EQ(num, bottom[1]->num()) << "Num of the inputs should be the same";
  CHECK_EQ(2, bottom[1]->channels()) << "Flow should have 2 channels: x-flow and y-flow";
  CHECK_EQ(width, bottom[1]->width()) << "Width of the inputs should be the same";
  CHECK_EQ(height, bottom[1]->height()) << "Height of the inputs should be the same";

  top[0]->Reshape(num, channels, height, width);
  transposed_image_.Reshape(num, height, width, channels);
}

#define min(a,b) ((a<b)?(a):(b))
#define max(a,b) ((a>b)?(a):(b))

template <typename Dtype>
void FlowWarpLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
    const vector<Blob<Dtype>*>& top)
{
    int width = top[0]->width();
    int height = top[0]->height();
    int channels = top[0]->channels();
    int num = top[0]->num();
    const int wh_size = width * height;
    const int whc_size = width * height * channels;

    Dtype* warped_data = top[0]->mutable_cpu_data(); // dest
    const Dtype* image_data = bottom[0]->cpu_data(); // source image
    const Dtype* flow_data = bottom[1]->cpu_data(); // source flow

    Dtype fillValue = this->layer_param().flow_warp_param().fill_value() == FlowWarpParameter_FillParameter_ZERO ? 0 : NAN;

    for(int n=0; n<num; n++)
    {
        int off = whc_size * n;
        for(int x=0; x<width; x++)
            for(int y=0; y<height; y++)
            {
                float fx = flow_data[2*wh_size*n + y*width + x];
                float fy = flow_data[2*wh_size*n + wh_size + y*width + x];

                float x2 = float(x) + fx;
                float y2 = float(y) + fy;

                if(x2>=0 && y2>=0 && x2<width && y2<height)
                {
                    int ix2_L = int(x2);
                    int iy2_T = int(y2);
                    int ix2_R = min(ix2_L+1, width-1);
                    int iy2_B = min(iy2_T+1, height-1);

                    float alpha=x2-ix2_L;
                    float beta=y2-iy2_T;

                    for(int c=0; c<channels; c++)
                    {
                        float TL = image_data[off + c*wh_size + iy2_T*width + ix2_L];
                        float TR = image_data[off + c*wh_size + iy2_T*width + ix2_R];
                        float BL = image_data[off + c*wh_size + iy2_B*width + ix2_L];
                        float BR = image_data[off + c*wh_size + iy2_B*width + ix2_R];

                        warped_data[off + c*wh_size + y*width + x] =
                            (1-alpha)*(1-beta)*TL +
                            alpha*(1-beta)*TR +
                            (1-alpha)*beta*BL +
                            alpha*beta*BR;
                    }
                }
                else
                {
                    for(int c=0; c<channels; c++)
                        warped_data[off + c*wh_size + y*width + x] = fillValue;
                }
            }
    }
}

template <typename Dtype>
void FlowWarpLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom)
{
    int width = top[0]->width();
    int height = top[0]->height();
    int channels = top[0]->channels();
    int num = top[0]->num();
    const int wh_size = width * height;
    const int whc_size = width * height * channels;

    const Dtype* warped_data = top[0]->cpu_data(); // dest
    const Dtype* warped_diff = top[0]->cpu_diff(); // dest
    const Dtype* image_data = bottom[0]->cpu_data(); // source image
    Dtype* image_diff = bottom[0]->mutable_cpu_diff(); // source image
    const Dtype* flow_data = bottom[1]->cpu_data(); // source flow
    Dtype* flow_diff = bottom[1]->mutable_cpu_diff(); // source flow

    for(int i=0; i<num*whc_size; i++)
        image_diff[i] = 0 ;

    for(int n=0; n<num; n++)
    {
        int off = whc_size * n;
        for(int x=0; x<width; x++)
            for(int y=0; y<height; y++)
            {
                float fx = flow_data[2*wh_size*n + y*width + x];
                float fy = flow_data[2*wh_size*n + wh_size + y*width + x];

                float x2 = float(x) + fx;
                float y2 = float(y) + fy;

                if(x2>=0 && y2>=0 && x2<width && y2<height)
                {
                    int ix2_L = int(x2);
                    int iy2_T = int(y2);
                    int ix2_R = min(ix2_L+1, width-1);
                    int iy2_B = min(iy2_T+1, height-1);

                    float alpha=x2-ix2_L;
                    float beta=y2-iy2_T;
                    for(int c=0; c<channels; c++)
                    {
                        float warped_diff_value = warped_diff[off + c*wh_size + y*width + x];
                        image_diff[off + c*wh_size + iy2_T*width + ix2_L] += warped_diff_value * (1-alpha)*(1-beta);
                        image_diff[off + c*wh_size + iy2_T*width + ix2_R] += warped_diff_value * alpha*(1-beta);
                        image_diff[off + c*wh_size + iy2_B*width + ix2_L] += warped_diff_value * (1-alpha)*beta;
                        image_diff[off + c*wh_size + iy2_B*width + ix2_R] += warped_diff_value * alpha*beta;
                    }

                    float gamma = iy2_B - y2;
                    float bot_diff = 0;
                    for(int c=0; c<channels; c++)
                    {
                        float temp = 0;
                        temp += gamma *     (image_data[off + c*wh_size + iy2_T*width + ix2_R] - image_data[off + c*wh_size + iy2_T*width + ix2_L]);
                        temp += (1-gamma) * (image_data[off + c*wh_size + iy2_B*width + ix2_R] - image_data[off + c*wh_size + iy2_B*width + ix2_L]);

                        bot_diff += warped_diff[off + c*wh_size + y*width + x] * temp;
                    }
                    flow_diff[2*wh_size*n + y*width + x] = bot_diff;

                    gamma = ix2_R - x2;
                    bot_diff = 0;
                    for(int c=0; c<channels; c++)
                    {
                        float temp = 0;
                        temp += gamma *     (image_data[off + c*wh_size + iy2_B*width + ix2_L] - image_data[off + c*wh_size + iy2_T*width + ix2_L]);
                        temp += (1-gamma) * (image_data[off + c*wh_size + iy2_B*width + ix2_R] - image_data[off + c*wh_size + iy2_T*width + ix2_R]);

                        bot_diff += warped_diff[off + c*wh_size + y*width + x] * temp;
                    }
                    flow_diff[2*wh_size*n + wh_size + y*width + x] = bot_diff;
                }
            }
    }

    if(!propagate_down[0]) caffe_memset(bottom[0]->count()*sizeof(Dtype), 0, image_diff);
    if(!propagate_down[1]) caffe_memset(bottom[1]->count()*sizeof(Dtype), 0, flow_diff);

//    {
//        printf("cpu flow u:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->data_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu flow v:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->data_at(0, 1, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu image:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[0]->data_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu flow diff u:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->diff_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu flow diff v:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[1]->diff_at(0, 1, y, x));
//            }
//            printf("\n");
//        }
//        printf("cpu image diff:\n");
//        for(int y=0; y<height; y++)
//        {
//            for(int x=0; x<width; x++)
//            {
//                printf("%f ", bottom[0]->diff_at(0, 0, y, x));
//            }
//            printf("\n");
//        }
//    }
}

INSTANTIATE_CLASS(FlowWarpLayer);
REGISTER_LAYER_CLASS(FlowWarp);

}  // namespace caffe

//                    }
//                }
//            }

//        for(int x=0; x<width; x++)
//            for(int y=0; y<height; y++)
//            {
//                float fx = flow_data[off + y*width + x];
//                float fy = flow_data[off + wh_size + y*width + x];

//                float x2 = float(x) + fx;
//                float y2 = float(y) + fy;

//                if(x2>=0 && y2>=0 && x2<width && y2<height)
//                {
//                    int ix2_L = int(x2);
//                    int iy2_T = int(y2);
//                    int ix2_R = min(ix2_L+1, width-1);
//                    int iy2_B = min(iy2_T+1, height-1);


#include "elementwise_operations_with_constants_test.hpp"
#include <boost/numeric/ublas/matrix.hpp>

int main()
{

  ///testing row major flaot prod
  bench_elementwise_constant<float, ublas::basic_row_major<>, 10, 10> b1;

  ///testing row major complex float prod
  bench_elementwise_constant<std::complex<float>, ublas::basic_row_major<>, 10, 10> b2;

  ///testing row major double prod
  bench_elementwise_constant<double, ublas::basic_row_major<>, 10, 10> b3;

  ///testing row major complex float elementwise operations with constants
  bench_elementwise_constant<std::complex<double>, ublas::basic_row_major<>, 10, 10> b4;

  ///testing column major flaot elementwise operations with constants
  bench_elementwise_constant<float, ublas::basic_column_major<>, 10, 10> b5;

  ///testing column major complex float elementwise operations with constants
  bench_elementwise_constant<std::complex<float>, ublas::basic_column_major<>, 10, 10> b6;

  ///testing column major double elementwise operations with constants
  bench_elementwise_constant<double, ublas::basic_column_major<>, 10, 10> b7;

  ///testing column major complex double elementwise operations with constants
  bench_elementwise_constant<std::complex<double>, ublas::basic_column_major<>, 10, 10> b8;

  std::cout << "Row major:" << std::endl;
  b1.run();
  b2.run();
  b3.run();
  b4.run();

  std::cout << "Column major:" << std::endl;
  b5.run();
  b6.run();
  b7.run();
  b8.run();

  return 0;

}


/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program 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 Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_MAPPING_BaseDeformationMAPPING_INL
#define SOFA_COMPONENT_MAPPING_BaseDeformationMAPPING_INL

#include "BaseDeformationMapping.h"
#include "BaseDeformationImpl.inl"
#include <SofaBaseVisual/VisualModelImpl.h>
#include <sofa/helper/gl/Color.h>
#include <sofa/helper/system/glu.h>
#include <sofa/helper/IndexOpenMP.h>

#ifdef _OPENMP
#include <omp.h>
#endif

#include <limits>

#include <Eigen/Core>
#include <Eigen/Dense>

namespace sofa
{
namespace component
{
namespace mapping
{

template <class JacobianBlockType>
BaseDeformationMappingT<JacobianBlockType>::BaseDeformationMappingT (core::State<In>* from , core::State<Out>* to)
    : Inherit ( from, to )
    , f_shapeFunction_name(initData(&f_shapeFunction_name,"shapeFunction","name of shape function (optional)"))
    , _shapeFunction(NULL)
    , _sampler(NULL)
    , f_index ( initData ( &f_index,"indices","parent indices for each child" ) )
    , f_w ( initData ( &f_w,"weights","influence weights of the Dofs" ) )
    , f_dw ( initData ( &f_dw,"weightGradients","weight gradients" ) )
    , f_ddw ( initData ( &f_ddw,"weightHessians","weight Hessians" ) )
    , f_F0 ( initData ( &f_F0,"M","Linear transformations from material to 3d space" ) )
    , f_cell ( initData ( &f_cell,"cell","indices required by shape function in case of overlapping elements" ) )
    , assemble ( initData ( &assemble,false, "assemble","Assemble the matrices (Jacobian/Geometric Stiffness) or use optimized Jacobian/vector multiplications" ) )
    , f_pos0 ( initData ( &f_pos0,"restPosition","initial spatial positions of children" ) )
    , missingInformationDirty(true)
    , KdTreeDirty(true)
    , triangles(0)
    , extTriangles(0)
    , extvertPosIdx(0)
    , showDeformationGradientScale(initData(&showDeformationGradientScale, (float)0.0, "showDeformationGradientScale", "Scale for deformation gradient display"))
    , showDeformationGradientStyle ( initData ( &showDeformationGradientStyle,"showDeformationGradientStyle","Visualization style for deformation gradients" ) )
    , showColorOnTopology ( initData ( &showColorOnTopology,"showColorOnTopology","Color mapping method" ) )
    , showColorScale(initData(&showColorScale, (float)1.0, "showColorScale", "Color mapping scale"))
    , d_geometricStiffness(initData(&d_geometricStiffness, 0u, "geometricStiffness", "0=no GS, 1=non symmetric, 2=symmetrized"))
    , d_parallel(initData(&d_parallel, false, "parallel", "use openmp parallelisation?"))
{
    helper::OptionsGroup methodOptions(3,"0 - None"
                                       ,"1 - trace(F^T.F)-3"
                                       ,"2 - sqrt(det(F^T.F))-1");
    methodOptions.setSelectedItem(0);
    showColorOnTopology.setValue(methodOptions);

    helper::OptionsGroup styleOptions(6,"0 - All axis"
                                      ,"1 - First axis"
                                      ,"2 - Second axis"
                                      ,"3 - Third axis"
                                      ,"4 - deformation"
                                      ,"5 - 1st piola stress" );
    styleOptions.setSelectedItem(0);
    showDeformationGradientStyle.setValue(styleOptions);
}

/*
template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::updateIndex(const size_t parentSize, const size_t childSize)
{
    if(this->f_printLog.getValue())
        std::cout<<this->getName()<< "::" << SOFA_CLASS_METHOD <<std::endl;

    this->f_index_parentToChild.clear();
    this->f_index_parentToChild.resize(parentSize);

    const VecVRef& index = this->f_index.getValue();

    //Check size just in case
    if(childSize != index.size())
    {
        std::cout << SOFA_CLASS_METHOD << " f_index has wrong size" << std::endl;
        serr << "index size : " << index.size() << sendl;
        serr << "child size : " << childSize << sendl;
        exit(EXIT_FAILURE);
    }

    //Go through f_index and use its value to update f_index_parentToChild
    for(size_t i=0; i<index.size(); ++i)
    {
        for(size_t j=0; j< index[i].size(); j++ )
        {
            int parentIndex = index[i][j];
            this->f_index_parentToChild[parentIndex].push_back(i); //Add child index
            this->f_index_parentToChild[parentIndex].push_back(j); //Add parent index
        }
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::updateIndex()
{
    if(this->f_printLog.getValue())
    {
        std::cout<<this->getName()<< "::" << SOFA_CLASS_METHOD <<std::endl;
    }

    int parentSize = this->getFromSize();
    int childSize = this->getToSize();
    this->f_index_parentToChild.clear();
    this->f_index_parentToChild.resize(parentSize);

    if(this->f_printLog.getValue())
    {
        std::cout << "parent size : " << parentSize << std::endl;
        std::cout << "child size : " << childSize << std::endl;
    }

    const VecVRef& index = this->f_index.getValue();

    //Check size just in case
    if( (unsigned)childSize != index.size() )
    {
        std::cout << SOFA_CLASS_METHOD << " f_index has wrong size" << std::endl;
        serr << "index size : " << index.size() << sendl;
        serr << "child size : " << childSize << sendl;
        exit(EXIT_FAILURE);
    }

    //Go through f_index and use its value to update f_index_parentToChild
    for(size_t i=0; i<index.size(); ++i)
    {
        for(size_t j=0; j< index[i].size(); j++ )
        {
            int parentIndex = index[i][j];
            this->f_index_parentToChild[parentIndex].push_back(i); //Add child index
            this->f_index_parentToChild[parentIndex].push_back(j); //Add parent index
        }
    }
}
*/

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::resizeAll(const InVecCoord& p0, const OutVecCoord& c0, const VecCoord& x0, const VecVRef& index, const VecVReal& w, const VecVGradient& dw, const VecVHessian& ddw, const VMaterialToSpatial& F0)
{
    size_t cSize = c0.size();
    if(cSize != x0.size() || cSize != index.size() || cSize != w.size() || cSize != dw.size() || cSize != ddw.size())
    {
        sout << SOFA_CLASS_METHOD << " : wrong sizes " << sendl;
    }

    helper::WriteOnlyAccessor<Data<VecCoord> > wa_x0 (this->f_pos0);
    wa_x0.resize(cSize);
    for(size_t i=0; i<cSize; ++i)
        wa_x0[i] = x0[i];

    helper::WriteOnlyAccessor<Data<VecVRef > > wa_index (this->f_index);
    wa_index.resize(cSize);
    for(size_t i=0; i<cSize; ++i)
        wa_index[i].assign(index[i].begin(), index[i].end());

    helper::WriteOnlyAccessor<Data<VecVReal > > wa_w (this->f_w);
    wa_w.resize(cSize);
    for(size_t i=0; i<cSize; ++i)
        wa_w[i].assign(w[i].begin(), w[i].end());

    helper::WriteOnlyAccessor<Data<VecVGradient > > wa_dw (this->f_dw);
    wa_dw.resize(cSize);
    for(size_t i=0; i<cSize; ++i)
        wa_dw[i].assign(dw[i].begin(), dw[i].end());

    helper::WriteOnlyAccessor<Data<VecVHessian > > wa_ddw (this->f_ddw);
    wa_ddw.resize(cSize);
    for(size_t i=0; i<cSize; ++i)
        wa_ddw[i].assign(ddw[i].begin(), ddw[i].end());

    helper::WriteOnlyAccessor<Data< VMaterialToSpatial > >  wa_F0(this->f_F0);
    wa_F0.resize(cSize);
    for(size_t i=0; i<cSize; ++i)
        wa_F0[i] = F0[i];

//    updateIndex(p0.size(), c0.size());

    initJacobianBlocks(p0, c0);
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::resizeOut()
{
    {
        // TODO this must be done before resizeOut() but is done again in Inherit::init();
        // also clean the numerous calls to apply
        core::behavior::BaseMechanicalState *state;
        if ((state = this->fromModel.get()->toBaseMechanicalState()))
            this->maskFrom = &state->forceMask;
        if ((state = this->toModel.get()->toBaseMechanicalState()))
            this->maskTo = &state->forceMask;
        else
            this->setNonMechanical();
    }

    helper::ReadAccessor<Data<OutVecCoord> > out (*this->toModel->read(core::ConstVecCoordId::position()));

    helper::WriteOnlyAccessor<Data<VecCoord> > pos0 (this->f_pos0);
    helper::WriteOnlyAccessor<Data< VMaterialToSpatial > > F0(this->f_F0);
    this->missingInformationDirty=true; this->KdTreeDirty=true; // need to update mapped spatial positions if needed for visualization

    size_t size;

    if( !_sampler ) this->getContext()->get(_sampler,core::objectmodel::BaseContext::Local);
    bool restPositionSet=false;
    helper::ReadAccessor<Data< OutVecCoord > >  rest(*this->toModel->read(core::ConstVecCoordId::restPosition()));

    if(_sampler) // retrieve initial positions from gauss point sampler (deformation gradient types)
    {
        size = _sampler->getNbSamples();
        if(rest.size()==size && size!=1)  restPositionSet=true;

        this->toModel->resize(size);
        pos0.resize(size);  for(size_t i=0; i<size; i++) pos0[i]=_sampler->getSamples()[i];
        F0.resize(size);
        if(restPositionSet)     // use custom rest positions defined in state (to set material directions or set residual deformations)
        {
            for(size_t i=0; i<rest.size(); ++i) F0[i]=OutDataTypesInfo<Out>::getF(rest[i]);
            sout<<rest.size()<<" rest positions imported "<<sendl;
        }
        else
        {
            for(size_t i=0; i<size; ++i) copy(F0[i],_sampler->getTransforms()[i]);
        }
        sout<<size <<" gauss points imported"<<sendl;
    }
    else  // retrieve initial positions from children dofs (vec types)
    {
        size = out.size();
        this->toModel->resize(size);
        pos0.resize(size);  for(size_t i=0; i<size; i++ )  Out::get(pos0[i][0],pos0[i][1],pos0[i][2],out[i]);
        //        F0.resize(size); for(size_t i=0; i<size; ++i) identity(F0[i]); // necessary ?
    }

    // init shape function
    if( !_shapeFunction )
    {
        sofa::core::objectmodel::BaseContext* context = this->getContext();
        std::vector<BaseShapeFunction*> sf; context->get<BaseShapeFunction>(&sf,core::objectmodel::BaseContext::SearchUp);
        for(unsigned int i=0;i<sf.size();i++)
        {
            if(this->f_shapeFunction_name.isSet()) {if(this->f_shapeFunction_name.getValue().compare(sf[i]->getName()) == 0) _shapeFunction=sf[i];}
            else if(sf[i]->f_position.getValue().size() == this->fromModel->getSize()) _shapeFunction=sf[i];
        }
    }

    if(0 != f_index.getValue().size() && pos0.size() == f_index.getValue().size() && f_w.getValue().size() == f_index.getValue().size()) // we already have the needed data, we directly use them
    {
        sout<<"using filled data" <<sendl;
    }
    else if(_shapeFunction) // if we do not have the needed data, and have a shape function, we use it to compute needed data (index, weights, etc.)
    {
        sout<<"found shape function "<<_shapeFunction->getName()<<sendl;
        helper::vector<mCoord> mpos0;
        mpos0.resize(pos0.size());
        for(size_t i=0; i<pos0.size(); ++i) defaulttype::StdVectorTypes<mCoord,mCoord>::set( mpos0[i], pos0[i][0] , pos0[i][1] , pos0[i][2]);

        // interpolate weights at sample positions
        if(this->f_cell.getValue().size()==size) _shapeFunction->computeShapeFunction(mpos0,*this->f_index.beginWriteOnly(),*this->f_w.beginWriteOnly(),*this->f_dw.beginWriteOnly(),*this->f_ddw.beginWriteOnly(),this->f_cell.getValue());
        else _shapeFunction->computeShapeFunction(mpos0,*this->f_index.beginWriteOnly(),*this->f_w.beginWriteOnly(),*this->f_dw.beginWriteOnly(),*this->f_ddw.beginWriteOnly());
        this->f_index.endEdit();      this->f_w.endEdit();        this->f_dw.endEdit();        this->f_ddw.endEdit();
    }
    else // if the prerequisites are not fulfilled we print an error
    {
        serr << "ShapeFunction<"<<ShapeFunctionType::Name()<<"> component not found" << sendl;
    }

//    updateIndex();

    // init jacobians
    initJacobianBlocks();

    // clear forces
    if(this->toModel->write(core::VecDerivId::force())) { helper::WriteOnlyAccessor<Data< OutVecDeriv > >  f(*this->toModel->write(core::VecDerivId::force())); for(size_t i=0;i<f.size();i++) f[i].clear(); }
    // clear velocities
    if(this->toModel->write(core::VecDerivId::velocity())) { helper::WriteOnlyAccessor<Data< OutVecDeriv > >  vel(*this->toModel->write(core::VecDerivId::velocity())); for(size_t i=0;i<vel.size();i++) vel[i].clear(); }

    //Apply mapping to init child positions
    reinit();

    // set deformation gradient state rest position when defined by gaussPointSampler
    if(_sampler && restPositionSet == false && this->toModel->read(core::VecCoordId::restPosition())->getValue().size()==size ) // not for states that do not have restpos (like visualmodel)
    {
        helper::ReadAccessor<Data< VMaterialToSpatial > >  ra_F0(this->f_F0);
        helper::WriteOnlyAccessor<Data< OutVecCoord > >  rest(*this->toModel->write(core::VecCoordId::restPosition()));
        for(size_t i=0; i<rest.size(); ++i) for(int j=0; j<spatial_dimensions; ++j) for(int k=0; k<material_dimensions; ++k) rest[i][j*material_dimensions+k] = (OutReal)ra_F0[i][j][k];
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::resizeOut(const helper::vector<Coord>& position0, helper::vector<helper::vector<unsigned int> > index,helper::vector<helper::vector<Real> > w, helper::vector<helper::vector<defaulttype::Vec<spatial_dimensions,Real> > > dw, helper::vector<helper::vector<defaulttype::Mat<spatial_dimensions,spatial_dimensions,Real> > > ddw, helper::vector<defaulttype::Mat<spatial_dimensions,spatial_dimensions,Real> > F0)
{
    {
        // TODO this must be done before resizeOut() but is done again in Inherit::init();
        // also clean the numerous calls to apply
        core::behavior::BaseMechanicalState *state;
        if( (state = this->fromModel.get()->toBaseMechanicalState()) )
            this->maskFrom = &state->forceMask;
        if( (state = this->toModel.get()->toBaseMechanicalState()) )
            this->maskTo = &state->forceMask;
        else
            this->setNonMechanical();
    }

    helper::WriteOnlyAccessor<Data<VecCoord> > pos0 (this->f_pos0);
    this->missingInformationDirty=true; this->KdTreeDirty=true; // need to update mapped spatial positions if needed for visualization

    size_t size = position0.size();

    // paste input values
    this->toModel->resize(size);
    pos0.resize(size);  for(size_t i=0; i<size; i++ )        pos0[i]=position0[i];

    helper::WriteOnlyAccessor<Data<VecVRef > > wa_index (this->f_index);   wa_index.resize(size);  for(size_t i=0; i<size; i++ )    wa_index[i].assign(index[i].begin(), index[i].end());
    helper::WriteOnlyAccessor<Data<VecVReal > > wa_w (this->f_w);          wa_w.resize(size);  for(size_t i=0; i<size; i++ )    wa_w[i].assign(w[i].begin(), w[i].end());
    helper::WriteOnlyAccessor<Data<VecVGradient > > wa_dw (this->f_dw);    wa_dw.resize(size);  for(size_t i=0; i<size; i++ )    wa_dw[i].assign(dw[i].begin(), dw[i].end());
    helper::WriteOnlyAccessor<Data<VecVHessian > > wa_ddw (this->f_ddw);   wa_ddw.resize(size);  for(size_t i=0; i<size; i++ )    wa_ddw[i].assign(ddw[i].begin(), ddw[i].end());
    helper::WriteOnlyAccessor<Data<VMaterialToSpatial> > wa_F0 (this->f_F0);    wa_F0.resize(size);  for(size_t i=0; i<size; i++ )    for(size_t j=0; j<spatial_dimensions; j++ ) for(size_t k=0; k<material_dimensions; k++ )   wa_F0[i][j][k]=F0[i][j][k];

    sout<<size <<" custom gauss points imported"<<sendl;

//    updateIndex();

    // init jacobians
    initJacobianBlocks();

    // clear forces
    if(this->toModel->write(core::VecDerivId::force())) { helper::WriteOnlyAccessor<Data< OutVecDeriv > >  f(*this->toModel->write(core::VecDerivId::force())); for(size_t i=0;i<f.size();i++) f[i].clear(); }
    // clear velocities
    if(this->toModel->write(core::VecDerivId::velocity())) { helper::WriteOnlyAccessor<Data< OutVecDeriv > >  vel(*this->toModel->write(core::VecDerivId::velocity())); for(size_t i=0;i<vel.size();i++) vel[i].clear(); }

    //Apply mapping to init child positions
    reinit();

    // update deformation gradient state rest position
    if( this->toModel->read(core::VecCoordId::restPosition())->getValue().size()==size ) // not for states that do not have restpos (like visualmodel)
    {
        if( !_sampler ) this->getContext()->get(_sampler,core::objectmodel::BaseContext::Local);
        if(_sampler )
        {
            helper::ReadAccessor<Data< VMaterialToSpatial > >  ra_F0(this->f_F0);
            helper::WriteOnlyAccessor<Data< OutVecCoord > >  rest(*this->toModel->write(core::VecCoordId::restPosition()));
            for(size_t i=0; i<rest.size(); ++i) for(int j=0; j<spatial_dimensions; ++j) for(int k=0; k<material_dimensions; ++k) rest[i][j*material_dimensions+k] = (OutReal)ra_F0[i][j][k];
        }
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::init()
{
    baseMatrices.resize( 1 ); // just a wrapping for getJs()
    baseMatrices[0] = &eigenJacobian;

    resizeOut();

    Inherit::init();

    // check that all children particles got a parent
    const VecVRef& indices = this->f_index.getValue();
    for (std::size_t i=0; i < indices.size(); ++i)
        if ( indices[i].empty() )
            serr << this->getPathName() << " Particle " << i << " has no parent" << sendl;
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::reinit()
{
    if(this->isMechanical() && this->assemble.getValue()) updateJ();

    // force apply
    // bg: do we need this ?
    //    apply(NULL, *this->toModel->write(core::VecCoordId::position()), *this->fromModel->read(core::ConstVecCoordId::position()));
    //    if(this->toModel->write(core::VecDerivId::velocity())) applyJ(NULL, *this->toModel->write(core::VecDerivId::velocity()), *this->fromModel->read(core::ConstVecDerivId::velocity()));

    Inherit::reinit();
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::updateJ()
{
    helper::ReadAccessor<Data<InVecCoord> > in (*this->fromModel->read(core::ConstVecCoordId::position()));
    //helper::ReadAccessor<Data<OutVecCoord> > out (*this->toModel->read(core::ConstVecCoordId::position()));
    const VecVRef& index = this->f_index.getValue();

    SparseMatrixEigen& J = eigenJacobian;

    J.resizeBlocks(jacobian.size(),in.size());

    for( size_t i=0 ; i<this->maskTo->size() ; ++i)
    {
        J.beginBlockRow(i);
        for(size_t j=0; j<jacobian[i].size(); j++)
            J.createBlock( index[i][j], jacobian[i][j].getJ());
        J.endBlockRow();
    }

    J.compress();

    //    maskedEigenJacobian.resize(0,0);
}

//template <class JacobianBlockType>
//void BaseDeformationMappingT<JacobianBlockType>::updateMaskedJ()
//{
//    size_t currentHash = this->maskTo->getHash();
//    if( previousMaskHash!=currentHash )
//    {
//        previousMaskHash = currentHash;
//        maskedEigenJacobian.resize(0,0);
//    }
//    if( !maskedEigenJacobian.rows() )
//    {
//        this->maskTo->maskedMatrix( maskedEigenJacobian.compressedMatrix, eigenJacobian.compressedMatrix, Out::deriv_total_size );
//        sout<<"updateMaskedJ "<<maskedEigenJacobian.compressedMatrix.nonZeros()<<sendl;
//    }
//}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::updateK( const core::MechanicalParams* mparams, core::ConstMultiVecDerivId childForceId )
{
    unsigned geometricStiffness = d_geometricStiffness.getValue();

    if( BlockType::constant || !geometricStiffness /*|| !assemble.getValue()*/ ) { K.resize(0,0); return; }

    const OutVecDeriv& childForce = childForceId[this->toModel.get(mparams)].read()->getValue();
    helper::ReadAccessor<Data<InVecCoord> > in (*this->fromModel->read(core::ConstVecCoordId::position()));
    const VecVRef& indices = this->f_index.getValue();

    K.resizeBlocks(in.size(),in.size());
    helper::vector<KBlock> diagonalBlocks; diagonalBlocks.resize(in.size());

    // TODO: need to take into account mask in geometric stiffness, I do no think so!??

    for(size_t i=0; i<jacobian.size(); i++)
    {
        for(size_t j=0; j<jacobian[i].size(); j++)
        {
            size_t index=indices[i][j];
            diagonalBlocks[index] += jacobian[i][j].getK(childForce[i], geometricStiffness==2);
        }
    }

    for(size_t i=0; i<in.size(); i++)
        K.insertBackBlock(i,i,diagonalBlocks[i]);
    K.compress();
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::apply(OutVecCoord& out, const InVecCoord& in)
{
    const VecVRef& indices = this->f_index.getValue();

#ifdef _OPENMP
#pragma omp parallel for if (this->d_parallel.getValue())
#endif
    for(helper::IndexOpenMP<unsigned int>::type i=0; i<jacobian.size(); i++)
    {
        out[i]=OutCoord();
        for(size_t j=0; j<jacobian[i].size(); j++)
        {
            size_t index=indices[i][j];
            jacobian[i][j].addapply(out[i],in[index]);
        }
    }

    if(this->assemble.getValue() && ( !BlockType::constant ) )  eigenJacobian.resize(0,0); // J needs to be updated later where the dof mask can be activated

    this->missingInformationDirty=true; this->KdTreeDirty=true; // need to update spatial positions of defo grads if needed for visualization
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::applyJ(OutVecDeriv& out, const InVecDeriv& in)
{
    if(this->assemble.getValue())
    {
        if( !eigenJacobian.rows() ) updateJ();
        eigenJacobian.mult(out,in);
    }
    else
    {
        const VecVRef& indices = this->f_index.getValue();

        for( size_t i=0 ; i<this->maskTo->size() ; ++i)
        {
            if( !this->maskTo->isActivated() || this->maskTo->getEntry(i) )
            {
                out[i]=OutDeriv();
                for(size_t j=0; j<jacobian[i].size(); j++)
                {
                    size_t index=indices[i][j];
                    jacobian[i][j].addmult(out[i],in[index]);
                }
            }
        }
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::apply(const core::MechanicalParams * /*mparams*/ , Data<OutVecCoord>& dOut, const Data<InVecCoord>& dIn)
{
    OutVecCoord& out = *dOut.beginWriteOnly();
    const InVecCoord& in = dIn.getValue();
    const VecVRef& indices = this->f_index.getValue();

#ifdef _OPENMP
#pragma omp parallel for if (this->d_parallel.getValue())
#endif
    for(helper::IndexOpenMP<unsigned int>::type i=0; i<jacobian.size(); i++)
    {
        out[i]=OutCoord();
        for(size_t j=0; j<jacobian[i].size(); j++)
        {
            size_t index=indices[i][j];
            jacobian[i][j].addapply(out[i],in[index]);
        }
    }
    dOut.endEdit();

    if(this->assemble.getValue() && ( !BlockType::constant ) ) eigenJacobian.resize(0,0); // J needs to be updated later where the dof mask can be activated

    this->missingInformationDirty=true; this->KdTreeDirty=true; // need to update spatial positions of defo grads if needed for visualization
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::applyJ(const core::MechanicalParams * /*mparams*/ , Data<OutVecDeriv>& dOut, const Data<InVecDeriv>& dIn)
{
    if(this->assemble.getValue())
    {
        if( !eigenJacobian.rows() ) updateJ();

        //        if( this->maskTo->isActivated() )
        //        {
        //            updateMaskedJ();
        //            maskedEigenJacobian.mult(dOut,dIn);
        //        }
        //        else
        eigenJacobian.mult(dOut,dIn);
    }
    else
    {
        OutVecDeriv& out = *dOut.beginWriteOnly();
        const InVecDeriv& in = dIn.getValue();
        const VecVRef& indices = this->f_index.getValue();

        for( size_t i=0 ; i<this->maskTo->size() ; ++i)
        {
            if( !this->maskTo->isActivated() || this->maskTo->getEntry(i) )
            {
                out[i]=OutDeriv();
                for(size_t j=0; j<jacobian[i].size(); j++)
                {
                    size_t index=indices[i][j];
                    jacobian[i][j].addmult(out[i],in[index]);
                }
            }
        }

        dOut.endEdit();
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::applyJT(const core::MechanicalParams * /*mparams*/ , Data<InVecDeriv>& dIn, const Data<OutVecDeriv>& dOut)
{
    if(this->assemble.getValue())
    {
        if( !eigenJacobian.rows() ) updateJ();

        //        if( this->maskTo->isActivated() )
        //        {
        //            updateMaskedJ();
        //            maskedEigenJacobian.addMultTranspose(dIn,dOut);
        //        }
        //        else
        eigenJacobian.addMultTranspose(dIn,dOut);
    }
    else
    {
        InVecDeriv& in = *dIn.beginEdit();
        const OutVecDeriv& out = dOut.getValue();
        const VecVRef& indices = this->f_index.getValue();

        for( size_t i=0 ; i<this->maskTo->size() ; ++i)
        {
            if( this->maskTo->getEntry(i) )
            {
                for(size_t j=0; j<jacobian[i].size(); j++)
                {
                    size_t index=indices[i][j];
                    jacobian[i][j].addMultTranspose(in[index],out[i]);
                }
            }
        }

        dIn.endEdit();
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::applyDJT(const core::MechanicalParams* mparams, core::MultiVecDerivId parentDfId, core::ConstMultiVecDerivId childForceId )
{
    if( BlockType::constant || !d_geometricStiffness.getValue() ) return;

    Data<InVecDeriv>& parentForceData = *parentDfId[this->fromModel.get(mparams)].write();
    const Data<InVecDeriv>& parentDisplacementData = *mparams->readDx(this->fromModel);
    const Data<OutVecDeriv>& childForceData = *mparams->readF(this->toModel);

    helper::WriteAccessor<Data<InVecDeriv> > parentForce (parentForceData);
    helper::ReadAccessor<Data<InVecDeriv> > parentDisplacement (parentDisplacementData);
    helper::ReadAccessor<Data<OutVecDeriv> > childForce (childForceData);

    if( assemble.getValue() && K.compressedMatrix.nonZeros() ) // assembled version
    {
        assert( this->assemble.getValue() );
        K.addMult(parentForceData,parentDisplacementData,mparams->kFactor());
    }
    else
    {
        // if symmetrized version, force local assembly
        if( assemble.getValue() || d_geometricStiffness.getValue() == 2 )
        {
            updateK( mparams, childForceId );
            K.addMult(parentForceData,parentDisplacementData,mparams->kFactor());
            K.resize(0,0); // forgot about this matrix
        }
        else
        {

        const VecVRef& indices = this->f_index.getValue();
        for( size_t i=0 ; i<this->maskTo->size() ; ++i)
        {
            if( this->maskTo->getEntry(i) )
            {
                for(size_t j=0; j<jacobian[i].size(); j++)
                {
                    size_t index=indices[i][j];
                    jacobian[i][j].addDForce(parentForce[index],parentDisplacement[index],childForce[i], mparams->kFactor());
                }
            }
        }

//#ifdef _OPENMP
//#pragma omp parallel for if (this->d_parallel.getValue())
//#endif
//            for(helper::IndexOpenMP<unsigned int>::type i=0; i<this->f_index_parentToChild.size(); i++)
//            {
//                for(size_t j=0; j<this->f_index_parentToChild[i].size(); j+=2)
//                {
//                    size_t indexc=this->f_index_parentToChild[i][j];
//                    jacobian[indexc][this->f_index_parentToChild[i][j+1]].addDForce(parentForce[i],parentDisplacement[i],childForce[indexc], mparams->kFactor());
//                }
//            }

        }
    }
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::applyJT( const core::ConstraintParams * /*cparams*/, Data<InMatrixDeriv>& _out, const Data<OutMatrixDeriv>& _in )
{
    // TODO handle mask

    InMatrixDeriv& out = *_out.beginEdit();
    const OutMatrixDeriv& in = _in.getValue();
    const VecVRef& indices = this->f_index.getValue();

    typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end();

    for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt)
    {
        typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end();
        typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin();

        if (colIt != colItEnd)
        {
            typename InMatrixDeriv::RowIterator o = out.writeLine(rowIt.index());

            for ( ; colIt != colItEnd; ++colIt)
            {
                size_t indexIn = colIt.index();

                for(size_t j=0; j<jacobian[indexIn].size(); j++)
                {
                    size_t indexOut = indices[indexIn][j];

                    InDeriv tmp;
                    jacobian[indexIn][j].addMultTranspose( tmp, colIt.val() );

                    o.addCol( indexOut, tmp );
                }
            }
        }
    }

    _out.endEdit();
}

/** abstract implementation of BasePointMapper functions
    they call mapPosition/mapDefoGradient specialized functions (templated on specific jacobianBlockType)
**/

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::ForwardMapping(Coord& p,const Coord& p0)
{
    if ( !_shapeFunction ) return;

    // interpolate weights at sample positions
    mCoord mp0;        defaulttype::StdVectorTypes<mCoord,mCoord>::set( mp0, p0[0] , p0[1] , p0[2]);
    VRef ref; VReal w;
    _shapeFunction->computeShapeFunction(mp0,ref,w);

    // map using specific instanciation
    this->mapPosition(p,p0,ref,w);
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::BackwardMapping(Coord& p0,const Coord& p,const Real Thresh, const size_t NbMaxIt)
{
    if ( !_shapeFunction ) return;

    // iterate: p0(n+1) = F0.F^-1 (p-p(n)) + p0(n)
    size_t count=0;
    mCoord mp0;
    MaterialToSpatial F0;  VRef ref; VReal w; VGradient dw;
    Coord pnew;
    MaterialToSpatial F;
    defaulttype::Mat<material_dimensions,spatial_dimensions,Real> Finv;

    identity(F0);
    while(count<NbMaxIt)
    {
        defaulttype::StdVectorTypes<mCoord,mCoord>::set( mp0, p0[0] , p0[1] , p0[2]);
        _shapeFunction->computeShapeFunction(mp0,ref,w,&dw);
        if(!w[0]) { p0=Coord(); return; } // outside object

        this->mapPosition(pnew,p0,ref,w);
        if((p-pnew).norm2()<Thresh) return; // has converged
        this->mapDeformationGradient(F,p0,F0,ref,w,dw);

        invert(Finv,F);
        p0+=F0*Finv*(p-pnew);
        count++;
    }
}

template <class JacobianBlockType>
unsigned int BaseDeformationMappingT<JacobianBlockType>::getClosestMappedPoint(const Coord& p, Coord& x0,Coord& x, bool useKdTree)
{
    helper::ReadAccessor<Data<OutVecCoord> > out (*this->toModel->read(core::ConstVecCoordId::position()));
    if(this->missingInformationDirty)
    {
        if(!OutDataTypesInfo<Out>::positionMapped) mapPositions();
        this->missingInformationDirty=false;
    }

    size_t index=0;
    if(useKdTree)
    {
        if(this->KdTreeDirty)
        {
            if(OutDataTypesInfo<Out>::positionMapped) { f_pos.resize(out.size()); for(size_t i=0; i<out.size(); i++ )  Out::get(f_pos[i][0],f_pos[i][1],f_pos[i][2],out[i]);  } // copy to f_pos
            this->f_KdTree.build(f_pos);
            this->KdTreeDirty=false;
        }
        index=this->f_KdTree.getClosest(p,f_pos);
        x=f_pos[index];
    }
    else
    {
        Real dmin=std::numeric_limits<Real>::max();
        for(size_t i=0; i<out.size(); i++)
        {
            Coord P; if(OutDataTypesInfo<Out>::positionMapped) Out::get(P[0],P[1],P[2],out[i]); else P=f_pos[i];
            Real d=(p-P).norm2();
            if(d<dmin) {dmin=d; index=i; x=P;}
        }
    }
    x0=f_pos0.getValue()[index];
    return index;
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::draw(const core::visual::VisualParams* vparams)
{
#ifndef SOFA_NO_OPENGL
    if (!vparams->displayFlags().getShowMechanicalMappings() && !showDeformationGradientScale.getValue() && showColorOnTopology.getValue().getSelectedId()==0) return;

    glPushAttrib ( GL_LIGHTING_BIT );

    helper::ReadAccessor<Data<InVecCoord> > in (*this->fromModel->read(core::ConstVecCoordId::position()));
    helper::ReadAccessor<Data<OutVecCoord> > out (*this->toModel->read(core::ConstVecCoordId::position()));
    helper::ReadAccessor<Data<VecVRef > > ref (this->f_index);
    helper::ReadAccessor<Data<VecVReal > > w (this->f_w);

    if(this->missingInformationDirty)
    {
        if(!OutDataTypesInfo<Out>::positionMapped) mapPositions();
        if(!OutDataTypesInfo<Out>::FMapped) if(showDeformationGradientScale.getValue() || showColorOnTopology.getValue().getSelectedId()!=0) mapDeformationGradients();
        this->missingInformationDirty=false;
    }

    if (vparams->displayFlags().getShowMechanicalMappings())
    {
        helper::vector< defaulttype::Vector3 > edge;     edge.resize(2);
        defaulttype::Vec<4,float> col;

        for(size_t i=0; i<out.size(); i++ )
        {
            if(OutDataTypesInfo<Out>::positionMapped) Out::get(edge[1][0],edge[1][1],edge[1][2],out[i]);
            else edge[1]=f_pos[i];
            for(size_t j=0; j<ref[i].size(); j++ )
                if(w[i][j])
                {
                    In::get(edge[0][0],edge[0][1],edge[0][2],in[ref[i][j]]);
                    sofa::helper::gl::Color::getHSVA(&col[0],240.f*(float)w[i][j],1.f,.8f,1.f);
                    vparams->drawTool()->drawLines ( edge, 1, col );
                }
        }
    }
    if (showDeformationGradientScale.getValue())
    {
        const Data<OutVecDeriv>* outf = this->toModel->read(core::ConstVecDerivId::force());
        glEnable ( GL_LIGHTING );
        float scale=showDeformationGradientScale.getValue();
        defaulttype::Vec<4,float> col( 0.5, 0.5, 0.0, 1.0 );
        defaulttype::Mat<3,3,float> F;
        defaulttype::Vec<3,float> p;

        static const int subdiv = 8;

        for(size_t i=0; i<out.size(); i++ )
        {
            if(OutDataTypesInfo<Out>::FMapped) F=(defaulttype::Mat<3,3,float>)OutDataTypesInfo<Out>::getF(out[i]); else F=(defaulttype::Mat<3,3,float>)f_F[i];
            if(OutDataTypesInfo<Out>::positionMapped) Out::get(p[0],p[1],p[2],out[i]); else p=f_pos[i];

            if(showDeformationGradientStyle.getValue().getSelectedId()==0)
                for(int j=0; j<material_dimensions; j++)
                {
                    defaulttype::Vec<3,float> u=F.transposed()(j)*0.5*scale;
                    vparams->drawTool()->drawCylinder(p-u,p+u,0.05f*scale,col,subdiv);
                }
            else if(showDeformationGradientStyle.getValue().getSelectedId()==1)
            {
                defaulttype::Vec<3,float> u=F.transposed()(0)*0.5*scale;
                vparams->drawTool()->drawCylinder(p-u,p+u,0.05f*scale,col,subdiv);
            }
            else if(showDeformationGradientStyle.getValue().getSelectedId()==2)
            {
                defaulttype::Vec<3,float> u=F.transposed()(1)*0.5*scale;
                vparams->drawTool()->drawCylinder(p-u,p+u,0.05f*scale,col,subdiv);
            }
            else if(showDeformationGradientStyle.getValue().getSelectedId()==3)
            {
                defaulttype::Vec<3,float> u=F.transposed()(2)*0.5*scale;
                vparams->drawTool()->drawCylinder(p-u,p+u,0.05f*scale,col,subdiv);
            }
            else if(showDeformationGradientStyle.getValue().getSelectedId()==4) // strain
            {
                vparams->drawTool()->setMaterial(col);
                drawEllipsoid(F,p,0.5*scale);
            }
            else if(showDeformationGradientStyle.getValue().getSelectedId()==5 && outf) // stress
                if(OutDataTypesInfo<Out>::FMapped)
                {
                    F=(defaulttype::Mat<3,3,float>)OutDataTypesInfo<Out>::getF(outf->getValue()[i]);
                    vparams->drawTool()->setMaterial(col);
                    drawEllipsoid(F,p,0.5f*scale);
                }

        }
    }

    if(showColorOnTopology.getValue().getSelectedId() )
    {
        if( !this->extTriangles && !this->triangles )
        {
            component::visualmodel::VisualModelImpl *visual = NULL;
            this->getContext()->get( visual, core::objectmodel::BaseContext::Local);
            if(visual) {this->extTriangles = &visual->getTriangles(); this->extvertPosIdx = &visual->m_vertPosIdx.getValue(); this->triangles=0; }
            else
            {
                core::topology::BaseMeshTopology *topo = NULL;
                this->getContext()->get( topo, core::objectmodel::BaseContext::Local);
                if(topo) {this->triangles = &topo->getTriangles();  this->extTriangles=0; }
            }
        }

        if( this->extTriangles || this->triangles )
        {
            std::vector< Real > val(out.size());
            MaterialToSpatial F;
            for(size_t i=0; i<out.size(); i++ )
            {
                if(OutDataTypesInfo<Out>::FMapped) F=OutDataTypesInfo<Out>::getF(out[i]); else F=f_F[i];

                if(showColorOnTopology.getValue().getSelectedId()==1) val[i]=(defaulttype::trace(F.transposed()*F)-3.);
                else  val[i]=sqrt(defaulttype::determinant(F.transposed()*F))-1.;

                //if (val[i]<0) val[i]=2*val[i]/(val[i]+1.);
                val[i]*=240 * this->showColorScale.getValue();
                val[i]+=120;
                if (val[i]<0) val[i]=0;
                if (val[i]>240) val[i]=240;
            }

            size_t nb =0;
            if(triangles) nb+=triangles->size();
            if(extTriangles) nb+=extTriangles->size();

            std::vector< defaulttype::Vector3 > points(3*nb),normals;
            std::vector< defaulttype::Vec<4,float> > colors(3*nb);
            size_t count=0;

            if(triangles)
                for ( size_t i = 0; i < triangles->size(); i++)
                    for ( size_t j = 0; j < 3; j++)
                    {
                        size_t index = (*triangles)[i][j];
                        if(OutDataTypesInfo<Out>::positionMapped) Out::get(points[count][0],points[count][1],points[count][2],out[index]); else points[count]=f_pos[index];
                        sofa::helper::gl::Color::getHSVA(&colors[count][0],(float)val[index],1.f,.8f,1.f);
                        count++;
                    }
            if(extTriangles)
                for ( size_t i = 0; i < extTriangles->size(); i++)
                    for ( size_t j = 0; j < 3; j++)
                    {
                        size_t index = (*extTriangles)[i][j];
                        if(this->extvertPosIdx) index=(*extvertPosIdx)[index];
                        if(OutDataTypesInfo<Out>::positionMapped) Out::get(points[count][0],points[count][1],points[count][2],out[index]); else points[count]=f_pos[index];
                        sofa::helper::gl::Color::getHSVA(&colors[count][0],(float)val[index],1.f,.8f,1.f);
                        count++;
                    }

            glDisable( GL_LIGHTING);
            vparams->drawTool()->drawTriangles(points, normals, colors);
        }
    }
    glPopAttrib();
#endif /* SOFA_NO_OPENGL */
}

template <class JacobianBlockType>
const defaulttype::BaseMatrix* BaseDeformationMappingT<JacobianBlockType>::getJ(const core::MechanicalParams * /*mparams*/)
{
    if(!this->assemble.getValue() || !BlockType::constant || !eigenJacobian.rows()) updateJ();

    //    if( this->maskTo->isActivated() )
    //    {
    //        updateMaskedJ();
    //        return &maskedEigenJacobian;
    //    }

    return &eigenJacobian;
}

template <class JacobianBlockType>
const helper::vector<sofa::defaulttype::BaseMatrix*>* BaseDeformationMappingT<JacobianBlockType>::getJs()
{
    if(!this->assemble.getValue() || !BlockType::constant || !eigenJacobian.rows()) updateJ();

    //    if( this->maskTo->isActivated() )
    //    {
    //        updateMaskedJ();
    //        baseMatrices[0] = &maskedEigenJacobian;
    //    }
    //    else
    //    {
    //        baseMatrices[0] = &eigenJacobian;
    //    }

    return &baseMatrices;
}

template <class JacobianBlockType>
const defaulttype::BaseMatrix* BaseDeformationMappingT<JacobianBlockType>::getK()
{
    if( BlockType::constant || !K.compressedMatrix.nonZeros() ) return NULL;
    else return &K;
}

template <class JacobianBlockType>
typename BaseDeformationMappingT<JacobianBlockType>::SparseMatrix& BaseDeformationMappingT<JacobianBlockType>::getJacobianBlocks()
{
    if(!this->assemble.getValue() || !BlockType::constant) updateJ();
    return jacobian;
}

template <class JacobianBlockType>
void BaseDeformationMappingT<JacobianBlockType>::updateForceMask()
{
    const VecVRef& indices = this->f_index.getValue();
    for( size_t i=0 ; i<this->maskTo->size() ; ++i)
    {
        if( this->maskTo->getEntry(i) )
        {
            for(size_t j=0; j<jacobian[i].size(); j++)
            {
                size_t index = indices[i][j];
                this->maskFrom->insertEntry( index );
            }
        }
    }

    //    serr<<"updateForceMask "<<this->maskTo->nbActiveDofs()<<" "<<this->maskFrom->nbActiveDofs()<<sendl;
}

} // namespace mapping
} // namespace component
} // namespace sofa

#endif


#include <cmath>
#include <fstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <complex>
#include <tuple>
#include <algorithm>
#include <chrono>
#include <boost/program_options.hpp>
#include <boost/spirit/include/karma.hpp>
#include <boost/container/flat_map.hpp>
#include <boost/multi_index_container.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>
#include <sndfile.h>
#include <OpenImageIO/imageio.h>

#include "common.hpp"
#include "channel_state.hpp"
#include "fm_operator.hpp"

#include "load_monoral.hpp"
#include "generate_tone.hpp"
#include "dna.hpp"
#include "get_image_distance.hpp"
#include "spectrum_image.hpp"
#include "fft.hpp"

struct by_sum;
struct by_score;

struct sorted_dnas_element_t {
  sorted_dnas_element_t( double sum_, double score_, const dna data_ ) : sum( sum_ ), score( score_ ), data( data_ ) {}
  double sum;
  double score;
  dna data;
};
using sorted_dnas_t =
  boost::multi_index::multi_index_container<
    sorted_dnas_element_t,
    boost::multi_index::indexed_by<
      boost::multi_index::ordered_non_unique<
        boost::multi_index::tag< by_sum >,
        boost::multi_index::member< sorted_dnas_element_t, double, &sorted_dnas_element_t::sum >
      >,
      boost::multi_index::ordered_non_unique<
        boost::multi_index::tag< by_score >,
        boost::multi_index::member< sorted_dnas_element_t, double, &sorted_dnas_element_t::score >
      >
    >
  >;

template <typename T>
struct output_float_policy : boost::spirit::karma::real_policies< T >
{
  static unsigned precision(T) { return 20u; }
};

int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("output,o", boost::program_options::value<std::string>(),  "出力ディレクトリ")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("mipmap,m", boost::program_options::value<int>()->default_value(0),  "初期ミップマップレベル")
    ("has-release,r", boost::program_options::value<bool>()->default_value(true),  "NOTE_OFFを有する楽器か")
    ("cycle,c", boost::program_options::value<unsigned int>()->default_value(4000),  "世代数")
    ("stickiness,s", boost::program_options::value<unsigned int>()->default_value(7),  "何世代トップが変化しなかったら次の分解能に移るか")
    ("interval,t", boost::program_options::value<unsigned int>()->default_value(2),  "時間方向の間隔")
    ("weight,w", boost::program_options::value<int>()->default_value(-5),  "時間方向の重み");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") || !params.count("output") || !params.count( "note" ) ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const std::string output_dir = params["output"].as<std::string>();
  const int weight = params["weight"].as<int>();
  const unsigned int interval = params["interval"].as<unsigned int>();
  init_fft();
  const auto window = generate_window();
  const auto audio = load_monoral( input_filename );
  const int x = 256;
  /*const std::array< spectrum_image, 14 > references{{
    spectrum_image( window, audio, 32, 44100, 128, 2 ),
    spectrum_image( window, audio, 64, 44100, 256, 2 ),
    spectrum_image( window, audio, 64, 44100, 256, 5 ),
    spectrum_image( window, audio, 128, 44100, 512, 5 ),
    spectrum_image( window, audio, 128, 44100, 512, 10 ),
    spectrum_image( window, audio, 256, 44100, 1024, 10 ),
    spectrum_image( window, audio, 256, 44100, 1024, 25 ),
    spectrum_image( window, audio, 512, 44100, 2048, 25 ),
    spectrum_image( window, audio, 512, 44100, 2048, 50 ),
    spectrum_image( window, audio, 1024, 44100, 4096, 50 ),
    spectrum_image( window, audio, 1024, 44100, 4096, 100 ),
    spectrum_image( window, audio, 2048, 44100, 8192, 100 ),
    spectrum_image( window, audio, 2048, 44100, 8192, 300 ),
    spectrum_image( window, audio, 4096, 44100, 8192, 900 ),
  }};*/
  const std::array< spectrum_image, 15 > references{{
    spectrum_image( window, audio, 32, 44100, 128, 15, weight, interval ),
    spectrum_image( window, audio, 64, 44100, 256, 14, weight, interval ),
    spectrum_image( window, audio, 64, 44100, 256, 13, weight, interval ),
    spectrum_image( window, audio, 128, 44100, 512, 12, weight, interval ),
    spectrum_image( window, audio, 128, 44100, 512, 11, weight, interval ),
    spectrum_image( window, audio, 256, 44100, 1024, 10, weight, interval ),
    spectrum_image( window, audio, 256, 44100, 1024, 9, weight, interval ),
    spectrum_image( window, audio, 512, 44100, 2048, 8, weight, interval ),
    spectrum_image( window, audio, 512, 44100, 2048,  7, weight, interval ),
    spectrum_image( window, audio, 1024, 44100, 4096, 6, weight, interval ),
    spectrum_image( window, audio, 1024, 44100, 4096, 5, weight, interval ),
    spectrum_image( window, audio, 2048, 44100, 8192, 4, weight, interval ),
    spectrum_image( window, audio, 2048, 44100, 8192, 3, weight, interval ),
    spectrum_image( window, audio, 4096, 44100, 8192, 2, weight, interval ),
    spectrum_image( window, audio, 4096, 44100, 8192, 1, weight, interval ),
  }};
  const auto &eref = references[ 14 ];
  const std::array< int, 15 > survive_count{{
    17,
    16,
    16,
    15,
    15,
    14,
    14,
    13,
    13,
    12,
    12,
    11,
    11,
    10,
    10
  }};
  std::cout << "ready" << std::endl;
  std::random_device seed_generator;
  std::mt19937 random_generator( seed_generator() );
  std::vector< dna > dnas( survive_count[ 0 ] * survive_count[ 0 ] );
  double sum = 0.0;
  size_t mipmap_level = params["mipmap"].as<int>();
  const float attack_time = ( eref.get_attack_time() - eref.get_delay_time() );
  const float release_time = ( eref.get_total_time() - eref.get_release_time() );
  std::cout << __FILE__ << " " << __LINE__ << " " << attack_time << " " << release_time << std::endl;
  size_t stable = 0u;
  std::vector< double > cached_scores;
  std::vector< dna > survived;
  const bool has_release = params["has-release"].as<bool>();
  std::vector< double > scores;
  const unsigned int cycles = params["cycle"].as<unsigned int>() + 1u;
  const unsigned int stickiness = params["stickiness"].as<unsigned int>();
  for( size_t cycle = 0u; cycle != cycles; ++cycle ) {
    scores.clear();
    scores.reserve( dnas.size() );
    size_t counter = 0u;
    //const auto begin = std::chrono::high_resolution_clock::now();
    for( size_t i = 0u; i != dnas.size(); ++i ) {
      if( !cached_scores.empty() && ( i % ( cached_scores.size() + 1 ) ) == 0u ) scores.push_back( cached_scores[ i / ( cached_scores.size() + 1 ) ] );
      else {
        const auto audio = generate_tone(
          params["note"].as<int>(),
          eref.get_delay_time()*tinyfm3::frequency,
          eref.get_release_time()*tinyfm3::frequency,
          eref.get_total_time()*tinyfm3::frequency,
          dnas[ i ]( attack_time, release_time, has_release ),
          has_release
        );
        double distance = get_distance(
          references[ mipmap_level ],
          window,
          audio
        );
        scores.push_back( 1.0/(distance*distance) );
      }
    }
    survived.clear();
    survived.reserve( survive_count[ mipmap_level ] );
    double top_score = 0.0;
    size_t top_index = 0;
    double previous_top_score = 1.0/scores[ 0.f ];
    cached_scores.clear();
    cached_scores.reserve( survive_count[ mipmap_level ] );
    {
      const size_t elite_count = std::min( survive_count[ mipmap_level ], int( mipmap_level / 2u + 1u ) );
      for( size_t i = 0u; i != elite_count; ++i ) {
        const auto top = std::distance( scores.begin(), std::max_element( scores.begin(), scores.end() ) );
        if( i == 0u ) {
	  top_score = 1.0/scores[ top ];
          top_index = top;
	}
        survived.emplace_back( std::move( dnas[ top ] ) );
        cached_scores.emplace_back( scores[ top ] );
        dnas.erase( std::next( dnas.begin(), top ) );
        scores.erase( std::next( scores.begin(), top ) );
      }
      for( size_t i = 0u; i != survive_count[ mipmap_level ] - elite_count; ++i ) {
        std::discrete_distribution< size_t > distribution( scores.begin(), scores.end() );
        size_t pos = distribution( random_generator );
	survived.emplace_back( std::move( dnas[ pos ] ) );
        cached_scores.emplace_back( scores[ pos ] );
        dnas.erase( std::next( dnas.begin(), pos ) );
        scores.erase( std::next( scores.begin(), pos ) );
      }
      if( fabs( top_score - previous_top_score ) < 0.00000001 ) ++stable;
      else stable = 0u;
      if( stable > stickiness && mipmap_level < references.size() - 1u ) {
        cached_scores.clear();
        mipmap_level = mipmap_level + 1u;
	stable = 0u;
      }
    }
    dnas.clear();
    const int mutation_rate = ( cycle % 20 ) ? 80+cycle/5 : 8+cycle/50;
    for( size_t l = 0u; l != survived.size(); ++l ) {
      for( size_t r = 0u; r != survived.size(); ++r ) {
        if( l != r ) dnas.emplace_back( survived[ l ].crossover( survived[ r ], mutation_rate ) );
        else dnas.emplace_back( survived[ l ] );
      }
    }
    //const auto end = std::chrono::high_resolution_clock::now();
    std::cout << cycle << " " << top_index << " " << top_score << " " << mipmap_level << /*" " << std::chrono::duration_cast< std::chrono::microseconds >( end - begin ).count() <<*/ std::endl;
    if ( !( cycle % 10 ) ) {
      namespace karma = boost::spirit::karma;
      std::string filename;
      karma::generate( std::back_inserter( filename ), karma::string << "/" << karma::right_align( 4, '0' )[ karma::uint_ ] << ".conf", boost::fusion::make_vector( output_dir, cycle ) );
      std::string serialized;
      const auto config = survived.front()( attack_time, release_time, has_release );
      karma::real_generator< float, output_float_policy< float > > float_p;
      karma::generate( std::back_inserter( serialized ), float_p % ',', config );
      std::fstream file( filename.c_str(), std::ios::out );
      file.write( serialized.c_str(), serialized.size() );
      file.close();
    }
  }
}


#include <sstream>
#include <iostream>
#include <vector>
#include <cassert>
#include <cmath>

#ifdef HAVE_MPI
#include <mpi.h>
#endif

#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "verbose.h"
#include "hg.h"
#include "prob.h"
#include "inside_outside.h"
#include "ff_register.h"
#include "decoder.h"
#include "filelib.h"
#include "optimize.h"
#include "fdict.h"
#include "weights.h"
#include "sparse_vector.h"

using namespace std;
using boost::shared_ptr;
namespace po = boost::program_options;

void SanityCheck(const vector<double>& w) {
  for (int i = 0; i < w.size(); ++i) {
    assert(!isnan(w[i]));
    assert(!isinf(w[i]));
  }
}

struct FComp {
  const vector<double>& w_;
  FComp(const vector<double>& w) : w_(w) {}
  bool operator()(int a, int b) const {
    return fabs(w_[a]) > fabs(w_[b]);
  }
};

void ShowLargestFeatures(const vector<double>& w) {
  vector<int> fnums(w.size());
  for (int i = 0; i < w.size(); ++i)
    fnums[i] = i;
  vector<int>::iterator mid = fnums.begin();
  mid += (w.size() > 10 ? 10 : w.size());
  partial_sort(fnums.begin(), mid, fnums.end(), FComp(w));
  cerr << "TOP FEATURES:";
  for (vector<int>::iterator i = fnums.begin(); i != mid; ++i) {
    cerr << ' ' << FD::Convert(*i) << '=' << w[*i];
  }
  cerr << endl;
}

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("input_weights,w",po::value<string>(),"Input feature weights file")
        ("training_data,t",po::value<string>(),"Training data")
        ("decoder_config,c",po::value<string>(),"Decoder configuration file")
        ("output_weights,o",po::value<string>()->default_value("-"),"Output feature weights file");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || !(conf->count("training_data")) || !conf->count("decoder_config")) {
    cerr << dcmdline_options << endl;
#ifdef HAVE_MPI
    MPI::Finalize();
#endif
    exit(1);
  }
}

void ReadTrainingCorpus(const string& fname, int rank, int size, vector<string>* c) {
  ReadFile rf(fname);
  istream& in = *rf.stream();
  string line;
  int lc = 0;
  while(in) {
    getline(in, line);
    if (!in) break;
    if (lc % size == rank) c->push_back(line);
    ++lc;
  }
}

static const double kMINUS_EPSILON = -1e-6;

struct TrainingObserver : public DecoderObserver {
  void Reset() {
    total_complete = 0;
    cur_obj = 0;
    tot_obj = 0;
    tot.clear();
  }

  void SetLocalGradientAndObjective(SparseVector<double>* g, double* o) const {
    *o = tot_obj;
    *g = tot;
  }

  virtual void NotifyDecodingStart(const SentenceMetadata& smeta) {
    cur_obj = 0;
    state = 1;
  }

  void ExtractExpectedCounts(Hypergraph* hg) {
    vector<prob_t> posts;
    cur.clear();
    const prob_t z = hg->ComputeEdgePosteriors(1.0, &posts);
    cur_obj = log(z);
    for (int i = 0; i < posts.size(); ++i) {
      const SparseVector<double>& efeats = hg->edges_[i].feature_values_;
      const double post = static_cast<double>(posts[i] / z);
      for (SparseVector<double>::const_iterator j = efeats.begin(); j != efeats.end(); ++j)
        cur.add_value(j->first, post);
    }
  }

  // compute model expectations, denominator of objective
  virtual void NotifyTranslationForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    assert(state == 1);
    state = 2;
    ExtractExpectedCounts(hg);
  }

  // replace translation forest, since we're doing EM training (we don't know which)
  virtual void NotifyAlignmentForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    assert(state == 2);
    state = 3;
    ExtractExpectedCounts(hg);
  }

  virtual void NotifyDecodingComplete(const SentenceMetadata& smeta) {
    ++total_complete;
    tot_obj += cur_obj;
    tot += cur;
  }

  int total_complete;
  double cur_obj;
  double tot_obj;
  SparseVector<double> cur, tot;
  int state;
};

void ReadConfig(const string& ini, vector<string>* out) {
  ReadFile rf(ini);
  istream& in = *rf.stream();
  while(in) {
    string line;
    getline(in, line);
    if (!in) continue;
    out->push_back(line);
  }
}

void StoreConfig(const vector<string>& cfg, istringstream* o) {
  ostringstream os;
  for (int i = 0; i < cfg.size(); ++i) { os << cfg[i] << endl; }
  o->str(os.str());
}

struct OptimizableMultinomialFamily {
  struct CPD {
    CPD() : z() {}
    double z;
    map<WordID, double> c2counts;
  };
  map<WordID, CPD> counts;
  double Value(WordID conditioning, WordID generated) const {
    map<WordID, CPD>::const_iterator it = counts.find(conditioning);
    assert(it != counts.end());
    map<WordID,double>::const_iterator r = it->second.c2counts.find(generated);
    if (r == it->second.c2counts.end()) return 0;
    return r->second;
  }
  void Increment(WordID conditioning, WordID generated, double count) {
    CPD& cc = counts[conditioning];
    cc.z += count;
    cc.c2counts[generated] += count;
  }
  void Optimize() {
    for (map<WordID, CPD>::iterator i = counts.begin(); i != counts.end(); ++i) {
      CPD& cpd = i->second;
      for (map<WordID, double>::iterator j = cpd.c2counts.begin(); j != cpd.c2counts.end(); ++j) {
        j->second /= cpd.z;
        // cerr << "P(" << TD::Convert(j->first) << " | " << TD::Convert(i->first) << " ) =  " << j->second << endl;
      }
    }
  }
  void Clear() {
    counts.clear();
  }
};

struct CountManager {
  CountManager(size_t num_types) : oms_(num_types) {}
  virtual ~CountManager();
  virtual void AddCounts(const SparseVector<double>& c) = 0;
  void Optimize(SparseVector<double>* weights) {
    for (int i = 0; i < oms_.size(); ++i) {
      oms_[i].Optimize();
    }
    GetOptimalValues(weights);
    for (int i = 0; i < oms_.size(); ++i) {
      oms_[i].Clear();
    }
  }
  virtual void GetOptimalValues(SparseVector<double>* wv) const = 0;
  vector<OptimizableMultinomialFamily> oms_;
};
CountManager::~CountManager() {}

struct TaggerCountManager : public CountManager {
  // 0 = transitions, 2 = emissions
  TaggerCountManager() : CountManager(2) {}
  void AddCounts(const SparseVector<double>& c);
  void GetOptimalValues(SparseVector<double>* wv) const {
    for (set<int>::const_iterator it = fids_.begin(); it != fids_.end(); ++it) {
      int ftype;
      WordID cond, gen;
      bool is_optimized = TaggerCountManager::GetFeature(*it, &ftype, &cond, &gen);
      assert(is_optimized);
      wv->set_value(*it, log(oms_[ftype].Value(cond, gen)));
    }
  }
  // Id:0:a=1 Bi:a_b=1 Bi:b_c=1 Bi:c_d=1 Uni:a=1 Uni:b=1 Uni:c=1 Uni:d=1 Id:1:b=1 Bi:BOS_a=1 Id:2:c=1
  static bool GetFeature(const int fid, int* feature_type, WordID* cond, WordID* gen) {
    const string& feat = FD::Convert(fid);
    if (feat.size() > 5 && feat[0] == 'I' && feat[1] == 'd' && feat[2] == ':') {
      // emission
      const size_t p = feat.rfind(':');
      assert(p != string::npos);
      *cond = TD::Convert(feat.substr(p+1));
      *gen = TD::Convert(feat.substr(3, p - 3));
      *feature_type = 1;
      return true;
    } else if (feat[0] == 'B' && feat.size() > 5 && feat[2] == ':' && feat[1] == 'i') {
      // transition
      const size_t p = feat.rfind('_');
      assert(p != string::npos);
      *gen = TD::Convert(feat.substr(p+1));
      *cond = TD::Convert(feat.substr(3, p - 3));
      *feature_type = 0;
      return true;
    } else if (feat[0] == 'U' && feat.size() > 4 && feat[1] == 'n' && feat[2] == 'i' && feat[3] == ':') {
      // ignore
      return false;
    } else {
      cerr << "Don't know how to deal with feature of type: " << feat << endl;
      abort();
    }
  }
  set<int> fids_;
};

void TaggerCountManager::AddCounts(const SparseVector<double>& c) {
  for (SparseVector<double>::const_iterator it = c.begin(); it != c.end(); ++it) {
    const double& val = it->second;
    int ftype;
    WordID cond, gen;
    if (GetFeature(it->first, &ftype, &cond, &gen)) {
      oms_[ftype].Increment(cond, gen, val);
      fids_.insert(it->first);
    }
  }
}

int main(int argc, char** argv) {
#ifdef HAVE_MPI
  MPI::Init(argc, argv);
  const int size = MPI::COMM_WORLD.Get_size();
  const int rank = MPI::COMM_WORLD.Get_rank();
#else
  const int size = 1;
  const int rank = 0;
#endif
  SetSilent(true);  // turn off verbose decoder output
  register_feature_functions();

  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);

  TaggerCountManager tcm;

  // load cdec.ini and set up decoder
  vector<string> cdec_ini;
  ReadConfig(conf["decoder_config"].as<string>(), &cdec_ini);
  istringstream ini;
  StoreConfig(cdec_ini, &ini);
  if (rank == 0) cerr << "Loading grammar...\n";
  Decoder* decoder = new Decoder(&ini);
  if (decoder->GetConf()["input"].as<string>() != "-") {
    cerr << "cdec.ini must not set an input file\n";
#ifdef HAVE_MPI
    MPI::COMM_WORLD.Abort(1);
#endif
  }
  if (rank == 0) cerr << "Done loading grammar!\n";
  Weights w;
  if (conf.count("input_weights"))
    w.InitFromFile(conf["input_weights"].as<string>());

  double objective = 0;
  bool converged = false;

  vector<double> lambdas;
  w.InitVector(&lambdas);
  vector<string> corpus;
  ReadTrainingCorpus(conf["training_data"].as<string>(), rank, size, &corpus);
  assert(corpus.size() > 0);

  int iteration = 0;
  TrainingObserver observer;
  while (!converged) {
    ++iteration;
    observer.Reset();
    if (rank == 0) {
      cerr << "Starting decoding... (~" << corpus.size() << " sentences / proc)\n";
    }
    decoder->SetWeights(lambdas);
    for (int i = 0; i < corpus.size(); ++i)
      decoder->Decode(corpus[i], &observer);

    SparseVector<double> x;
    observer.SetLocalGradientAndObjective(&x, &objective);
    cerr << "COUNTS = " << x << endl;
    cerr << "   OBJ = " << objective << endl;
    tcm.AddCounts(x);

#if 0
#ifdef HAVE_MPI
    MPI::COMM_WORLD.Reduce(const_cast<double*>(&gradient.data()[0]), &rcv_grad[0], num_feats, MPI::DOUBLE, MPI::SUM, 0);
    MPI::COMM_WORLD.Reduce(&objective, &to, 1, MPI::DOUBLE, MPI::SUM, 0);
    swap(gradient, rcv_grad);
    objective = to;
#endif
#endif

    if (rank == 0) {
      SparseVector<double> wsv;
      tcm.Optimize(&wsv);

      w.InitFromVector(wsv);
      w.InitVector(&lambdas);

      ShowLargestFeatures(lambdas);

      converged = iteration > 100;
      if (converged) { cerr << "OPTIMIZER REPORTS CONVERGENCE!\n"; }

      string fname = "weights.cur.gz";
      if (converged) { fname = "weights.final.gz"; }
      ostringstream vv;
      vv << "Objective = " << objective << "  (ITERATION=" << iteration << ")";
      const string svv = vv.str();
      w.WriteToFile(fname, true, &svv);
    }  // rank == 0
    int cint = converged;
#ifdef HAVE_MPI
    MPI::COMM_WORLD.Bcast(const_cast<double*>(&lambdas.data()[0]), num_feats, MPI::DOUBLE, 0);
    MPI::COMM_WORLD.Bcast(&cint, 1, MPI::INT, 0);
    MPI::COMM_WORLD.Barrier();
#endif
    converged = cint;
  }
#ifdef HAVE_MPI
  MPI::Finalize();
#endif
  return 0;
}


#include <boost/parameter.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>

BOOST_PARAMETER_NAME((_graph, graphs) graph)
BOOST_PARAMETER_NAME((_visitor, graphs) visitor)
BOOST_PARAMETER_NAME((_root_vertex, graphs) root_vertex)
BOOST_PARAMETER_NAME((_index_map, graphs) index_map)
BOOST_PARAMETER_NAME((_color_map, graphs) color_map)

using boost::mpl::_;
// We first need to define a few metafunction that we use in the
// predicates below.

template <class G>
struct traversal_category
{
    typedef typename boost::graph_traits<G>::traversal_category type;
};

template <class G>
struct vertex_descriptor
{
    typedef typename boost::graph_traits<G>::vertex_descriptor type;
};

template <class G>
struct value_type
{
    typedef typename boost::property_traits<G>::value_type type;
};

template <class G>
struct key_type
{
    typedef typename boost::property_traits<G>::key_type type;
};

template<class Size, class IndexMap>
boost::iterator_property_map<
    boost::default_color_type*, IndexMap
  , boost::default_color_type, boost::default_color_type&
>
default_color_map(Size num_vertices, IndexMap const& index_map)
{
   std::vector<boost::default_color_type> colors(num_vertices);
   return &colors[0];
}

BOOST_PARAMETER_FUNCTION(
    (void), depth_first_search, graphs

  , (required
      (graph
       , *(boost::mpl::and_<
               boost::is_convertible<
                   traversal_category<_>, boost::incidence_graph_tag
               >
             , boost::is_convertible<
                   traversal_category<_>, boost::vertex_list_graph_tag
               >
           >) ))

    (optional
      (visitor, *, boost::dfs_visitor<>()) // not checkable

      (root_vertex
        , (vertex_descriptor<graphs::graph::_>)
        , *vertices(graph).first)

      (index_map
        , *(boost::mpl::and_<
              boost::is_integral<value_type<_> >
            , boost::is_same<
                  vertex_descriptor<graphs::graph::_>, key_type<_>
              >
          >)
        , get(boost::vertex_index,graph))

      (in_out(color_map)
        , *(boost::is_same<
              vertex_descriptor<graphs::graph::_>, key_type<_>
          >)
       , default_color_map(num_vertices(graph), index_map) )
    )
)
{}

int main()
{
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS> G;

    enum {u, v, w, x, y, z, N};
    typedef std::pair<int, int> E;
    E edges[] = {E(u, v), E(u, x), E(x, v), E(y, x), E(v, y), E(w, y),
                 E(w,z), E(z, z)};
    G g(edges, edges + sizeof(edges) / sizeof(E), N);

    ::depth_first_search(g);
    ::depth_first_search(g, _root_vertex = (int)x);
}


#pragma once

#include "inexor/vulkan-renderer/world/bit_stream.hpp"

#include <boost/dynamic_bitset.hpp>
#include <boost/signals2.hpp>
#include <glm/vec3.hpp>

#include <array>
#include <cstdint>
#include <optional>

namespace inexor::vulkan_renderer::world {
/// How often a cube can be indented, results in MAX_INDENTATION+1 steps.
constexpr std::uint8_t MAX_INDENTATION = 8;

/// The default size of a cube / the octree size boundaries.
constexpr float DEFAULT_CUBE_SIZE = 1;

/// The default position of the cube in the coordinate system.
constexpr glm::vec3 DEFAULT_CUBE_POSITION = {0., 0., 0.};

/// The types a cube can have and its bit-representation.
enum class CubeType {
    /// The cube has no surface and no vertices.
    EMPTY = 0b00,
    /// The cube is a "real" cube where each edge has the same length.
    FULL = 0b01,
    /// The cube has at least on edge which has been indented from at least one axis.
    /// That means that the cube may or may not be a perfect cube with same-length edges.
    INDENTED = 0b10,
    /// The cube is divided in 8 octants (same-sized cubes).
    OCTANT = 0b11
};

class Indentation {
private:
    /// Run on-change events.
    void change();

    /// Copy the values from another indentation to this one.
    /// @param indentation The indentation to copy the values from.
    /// @return Whether any value has changed.
    bool copy_values(const Indentation &indentation);

    /// Parse one axis from a BitStream.
    /// @param stream The BitStream to parse one axis from.
    /// @return The indentation level on that axis.
    static std::uint8_t parse_one(BitStream &stream);

    /// Indentation level on the x-axis.
    std::uint8_t x_level = 0;

    /// Indentation level on the y-axis.
    std::uint8_t y_level = 0;

    /// Indentation level on the z-axis.
    std::uint8_t z_level = 0;

public:
    /// Create an Indentation to assign to a cube corner.
    Indentation();

    /// Create an Indentation to assign to a cube corner.
    /// @param x Indentation leven on the x-axis
    /// @param y Indentation leven on the y-axis
    /// @param z Indentation leven on the z-axis
    Indentation(std::uint8_t x, std::uint8_t y, std::uint8_t z);

    Indentation(const Indentation &indentation);

    Indentation(Indentation &&indentation) noexcept;

    /// Signal emitted when one of the indentation levels changes.
    /// Argument: the indentation emitting the signal ("this").
    boost::signals2::signal<void(Indentation *)> on_change;

    Indentation &operator=(Indentation &&lhs) noexcept;

    Indentation &operator=(const Indentation &rhs);

    Indentation &operator=(const glm::tvec3<uint8_t> &rhs);

    Indentation &operator+=(const glm::tvec3<int8_t> &other);

    Indentation &operator-=(const glm::tvec3<int8_t> &other);

    [[nodiscard]] bool equal_values(const Indentation &other) const;

    [[nodiscard]] bool equal_values(const glm::tvec3<uint8_t> &other) const;

    /// Set the indentations depth for the axis.
    /// @param x Indentation leven on the x-axis.
    /// @param y Indentation leven on the y-axis.
    /// @param z Indentation leven on the z-axis.
    void set(std::optional<std::uint8_t> x, std::optional<std::uint8_t> y, std::optional<std::uint8_t> z);

    /// Set the indentation level for the x axis.
    /// @param x Indentation level on the x axis.
    void set_x(std::uint8_t x);

    /// Set the indentation level for the y axis.
    /// @param x Indentation level on the y axis.
    void set_y(std::uint8_t y);

    /// Set the indentation level for the z axis.
    /// @param x Indentation level on the z axis.
    void set_z(std::uint8_t z);

    /// Parse an indentation from a bitstream.
    /// @param stream The stream to extract the bitstream from.
    /// @return The indentation on all three axes.
    static Indentation parse(BitStream &stream);

    /// Get the x-axis indentation level.
    /// @return the x-axis indentation level.
    [[nodiscard]] std::uint8_t x() const;

    /// Get the y-axis indentation level.
    /// @return the y-axis indentation level.
    [[nodiscard]] std::uint8_t y() const;

    /// Get the z-axis indentation level.
    /// @return the z-axis indentation level.
    [[nodiscard]] std::uint8_t z() const;

    /// Get the indentation levels on all three axes as a glm::tvec3<std::uint8_t>.
    /// @return indentation levels on all three axes
    [[nodiscard]] glm::tvec3<std::uint8_t> vec() const;

    // TODO: Get bits from values.
    // [[nodiscard]] dynamic_bitset<> bits();
};

/// A cube or octree representing the maps geometry.
///
/// Values connected to corners of cubes are saved in the following order.
///
/// | Order | X      | Y      | Z      |
/// |-------|--------|--------|--------|
/// | 1.    | lower  | lower  | lower  |
/// | 2.    | lower  | lower  | higher |
/// | 3.    | lower  | higher | lower  |
/// | 4.    | lower  | higher | higher |
/// | 5.    | higher | lower  | lower  |
/// | 6.    | higher | lower  | higher |
/// | 7.    | higher | higher | lower  |
/// | 8.    | higher | higher | higher |
///
/// Values connected to sides of cubes are saved in the following order.
///
/// | Order | X      | Y      | Z      |
/// |-------|--------|--------|--------|
/// | 1.    | axis   | axis   | lower  |
/// | 2.    | axis   | axis   | higher |
/// | 3.    | axis   | lower  | axis   |
/// | 4.    | axis   | higher | axis   |
/// | 5.    | lower  | axis   | axis   |
/// | 6.    | higher | axis   | axis   |
class Cube {
private:
    Cube(CubeType type, float size, const glm::vec3 &position,
         const std::optional<std::array<Indentation, 8>> &indentations,
         std::optional<std::array<std::shared_ptr<Cube>, 8>> octants);

    /// Insert all polygons into memory.
    /// @param polygons Pointer to the memory where the polygons should be saved to.
    void all_polygons(std::array<glm::vec3, 3> *&polygons);

    /// Run on-change events.
    void change();

    /// Run on-change events.
    void change(Indentation *indentation);

    /// Copy the values from another cube to this one.
    /// @param cube The cube to copy the values from.
    /// @return Whether any value has changed (this != &cube).
    bool copy_values(const Cube &cube);

    /// Get all vertices of this cube (not its children).
    /// @return vertices of this cube.
    [[nodiscard]] std::array<glm::vec3, 8> vertices();

    /// Get the polygons of this cube as if it is a full cube.
    /// @return polygons of this cube as if it is a full cube
    std::array<std::array<glm::vec3, 3>, 12> full_polygons();

    /// Get the vertices in a structure which is ordered in triangles of the order of a full cube.
    /// @param v The vertices of the the sides of a cube.
    /// @return polygons of this cube in the order of a full cube.
    std::array<std::array<glm::vec3, 3>, 12> full_polygons(std::array<glm::vec3, 8> &v);

    /// Get the polygons of this cube (only when it is an indented cube).
    /// @return polygons of this cube
    std::array<std::array<glm::vec3, 3>, 12> indented_polygons();

    /// Get the indentation levels for each side of the cube.
    /// @return The indentation lebvels for each side of the cube.
    std::array<glm::tvec3<std::uint8_t>, 8> indentation_levels();

    /// Cache of this cubes polygons. Not of its octants (i.e., empty of the cube is of type CubeType::OCTANTS).
    std::array<std::array<glm::vec3, 3>, 12> polygons_cache = {}; // Vertices of this cube (not its octants)

    /// Whether this->polygons_cache is valid and might be used.
    bool valid_cache = false;

    /// Whether this octree is reactive (i.e. updates when childs update).
    bool is_reactive = false;

    /// Type of the cube.
    CubeType cube_type = CubeType::EMPTY;

    /// The position of the cube in the coordinate system (i.e., the vector from (0, 0, 0) to the bounds of the cube
    /// with the lowest values on x, y, and z-axis).
    glm::vec3 cube_position = {0.0f, 0.0f, 0.0f};

    /// The maximum size of the cube (i.e. if the cube is not indented).
    float cube_size = 32;

public:
    /// Signal emitted when any of the geometry of this cube or its child cubes changes.
    /// Argument: the cube which was originally changed ("this" or a child-cube).
    boost::signals2::signal<void(Cube *)> on_change;

    /// The indentations of this cube if this cube is of CubeType::INDENTED.
    /// Ordered as following:
    /// 0. Corner with lower x-axis-value, lower y-value, lower z-value.
    /// 1. Corner with lower x-axis-value, lower y-value, higher z-value.
    /// 2. Corner with lower x-axis-value, higher y-value, lower z-value.
    /// 3. Corner with lower x-axis-value, higher y-value, higher z-value.
    /// 4. Corner with higher x-axis-value, lower y-value, lower z-value.
    /// 5. Corner with higher x-axis-value, lower y-value, higher z-value.
    /// 6. Corner with higher x-axis-value, higher y-value, lower z-value.
    /// 7. Corner with higher x-axis-value, higher y-value, higher z-value.
    std::optional<std::array<Indentation, 8>> indentations = std::nullopt;

    /// The octants of this cube if this cube is of CubeType::OCTANTS.
    /// Ordered as following:
    /// 0. Octant with lower x-axis-value, lower y-value, lower z-value.
    /// 1. Octant with lower x-axis-value, lower y-value, higher z-value.
    /// 2. Octant with lower x-axis-value, higher y-value, lower z-value.
    /// 3. Octant with lower x-axis-value, higher y-value, higher z-value.
    /// 4. Octant with higher x-axis-value, lower y-value, lower z-value.
    /// 5. Octant with higher x-axis-value, lower y-value, higher z-value.
    /// 6. Octant with higher x-axis-value, higher y-value, lower z-value.
    /// 7. Octant with higher x-axis-value, higher y-value, higher z-value.
    std::optional<std::array<std::shared_ptr<Cube>, 8>> octants = std::nullopt;

    /// Create a cube.
    /// @param type The type of the cube. The cube needs further adjustment after construction if not of CubeType::FULL
    /// or CubeType::EMPTY.
    /// @param size The maximum size of the cube.
    /// @param position The position of the cube in the coordinate system (i.e., the vector from (0, 0, 0) to the bounds
    /// of the cube with the lowest values on x, y, and z-axis).
    Cube(CubeType type, float size, const glm::vec3 &position);

    /// Create a CubeType::INDENTED cube.
    /// @param indentations The indentations of the cube.
    /// @param size The maximum size of the cube.
    /// @param position The position of the cube in the coordinate system (i.e., the vector from (0, 0, 0) to the bounds
    /// of the cube with the lowest values on x, y, and z-axis).
    Cube(std::array<Indentation, 8> &indentations, float size, const glm::vec3 &position);

    /// Create a CubeType::Octants cube.
    ///
    /// @param octants The octants (i.e. sub-cubes) of the cube.
    /// @param size The maximum size of the cube.
    /// @param position The position of the cube in the coordinate system (i.e., the vector from (0, 0, 0) to the bounds
    /// of the cube with the lowest values on x, y, and z-axis).
    Cube(std::array<std::shared_ptr<Cube>, 8> &octants, float size, const glm::vec3 &position);

    Cube(const Cube &cube);

    Cube(Cube &&cube) noexcept;

    Cube &operator=(Cube &&lhs) noexcept;

    Cube &operator=(const Cube &rhs);

    /// Parse an octree from binary data.
    /// @param data The data to parse the octree from.
    /// @return Cube object representing the cubes / octrees from the data.
    static Cube parse(std::vector<unsigned char> &data);

    /// Parse an octree from a BitStream.
    /// @param stream The BitStream to parse the octree from.
    /// @return Cube object representing the cubes / octrees from the stream.
    static Cube parse(BitStream &stream);

    /// Parse an octree from a BitStream.
    /// @param stream The BitStream to parse the octree from.
    /// @param size The maximum size of the cube.
    /// @param position The position of the cube in the coordinate system (i.e., the vector from (0, 0, 0) to the bounds
    /// of the cube with the lowest values on x, y, and z-axis).
    /// @return Cube object representing the cubes / octrees from the stream.
    static Cube parse(BitStream &stream, float size, const glm::vec3 &position);

    /// Get the type of the cube.
    /// @return type of the cube.
    [[nodiscard]] CubeType type();

    /// Get the number of leaves, this octree contains.
    /// Leaves are cubes of CubeType::INDENTED or CubeTYPE::FULL.
    /// @return Number of leaves, this octree contains.
    [[nodiscard]] std::uint64_t leaves();

    // [[nodiscard]] dynamic_bitset<> bits(); // Bit representation of this cube
    // [[nodiscard]] vector<array<glm::vec3, 8>> vertices(); // All vertices this cube contains.
    // [[nodiscard]] vector<array<glm::vec3, 4>> sides(); // All sides this cube contains.
    /// Get all polygons (triangles) of each cube of this octree.
    /// @return A vector which contains the three vertices representing a triangle.
    [[nodiscard]] std::vector<std::array<glm::vec3, 3>> polygons(); // All polygons this cube contains.

    /// Invalidate the cache of this cube / octree (not its children).
    void invalidate_cache();

    /// Make this octree reactive (update its values when one of its attributes changes).
    /// @param force Whether to make it reactive again even though the connections were established before.
    void make_reactive(bool force = false);
};
} // namespace inexor::vulkan_renderer::world


#ifndef UUID_73051518_6403_4F4E_97A5_BE49AD1C26A8
#define UUID_73051518_6403_4F4E_97A5_BE49AD1C26A8

#include <boost/container/vector.hpp>

#include <algorithm>
#include <cassert>
#include <functional>
#include <type_traits>
#include <utility>

// We are going to support arbitrary element types in our vector,
// including custom non-trivially copyable types. We have to consider:
// - that element types are arbitrary complex and should be perfectly
//   forwarded everywhere.
// - addition and multiplication doesn't have to be commutative.
// - optimizing division to multiplication by inverse may be invalid.

// Type of result of applying binary operation Op to T and U.

template<typename Op,typename T,typename U>
using op_result_t = decltype(Op{}(std::declval<T>(),std::declval<U>()));

// Calculate inverse of x when it's possible to optimize a/b to a*(b^(-1)).
// We prefer to not specialize this for integers to return floats as that will give
// a different result type (int/int=double) which make sence for math
// (and Python), but not C++.

template<typename T,
         typename = std::enable_if_t<std::is_floating_point_v<T>>>
constexpr T multiplicative_inverse(T x)
{
    return T(1)/x;
}

template<typename T>
using multiplicative_inverse_result_t = decltype(multiplicative_inverse(std::declval<T>()));

// A trait that figures out if multiplication_inverse is callable for given T.

template<typename T,typename = void>
struct has_multiplicative_inverse : std::false_type {};

template<typename T>
struct has_multiplicative_inverse<T,
    std::void_t<multiplicative_inverse_result_t<T>>> : std::true_type {};

template<typename T>
constexpr inline bool has_multiplicative_inverse_v = has_multiplicative_inverse<T>{};

#endif


#include "Attributes.hpp"

#include <boost/range/algorithm/find.hpp>

#include "Derivatives.hpp"

namespace FeltElements
{
namespace Attribute
{
namespace MeshBody
{
MaterialProperties::MaterialProperties(Mesh & mesh) : ThisBase(mesh)
{
	(*(*this)) = Body::Material{0, 0, 0};
}

Forces::Forces(Mesh & mesh) : MeshBase(mesh)
{
	(*(*this)) = Body::Forces{0, {0, 0, 0}};
}
}  // namespace MeshBody

namespace Surface
{
Traction::Traction(Mesh & mesh) : ThisBase{mesh}
{
	for (auto hfh : boost::make_iterator_range(mesh.halffaces())) (*this)[hfh] = 0;
}
}  // namespace Surface

namespace Vertex
{
MaterialPosition::MaterialPosition(Mesh & mesh) : ThisBase(mesh) {}
SpatialPosition::SpatialPosition(Mesh & mesh) : ThisBase(mesh) {}

FixedDOF::FixedDOF(Mesh & mesh) : ThisBase(mesh)
{
	for (auto itvtxh = mesh.vertices_begin(); itvtxh != mesh.vertices_end(); itvtxh++)
		(*this)[*itvtxh] = 0;
}
}  // namespace Vertex

namespace Cell
{
VertexHandles::VertexHandles(Mesh & mesh) : ThisBase{mesh}
{
	for (auto cellh : boost::make_iterator_range(mesh.cells()))
	{
		auto const & vtxhs = mesh.get_cell_vertices(cellh);
		std::copy_n(vtxhs.begin(), vtxhs.size(), (*this)[cellh].begin());
	}
}

MaterialShapeDerivative::MaterialShapeDerivative(
	Mesh & mesh, VertexHandles const & vtxhs, Vertex::MaterialPosition const & X_)
	: ThisBase{mesh}
{
	for (auto cellh : boost::make_iterator_range(mesh.cells()))
		(*this)[cellh] = Derivatives::dN_by_dX(X_.for_element(vtxhs[cellh]));
}

NodalForces::NodalForces(Mesh & mesh) : ThisBase{mesh}
{
	for (auto cellh : boost::make_iterator_range(mesh.cells())) (*this)[cellh] = 0;
}

Stiffness::Stiffness(Mesh & mesh) : ThisBase(mesh)
{
	for (auto cellh : boost::make_iterator_range(mesh.cells())) (*this)[cellh] = 0;
}

Boundary::Boundary(Mesh & mesh, VertexHandles const & vtxhs) : CellBase(mesh)
{
	for (auto cellh : boost::make_iterator_range(mesh.cells()))
	{
		auto const & cell_vtxhs = vtxhs[cellh];

		for (auto hfh : boost::make_iterator_range(mesh.cell_halffaces(cellh)))
		{
			if (!mesh.is_boundary(Mesh::face_handle(hfh)))
				continue;

			// Indices of vertex handles for this face within this cell's list of vertex handles.
			BoundaryElement::VtxhIdxs vtxh_idxs;
			// Get opposite half-face so winding order is on the outside (i.e. normal calculated
			// from cross product points outward).
			Mesh::Face const & boundary_face = mesh.opposite_halfface(hfh);

			for (auto const & [vtx_idx, heh] : boost::adaptors::index(boundary_face.halfedges()))
			{
				// Handle to vertex at start endpoint of this halfedge.
				Vtxh const & vtxh = mesh.halfedge(heh).from_vertex();
				using VtxhIdx = decltype(vtxh_idxs)::size_type;
				// Face vertex's index within cell's list of vertex handles.
				vtxh_idxs[static_cast<VtxhIdx>(vtx_idx)] = index_of(cell_vtxhs, vtxh);
			}

			(*this)[cellh].push_back(vtxh_idxs);
		}
	}
}
}  // namespace Cell
}  // namespace Attribute

Attributes::Attributes(Mesh & mesh)
	: material{mesh},
	  forces{mesh},
	  x{mesh},
	  X{mesh},
	  fixed_dof(mesh),
	  vtxhs{mesh},
	  boundary_faces_vtxh_idxs{mesh, vtxhs},
	  dN_by_dX{mesh, vtxhs, X},
	  R{mesh},
	  K{mesh}
{
}
}  // namespace FeltElements