#include "modules/neuralNetwork/layer/recurrent/gru/gru.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
namespace layer
{
namespace recurrent
{
;

void GRU::initialize()
{
  Recurrent::initialize();

  // Setting number of recurrent gates to 3 (GRU)
  _gateCount = 3;

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN") _rnnMode = CUDNN_GRU;
#endif
}

void GRU::createForwardPipeline()
{
  // Calling base layer function
  Layer::createForwardPipeline();

  // Checking Layer sizes
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Checking Layer sizes
    const memory::dim T = 1;                            // time steps
    const memory::dim G = _gateCount;                   // Gates
    const memory::dim N = _batchSize;                   // Batch size
    const memory::dim IC = _prevLayer->_outputChannels; // channels
    const memory::dim OC = _outputChannels;             // channels
    const memory::dim L = _depth;                       // layers
    const memory::dim D = 1;                            // directions

    // Creating descriptor for layer memory
    const memory::dims layerInputDims = {T, N, IC};
    auto layerInputMemDesc = memory::desc(layerInputDims, memory::data_type::f32, memory::format_tag::tnc);

    // Creating descriptor for layer memory
    const memory::dims layerOutputDims = {T, N, OC};
    auto layerOutputMemDesc = memory::desc(layerOutputDims, memory::data_type::f32, memory::format_tag::tnc);

    // Creating descriptor for the hidden state memory
    const memory::dims stateLayerDims = {L, D, N, OC};
    auto stateMemDesc = memory::desc(stateLayerDims, memory::data_type::f32, memory::format_tag::ldnc);

    // Creating descriptor for the weights memory
    memory::dims weightInputDims = {L, D, IC, G, OC};
    auto weightInputMemDesc = memory::desc(weightInputDims, memory::data_type::f32, memory::format_tag::any);

    memory::dims weightRecurrentDims = {L, D, OC, G, OC};
    auto weightRecurrentMemDesc = memory::desc(weightRecurrentDims, memory::data_type::f32, memory::format_tag::any);

    // Creating memory for the hidden state
    _hiddenStateMem.resize(_nn->_timestepCount);
    for (size_t i = 0; i < _nn->_timestepCount; i++) _hiddenStateMem[i] = memory(stateMemDesc, _nn->_dnnlEngine);

    // Crating null hidden state mems for initial timestep
    _nullStateInputMem = memory(stateMemDesc, _nn->_dnnlEngine);
    _nullStateOutputMem = memory(stateMemDesc, _nn->_dnnlEngine);

    // Setting them to zero
    std::vector<float> nullState(L * D * N * OC, 0.0f);
    write_to_dnnl_memory(nullState.data(), _nullStateInputMem);
    write_to_dnnl_memory(nullState.data(), _nullStateOutputMem);

    // Creating descriptor for the GRU operation
    auto forwardGRUDesc = gru_forward::desc(
      _propKind,                     // aprop_kind
      rnn_direction::unidirectional, // direction
      layerInputMemDesc,             // src_layer_desc
      stateMemDesc,                  // src_iter_desc
      weightInputMemDesc,            // weights_layer_desc
      weightRecurrentMemDesc,        // weights_iter_desc
      _biasMem.get_desc(),           // bias_desc
      layerOutputMemDesc,            // dst_layer_desc
      stateMemDesc                   // dst_iter_desc
    );

    // Create GRU primitive descriptor.
    dnnl::primitive_attr gruPrimitiveAttributes;
    _forwardGRUPrimitiveDesc = gru_forward::primitive_desc(forwardGRUDesc, gruPrimitiveAttributes, _nn->_dnnlEngine);

    // Create the primitive.
    _forwardGRUPrimitive = gru_forward(_forwardGRUPrimitiveDesc);

    // Now allocating workspace
    _workspaceMem.resize(_nn->_timestepCount);
    for (size_t i = 0; i < _nn->_timestepCount; i++)
      _workspaceMem[i] = memory(_forwardGRUPrimitiveDesc.workspace_desc(), _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // Obtaining batch size
    const size_t L = _depth;
    const size_t N = _batchSize;
    const size_t IC = _prevLayer->_outputChannels;
    const size_t OC = _outputChannels;

    int dimA[3];
    int strideA[3];

    dimA[0] = L;  // Hidden Layer count
    dimA[1] = N;  // Minibatch size
    dimA[2] = OC; // Hidden Size

    strideA[0] = dimA[2] * dimA[1];
    strideA[1] = dimA[2];
    strideA[2] = 1;

    // Allocating hidden state descriptor
    cudnnErrCheck(cudnnCreateTensorDescriptor(&_hTensorDesc));
    cudnnErrCheck(cudnnSetTensorNdDescriptor(_hTensorDesc, CUDNN_DATA_FLOAT, 3, dimA, strideA));

    // Allocating hidden state tensors
    _hStateTensor.resize(_nn->_timestepCount);
    for (size_t i = 0; i < _nn->_timestepCount; i++) cudaErrCheck(cudaMalloc((void **)&_hStateTensor[i], L * N * OC * sizeof(float)));

    // Creating RNN data descriptors for input and output
    cudnnErrCheck(cudnnCreateRNNDataDescriptor(&_inputRNNDataDesc));
    cudnnErrCheck(cudnnCreateRNNDataDescriptor(&_outputRNNDataDesc));

    // Setting and copying sequence length array to device
    std::vector<int> seqLengthArray(N, 1);
    cudaErrCheck(cudaMalloc((void **)&_devSequenceLengths, N * sizeof(int)));
    cudaErrCheck(cudaMemcpy(_devSequenceLengths, seqLengthArray.data(), N * sizeof(int), cudaMemcpyHostToDevice));

    // Setting intput/output RNN data descriptors
    cudnnErrCheck(cudnnSetRNNDataDescriptor(
      _inputRNNDataDesc,
      CUDNN_DATA_FLOAT,
      CUDNN_RNN_DATA_LAYOUT_SEQ_MAJOR_PACKED,
      1, // Max Sequence Length
      N,
      IC,
      seqLengthArray.data(),
      NULL));

    cudnnErrCheck(cudnnSetRNNDataDescriptor(
      _outputRNNDataDesc,
      CUDNN_DATA_FLOAT,
      CUDNN_RNN_DATA_LAYOUT_SEQ_MAJOR_PACKED,
      1, // Max Sequence Length
      N,
      OC,
      seqLengthArray.data(),
      NULL));

    // Now allocating workspace
    cudnnErrCheck(cudnnGetRNNTempSpaceSizes(
      _nn->_cuDNNHandle,
      _rnnDesc,
      _forwardMode,
      _inputRNNDataDesc,
      &_workSpaceSize,
      &_reserveSpaceSize));

    _workSpaceTensor.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++) cudaErrCheck(cudaMalloc((void **)&_workSpaceTensor[t], _workSpaceSize));

    _reserveSpaceTensor.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++) cudaErrCheck(cudaMalloc((void **)&_reserveSpaceTensor[t], _reserveSpaceSize));
  }
#endif
}

void GRU::createBackwardPipeline()
{
  // Calling base layer function
  Recurrent::createBackwardPipeline();

  // Checking Layer sizes
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Checking Layer sizes
    const memory::dim T = 1;                            // time steps
    const memory::dim G = _gateCount;                   // layers
    const memory::dim N = _batchSize;                   // Batch size
    const memory::dim IC = _prevLayer->_outputChannels; // channels
    const memory::dim OC = _outputChannels;             // channels
    const memory::dim L = _depth;                       // layers
    const memory::dim D = 1;                            // directions

    // Creating memory for the hidden state
    _hiddenStateGradientMem.resize(_nn->_timestepCount);
    for (size_t i = 0; i < _nn->_timestepCount; i++) _hiddenStateGradientMem[i] = memory(_hiddenStateMem[i].get_desc(), _nn->_dnnlEngine);

    // Creating descriptor for layer memory
    const memory::dims layerInputDims = {T, N, IC};
    auto layerInputMemDesc = memory::desc(layerInputDims, memory::data_type::f32, memory::format_tag::tnc);

    // Creating descriptor for layer memory
    const memory::dims layerOutputDims = {T, N, OC};
    auto layerOutputMemDesc = memory::desc(layerOutputDims, memory::data_type::f32, memory::format_tag::tnc);

    // Creating descriptor for the hidden state memory
    const memory::dims stateLayerDims = {L, D, N, OC};
    auto stateMemDesc = memory::desc(stateLayerDims, memory::data_type::f32, memory::format_tag::ldnc);

    // Creating descriptor for the weights memory
    memory::dims weightInputDims = {L, D, IC, G, OC};
    auto weightInputMemDesc = memory::desc(weightInputDims, memory::data_type::f32, memory::format_tag::any);

    memory::dims weightRecurrentDims = {L, D, OC, G, OC};
    auto weightRecurrentMemDesc = memory::desc(weightRecurrentDims, memory::data_type::f32, memory::format_tag::any);

    // Creating descriptor for the GRU operation
    auto backwardGRUDesc = gru_backward::desc(
      prop_kind::backward,           // aprop_kind
      rnn_direction::unidirectional, // direction
      layerInputMemDesc,             // src_layer_desc
      stateMemDesc,                  // src_iter_desc
      weightInputMemDesc,            // weights_layer_desc
      weightRecurrentMemDesc,        // weights_iter_desc
      _biasMem.get_desc(),           // bias_desc
      layerOutputMemDesc,            // dst_layer_desc
      stateMemDesc,                  // dst_iter_desc
      layerInputMemDesc,             // diff_src_layer_desc
      stateMemDesc,                  // diff_src_iter_desc
      weightInputMemDesc,            // diff_weights_layer_desc
      weightRecurrentMemDesc,        // diff_weights_iter_desc
      _biasGradientMem.get_desc(),   // diff_bias_desc
      layerOutputMemDesc,            // diff_dst_layer_desc
      stateMemDesc                   // diff_dst_iter_desc
    );

    // Create GRU primitive descriptor.
    _backwardGRUPrimitiveDesc = gru_backward::primitive_desc(backwardGRUDesc, _nn->_dnnlEngine, _forwardGRUPrimitiveDesc);

    // Create the primitive.
    _backwardGRUPrimitive = gru_backward(_backwardGRUPrimitiveDesc);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // Obtaining batch size
    size_t L = _depth;
    size_t N = _batchSize;
    size_t C = _outputChannels;

    // Allocating hidden state tensors
    _hGradientTensor.resize(_nn->_timestepCount);
    for (size_t i = 0; i < _nn->_timestepCount; i++) cudaErrCheck(cudaMalloc((void **)&_hGradientTensor[i], L * N * C * sizeof(float)));
  }
#endif
}

void GRU::forwardData(const size_t t)
{
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Configuring forward arguments
    std::unordered_map<int, memory> forwardGRUArgs;
    forwardGRUArgs.insert({DNNL_ARG_SRC_LAYER, _prevLayer->_outputMem[t]});
    forwardGRUArgs.insert({DNNL_ARG_WEIGHTS_LAYER, _weightsLayerMem});
    forwardGRUArgs.insert({DNNL_ARG_WEIGHTS_ITER, _weightsRecurrentMem});
    forwardGRUArgs.insert({DNNL_ARG_BIAS, _biasMem});
    forwardGRUArgs.insert({DNNL_ARG_DST_LAYER, _outputMem[t]});
    forwardGRUArgs.insert({DNNL_ARG_SRC_ITER, t == 0 ? _nullStateInputMem : _hiddenStateMem[t - 1]}); // Input
    forwardGRUArgs.insert({DNNL_ARG_DST_ITER, _hiddenStateMem[t]});                                   // Output
    forwardGRUArgs.insert({DNNL_ARG_WORKSPACE, _workspaceMem[t]});

    // Primitive execution
    _forwardGRUPrimitive.execute(_nn->_dnnlStream, forwardGRUArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    const size_t N = _batchSize;

    // Creating array of sequence lengths necessary for CuDNN
    std::vector<int> seqLengthArray(N, 1);

    cudnnErrCheck(cudnnRNNForward(
      _nn->_cuDNNHandle, // handle
      _rnnDesc,          // rnnDesc
      _forwardMode,
      _devSequenceLengths,                  // devSeqLengths
      _inputRNNDataDesc,                    // xDesc
      _prevLayer->_outputTensor[t],         // x
      _outputRNNDataDesc,                   // yDesc
      _outputTensor[t],                     // y
      _hTensorDesc,                         // hDesc
      t == 0 ? NULL : _hStateTensor[t - 1], // hx
      _hStateTensor[t],                     // hy
      NULL,                                 // cDesc -- Not necessary for GRU
      NULL,                                 // cx -- Not necessary for GRU
      NULL,                                 // cy -- Not necessary for GRU
      _weightsSize,
      _weightsTensor,
      _workSpaceSize,
      _workSpaceTensor[t],
      _reserveSpaceSize,
      _reserveSpaceTensor[t]));
  }
#endif
}

void GRU::backwardData(const size_t t)
{
  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer backward data propagation but NN was configured for inference only.\n");

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Cleaning current weight gradients
    const memory::dim L = _depth;
    const memory::dim G = _gateCount;      // Gates
    const memory::dim C = _outputChannels; // channels
    std::vector<float> nullWeightGradients(L * G * C * C, 0.0f);
    write_to_dnnl_memory(nullWeightGradients.data(), _weightsLayerGradientMem);
    write_to_dnnl_memory(nullWeightGradients.data(), _weightsRecurrentGradientMem);

    std::vector<float> nullBiasGradients(L * G * C, 0.0f);
    write_to_dnnl_memory(nullBiasGradients.data(), _biasGradientMem);

    _backwardGRUPrimitive.execute(_nn->_dnnlStream,
                                  {{DNNL_ARG_SRC_LAYER, _prevLayer->_outputMem[t]},
                                   {DNNL_ARG_SRC_ITER, t == 0 ? _nullStateInputMem : _hiddenStateMem[t - 1]},
                                   {DNNL_ARG_WEIGHTS_LAYER, _weightsLayerMem},
                                   {DNNL_ARG_WEIGHTS_ITER, _weightsRecurrentMem},
                                   {DNNL_ARG_BIAS, _biasMem},
                                   {DNNL_ARG_DST_LAYER, _outputMem[t]},
                                   {DNNL_ARG_DST_ITER, t == _nn->_timestepCount - 1 ? _nullStateInputMem : _hiddenStateMem[t]},
                                   {DNNL_ARG_DIFF_WEIGHTS_LAYER, _weightsLayerGradientMem},
                                   {DNNL_ARG_DIFF_WEIGHTS_ITER, _weightsRecurrentGradientMem},
                                   {DNNL_ARG_DIFF_BIAS, _biasGradientMem},
                                   {DNNL_ARG_DIFF_SRC_LAYER, _prevLayer->_outputGradientMem[t]},
                                   {DNNL_ARG_DIFF_SRC_ITER, t == 0 ? _nullStateOutputMem : _hiddenStateGradientMem[t - 1]},
                                   {DNNL_ARG_DIFF_DST_LAYER, _outputGradientMem[t]},
                                   {DNNL_ARG_DIFF_DST_ITER, t == _nn->_timestepCount - 1 ? _nullStateInputMem : _hiddenStateGradientMem[t]},
                                   {DNNL_ARG_WORKSPACE, _workspaceMem[t]}});
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudnnErrCheck(cudnnRNNBackwardData_v8(
      _nn->_cuDNNHandle,                                         // handle
      _rnnDesc,                                                  // rnnDesc
      _devSequenceLengths,                                       // devSeqLengths
      _outputRNNDataDesc,                                        // yDesc
      _outputTensor[t],                                          // y
      _outputGradientTensor[t],                                  // dy
      _inputRNNDataDesc,                                         // xDesc
      _prevLayer->_outputGradientTensor[t],                      // dx
      _hTensorDesc,                                              // hDesc
      t == 0 ? NULL : _hStateTensor[t - 1],                      // hx
      t == _nn->_timestepCount - 1 ? NULL : _hGradientTensor[t], // dhy
      t == 0 ? NULL : _hGradientTensor[t - 1],                   // dhx
      NULL,                                                      // cDesc -- Not necessary for GRU
      NULL,                                                      // cx -- Not necessary for GRU
      NULL,                                                      // dcy -- Not necessary for GRU
      NULL,                                                      // dcx -- Not necessary for GRU
      _weightsSize,
      _weightsTensor,
      _workSpaceSize,
      _workSpaceTensor[t],
      _reserveSpaceSize,
      _reserveSpaceTensor[t]));
  }
#endif
}

void GRU::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 Recurrent::setConfiguration(js);
 _type = "layer/recurrent/gru";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: gru: \n%s\n", js.dump(2).c_str());
}

void GRU::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
 Recurrent::getConfiguration(js);
}

void GRU::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Recurrent::applyModuleDefaults(js);
}

void GRU::applyVariableDefaults()
{

 Recurrent::applyVariableDefaults();
}

;

} //recurrent
} //layer
} //neuralNetwork
} //korali
;


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

#include <votca/xtp/gwbse.h>

#include <boost/format.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <votca/tools/constants.h>

//#include "mathimf.h"

using boost::format;
using namespace boost::filesystem;

namespace votca {
    namespace xtp {
        namespace ub = boost::numeric::ublas;

        // +++++++++++++++++++++++++++++ //
        // MBPT MEMBER FUNCTIONS         //
        // +++++++++++++++++++++++++++++ //

        void GWBSE::FullQPHamiltonian(){

            // constructing full QP Hamiltonian, storage in vxc
            _vxc = -_vxc + _sigma_x + _sigma_c;
            // diagonal elements are given by _qp_energies
            for (unsigned _m = 0; _m < _vxc.size1(); _m++ ){
              _vxc( _m,_m ) = _qp_energies( _m + _qpmin );
            }

             // sigma matrices can be freed
            _sigma_x.resize(0);
            _sigma_c.resize(0);

            if ( _do_qp_diag ){
                _qp_diag_energies.resize(_vxc.size1());
                _qp_diag_coefficients.resize(_vxc.size1(), _vxc.size1());
                linalg_eigenvalues(_vxc, _qp_diag_energies, _qp_diag_coefficients);
            }
           return;
        }

        void GWBSE::sigma_diag(const TCMatrix& _Mmn){

            unsigned _levelsum = _Mmn[0].size2(); // total number of bands
            unsigned _gwsize = _Mmn[0].size1(); // size of the GW basis
            const double pi = boost::math::constants::pi<double>();

             #pragma omp parallel for
                for (unsigned _gw_level = 0; _gw_level < _qptotal; _gw_level++) {
                    const ub::matrix<real_gwbse>& Mmn = _Mmn[ _gw_level + _qpmin ];
                    double sigma_x=0;
                        for ( unsigned _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++ ){
                            // loop over all occupied bands used in screening
                            for ( unsigned _i_occ = 0 ; _i_occ <= _homo ; _i_occ++ ){
                                sigma_x-= Mmn( _i_gw , _i_occ ) * Mmn( _i_gw , _i_occ );
                            } // occupied bands
                        } // gwbasis functions
                    _sigma_x(_gw_level,_gw_level)=( 1.0 - _ScaHFX ) * sigma_x;
                }

                if(_g_sc_max_iterations==0) {_g_sc_max_iterations=1;}
                ub::vector<double>& dftenergies=_orbitals->MOEnergies();
                // initial _qp_energies are dft energies
                ub::vector<double>_qp_old=_qp_energies;

                bool energies_converged=false;

	    // only diagonal elements except for in final iteration
            for (unsigned _g_iter = 0; _g_iter < _g_sc_max_iterations; _g_iter++) {
                // loop over all GW levels

                #pragma omp parallel for
                for (unsigned _gw_level = 0; _gw_level < _qptotal; _gw_level++) {
                    const ub::matrix<real_gwbse>& Mmn = _Mmn[ _gw_level + _qpmin ];
                    const double qpmin = _qp_old(_gw_level + _qpmin);

                    double sigma_c=0.0;

                    // loop over all functions in GW basis
                    for (unsigned _i_gw = 0; _i_gw < _gwsize; _i_gw++) {
                        // the ppm_weights smaller 1.e-5 are set to zero in rpa.cc PPM_construct_parameters
                        if (_ppm_weight(_i_gw) < 1.e-9) { continue;}
                        const double ppm_freq = _ppm_freq(_i_gw);
                        const double fac = _ppm_weight(_i_gw) * ppm_freq;
                        // loop over all bands
                        for (unsigned _i = 0; _i < _levelsum; _i++) {

                            double occ = 1.0;
                            if (_i > _homo) occ = -1.0; // sign for empty levels

                            // energy denominator
                            const double _denom = qpmin - _qp_old(_i) + occ*ppm_freq;

                            double _stab = 1.0;
                            if (std::abs(_denom) < 0.25) {

                                _stab = 0.5 * (1.0 - std::cos(4.0 * pi * std::abs(_denom)));

                            }

                            const double _factor =0.5* fac * _stab / _denom; //Hartree

                            // sigma_c diagonal elements
                            sigma_c += _factor * Mmn(_i_gw, _i) * Mmn(_i_gw, _i);

                        }// bands

                    }// GW functions
                    _sigma_c(_gw_level, _gw_level)=sigma_c;
                    // update _qp_energies

                    _qp_energies(_gw_level + _qpmin) = dftenergies(_gw_level + _qpmin) + sigma_c + _sigma_x(_gw_level, _gw_level) - _vxc(_gw_level, _gw_level);

                }// all bands
                ub::vector<double> diff= _qp_old - _qp_energies;
                energies_converged = true;
                double diff_max=0;
                unsigned state_max=0;
                for (unsigned l = 0; l < diff.size(); l++) {
                    if(std::abs(diff(l))>std::abs(diff_max)){
                            diff_max=diff(l);
                            state_max=l;
                    }
                    if (std::abs(diff(l))>_g_sc_limit) {
                        energies_converged = false;
                    }
                }
                if(tools::globals::verbose){
                    double _DFTgap =dftenergies(_homo + 1) - dftenergies(_homo);
                    double _QPgap = _qp_energies( _homo +1 ) - _qp_energies( _homo  );
                    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " QP_Iteration: " << _g_iter+1 <<" shift="<<_QPgap - _DFTgap <<" E_diff max="<<diff_max<<" StateNo:"<<state_max << flush;
                }
                double alpha=0.0;
                _qp_energies=(1-alpha)*_qp_energies+alpha*_qp_old;

                if (energies_converged) {
                    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Converged after " << _g_iter+1 << " G iterations." << flush;
                    break;
                }else if(_g_iter==_g_sc_max_iterations-1){
                        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " G-self-consistency cycle not converged after " << _g_sc_max_iterations << " iterations." << flush;
                        break;

                } else {
                    _qp_old = _qp_energies;
                }

            } // iterations

            return;
        }

         void GWBSE::sigma_offdiag(const TCMatrix& _Mmn) {
            unsigned _levelsum = _Mmn[0].size2(); // total number of bands
            unsigned _gwsize = _Mmn[0].size1(); // size of the GW basis
            const double pi = boost::math::constants::pi<double>();

            #pragma omp parallel for
            for (unsigned _gw_level1 = 0; _gw_level1 < _qptotal; _gw_level1++) {
                const ub::matrix<real_gwbse>& Mmn1 =  _Mmn[ _gw_level1 + _qpmin ];
                for (unsigned _gw_level2 = 0; _gw_level2 < _gw_level1; _gw_level2++) {
                    const ub::matrix<real_gwbse>& Mmn2 =  _Mmn[ _gw_level2 + _qpmin ];
                    double sigma_x=0;
                    for ( unsigned _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++ ){
                        // loop over all occupied bands used in screening
                        for ( unsigned _i_occ = 0 ; _i_occ <= _homo ; _i_occ++ ){
                            sigma_x -= Mmn1( _i_gw , _i_occ ) * Mmn2( _i_gw , _i_occ );
                        } // occupied bands
                    } // gwbasis functions
                    _sigma_x(_gw_level1, _gw_level2)=( 1.0 - _ScaHFX ) * sigma_x;
                }
            }

            #pragma omp parallel for
            for (unsigned _gw_level1 = 0; _gw_level1 < _qptotal; _gw_level1++) {
                const double qpmin1 = _qp_energies(_gw_level1 + _qpmin);
                const ub::matrix<real_gwbse>& Mmn1 = _Mmn[ _gw_level1 + _qpmin ];
                for (unsigned _gw_level2 = 0; _gw_level2 < _gw_level1; _gw_level2++) {
                    const double qpmin2 = _qp_energies(_gw_level1 + _qpmin);
                    const ub::matrix<real_gwbse>& Mmn2 = _Mmn[ _gw_level2 + _qpmin ];
                    double sigma_c = 0;
                    for (unsigned _i_gw = 0; _i_gw < _gwsize; _i_gw++) {
                        // the ppm_weights smaller 1.e-5 are set to zero in rpa.cc PPM_construct_parameters
                        if (_ppm_weight(_i_gw) < 1.e-9) {
                            continue;
                        }
                        const double ppm_freq = _ppm_freq(_i_gw);
                        const double fac = _ppm_weight(_i_gw) * ppm_freq;
                        // loop over all screening levels
                        for (unsigned _i = 0; _i < _levelsum; _i++) {

                            double occ = 1.0;
                            if (_i > _homo) occ = -1.0; // sign for empty levels

                            // energy denominator
                            const double _denom1 = qpmin1 - _qp_energies(_i) + occ * ppm_freq;
                            const double _denom2 = qpmin2 - _qp_energies(_i) + occ * ppm_freq;

                            double _stab1 = 1.0;
                            if (std::abs(_denom1) < 0.25) {
                                _stab1 = 0.5 * (1.0 - std::cos(4.0 * pi * std::abs(_denom1)));
                            }
                            const double factor1 = 0.5 * fac * _stab1 / _denom1; //Hartree
                            double _stab2 = 1.0;
                            if (std::abs(_denom2) < 0.25) {
                                _stab2 = 0.5 * (1.0 - std::cos(4.0 * pi * std::abs(_denom2)));
                            }
                            const double factor2 = 0.5 * fac * _stab2 / _denom2; //Hartree
                            sigma_c +=Mmn1(_i_gw, _i) * Mmn2(_i_gw, _i)*0.5*(factor1+factor2);
                        }// screening levels
                    }// GW functions

                    _sigma_c(_gw_level1, _gw_level2) = sigma_c;

                }// GW row
            }//GW col

        return;
        }

        void GWBSE::sigma_prepare_threecenters(TCMatrix& _Mmn){
            #if (GWBSE_DOUBLE)
                const ub::matrix<double>& ppm_phi=_ppm_phi;
            #else
                const ub::matrix<float> ppm_phi=_ppm_phi;
            #endif

            #pragma omp parallel for
            for ( int _m_level = 0 ; _m_level < _Mmn.get_mtot(); _m_level++ ){
                // get Mmn for this _m_level
                // and multiply with _ppm_phi = eigenvectors of epsilon
              _Mmn[ _m_level ] = ub::prod(  ppm_phi , _Mmn[_m_level] );
            }
            return;
        }

    }

};


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/adptbx.h>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>

namespace cctbx { namespace adptbx { namespace boost_python {

  void wrap_hirshfeld();

namespace {

  sym_mat3<double>
  debye_waller_factor_u_star_gradient_coefficients_double(
    miller::index<> const& h)
  {
    return debye_waller_factor_u_star_gradient_coefficients(
      h, scitbx::type_holder<double>());
  }

  af::shared<double>
  debye_waller_factor_u_star_curvature_coefficients_double(
    miller::index<> const& h)
  {
    return debye_waller_factor_u_star_curvature_coefficients(
      h, scitbx::type_holder<double>());
  }

  struct eigensystem_wrappers
  {
    typedef eigensystem<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("eigensystem", no_init)
        .def(init<sym_mat3<double> const&>((arg("sym_mat3"))))
        .def("vectors", &w_t::vectors, ccr())
        .def("values", &w_t::values, ccr())
      ;
    }
  };

  void init_module()
  {
    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;

    def("u_as_b", (double(*)(double)) u_as_b);
    def("b_as_u", (double(*)(double)) b_as_u);
    def("u_as_b", (sym_mat3<double>(*)(sym_mat3<double> const&)) u_as_b);
    def("b_as_u", (sym_mat3<double>(*)(sym_mat3<double> const&)) b_as_u);

#define CCTBX_DEF(function_name) \
    def(# function_name, \
      (sym_mat3<double>(*)(uctbx::unit_cell const&, sym_mat3<double> const&)) \
      function_name);
    CCTBX_DEF(u_cif_as_u_star)
    CCTBX_DEF(u_star_as_u_cif)
    CCTBX_DEF(u_cart_as_u_star)
    CCTBX_DEF(u_star_as_u_cart)
    CCTBX_DEF(u_cart_as_u_cif)
    CCTBX_DEF(u_cif_as_u_cart)
    def("u_star_as_beta",
      (sym_mat3<double>(*)(sym_mat3<double> const&)) u_star_as_beta);
    def("beta_as_u_star",
      (sym_mat3<double>(*)(sym_mat3<double> const&)) beta_as_u_star);
    CCTBX_DEF(u_cart_as_beta)
    CCTBX_DEF(beta_as_u_cart)
    CCTBX_DEF(u_cif_as_beta)
    CCTBX_DEF(beta_as_u_cif)
#undef CCTBX_DEF

    def("u_cart_as_u_iso",
      (double(*)(sym_mat3<double> const&)) u_cart_as_u_iso);
    def("u_iso_as_u_cart",
      (sym_mat3<double>(*)(double const&)) u_iso_as_u_cart);

#define CCTBX_DEF(function_name) \
    def(# function_name, \
      (double(*)(uctbx::unit_cell const&, sym_mat3<double> const&)) \
      function_name);
    CCTBX_DEF(u_star_as_u_iso)
    CCTBX_DEF(u_cif_as_u_iso)
    CCTBX_DEF(beta_as_u_iso)
#undef CCTBX_DEF

#define CCTBX_DEF(function_name) \
    def(# function_name, \
      (sym_mat3<double>(*)(uctbx::unit_cell const&, double const&)) \
      function_name);
    CCTBX_DEF(u_iso_as_u_star)
    CCTBX_DEF(u_iso_as_u_cif)
    CCTBX_DEF(u_iso_as_beta)
#undef CCTBX_DEF

    class_<factor_u_cart_u_iso<> >("factor_u_cart_u_iso", no_init)
      .def(init<sym_mat3<double> const&>((arg("u_cart"))))
      .def_readonly("u_iso", &factor_u_cart_u_iso<>::u_iso)
      .add_property("u_cart_minus_u_iso",
        make_getter(&factor_u_cart_u_iso<>::u_cart_minus_u_iso, rbv()))
    ;
    class_<factor_u_star_u_iso<> >("factor_u_star_u_iso", no_init)
      .def(init<
        uctbx::unit_cell const&, sym_mat3<double> const&>((
          arg("unit_cell"), arg("u_star"))))
      .def_readonly("u_iso", &factor_u_star_u_iso<>::u_iso)
      .add_property("u_star_minus_u_iso",
        make_getter(&factor_u_star_u_iso<>::u_star_minus_u_iso, rbv()))
    ;
    class_<factor_u_cif_u_iso<> >("factor_u_cif_u_iso", no_init)
      .def(init<
        uctbx::unit_cell const&, sym_mat3<double> const&>((
          arg("unit_cell"), arg("u_cif"))))
      .def_readonly("u_iso", &factor_u_cif_u_iso<>::u_iso)
      .add_property("u_cif_minus_u_iso",
        make_getter(&factor_u_cif_u_iso<>::u_cif_minus_u_iso, rbv()))
    ;
    class_<factor_beta_u_iso<> >("factor_beta_u_iso", no_init)
      .def(init<
        uctbx::unit_cell const&, sym_mat3<double> const&>((
          arg("unit_cell"), arg("beta"))))
      .def_readonly("u_iso", &factor_beta_u_iso<>::u_iso)
      .add_property("beta_minus_u_iso",
        make_getter(&factor_beta_u_iso<>::beta_minus_u_iso, rbv()))
    ;

    def("debye_waller_factor_b_iso",
      (double(*)(
        double,
        double, double, bool))
          debye_waller_factor_b_iso, (
            arg("stol_sq"),
            arg("b_iso"),
            arg("exp_arg_limit")=50,
            arg("truncate_exp_arg")=false));
    def("debye_waller_factor_b_iso",
      (af::shared<double>(*)(
        af::const_ref<double> const&,
        double const&, double const&, bool))
          debye_waller_factor_b_iso, (
            arg("stol_sq"),
            arg("b_iso"),
            arg("exp_arg_limit")=50,
            arg("truncate_exp_arg")=false));
    def("debye_waller_factor_u_iso",
      (double(*)(double, double)) debye_waller_factor_u_iso);
    def("debye_waller_factor_b_iso",
      (double(*)(uctbx::unit_cell const&, miller::index<> const&, double))
      debye_waller_factor_b_iso);
    def("debye_waller_factor_u_iso",
      (double(*)(uctbx::unit_cell const&, miller::index<> const&, double))
      debye_waller_factor_u_iso);
    def("debye_waller_factor_u_star",
      (double(*)(
        miller::index<> const&,
        sym_mat3<double> const&, double const&, bool))
          debye_waller_factor_u_star, (
            arg("h"),
            arg("u_star"),
            arg("exp_arg_limit")=50,
            arg("truncate_exp_arg")=false));
    def("debye_waller_factor_u_star",
      (af::shared<double>(*)(
        af::const_ref<miller::index<> > const&,
        sym_mat3<double> const&, double const&, bool))
          debye_waller_factor_u_star, (
            arg("miller_indices"),
            arg("u_star"),
            arg("exp_arg_limit")=50,
            arg("truncate_exp_arg")=false));
    def("debye_waller_factor_u_star_gradient_coefficients",
      debye_waller_factor_u_star_gradient_coefficients_double, (
        arg("h")));
    def("debye_waller_factor_u_star_curvature_coefficients",
      debye_waller_factor_u_star_curvature_coefficients_double, (
        arg("h")));
    def("debye_waller_factor_beta",
      (double(*)(miller::index<> const&, sym_mat3<double> const&))
      debye_waller_factor_beta);
    def("debye_waller_factor_u_cif",
      (double(*)(uctbx::unit_cell const&,
                 miller::index<> const&,
                 sym_mat3<double> const&))
      debye_waller_factor_u_cif);
    def("debye_waller_factor_u_cart",
      (double(*)(uctbx::unit_cell const&,
                 miller::index<> const&,
                 sym_mat3<double> const&))
      debye_waller_factor_u_cart);

    def("grad_u_star_as_u_cart",
      (sym_mat3<double>(*)(
        uctbx::unit_cell const&,
        sym_mat3<double> const&)) grad_u_star_as_u_cart);
    def("grad_u_star_as_u_cart",
      (af::shared<sym_mat3<double> >(*)(
        uctbx::unit_cell const&,
        af::const_ref<sym_mat3<double> > const&)) grad_u_star_as_u_cart);
    def("grad_u_cart_as_u_star",
      (sym_mat3<double>(*)(
        uctbx::unit_cell const&,
        sym_mat3<double> const&)) grad_u_cart_as_u_star);
    def("grad_u_cart_as_u_star",
      (af::shared<sym_mat3<double> >(*)(
        uctbx::unit_cell const&,
        af::const_ref<sym_mat3<double> > const&)) grad_u_cart_as_u_star);

    def("eigenvalues", (vec3<double>(*)(sym_mat3<double> const&)) eigenvalues);
    def("is_positive_definite",
      (bool(*)(vec3<double> const&)) is_positive_definite);
    def("is_positive_definite",
      (bool(*)(vec3<double> const&, double const&)) is_positive_definite);
    def("is_positive_definite",
      (bool(*)(sym_mat3<double> const&)) is_positive_definite);
    def("is_positive_definite",
      (bool(*)(sym_mat3<double> const&, double const&)) is_positive_definite);

    def("is_positive_definite",
      (af::shared<bool>(*)(af::const_ref<sym_mat3<double> > const&,
                                         double const&)) is_positive_definite);

    def("eigenvalue_filtering",
      (sym_mat3<double>(*)(
        sym_mat3<double> const&, double const&, double const&))
          eigenvalue_filtering, (
            arg("u_cart"),
            arg("u_min")=0,
            arg("u_max")=0));

    eigensystem_wrappers::wrap();

    def("c_u_c_transpose",
      (sym_mat3<double>(*)(mat3<double> const&, sym_mat3<double> const&))
      c_u_c_transpose);

    wrap_hirshfeld();

    class_<projection_sum<> >("projection_sum", no_init)
      .def(init<
        vec3<double> const&, vec3<double> const&,
        sym_mat3<double> const&, sym_mat3<double> const&,
        uctbx::unit_cell const& >((
          arg("site1"), arg("site2"), arg("ustar1"), arg("ustar2"),
          arg("unit_cell"))))
      .def("delta_z", &projection_sum<>::delta_z)
      .def("z_12", &projection_sum<>::z_12)
      .def("z_21", &projection_sum<>::z_21)
    ;
}

} // namespace <anonymous>
}}} // namespace cctbx::adptbx::boost_python

BOOST_PYTHON_MODULE(cctbx_adptbx_ext)
{
  cctbx::adptbx::boost_python::init_module();
}


#pragma once

#include <Eigen/Dense>

#include "epigraph.hpp"
#include "systemDynamics.hpp"
#include "trajectoryData.hpp"
#include "discretizationData.hpp"

namespace scpp
{

template <typename DERIVED, size_t STATE_DIM, size_t INPUT_DIM, size_t PARAM_DIM>
class SystemModel : public SystemDynamics<STATE_DIM, INPUT_DIM, PARAM_DIM>
{
public:
    using BASE = SystemDynamics<STATE_DIM, INPUT_DIM, PARAM_DIM>;

    using trajectory_data_t = TrajectoryData<BASE>;
    using discretization_data_t = DiscretizationData<BASE>;

    using ptr_t = std::shared_ptr<DERIVED>;

    using typename BASE::control_matrix_t;
    using typename BASE::control_matrix_v_t;
    using typename BASE::dynamic_matrix_t;
    using typename BASE::dynamic_vector_map_t;
    using typename BASE::dynamic_vector_t;
    using typename BASE::input_vector_t;
    using typename BASE::input_vector_v_t;
    using typename BASE::param_vector_t;
    using typename BASE::state_matrix_t;
    using typename BASE::state_matrix_v_t;
    using typename BASE::state_vector_t;
    using typename BASE::state_vector_v_t;

    using typename BASE::control_matrix_ad_t;
    using typename BASE::domain_vector_ad_t;
    using typename BASE::dynamic_vector_ad_t;
    using typename BASE::input_vector_ad_t;
    using typename BASE::param_vector_ad_t;
    using typename BASE::state_matrix_ad_t;
    using typename BASE::state_vector_ad_t;

    enum : size_t
    {
        state_dim = STATE_DIM,
        input_dim = INPUT_DIM,
        param_dim = PARAM_DIM,
    };

    /**
     * @brief Construct a new System Model object
     *
     */
    SystemModel(){};

    virtual ~SystemModel(){};

    /**
     * @brief Function to initialize the trajectory of a derived model. Has to be implemented by the derived class. Only required for SC models,
     *
     */
    virtual void getInitializedTrajectory(trajectory_data_t &)
    {
        throw std::runtime_error("getInitializedTrajectory: This function has to be implemented by the derived class.");
    };

    /**
     * @brief Function to add constraints of a model. Has to be implemented by the derived class.
     *
     * @param socp  The SOCP.
     * @param X     Last state trajectory.
     * @param U     Last input trajectory.
     */
    virtual void addApplicationConstraints(std::shared_ptr<cvx::OptimizationProblem>,
                                           state_vector_v_t &,
                                           input_vector_v_t &){};

    /**
     * @brief Gets the new parameters in the system flow map.
     *
     */
    virtual void getNewModelParameters(param_vector_t &){};

    /**
     * @brief Updates the parameters in the system flow map.
     *
     */
    void updateModelParameters()
    {
        param_vector_t model_params;
        getNewModelParameters(model_params);
        BASE::updateModelParameters(model_params);
    };

    /**
     * @brief Function to remove mass and length dimensions from all function parameters.
     *
     */
    virtual void nondimensionalize()
    {
        throw std::runtime_error("nondimensionalize: This function has to be implemented by the derived class.");
    };

    /**
     * @brief Function to add mass and length dimensions to all function parameters.
     *
     */
    virtual void redimensionalize()
    {
        throw std::runtime_error("redimensionalize: This function has to be implemented by the derived class.");
    };

    /**
     * @brief Get the operating point of the system. Usually an equilibrium point for linearization.
     *
     */
    virtual void getOperatingPoint(state_vector_t &, input_vector_t &)
    {
        throw std::runtime_error("getOperatingPoint: This function has to be implemented by the derived class.");
    };

    /**
     * @brief Function to remove mass and length dimensions from state and input trajectory.
     *
     */
    virtual void nondimensionalizeTrajectory(trajectory_data_t &)
    {
        throw std::runtime_error("nondimensionalizeTrajectory: This function has to be implemented by the derived class.");
    };

    /**
     * @brief Function to add mass and length dimensions to state and input trajectory.
     *
     */
    virtual void redimensionalizeTrajectory(trajectory_data_t &)
    {
        throw std::runtime_error("redimensionalizeTrajectory: This function has to be implemented by the derived class.");
    };

    static const std::string getModelName()
    {
        return DERIVED::modelName;
    };

    void setParameterFolder(const std::string &path)
    {
        param_folder_path = path;
    };

    const std::string getParameterFolder() const
    {
        return param_folder_path + getModelName();
    };

private:
    std::string param_folder_path = "../scpp_models/config/";
};

} // namespace scpp


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/scaling/scaling.h>
#include <mmtbx/scaling/absolute_scaling.h>
#include <mmtbx/scaling/relative_scaling.h>
#include <mmtbx/scaling/twinning.h>
#include <mmtbx/scaling/outlier.h>

namespace mmtbx { namespace scaling {
namespace boost_python{

  // outlier detection
  void wrap_outlier();
  // twinnning related stuff
  void wrap_twinning();
  // relative scaling
  void wrap_local_scaling_moment_based();
  void wrap_local_scaling_ls_based();
  void wrap_local_scaling_nikonov();
  void wrap_least_squares_on_i();
  void wrap_least_squares_on_f();
  void wrap_least_squares_on_i_wt();
  void wrap_least_squares_on_f_wt();

namespace {

  void init_module()
  {
    using namespace boost::python;

    // Wilson parameters and other handy stuff
    def("get_d_star_sq_low_limit", get_d_star_sq_low_limit);
    def("get_d_star_sq_high_limit", get_d_star_sq_high_limit);

    def("get_gamma_prot", ( scitbx::af::shared<double>(*)(
        scitbx::af::const_ref<double> const&))
        get_gamma_prot);
    def("gamma_prot", (double(*)(double const&)) gamma_prot);

    def("sigma_prot_sq", (double(*)(double const&, double const&))
        sigma_prot_sq);
    def("get_sigma_prot_sq", (scitbx::af::shared<double>(*)(
                          scitbx::af::const_ref<double> const&,
                          double const&)) get_sigma_prot_sq);

    // Isotropic scaling
    def("wilson_single_nll",
        (double(*)(double const&,
                   double const&,
                   double const&,
                   double const&,
                   double const&,
                   double const&,
                   bool const&,
                   double const&,
                   double const&,
                   bool const&
                   ))
        absolute_scaling::wilson_single_nll, (
          arg("d_star_sq"),
          arg("f_obs"),
          arg("sigma_f_obs"),
          arg("epsilon"),
          arg("sigma_sq"),
          arg("gamma_prot"),
          arg("centric"),
          arg("p_scale"),
          arg("p_B_wilson"),
          arg("transform")=true));

    def("wilson_total_nll",
        (double(*)(scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<bool> const&,
                   double const&,
                   double const&,
                   bool))
        absolute_scaling::wilson_total_nll, (
          arg("d_star_sq"),
          arg("f_obs"),
          arg("sigma_f_obs"),
          arg("epsilon"),
          arg("sigma_sq"),
          arg("gamma_prot"),
          arg("centric"),
          arg("p_scale"),
          arg("p_B_wilson"),
          arg("transform")=true));

    def("wilson_single_nll_gradient", (scitbx::af::tiny<double,2>(*)(
                                        double const&,
                                        double const&,
                                        double const&,
                                        double const&,
                                        double const&,
                                        double const&,
                                        bool const&,
                                        double const&,
                                        double const&))
        absolute_scaling::wilson_single_nll_gradient,
          (arg("d_star_sq"),
           arg("f_obs"),
           arg("sigma_f_obs"),
           arg("epsilon"),
           arg("sigma_sq"),
           arg("gamma_prot"),
           arg("centric"),
           arg("p_scale"),
           arg("p_B_wilson"))
        );

    def("wilson_total_nll_gradient", (scitbx::af::tiny<double,2>(*)(
                                      scitbx::af::const_ref<double> const&,
                                      scitbx::af::const_ref<double> const&,
                                      scitbx::af::const_ref<double> const&,
                                      scitbx::af::const_ref<double> const&,
                                      scitbx::af::const_ref<double> const&,
                                      scitbx::af::const_ref<double> const&,
                                      scitbx::af::const_ref<bool> const&,
                                      double const&,
                                      double const&))
        absolute_scaling::wilson_total_nll_gradient,
          (arg("d_star_sq"),
           arg("f_obs"),
           arg("sigma_f_obs"),
           arg("epsilon"),
           arg("sigma_sq"),
           arg("gamma_prot"),
           arg("centric"),
           arg("p_scale"),
           arg("p_B_wilson"))
        );

    def ("ml_normalise", (scitbx::af::shared<double>(*)(
                          scitbx::af::const_ref<double> const&,
                          scitbx::af::const_ref<double> const&,
                          scitbx::af::const_ref<double> const&,
                          scitbx::af::const_ref<double> const&,
                          scitbx::af::const_ref<double> const&,
                          scitbx::af::const_ref<bool> const&,
                          double const&,
                          double const&,
                          bool const&))
         absolute_scaling::ml_normalise, (
           arg("d_star_sq"),
           arg("f_obs"),
           arg("epsilon"),
           arg("sigma_sq"),
           arg("gamma_prot"),
           arg("centric"),
           arg("p_scale"),
           arg("p_B_wilson"),
           arg("wiggle")=true));

    // Anisotropic scaling
    def("wilson_get_aniso_scale",
        (double(*)(cctbx::miller::index<> const&,
                   double const&,
                   double const&,
                   scitbx::sym_mat3<double> const&))
        absolute_scaling::wilson_get_aniso_scale);

    def("wilson_single_nll_aniso",
        (double(*)(cctbx::miller::index<> const&,
                   double const&,
                   double const&,
                   double const&,
                   double const&,
                   double const&,
                   bool const&,
                   double const&,
                   cctbx::uctbx::unit_cell const&,
                   scitbx::sym_mat3<double> const&))
        absolute_scaling::wilson_single_nll_aniso);

    def("wilson_total_nll_aniso",
        (double(*)(scitbx::af::const_ref< cctbx::miller::index<> > const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<double> const&,
                   scitbx::af::const_ref<bool> const&,
                   double const&,
                   cctbx::uctbx::unit_cell const&,
                   scitbx::sym_mat3<double> const&))
        absolute_scaling::wilson_total_nll_aniso);

    def("wilson_single_nll_aniso_gradient",
        (scitbx::af::shared<double>(*)
         ( cctbx::miller::index<> const&,
           double const&,
           double const&,
           double const&,
           double const&,
           double const&,
           bool const&,
           double const&,
           cctbx::uctbx::unit_cell const&,
           scitbx::sym_mat3<double> const&))
        absolute_scaling::wilson_single_nll_aniso_gradient);

    def("wilson_total_nll_aniso_gradient_orbit",
        (scitbx::af::shared<double>(*)
         (scitbx::af::const_ref< cctbx::miller::index<> > const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<bool> const&,
          double const&,
          cctbx::uctbx::unit_cell const&,
          cctbx::sgtbx::space_group const&,
          scitbx::sym_mat3<double> const&))
        absolute_scaling::wilson_total_nll_aniso_gradient_orbit);

    def("wilson_total_nll_aniso_gradient",
        (scitbx::af::shared<double>(*)
         (scitbx::af::const_ref< cctbx::miller::index<> > const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<double> const&,
          scitbx::af::const_ref<bool> const&,
          double const&,
          cctbx::uctbx::unit_cell const&,
          scitbx::sym_mat3<double> const&))
        absolute_scaling::wilson_total_nll_aniso_gradient);

    def("ml_normalise_aniso",
        (scitbx::af::shared<double>(*)
         (scitbx::af::const_ref< cctbx::miller::index<> >const&,
          scitbx::af::const_ref<double> const&,
          double const&,
          cctbx::uctbx::unit_cell const& ,
          scitbx::sym_mat3<double> const&,
          bool const&))
         absolute_scaling::ml_normalise_aniso, (
           arg("hkl"),
           arg("f_obs"),
           arg("p_scale"),
           arg("unit_cell"),
           arg("u_star"),
           arg("volume_correction_rwgk")=false));

    def("kernel_normalisation",
        (scitbx::af::shared<double>(*)
         (scitbx::af::const_ref< double > const&,
          scitbx::af::const_ref< double > const&,
          scitbx::af::const_ref< double > const&,
          scitbx::af::const_ref< double > const&,
          double const&))
        absolute_scaling::kernel_normalisation,
        (arg("d_star_sq_hkl"),
         arg("I_hkl"),
         arg("epsilon"),
         arg("d_star_sq_array"),
         arg("kernel_width")));

    //outlier detection
    wrap_outlier();
    // twinning related stuff
    wrap_twinning();
    // relative scaling
    wrap_local_scaling_moment_based();
    wrap_local_scaling_ls_based();
    wrap_local_scaling_nikonov();
    wrap_least_squares_on_i();
    wrap_least_squares_on_f();
    wrap_least_squares_on_i_wt();
    wrap_least_squares_on_f_wt();
  }

}}}} // mmtbx::scaling::<anonymous>

BOOST_PYTHON_MODULE(mmtbx_scaling_ext)
{
  mmtbx::scaling::boost_python::init_module();
}


#pragma once

#include <Eigen/Eigen>

namespace LRR
{
  namespace Rendering
  {
    using namespace Eigen;

    struct VertexShaderOutput
    {
      Vector4f vertexPosition;
    };

    struct FragmentShaderOutput
    {
      Vector3f fragmentColor;
    };

    class Shader
    {
    public:
      Shader() = default;
      Shader(Shader const &other) = default;
      virtual ~Shader() = default;

      virtual VertexShaderOutput OnVertex() = 0;
      virtual FragmentShaderOutput OnFragment() = 0;

      inline Vector3f const &CurrentVertex() const
      { return mCurrentVertex; }

      inline void CurrentVertex(Vector3f const &vertex)
      { mCurrentVertex = vertex; }

      inline Vector2f const &FragCoord() const
      { return mFragCoord; }

      inline void FragCoord(Vector2f const &fragCoord)
      { mFragCoord = fragCoord; }
    protected:
      Vector3f mCurrentVertex;
      Vector2f mFragCoord;
    };
  }
}


#include <algorithm>
#include <iostream>
#include <map>
#include <vector>
#include <gtest/gtest.h>

// [Getting error "BOOST_PARAMETER_MAX_ARITY must be at least 12 for CGAL::Mesh_3" in CGAL 4.9](http://cgal-discuss.949826.n4.nabble.com/Getting-error-quot-BOOST-PARAMETER-MAX-ARITY-must-be-at-least-12-for-CGAL-Mesh-3-quot-in-CGAL-4-9-td4662304.html)
#define BOOST_PARAMETER_MAX_ARITY 12

#include <CGAL/Polyhedron_3.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/iterator/zip_iterator.hpp>

using Kernel = CGAL::Simple_cartesian<double>;
using Aff_transformation_3 = CGAL::Aff_transformation_3<Kernel>;
using Polyhedron = CGAL::Polyhedron_3<Kernel>;
using Color = CGAL::Color;
using HalfedgeDS = Polyhedron::HalfedgeDS;
using HDS = Polyhedron::Halfedge_data_structure;
using Decorator = CGAL::HalfedgeDS_decorator<HDS>;

using Items = std::map<std::string, Polyhedron>;

class SyntheticSeamCollinearTest : public testing::Test
{
protected:

  Items getItems(const std::string &file_name)
  {
    std::string test_models_path = pathFromUri("package://analyzer/resources/");
    auto items = parseScene(test_models_path, test_models_path + file_name);
    return items;
  }

  SearchTree makeSearchTree(const std::vector<LineSet>& weld_segments)
  {
    std::vector<Point> points;
    std::vector<size_t> seg_indices;
    for (size_t i = 0; i < weld_segments.size(); ++i)
    {
      const auto &seg = weld_segments[i];
      Point p1 = seg.front();
      Point p2 = seg.back();
      points.push_back(std::move(p1));
      points.push_back(std::move(p2));
      seg_indices.push_back(i);
      seg_indices.push_back(i);
    }

    return SearchTree(
      boost::make_zip_iterator(boost::make_tuple( points.begin(), seg_indices.begin() )),
      boost::make_zip_iterator(boost::make_tuple( points.end(),   seg_indices.end() ) )
    );
  }

};

class MyTriMeshRefiner
{
private:
  std::vector<std::vector<size_t>> faces_;
  std::vector<Point3> vertices_;
  std::map<Point3, size_t> vertexIndicesMap_;
  size_t vertexCounter_;

public:

  size_t n_Faces() const { return faces_.size(); }

  size_t n_Vertices() const { return vertices_.size(); }

  size_t addVertex(Point3 p)
  {
    if (auto it = vertexIndicesMap_.find(p); it == vertexIndicesMap_.end()) // not found
    {
      vertexIndicesMap_.insert({p, vertexCounter_++});
      vertices_.push_back(std::move(p));
      return vertexCounter_ - 1;
    }
    else
    {
      return it->second;
    }
  }

  MyTriMeshRefiner(const Polyhedron& poly)
  : vertexCounter_(0)
  {
    for (const auto &face : faces(poly))
    {
      auto begin = face->halfedge();
      auto edge = begin;
      // Each vertex
      std::vector<size_t> tri;
      do
      {
        Point3 p = edge->vertex()->point();
        size_t vertex_idx = addVertex(p);
        tri.push_back(vertex_idx);
        edge = edge->next();
      } while (edge != begin);
      faces_.push_back(std::move(tri));
    }
  }

  void refine()
  {
    std::vector<std::vector<size_t>> new_faces;
    for (const auto &face : faces_)
    {
      size_t a_idx = face[0],
             b_idx = face[1],
             c_idx = face[2];
      Point3 a = vertices_[a_idx],
                  b = vertices_[b_idx],
                  c = vertices_[c_idx],
                  ab = CGAL::midpoint(a, b),
                  bc = CGAL::midpoint(b, c),
                  ca = CGAL::midpoint(c, a);
      size_t ab_idx = addVertex(ab),
             bc_idx = addVertex(bc),
             ca_idx = addVertex(ca);
      std::vector tri_1 = {a_idx,  ab_idx, ca_idx},
                  tri_2 = {ab_idx, b_idx,  bc_idx},
                  tri_3 = {ca_idx, bc_idx, c_idx},
                  tri_4 = {ab_idx, bc_idx, ca_idx};
      new_faces.push_back(std::move(tri_1));
      new_faces.push_back(std::move(tri_2));
      new_faces.push_back(std::move(tri_3));
      new_faces.push_back(std::move(tri_4));
    }
    std::swap(faces_, new_faces);
  }

  Polyhedron toPolyhedron()
  {
    // make a copy as `buildPolyhedron` gets parameters by non-constant reference
    auto points = vertices_;
    auto polygons = faces_;
    return buildPolyhedron(points, polygons);
  }
};

TEST_F(SyntheticSeamCollinearTest, CaseRotation)
{
  Items items = getItems("rotation.x3d");
  ASSERT_FALSE(items.empty());

  std::vector<Polyhedron> polys, refined_polys;
  for (const auto &item : items)
  {
    Polyhedron poly = item.second;

    MyTriMeshRefiner mesh_refiner(poly);
    mesh_refiner.refine();
    mesh_refiner.refine();

    Polyhedron refined_poly = mesh_refiner.toPolyhedron();
    polys.push_back(std::move(poly));
    refined_polys.push_back(std::move(refined_poly));
  }

  std::vector<LineSet> weld_segments, weld_segments_to_unify;
  findSeams(polys, weld_segments);
  findSeams(refined_polys, weld_segments_to_unify);

  ASSERT_FALSE(weld_segments.empty());
  ASSERT_FALSE(weld_segments_to_unify.empty());
  ASSERT_EQ(weld_segments.size(), weld_segments_to_unify.size());

  SearchTree stree1 = makeSearchTree(weld_segments);
  SearchTree stree2 = makeSearchTree(weld_segments_to_unify);

  for (size_t i = 0; i < weld_segments.size(); ++i)
  {
    auto seg = weld_segments[i];
    const Point& p1 = seg.front();
    const Point& p2 = seg.back();
    constexpr double rad = 0.025, eps = 0.0001;
    FuzzySphere fs1(p1, rad, eps);
    FuzzySphere fs2(p2, rad, eps);
    std::vector<Point_and_int> found1, found2, found_intersection;

    stree2.search(std::back_inserter(found1), fs1);
    stree2.search(std::back_inserter(found2), fs2);
    std::set_intersection(found1.begin(), found1.end(), found2.begin(), found2.end(),
                          std::back_inserter(found_intersection),
          [](Point_and_int a, Point_and_int b)
          {
            size_t idx1 = boost::get<1>(a),
                   idx2 = boost::get<1>(b);
            return idx1 < idx2;
          });

    // check 1:1 correspondence between segments in weld_segments and weld_segments_to_unify
    ASSERT_EQ(found_intersection.size(), 1);
    size_t idx_found = boost::get<1>(*found_intersection.begin());
    auto refined_seg = weld_segments_to_unify[idx_found];
    const Point& p1_refined = refined_seg.front();
    const Point& p2_refined = refined_seg.back();

    bool cond = ( (twoPointsEqual(p1, p1_refined) && twoPointsEqual(p2, p2_refined))
                ||(twoPointsEqual(p1, p2_refined) && twoPointsEqual(p2, p1_refined)) );

    ASSERT_TRUE(cond);
  }
}


#include <complex>
#include <memory>
#include <fftw3.h>
#include <armadillo>
using namespace std;
using namespace arma;

#ifndef __DEFT_HPP__
#define __DEFT_HPP__

class deft{

public:

    // constructors
    deft(const size_t, const size_t, const size_t, const double*, const double*, const double*);
    deft(const deft&);

    // read data
    double at(const size_t, const size_t, const size_t) const;
    double operator()(const size_t, const size_t, const size_t) const;

    // copy data
    void copy_data_from(const double*);

    // assignment
    void equals(const double);
    void operator=(const double);
    void equals(const deft);
    void operator=(const deft);

    // addition
    void addEquals(const double);
    void operator+=(const double);
    void addEquals(const deft);
    void operator+=(const deft);

    // subtraction
    void subtractEquals(const double);
    void operator-=(const double);
    void subtractEquals(const deft);
    void operator-=(const deft);
    // multiplication
    void multiplyEquals(const double);
    void operator*=(const double);
    void multiplyEquals(const deft);
    void operator*=(const deft);

    // division
    void divideEquals(const double);
    void operator/=(const double);
    void divideEquals(const deft);
    void operator/=(const deft);

    // elementwise math
    void pow(const double);

    // fourier transform operations
    void computeFT();
    void computeIFT();

    // derivatives (using fourier transforms)
    void compute_gradient_x();
    void compute_gradient_y();
    void compute_gradient_z();
    void compute_gradient_squared();
    void compute_laplacian();

    // integrate
    double integrate() const;

    // update cell geometry and reciprocal lattice vectors
    void updateGeometry(const double*, const double*, const double*);

    // read cell information
    double cellVecX(const size_t) const;
    double cellVecY(const size_t) const;
    double cellVecZ(const size_t) const;
    double cellLenX() const;
    double cellLenY() const;
    double cellLenZ() const;
    double vol() const;
    double dv() const;
    double kVecX(const size_t) const;
    double kVecX(const size_t, const size_t, const size_t) const;
    double kVecY(const size_t) const;
    double kVecY(const size_t, const size_t, const size_t) const;
    double kVecZ(const size_t) const;
    double kVecZ(const size_t, const size_t, const size_t) const;
    double kVecLen(const size_t) const;
    double kVecLen(const size_t, const size_t, const size_t) const;

    // interpolate
    deft* interpolate(const size_t new_x, const size_t new_y, const size_t new_z);

    // sum a function over a lattice
    void sum_over_lattice(mat loc, double (*func)(double));

private:
public:

    // real-space dimensions and data
    const size_t _xDim;
    const size_t _yDim;
    const size_t _zDim;
    const size_t _dimXYZ;
    // TODO: decide whether this should remain a pointer
    cube* _data;

private:

    // cell geometry
    shared_ptr<vec> _cellVecX;
    shared_ptr<vec> _cellVecY;
    shared_ptr<vec> _cellVecZ;
    shared_ptr<double> _cellLenX;
    shared_ptr<double> _cellLenY;
    shared_ptr<double> _cellLenZ;
    shared_ptr<double> _vol;
    shared_ptr<double> _dv;

public:

    // fourier-space dimensions and data
    const size_t _xDimFT;
    const size_t _yDimFT;
    const size_t _zDimFT;
    // TODO: add ft_numXYZ
    cx_cube* _dataFT;

private:

    // reciprocal lattice vectors
    shared_ptr<cube> _kVecX;
    shared_ptr<cube> _kVecY;
    shared_ptr<cube> _kVecZ;
    shared_ptr<cube> _kVecLen;

    // fftw info
    fftw_plan _planR2C;
    fftw_plan _planC2R;

};

#endif  //  __DEFT_HPP__


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidDataSet.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/FluidTensor.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <queue>
#include <memory>
#include <string>

namespace fluid {
namespace algorithm {

class KDTree
{

public:
  using string = std::string;

  using DataSet = FluidDataSet<string, double, 1>;
  using ConstRealVectorView = FluidTensorView<const double, 1>;
  struct Node;
  using NodePtr = std::shared_ptr<Node>;
  using knnCandidate = std::pair<double, const Node*>;
  using knnQueue = std::priority_queue<knnCandidate, std::vector<knnCandidate>,
                                       std::less<knnCandidate>>;
  using iterator = const std::vector<index>::iterator;

  struct Node
  {
    const string     id;
    const RealVector data;
    NodePtr          left{nullptr}, right{nullptr};
  };

  struct FlatData
  {
    FluidTensor<index, 2>  tree;
    FluidTensor<string, 1> ids;
    FluidTensor<double, 2> data;
    FlatData(index n, index m) : tree(n, 2), ids(n), data(n, m) {}
  };

  explicit KDTree() = default;
  ~KDTree() = default;

  KDTree(const DataSet& dataset)
  {
    using namespace std;
    mNPoints = dataset.size();
    mDims = dataset.pointSize();
    if (mDims > 0 && mNPoints > 0)
    {
      vector<index> indices(asUnsigned(dataset.size()));
      iota(indices.begin(), indices.end(), 0);
      mRoot = buildTree(indices, indices.begin(), indices.end(), dataset, 0);
    }
    mInitialized = true;
  }

  void addNode(string id, ConstRealVectorView data)
  {
    mRoot = addNode(mRoot.get(), id, data, 0);
    mNPoints++;
  }

  DataSet kNearest(ConstRealVectorView data, index k = 1,
                   double radius = 0) const
  {
    assert(data.size() == mDims);
    knnQueue queue;
    auto     result = DataSet(1);
    kNearest(mRoot.get(), data, queue, k, radius, 0);
    index                     numFound = asSigned(queue.size());
    std::vector<knnCandidate> sorted(asUnsigned(numFound));
    for (index i = numFound - 1; i >= 0; i--)
    {
      sorted[asUnsigned(i)] = queue.top();
      queue.pop();
    }
    for (index i = 0; i < numFound; i++)
    {
      auto dist = FluidTensor<double, 1>{sorted[asUnsigned(i)].first};
      auto id = sorted[asUnsigned(i)].second->id;
      result.add(id, dist);
    }
    return result;
  }

  void  print() const { print(mRoot.get(), 0); }
  index dims() const { return mDims; }
  index size() const { return mNPoints; }
  bool  initialized() const { return mInitialized; }

  void clear()
  {
    mRoot = nullptr;
    mInitialized = false;
  }

  FlatData toFlat() const
  {
    FlatData store(mNPoints, mDims);
    flatten(0, mRoot.get(), store);
    return store;
  }

  void fromFlat(FlatData vectors)
  {
    mRoot = unflatten(vectors, 0);
    mNPoints = vectors.data.rows();
    mDims = vectors.data.cols();
    mInitialized = true;
  }

private:
  NodePtr buildTree(std::vector<index>& indices, iterator from, iterator to,
                    const DataSet& dataset, index depth) const
  {
    using namespace std;
    if (from == to)
      return nullptr;
    else if (std::distance(from, to) == 1)
    {
      return makeNode(dataset.getIds()(*from), dataset.getData().row(*from));
    }
    const index d = depth % mDims;
    sort(from, to, [&](index a, index b) {
      return dataset.getData().row(a)(d) < dataset.getData().row(b)(d);
    });
    const index range = std::distance(from, to);
    const index median = range / 2;
    NodePtr     current = makeNode(dataset.getIds().row(*(from + median)),
                               dataset.getData().row(*(from + median)));
    if (median > 0)
      current->left =
          buildTree(indices, from, from + median, dataset, depth + 1);
    if (range - median > 1)
      current->right =
          buildTree(indices, from + median + 1, to, dataset, depth + 1);
    return current;
  }

  NodePtr makeNode(string id, ConstRealVectorView data) const
  {
    return std::make_shared<Node>(Node{id, RealVector{data}, nullptr, nullptr});
  }

  NodePtr addNode(Node* current, string id, ConstRealVectorView data,
                  const index depth) const
  {
    if (current == nullptr) { return makeNode(id, data); }

    const index d = depth % mDims;
    if (data(d) < current->data(d))
    { current->left = addNode(current->left.get(), id, data, depth + 1); }
    else
    {
      current->right = addNode(current->right.get(), id, data, depth + 1);
    }
    return NodePtr(current);
  }

  double distance(ConstRealVectorView p1, ConstRealVectorView p2) const
  {
    using namespace Eigen;
    auto v1 = _impl::asEigen<Array>(p1);
    auto v2 = _impl::asEigen<Array>(p2);
    return (v1 - v2).matrix().norm();
  }

  void print(Node* current, index depth) const
  {
    for (index i = 0; i < depth; ++i) std::cout << "  ";
    if (current == nullptr)
    {
      std::cout << " null" << std::endl;
      return;
    }
    std::cout << " " << current->id << std::endl;
    for (index i = 0; i < depth; ++i) std::cout << "  ";
    std::cout << " left" << std::endl;
    print(current->left.get(), depth + 1);
    for (index i = 0; i < depth; ++i) std::cout << "  ";
    std::cout << " right" << std::endl;
    print(current->right.get(), depth + 1);
  }

  void kNearest(const Node* current, ConstRealVectorView data, knnQueue& knn,
                index k, double radius, index depth) const
  {
    if (current == nullptr) return;
    const double currentDist = distance(current->data, data);
    bool         withinRadius = radius > 0 ? currentDist < radius : true;
    if (withinRadius && (knn.size() < asUnsigned(k) || k == 0))
    { knn.push(std::make_pair(currentDist, current)); }
    else if (withinRadius && currentDist < knn.top().first)
    {
      knn.pop();
      knn.push(std::make_pair(currentDist, current));
    }
    const index  d = depth % mDims;
    const double dimDif = current->data(d) - data(d);
    Node*        firstBranch = current->left.get();
    Node*        secondBranch = current->right.get();
    if (dimDif <= 0)
    {
      firstBranch = current->right.get();
      secondBranch = current->left.get();
    }
    kNearest(firstBranch, data, knn, k, radius, depth + 1);
    if (k == 0 || knn.size() < asUnsigned(k) ||
        dimDif < knn.top().first) // ball centered at query with diametre
                                  // kthDist intersects with current partition
                                  // (or need to get more neighbors)
    { kNearest(secondBranch, data, knn, k, radius, depth + 1); }
  }

  index flatten(index nodeId, const Node* current, FlatData& store) const
  {
    if (current == nullptr) { return nodeId; }
    store.ids(nodeId) = current->id;
    store.data.row(nodeId) = current->data;

    index nextNodeId = nodeId + 1;
    if (current->left == nullptr) { store.tree(nodeId, 0) = -1; }
    else
    {
      store.tree(nodeId, 0) = nextNodeId;
      nextNodeId = flatten(nextNodeId, current->left.get(), store);
    }
    if (current->right == nullptr) { store.tree(nodeId, 1) = -1; }
    else
    {
      store.tree(nodeId, 1) = nextNodeId;
      nextNodeId = flatten(nextNodeId, current->right.get(), store);
    }
    return nextNodeId;
  }

  NodePtr unflatten(const FlatData& store, index index) const
  {
    if (index == -1) return nullptr;
    NodePtr current = makeNode(store.ids[index], store.data[index]);
    current->left = unflatten(store, store.tree(index, 0));
    current->right = unflatten(store, store.tree(index, 1));
    return current;
  }

  NodePtr mRoot{nullptr};
  index   mDims;
  index   mNPoints{0};
  bool    mInitialized{false};
};
} // namespace algorithm
} // namespace fluid


//
// Created by Zhongshi Jiang on 4/20/17.
//
#include "ReWeightedARAP.h"
#include "ScafData.h"
#include "util/triangle_utils.h"

#include "igl/arap.h"
#include "igl/cat.h"
#include "igl/doublearea.h"
#include "igl/grad.h"
#include "igl/local_basis.h"
#include "igl/per_face_normals.h"
#include "igl/slice_into.h"
#include "igl/serialize.h"
#include <igl/columnize.h>

#include <igl/flip_avoiding_line_search.h>
#include <igl/boundary_facets.h>
#include <igl/unique.h>
#include <igl/slim.h>
#include <igl/grad.h>
#include <igl/is_symmetric.h>
#include <igl/polar_svd.h>
#include <igl/boundary_loop.h>
#include <igl/cotmatrix.h>
#include <igl/edge_lengths.h>
#include <igl/local_basis.h>
#include <igl/readOBJ.h>
#include <igl/repdiag.h>
#include <igl/vector_area_matrix.h>
#include <iostream>
#include <igl/slice.h>
#include <igl/colon.h>
#include <igl/min_quad_with_fixed.h>

#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <Eigen/SparseCholesky>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <map>
#include <set>
#include <vector>
#include <igl/Timer.h>
#include <igl/edge_flaps.h>

void ReWeightedARAP::solve_weighted_arap(Eigen::MatrixXd &uv)
{
  using namespace Eigen;
  using namespace std;
  int dim = d_.dim;
  auto v_n = d_.v_num;
  igl::Timer timer;
  timer.start();

  const VectorXi &bnd_ids = d_.frame_ids;

  const auto bnd_n = bnd_ids.size();
  assert(bnd_n > 0);
  MatrixXd bnd_pos;
  igl::slice(d_.w_uv, bnd_ids, 1, bnd_pos);

  ArrayXi known_ids(bnd_n * dim);
  ArrayXi unknown_ids((v_n - bnd_n) * dim);

  { // get the complement of bnd_ids.
    int assign = 0, i = 0;
    for (int get = 0; i < v_n && get < bnd_ids.size(); i++)
    {
      if (bnd_ids(get) == i)
        get++;
      else
        unknown_ids(assign++) = i;
    }
    while (i < v_n)
      unknown_ids(assign++) = i++;
    assert(assign + bnd_ids.size() == v_n);
  }

  VectorXd known_pos(bnd_ids.size() * dim);
  for (int d = 0; d < dim; d++)
  {
    auto n_b = bnd_ids.rows();
    known_ids.segment(d * n_b, n_b) = bnd_ids.array() + d * v_n;
    known_pos.segment(d * n_b, n_b) = bnd_pos.col(d);
    unknown_ids.block(d * (v_n - n_b), 0, v_n - n_b, unknown_ids.cols()) =
        unknown_ids.topRows(v_n - n_b) + d * v_n;
  }
  //std::cout<<"Slicing Knowns "<<timer.getElapsedTime()<<std::endl;
  //timer.start();

  Eigen::SparseMatrix<double> L;
  Eigen::VectorXd rhs;

  // fixed frame solving:
  // x_e as the fixed frame, x_u for unknowns (mesh + unknown scaffold)
  // min ||(A_u*x_u + A_e*x_e) - b||^2
  // => A_u'*A_u*x_u + A_u'*A_e*x_e = Au'*b
  // => A_u'*A_u*x_u  = Au'* (b - A_e*x_e) := Au'* b_u
  // => L * x_u = rhs
  //
  // separate matrix build:
  // min ||A_m x_m - b_m||^2 + ||A_s x_all - b_s||^2 + soft + proximal
  // First change dimension of A_m to fit for x_all
  // (Not just at the end, since x_all is flattened along dimensions)
  // L = A_m'*A_m + A_s'*A_s + soft + proximal
  // rhs = A_m'* b_m + A_s' * b_s + soft + proximal
  //
  using namespace std;
  Eigen::SparseMatrix<double> L_m, L_s;
  Eigen::VectorXd rhs_m, rhs_s;
  build_surface_linear_system(L_m, rhs_m);  // complete Am, with soft
  build_scaffold_linear_system(L_s, rhs_s); // complete As, without proximal
  // we don't need proximal term

  L = L_m + L_s;
  rhs = rhs_m + rhs_s;
  L.makeCompressed();

  //std::cout<<"Constructing matrices "<<timer.getElapsedTime()<<std::endl;
  //  VectorXd uv_flat(dim * v_n);
  //  for (int i = 0; i < dim; i++)
  //    for (int j = 0; j < v_n; j++)
  //      uv_flat(v_n * i + j) = d_.w_uv(j, i);
  //
  //  VectorXd uv_flat_unknown(dim * (v_n - bnd_ids.size()));
  //  igl::slice(uv_flat, unknown_ids, 1, uv_flat_unknown);

  timer.start();
  Eigen::VectorXd unknown_Uc((v_n - d_.frame_ids.size()) * dim), Uc(dim * v_n);
  bool solve_with_cg = (d_.dim == 3);
  if (solve_with_cg)
  {
    for (auto t:{1e-6}) {
      timer.start();
      ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::Lower | Upper>
          CGsolver;
      CGsolver.setTolerance(t);
      unknown_Uc = CGsolver.compute(L).solve(rhs);
      cout << t << "CGSolve = " << timer.getElapsedTime() << endl;
      std::cout << "#iterations:     " << CGsolver.iterations() << std::endl;
      std::cout << "estimated error: " << CGsolver.error() << std::endl;
    }
  }
  else
  {
    SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    unknown_Uc = solver.compute(L).solve(rhs);
    //cout << "Direct Solve = " << timer.getElapsedTime() << endl;
  }
  //timer.start();
  igl::slice_into(unknown_Uc, unknown_ids.matrix(), 1, Uc);
  igl::slice_into(known_pos, known_ids.matrix(), 1, Uc);

  uv = Map<Matrix<double,-1,-1,Eigen::ColMajor>>(Uc.data(),v_n ,dim);
  //for (int i = 0; i < dim; i++)
  //  uv.col(i) = Uc.block(i * v_n, 0, v_n, 1);
  //cout << "Slice back = " << timer.getElapsedTime() << endl;
}

void ReWeightedARAP::build_surface_linear_system(Eigen::SparseMatrix<double> &L,
                                                 Eigen::VectorXd &rhs) const
{

  using namespace Eigen;
  using namespace std;

  const int v_n = d_.v_num - (d_.frame_ids.size());
  const int dim = d_.dim;
  const int f_n = d_.mf_num;
  if (d_.dim == 3 && d_.m_T.cols() == 3)
  { // arap
    Eigen::SparseMatrix<double> L_cot = (-arap_energy_p * CotMat).eval();
    L_cot.conservativeResize(v_n, v_n);
    igl::repdiag(L_cot, 3, L);

    auto &data = arap_data;

    const int Rdim = data.dim;
    MatrixXd R(Rdim, data.CSM.rows());

    for (int i = 0; i < arap_rots.size(); i++)
    {
      R.block(0, 3 * i, 3, 3) = arap_rots[i];
    }

    // Number of rotations: #vertices or #elements
    int num_rots = data.K.cols() / Rdim / Rdim;
    // distribute group rotations to vertices in each group

    VectorXd Rcol;
    igl::columnize(R, num_rots, 2, Rcol);
    VectorXd Bcol = data.K * Rcol;
    assert(Bcol.size() == data.n * data.dim);

    Map<MatrixXd> arap_Bc(Bcol.data(), data.n, data.dim); //column order

    rhs = Eigen::VectorXd::Zero(v_n * 3);
    for (int d = 0; d < dim; d++)
      rhs.segment(d * v_n, data.n) = arap_energy_p * arap_Bc.col(d);
    return;
  }

  // to get the  complete A
  Eigen::VectorXd sqrtM = d_.m_M.array().sqrt();
  Eigen::SparseMatrix<double> A(dim * dim * f_n, dim * v_n);

  auto decoy_Dx_m = Dx_m; decoy_Dx_m.conservativeResize(W_m.rows(), v_n);
  auto decoy_Dy_m = Dy_m; decoy_Dy_m.conservativeResize(W_m.rows(), v_n);
  auto decoy_Dz_m = Dz_m;
  if (dim == 3) decoy_Dz_m.conservativeResize(W_m.rows(), v_n);
  buildAm(sqrtM, decoy_Dx_m, decoy_Dy_m, decoy_Dz_m, W_m, A);

  Eigen::SparseMatrix<double> At = A.transpose();
  At.makeCompressed();

  Eigen::SparseMatrix<double> id_m(At.rows(), At.rows());
  id_m.setIdentity();

  L = At * A;

  Eigen::VectorXd frhs;
  buildRhs(sqrtM, W_m, Ri_m, frhs);
  rhs = At * frhs;

  // add soft constraints.
  for (auto const &x : d_.soft_cons)
  {
    int v_idx = x.first;

    for (int d = 0; d < dim; d++)
    {
      rhs(d * (v_n) + v_idx) += d_.soft_const_p * x.second(d); // rhs
      L.coeffRef(d * v_n + v_idx,
                 d * v_n + v_idx) += d_.soft_const_p; // diagonal
    }
  }
}

void ReWeightedARAP::build_scaffold_linear_system(Eigen::SparseMatrix<double>
                                                      &L,
                                                  Eigen::VectorXd &rhs) const
{
  using namespace Eigen;

  const int f_n = W_s.rows();
  const int v_n = Dx_s.cols();
  const int dim = d_.dim;

  Eigen::VectorXd sqrtM = d_.s_M.array().sqrt();
  Eigen::SparseMatrix<double> A(dim * dim * f_n, dim * v_n);
  buildAm(sqrtM, Dx_s, Dy_s, Dz_s, W_s, A);

  const VectorXi &bnd_ids = d_.frame_ids;

  auto bnd_n = bnd_ids.size();
  assert(bnd_n > 0);
  MatrixXd bnd_pos;
  igl::slice(d_.w_uv, bnd_ids, 1, bnd_pos);

  ArrayXi known_ids(bnd_ids.size() * dim);
  ArrayXi unknown_ids((v_n - bnd_ids.rows()) * dim);

  { // get the complement of bnd_ids.
    int assign = 0, i = 0;
    for (int get = 0; i < v_n && get < bnd_ids.size(); i++)
    {
      if (bnd_ids(get) == i)
        get++;
      else
        unknown_ids(assign++) = i;
    }
    while (i < v_n)
      unknown_ids(assign++) = i++;
    assert(assign + bnd_ids.size() == v_n);
  }

  VectorXd known_pos(bnd_ids.size() * dim);
  for (int d = 0; d < dim; d++)
  {
    auto n_b = bnd_ids.rows();
    known_ids.segment(d * n_b, n_b) = bnd_ids.array() + d * v_n;
    known_pos.segment(d * n_b, n_b) = bnd_pos.col(d);
    unknown_ids.block(d * (v_n - n_b), 0, v_n - n_b, unknown_ids.cols()) =
        unknown_ids.topRows(v_n - n_b) + d * v_n;
  }
  Eigen::VectorXd sqrt_M = d_.s_M.array().sqrt();

  // slice
  // 'manual slicing for A(:, unknown/known)'
  Eigen::SparseMatrix<double> Au, Ae;
  {
    using TY = double;
    using TX = double;
    auto &X = A;

    int xm = X.rows();
    int xn = X.cols();
    int ym = xm;
    int yn = unknown_ids.size();
    int ykn = known_ids.size();

    std::vector<int> CI(xn, -1);
    std::vector<int> CKI(xn, -1);
    // initialize to -1
    for (int i = 0; i < yn; i++)
      CI[unknown_ids(i)] = (i);
    for (int i = 0; i < ykn; i++)
      CKI[known_ids(i)] = i;
    Eigen::DynamicSparseMatrix<TY, Eigen::ColMajor> dyn_Y(ym, yn);
    Eigen::DynamicSparseMatrix<TY, Eigen::ColMajor> dyn_K(ym, ykn);
    // Take a guess at the number of nonzeros (this assumes uniform distribution
    // not banded or heavily diagonal)
    dyn_Y.reserve(A.nonZeros());
    dyn_K.reserve(A.nonZeros() * ykn / xn);
    // Iterate over outside
    for (int k = 0; k < X.outerSize(); ++k)
    {
      // Iterate over inside
      if (CI[k] != -1)
        for (typename Eigen::SparseMatrix<TX>::InnerIterator it(X, k); it;
             ++it)
        {
          dyn_Y.coeffRef(it.row(), CI[it.col()]) = it.value();
        }
      else
        for (typename Eigen::SparseMatrix<TX>::InnerIterator it(X, k); it;
             ++it)
        {
          dyn_K.coeffRef(it.row(), CKI[it.col()]) = it.value();
        }
    }
    Au = Eigen::SparseMatrix<TY>(dyn_Y);
    Ae = Eigen::SparseMatrix<double>(dyn_K);
  }

  Eigen::SparseMatrix<double> Aut = Au.transpose();
  Aut.makeCompressed();

  Eigen::SparseMatrix<double> id(Aut.rows(), Aut.rows());
  id.setIdentity();

  L = Aut * Au;

  Eigen::VectorXd frhs;
  buildRhs(sqrtM, W_s, Ri_s, frhs);

  rhs = Aut * (frhs - Ae * known_pos);
}

void ReWeightedARAP::buildAm(const Eigen::VectorXd &sqrt_M,
                             const Eigen::SparseMatrix<double> &Dx,
                             const Eigen::SparseMatrix<double> &Dy,
                             const Eigen::SparseMatrix<double> &Dz,
                             const Eigen::MatrixXd &W,
                             Eigen::SparseMatrix<double> &Am)
{
  std::vector<Eigen::Triplet<double>> IJV;

  Eigen::SparseMatrix<double> MDx = sqrt_M.asDiagonal() * Dx;
  Eigen::SparseMatrix<double> MDy = sqrt_M.asDiagonal() * Dy;

  Eigen::SparseMatrix<double> MDz;
  if (Dz.rows() != 0) MDz = sqrt_M.asDiagonal() * Dz;

  igl::slim_buildA(MDx, MDy, MDz, W, IJV);

  Am.setFromTriplets(IJV.begin(), IJV.end());
  Am.makeCompressed();
 }

void ReWeightedARAP::buildRhs(const Eigen::VectorXd &sqrt_M,
                              const Eigen::MatrixXd &W,
                              const Eigen::MatrixXd &Ri,
                              Eigen::VectorXd &f_rhs)
{
  const int dim = (W.cols() == 4) ? 2 : 3;
  const int f_n = W.rows();
  f_rhs.resize(dim * dim * f_n);

  if (dim == 2)
  {
    /*b = [W11*R11 + W12*R21; (formula (36))
         W11*R12 + W12*R22;
         W21*R11 + W22*R21;
         W21*R12 + W22*R22];*/
    for (int i = 0; i < f_n; i++)
    {
      auto sqrt_area = sqrt_M(i);
      f_rhs(i + 0 * f_n) = sqrt_area * (W(i, 0) * Ri(i, 0) + W(i, 1) * Ri(i, 1));
      f_rhs(i + 1 * f_n) = sqrt_area * (W(i, 0) * Ri(i, 2) + W(i, 1) * Ri(i, 3));
      f_rhs(i + 2 * f_n) = sqrt_area * (W(i, 2) * Ri(i, 0) + W(i, 3) * Ri(i, 1));
      f_rhs(i + 3 * f_n) = sqrt_area * (W(i, 2) * Ri(i, 2) + W(i, 3) * Ri(i, 3));
    }
  }
  else
  {
    /*b = [W11*R11 + W12*R21 + W13*R31;
         W11*R12 + W12*R22 + W13*R32;
         W11*R13 + W12*R23 + W13*R33;
         W21*R11 + W22*R21 + W23*R31;
         W21*R12 + W22*R22 + W23*R32;
         W21*R13 + W22*R23 + W23*R33;
         W31*R11 + W32*R21 + W33*R31;
         W31*R12 + W32*R22 + W33*R32;
         W31*R13 + W32*R23 + W33*R33;];*/
    for (int i = 0; i < f_n; i++)
    {
      auto sqrt_area = sqrt_M(i);
      f_rhs(i + 0 * f_n) = sqrt_area *
                           (W(i, 0) * Ri(i, 0) + W(i, 1) * Ri(i, 1) + W(i, 2) * Ri(i, 2));
      f_rhs(i + 1 * f_n) = sqrt_area *
                           (W(i, 0) * Ri(i, 3) + W(i, 1) * Ri(i, 4) + W(i, 2) * Ri(i, 5));
      f_rhs(i + 2 * f_n) = sqrt_area *
                           (W(i, 0) * Ri(i, 6) + W(i, 1) * Ri(i, 7) + W(i, 2) * Ri(i, 8));
      f_rhs(i + 3 * f_n) = sqrt_area *
                           (W(i, 3) * Ri(i, 0) + W(i, 4) * Ri(i, 1) + W(i, 5) * Ri(i, 2));
      f_rhs(i + 4 * f_n) = sqrt_area *
                           (W(i, 3) * Ri(i, 3) + W(i, 4) * Ri(i, 4) + W(i, 5) * Ri(i, 5));
      f_rhs(i + 5 * f_n) = sqrt_area *
                           (W(i, 3) * Ri(i, 6) + W(i, 4) * Ri(i, 7) + W(i, 5) * Ri(i, 8));
      f_rhs(i + 6 * f_n) = sqrt_area *
                           (W(i, 6) * Ri(i, 0) + W(i, 7) * Ri(i, 1) + W(i, 8) * Ri(i, 2));
      f_rhs(i + 7 * f_n) = sqrt_area *
                           (W(i, 6) * Ri(i, 3) + W(i, 7) * Ri(i, 4) + W(i, 8) * Ri(i, 5));
      f_rhs(i + 8 * f_n) = sqrt_area *
                           (W(i, 6) * Ri(i, 6) + W(i, 7) * Ri(i, 7) + W(i, 8) * Ri(i, 8));
    }
  }
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <vector>
#include <iostream>
#include <iterator>
#include <math.h>
#include <boost/optional.hpp>
#include <fftw3.h>
#include <comma/visiting/traits.h>
#include <comma/application/command_line_options.h>
#include <comma/application/verbose.h>
#include <comma/csv/options.h>
#include <comma/csv/stream.h>
#include "detail/shuffle-tied.h"

std::size_t input_size=0;
bool filter_input=true;
bool logarithmic_output=true;
bool magnitude=false;
bool real=false;
bool split=false;
std::size_t bin_size=0;
boost::optional<double> bin_overlap;
bool tied=true;

void usage(bool detail)
{
    std::cerr<<"    perform fft on input data" << std::endl;
    std::cerr << std::endl;
    std::cerr<< "usage: " << comma::verbose.app_name() << " [ <options> ]" << std::endl;
    std::cerr << std::endl;
    std::cerr<< "    input fields: data; an array of double, size is specified with --size"  << std::endl;
    std::cerr<< "    output: array of pair (real, complex) of double; use --output-size to get size of array of doubles with the specified options"  << std::endl;
    std::cerr << std::endl;
    std::cerr << "options" << std::endl;
    std::cerr << "    --help,-h: show help" << std::endl;
    std::cerr << "    --verbose,-v: show detailed messages" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    --bin-size=[<size>]: cut data into several bins of this size and perform fft on each bin, when not speificed calculates fft on the whole data" << std::endl;
    std::cerr << "    --bin-overlap=[<overlap>]: if specified, each bin will contain this portion of the last bin's data, range: 0 (no overlap) to 1"<<std::endl;
    std::cerr << "    --linear: output as linear; when not specified, output will be scaled to lograithm of 10 for magnitude or real part (phase is not affected)" << std::endl;
    std::cerr << "    --no-filter: when not specified, filters input using a cut window to get limited output" << std::endl;
    std::cerr << "    --size=<size>: size of input vector" << std::endl;
    std::cerr << std::endl;
    std::cerr << "  output options:" << std::endl;
    std::cerr << "    --magnitude: output magnitude only" << std::endl;
    std::cerr<< "        output is binary array of double with half the size of input"  << std::endl;
    std::cerr << "    --real: output real part only" << std::endl;
    std::cerr<< "        output is binary array of double with half the size of input"  << std::endl;
    std::cerr << "    --split: output array of real followed by array of complex part; when not specified real and complex parts are interleaved" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    --shuffle,--shuffle-fields,--shuffled-fields=[<csv_fields>]: comma separated list of input fields to be written to stdout; if not specified prepend all input fields to the output" << std::endl;
    std::cerr << "    --untied: only write output (doesn't write input to stdout)" << std::endl;
    std::cerr << std::endl;
    std::cerr << "  utility options:" << std::endl;
    std::cerr << "    --output-fields: print output fields and exit; depends on input fields and size" << std::endl;
    std::cerr << "    --output-format: print binary format of output and exit; depends on input fields and size" << std::endl;
    std::cerr << "    --output-size: print size of output data array and exit; e.g. if output format is t,ui,256d, then output size is 256" << std::endl;
    std::cerr << std::endl;
    if(detail)
    {
        std::cerr << "csv options:" << std::endl;
        std::cerr<< comma::csv::options::usage() << std::endl;
        std::cerr << std::endl;
    }
    else
    {
        std::cerr << "use -v or --verbose to see more detail" << std::endl;
        std::cerr << std::endl;
    }
    std::cerr << "example" << std::endl;
    std::cerr << "      " << comma::verbose.app_name() << " --binary=\"t,16000f\" --fields=t,data --size=16000" << std::endl;
    std::cerr << std::endl;
    exit(0);
}

// math-fft --fields ,,data --format t,ui,16000f
// math-fft --fields ,,,,,data --format t,ui,s[$(( 16000 * 4  ))],t,ui,16000f

struct input_t
{
    std::vector<double> data;
    input_t() : data(input_size) {}
};

struct output_t
{
    std::vector<double> data;
    output_t()
    {
        std::size_t len=bin_size;
        if(magnitude || real) { len/=2; }
        data.resize(len);
    }
    void reset()
    {
        memset(&data[0],0,data.size()*sizeof(data[0]));
    }
};

namespace comma { namespace visiting {

template <> struct traits< input_t >
{
    template< typename K, typename V > static void visit( const K& k, input_t& p, V& v )
    {
        v.apply( "data", p.data );
    }
    template< typename K, typename V > static void visit( const K& k, const input_t& p, V& v )
    {
        v.apply( "data", p.data );
    }
};

template <> struct traits< output_t >
{
    template< typename K, typename V > static void visit( const K& k, const output_t& p, V& v )
    {
        v.apply( "output", p.data );
    }
};

} } // namespace comma { namespace visiting {

template<typename T>
T* allocate_fftw_array(std::size_t size)
{
    return reinterpret_cast<T*>(fftw_malloc(sizeof(T)*size));
}

// a quick and dirty helper class
struct fft
{
    std::vector<double> h;
    double* input;
    fftw_complex* output;
    fftw_plan plan;

    fft(std::size_t size) : h( size ), input(allocate_fftw_array<double>(size)) ,
        output(allocate_fftw_array<fftw_complex>(size / 2 + 1 )) ,
        plan( fftw_plan_dft_r2c_1d( size, input, output, 0 ) )
    {
        for( std::size_t i = 0; i < size; ++i ) { h[i] = 0.54 - 0.46 * std::cos( M_PI * 2 * i / size ); }
    }
    ~fft()
    {
        fftw_destroy_plan(plan);
        fftw_free( input ); // seems that fftw_destroy_plan() releases it
        fftw_free( output ); // seems that fftw_destroy_plan() releases it
    }
    void calculate() { fftw_execute( plan); }
    std::size_t output_size() const { return h.size() / 2; }
};

void calculate(const double* data, std::size_t size, std::vector<double>& output)
{
    fft fft(size);
    if(filter_input)
    {
        for(std::size_t i=0;i<size;i++) { fft.input[i] = fft.h[i] * data[i]; }
    }
    else { memcpy(fft.input, data, size * sizeof(double)); }

    fft.calculate();

    if(magnitude)
    {
        if(fft.output_size()>output.size()) { COMMA_THROW(comma::exception, "size mismatch, output "<<output.size()<<" fft output "<<fft.output_size()); }
        for(std::size_t j=0;j<fft.output_size();j++)
        {
            double a= std::abs( std::complex<double>( fft.output[j][0], fft.output[j][1] ) );
            if(logarithmic_output) { a = (a == 0) ? 0 : (std::log10(a)); }
            output[j]=a;
        }
    }
    else if(real)
    {
        if(fft.output_size()>output.size()) { COMMA_THROW(comma::exception, "size mismatch, output "<<output.size()<<" fft output "<<fft.output_size()); }
        for(std::size_t j=0;j<fft.output_size();j++)
        {
            double a= fft.output[j][0];
            if(logarithmic_output) { a = (a == 0) ? 0 : (std::log10(a)); }
            output[j]=a;
        }
    }
    else
    {
        std::size_t k=0;
        std::size_t step=2;
        std::size_t off=1;
        if(split)
        {
            step=1;
            off=output.size()/2;
        }
        if(2*fft.output_size()>output.size()) { COMMA_THROW(comma::exception, "size mismatch, output "<<output.size()<<" fft output "<<fft.output_size()); }
        for(std::size_t j=0; j<fft.output_size(); j++, k+=step)
        {
            double a= fft.output[j][0];
            if(logarithmic_output) { a = (a == 0) ? 0 : (std::log10(a)); }
            output[k]=a;
            output[k+off]=fft.output[j][1];
        }
    }
}

struct app
{
    app()
    {
    }
    void process(const comma::csv::options& csv, const boost::optional<std::string>& shuffle_fields)
    {
        if(bin_overlap && int(bin_size*(1-*bin_overlap)) <= 0) { COMMA_THROW( comma::exception, "bin size and overlap don't work" ); }
        input_t sample;
        comma::csv::input_stream<input_t> is(std::cin, csv, sample);
        comma::csv::output_stream<output_t> os(std::cout, csv.binary(), true);
        std::string array_sizes="data=";
        array_sizes+=boost::lexical_cast<std::string>(sample.data.size());
        ::shuffle_tied<input_t,output_t> shuffle_tied(is, os, csv, shuffle_fields,array_sizes);
        output_t output;
        while(std::cin.good())
        {
            //read a record
            const input_t* input=is.read();
            if(!input) { break; }
            //calculate
            for(std::size_t bin_offset=0;bin_offset<input_size;bin_offset+=(bin_overlap ? bin_size*(1-*bin_overlap) : bin_size))
            {
                output.reset();
                calculate(&input->data[bin_offset], std::min(bin_size,input_size-bin_offset), output.data);
                //write output
                if(tied)
                    shuffle_tied.append(output);
                else
                    os.write(output);
            }
        }
    }
    std::size_t get_output_size()
    {
        return output_t().data.size();
    }
    void output_format()
    {
        std::cout<<output_t().data.size()<<"d"<<std::endl;
    }
    void output_fields()
    {
        std::cout<<comma::join(comma::csv::names< output_t >(true),',')<<std::endl;
    }
};

template<typename T>
std::ostream& operator<< (std::ostream& o, const std::vector<T>& v)
{
    std::copy(v.begin(), v.end(), std::ostream_iterator<T>(o, " "));
    return o;
}
template<typename T>
void range_check(T value, T min, T max, const char* label)
{
    if(value<min || value>max) { COMMA_THROW(comma::exception, label<<" out of range "<<min<<" to "<<max ); }
}

int main( int argc, char** argv )
{
    comma::command_line_options options( argc, argv, usage );
    try
    {
        comma::csv::options csv(options,"data");
        filter_input= ! options.exists("--no-filter");
        logarithmic_output= ! options.exists("--linear");
        input_size=options.value<std::size_t>("--size");
        magnitude=options.exists("--magnitude");
        real=options.exists("--real");
        split=options.exists("--split");
        bin_size=options.value<std::size_t>("--bin-size", input_size);
        range_check<std::size_t>(bin_size,0,input_size,"bin_size");
        bin_overlap=options.optional<double>("--bin-overlap");
        if(bin_overlap) { range_check<double>(*bin_overlap,0,1,"bin_overlap"); }
        boost::optional<std::string> shuffle_fields=options.optional<std::string>("--shuffle,--shuffle-fields,--shuffled-fields");
        tied=!options.exists("--untied");
        if(shuffle_fields&&!tied) { COMMA_THROW(comma::exception,"--shuffle only works with tied stream (can't specify both --untied and --shuffle"); }
        std::vector<std::string> unnamed=options.unnamed("--verbose,-v,--output-size,--output-format,--output-fields,--no-filter,--linear,--magnitude,--real,--split,--untied",
                                                         "--binary,-b,--fields,-f,--delimiter,-d,--size,--bin-size,--bin-overlap,--shuffle,--shuffle-fields,--shuffled-fields");
        if(unnamed.size() != 0) { COMMA_THROW(comma::exception, "invalid option(s): " << unnamed ); }
        app app;
        if(options.exists("--output-size")) { std::cout<< app.get_output_size() << std::endl; return 0; }
        if(options.exists("--output-format")) { app.output_format(); return 0; }
        if(options.exists("--output-fields")) { app.output_fields(); return 0; }
        app.process(csv,shuffle_fields);
        return 0;
    }
    catch( std::exception& ex )
    {
        std::cerr << comma::verbose.app_name() << ": " << ex.what() << std::endl;
    }
    catch( ... )
    {
        std::cerr << comma::verbose.app_name() << ": " << "unknown exception" << std::endl;
    }
    return 1;
}