/*
 * @file test_gnss_ice.cpp
 * @brief Iterative GPS Range/Phase Estimator with collected data
 * @author Ryan Watson
 */

// GTSAM related includes.
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/gnssNavigation/GnssData.h>
#include <gtsam/gnssNavigation/GnssTools.h>
#include <gtsam/gnssNavigation/nonBiasStates.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/gnssNavigation/GNSSMultiModalFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

// LibCluster
#include <libcluster/merge.h>
#include <libcluster/probutils.h>
#include <libcluster/libcluster.h>
#include <libcluster/distributions.h>

// BOOST
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

// GPSTK
#include <gpstk/ConfDataReader.hpp>

// STD
#include <chrono>
#include <fstream>
#include <iostream>
#include <algorithm>

using namespace std;
using namespace gtsam;
using namespace gpstk;
using namespace boost;
using namespace merge;
using namespace std::chrono;
using namespace libcluster;
using namespace distributions;
namespace NM = gtsam::noiseModel;
namespace po = boost::program_options;
typedef noiseModel::Diagonal diagNoise;

// Intel Threading Building Block
#include <tbb/tbb.h>

using symbol_shorthand::X; // nonBiasStates ( dx, dy, dz, trop, cb )
using symbol_shorthand::G;   // bias states ( Phase Biases )

// remove this later
Eigen::MatrixXd Tw_inv;
Eigen::RowVectorXd means;
// remove this later

int main(int argc, char* argv[])
{
        // define std out print color
        bool skipped_update(false);
        vector<int> prn_vec;
        vector<int> factor_count_vec;
        vector<rnxData> data;
        const string red("\033[0;31m");
        const string green("\033[0;32m");
        string confFile, gnssFile, station;
        double xn, yn, zn, range, phase, rho, gnssTime, prev_time;
        int ob_count(0), state_skip(0), tmp(0);
        int startKey(0), currKey, startEpoch(0), svn, numBatch(0), state_count(0), update_count(0);
        int nThreads(-1), phase_break, break_count(0), nextKey, factor_count(-1), res_count(-1);
        bool printECEF, printENU, printAmb, first_ob(true);
        Eigen::MatrixXd residuals, all_residuals;
        vector<mixtureComponents> globalMixtureModel;

        string res_str = "all.residuals";
        ofstream res_os(res_str);

        string res_out_str = "outliers.residuals";
        ofstream res_out_os(res_out_str);

        cout.precision(12);

        po::options_description desc("Available options");
        desc.add_options()
                ("help,h", "Print help message")
                ("confFile,c", po::value<string>(&confFile)->default_value(""),
                "Input config file" );

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
        po::notify(vm);

        ConfDataReader confReader;
        confReader.open(confFile);

        if (confFile.empty() ) {
                cout << red << "\n\n Currently, you need to provide a conf file \n"
                     << "\n\n"  << green << desc << endl;
        }

        while ( (station = confReader.getEachSection()) != "" )
        {
                xn = confReader.fetchListValueAsDouble("nominalECEF",station);
                yn = confReader.fetchListValueAsDouble("nominalECEF",station);
                zn = confReader.fetchListValueAsDouble("nominalECEF",station);
                printENU = confReader.getValueAsBoolean("printENU", station);
                printAmb = confReader.getValueAsBoolean("printAmb", station);
                printECEF = confReader.getValueAsBoolean("printECEF", station);
                gnssFile = confReader("dataFile", station);
        }

        Point3 nomXYZ(xn, yn, zn);
        Point3 prop_xyz = nomXYZ;

        try {data = readGNSS_SingleFreq(gnssFile); }
        catch(std::exception& e)
        {
                cout << red << "\n\n Cannot read GNSS data file " << endl;
                exit(1);
        }

        std::auto_ptr<tbb::task_scheduler_init> init;
        if(nThreads > 0) {
                init.reset(new tbb::task_scheduler_init(nThreads));
        }
        else
                cout << green << " \n\n Using threads for all processors" << endl;

        ISAM2DoglegParams doglegParams;
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.01;
        parameters.relinearizeSkip = 1000;
        ISAM2 isam(parameters);

        double output_time = 0.0;
        double rangeWeight = 2.5;
        double phaseWeight = 0.25;

        ifstream file(gnssFile.c_str());
        string value;

        nonBiasStates prior_nonBias = (gtsam::Vector(5) << 0.0, 0.0, 0.0, 0.0, 0.0).finished();

        phaseBias bias_state(Z_1x1);
        gnssStateVector phase_arc(Z_34x1);
        gnssStateVector bias_counter(Z_34x1);
        for (int i=1; i<34; i++) {bias_counter(i) = bias_counter(i-1) + 10000; }

        nonBiasStates initEst(Z_5x1);
        nonBiasStates between_nonBias_State(Z_5x1);

        Values initial_values, result;

        noiseModel::Diagonal::shared_ptr nonBias_InitNoise = noiseModel::Diagonal::Variances((gtsam::Vector(5) << 0.2, 0.2, 0.2, 3e6, 1e-1).finished());

        noiseModel::Diagonal::shared_ptr nonBias_Reset = noiseModel::Diagonal::Variances((gtsam::Vector(5) << 3e4, 3e4, 3e4, 3e6, 1e-1).finished());

        noiseModel::Diagonal::shared_ptr nonBias_ProcessNoise = noiseModel::Diagonal::Variances((gtsam::Vector(5) << 1.0, 1.0, 1.0, 1e3, 1e-3).finished());

        noiseModel::Diagonal::shared_ptr initNoise = noiseModel::Diagonal::Variances((gtsam::Vector(1) << 3e6).finished());

        NonlinearFactorGraph *graph = new NonlinearFactorGraph();

        residuals.setZero(1000,2);

        // init. mixture model.
        // Init this from file later
        Eigen::RowVectorXd m(2);
        m << 0.0, 0.0;

        // Add comp 1.
        Eigen::MatrixXd c(2,2);
        c<< std::pow(rangeWeight,2), 0.0, 0.0, std::pow(phaseWeight,2);
        globalMixtureModel.push_back(boost::make_tuple(0, 0, 0.0, m, c));

        int lastStep = get<0>(data.back());

        std::vector<int> num_obs (1000, 0);

        for(unsigned int i = startEpoch; i < data.size(); i++ ) {

                auto start = high_resolution_clock::now();

                double gnssTime = get<0>(data[i]);
                int currKey = get<1>(data[i]);
                if (first_ob) {
                        first_ob=false;
                        startKey = currKey;
                        graph->add(PriorFactor<nonBiasStates>(X(currKey), initEst,  nonBias_InitNoise));
                        ++factor_count;
                        initial_values.insert(X(currKey), initEst);
                }
                int nextKey = get<1>(data[i+1]);
                int svn = get<2>(data[i]);
                Point3 satXYZ = get<3>(data[i]);
                double rho = get<4>(data[i]);
                double range = get<5>(data[i]);
                double phase = get<6>(data[i]);
                double phase_break = get<7>(data[i]);

                ob_count = ob_count + 1;

                gtsam::Vector2 obs;
                obs << range-rho, phase-rho;

                if ( currKey == startKey || phase_arc[svn]!=phase_break)
                {
                        bias_state[0] = phase-range;
                        if (currKey > startKey) { bias_counter[svn] = bias_counter[svn] +1; }
                        initial_values.insert(G(bias_counter[svn]), bias_state);

                        graph->add(boost::make_shared<PriorFactor<phaseBias> >(G(bias_counter[svn]), bias_state,  initNoise));

                        phase_arc[svn] = phase_break;
                        ++factor_count;
                }

                graph->add(boost::make_shared<GNSSMultiModalFactor>(X(currKey), G(bias_counter[svn]), obs, satXYZ, nomXYZ, globalMixtureModel));

                prn_vec.push_back(svn);
                factor_count_vec.push_back(++factor_count);

                if (currKey != nextKey && nextKey != 0) {

                        if (currKey > startKey ) {
                                if ( lastStep == nextKey ) { break; }

                                if (state_count < 600 || state_count > (lastStep-600))
                                {
                                        graph->add(PriorFactor<nonBiasStates>(X(currKey), initEst,  nonBias_InitNoise));

                                        ++factor_count;
                                }

                                graph->add(PriorFactor<nonBiasStates>(X(currKey), prior_nonBias, nonBias_Reset));
                                ++factor_count;

                        }

                        isam.update(*graph, initial_values);
                        result = isam.calculateEstimate();

                        // Only learn from residuals which don't agree with the model
                        int ind(0);
                        double prob, probMax(0.0);
                        gtsam::Matrix cov_min(2,2);
                        Eigen::RowVectorXd mean_min(2);
                        Eigen::VectorXd res(2);

                        for (int j = 0; j<factor_count_vec.size(); j++)
                        {

                                res = graph->at(factor_count_vec[j])->residual(result);

                                for (int k=0; k<globalMixtureModel.size(); k++)
                                {
                                        merge::mixtureComponents mixtureComp = globalMixtureModel[k];

                                        Eigen::RowVectorXd mean = mixtureComp.get<3>();

                                        double quadform  = (res).transpose() * (mixtureComp.get<4>()).inverse() * (res);
                                        double norm = std::pow(std::sqrt(2 * M_PI),-1) * std::pow((mixtureComp.get<4>()).determinant(), -0.5);

                                        prob =  norm * exp(-0.5 * quadform);

                                        if (prob >= probMax)
                                        {
                                                ind = k;
                                                probMax = prob;
                                                cov_min = mixtureComp.get<4>();
                                                mean_min = mixtureComp.get<3>();
                                        }
                                }

                                res_os << res(0) << " " << res(1) << endl;

                                // Use z-test to see if residuals is considered an outlier
                                double z_r = (res(0))/std::sqrt(cov_min(0,0));
                                double z_p = (res(1))/std::sqrt(cov_min(1,1));

                                // only consider residuals more than 'n' stds from model
                                if (std::abs(z_r) > 3.0 || std::abs(z_p) > 3.0)
                                {
                                        ++res_count;
                                        if (res_count > 999 )
                                        {
                                                residuals.conservativeResize(residuals.rows()+1, residuals.cols());

                                                residuals.row(residuals.rows()-1) = res.transpose();

                                                res_out_os << res(0) << " " << res(1) << endl;

                                                graph->remove(factor_count_vec[j]);
                                                ob_count-=1;
                                        }
                                        else
                                        {
                                                residuals.block(res_count,0,1,2) << res.transpose();

                                                res_out_os << res(0) << " " << res(1) << endl;

                                                graph->remove(factor_count_vec[j]);
                                                ob_count-=1;
                                        }
                                }
                                else
                                {
                                        // if obs. match a model. Update the number of points in
                                        // that cluster.
                                        num_obs.at(ind) = num_obs.at(ind)+1;
                                }
                        }

                        initial_values.clear();
                        if (ob_count >= 5) {

                                ++tmp;
                                ++state_count;

                                isam.update(*graph);
                                isam.update();
                                result = isam.calculateEstimate();

                                prior_nonBias = result.at<nonBiasStates>(X(currKey));
                                Point3 delta_xyz = (gtsam::Vector(3) << prior_nonBias.x(), prior_nonBias.y(), prior_nonBias.z()).finished();
                                prop_xyz = nomXYZ - delta_xyz;

                                if (printECEF) {
                                        cout << "xyz " << gnssTime << " " << prop_xyz.x() << " " << prop_xyz.y() << " " << prop_xyz.z() << endl;
                                }

                                if (printENU) {
                                        Point3 enu = xyz2enu(prop_xyz, nomXYZ);
                                        cout << "enu " << gnssTime << " " << enu.x() << " " << enu.y() << " " << enu.z() << endl;
                                }

                                if (printAmb) {
                                        cout << "gps " << " " << gnssTime << " ";
                                        for (int k=0; k<prn_vec.size(); k++) {
                                                cout << result.at<phaseBias>(G(bias_counter[prn_vec[k]])) << " ";
                                        }
                                        cout << endl;
                                }
                                state_skip = 1;
                                prev_time = gnssTime;

                        }
                        else{
                                ++state_skip;
                        }
                        output_time = output_time +1;

                        factor_count_vec.clear();
                        factor_count = -1;

                        if (res_count > 1000)
                        {

                                StickBreak weights;
                                vector<GaussWish> clusters;
                                Eigen::MatrixXd qZ;

                                learnVDP(residuals, qZ, weights, clusters);

                                // update the number of obs in each component
                                globalMixtureModel = updateObs(globalMixtureModel, num_obs);
                                std::fill(num_obs.begin(), num_obs.end(), 0);

                                // merge the curr and prior mixture models.
                                globalMixtureModel = mergeMixtureModel(residuals, qZ, globalMixtureModel, clusters, weights, 0.05, 20);

                                cout << "\n\n\n\n\n\n" << endl;
                                cout << "----------------- Merged MODEL ----------------" << endl;
                                for (int i=0; i<globalMixtureModel.size(); i++)
                                {
                                        mixtureComponents mc = (globalMixtureModel)[i];
                                        auto cov = mc.get<4>();
                                        cout << mc.get<0>() << " " << mc.get<1>() << " "  <<  mc.get<2>() << "    " << mc.get<3>() <<"     "<< cov(0,0) << " " << cov(0,1) << " " << cov(1,1) <<"     "<<"\n\n" << endl;
                                }

                                residuals.setZero(1000,2);
                                res_count = -1;

                        }

                        graph->resize(0);
                        prn_vec.clear();

                        auto stop = high_resolution_clock::now();
                        auto duration = duration_cast<microseconds>(stop - start);

                        cout << "Delta time: "
                             << duration.count() << " microseconds" << endl;

                        initial_values.insert(X(nextKey), prior_nonBias);
                        ob_count = 0;
                }

        }

        cout << "\n\n\n\n\n\n" << endl;
        cout << "----------------- Final Incremental Mixture MODEL ----------------" << endl;
        for (int i=0; i<globalMixtureModel.size(); i++)
        {
                mixtureComponents mc = globalMixtureModel[i];
                auto cov = mc.get<4>();
                cout << mc.get<0>() << " " << mc.get<1>() << " "  <<  mc.get<2>() << "    " << mc.get<3>() <<"     "<< cov(0,0) << " " << cov(0,1) << " " << cov(1,1) <<"     "<<"\n\n" << endl;
        }

        return 0;
}


/*
 * Copyright (c) Facebook, Inc. and its affiliates.
 *
 * 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 <thrift/lib/cpp/test/loadgen/RNG.h>

#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include <folly/ThreadLocal.h>

namespace apache {
namespace thrift {
namespace loadgen {

const bool RNG::has_fixed_range;

namespace {

// We use seedRNG just to choose seeds for new thread-local RNGs
boost::mt19937 seedRNG;

struct RNGImpl {
  RNGImpl() : rng(), rngWrapper(&rng) {
    // Pick a value from seedRNG to seed this new rng
    rng.seed(seedRNG());
  }

  /// The actual RNG
  boost::mt19937 rng;

  /**
   * An RNG wrapper object.
   *
   * We store a thread local RNG rather than always creating RNG objects onthe
   * fly so that RNG::getRNG() can return a reference rather than a new object.
   * Many of the boost random APIs require a reference, so this allows callers
   * to pass the result of RNG::getRNG() directly to boost, rather than having
   * to create a local RNG object on their stack.
   */
  RNG rngWrapper;
};

folly::ThreadLocal<RNGImpl> threadLocalRNG;

} // unnamed namespace

RNG& RNG::getRNG() {
  return threadLocalRNG.get()->rngWrapper;
}

void RNG::setGlobalSeed(result_type s) {
  seedRNG.seed(s);
}

uint32_t RNG::getU32() {
  boost::uniform_int<uint32_t> distribution;
  return distribution(getRNG());
}

uint32_t RNG::getU32(uint32_t max) {
  boost::uniform_int<uint32_t> distribution(0, max);
  return distribution(getRNG());
}

uint32_t RNG::getU32(uint32_t min, uint32_t max) {
  boost::uniform_int<uint32_t> distribution(min, max);
  return distribution(getRNG());
}

double RNG::getReal() {
  boost::uniform_real<double> distribution;
  return distribution(getRNG());
}

double RNG::getReal(double min, double max) {
  boost::uniform_real<double> distribution(min, max);
  return distribution(getRNG());
}

double RNG::getLogNormal(double mean, double sigma) {
  // If the sigma is negative, default to half of the mean.
  // This produces fairly nicely shaped distributions
  if (sigma < 0) {
    sigma = mean / 2;
  }
  boost::lognormal_distribution<double> dist(mean, sigma);
  boost::variate_generator<RNG, boost::lognormal_distribution<double>> gen(
      getRNG(), dist);
  return gen();
}

} // namespace loadgen
} // namespace thrift
} // namespace apache


#pragma once
#include "hashmap.hpp"
#include "json11.hpp"
#include "pulse.hpp"
// #include "dipole.hpp"
#include <Eigen/Dense>
#include <atomic>
#include <chrono>
#include <complex>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <gperftools/profiler.h>
#include <iomanip>
#include <iostream>
#include <openssl/md5.h>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <thread> // std::thread
#include <unistd.h>

#define NSYS 2

using namespace json11;
using namespace std;
using namespace Eigen;

typedef unsigned long long int ullint;
typedef long long int llint;
typedef Eigen::Matrix<complex<double>, NSYS, NSYS> MatrixNcd;
typedef Eigen::Matrix<ullint, Eigen::Dynamic, Eigen::Dynamic> nullmat;
typedef std::vector<MatrixNcd, Eigen::aligned_allocator<MatrixNcd>> nNNmat;
typedef Eigen::Matrix<int32_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXimat_r;
typedef Eigen::Matrix<uint16_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXuimat_r;
typedef Eigen::Matrix<uint16_t, 1, Eigen::Dynamic, Eigen::RowMajor> XXuivet_r;


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-ieee
    Function object implementing minnummag capabilities

    Returns the input value which have the least absolute value, ignoring nan.

    @par Semantic:

    @code
    auto r = minnummag(x,y);
    @endcode

    is similar to:

    @code
    auto r =  is_nan(x) ? y : (is_nan(y) ? x : minmag(x, y));;
    @endcode

    @see min, minnum, minmag, is_nan

  **/
  Value minnummag(Value const & x, Value const& y);
} }
#endif

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

#endif


#include <voxelized_geometry_tools/cpu_pointcloud_voxelization.hpp>

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <atomic>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <memory>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/voxel_grid.hpp>
#include <voxelized_geometry_tools/collision_map.hpp>
#include <voxelized_geometry_tools/pointcloud_voxelization_interface.hpp>

namespace voxelized_geometry_tools
{
namespace pointcloud_voxelization
{
VoxelizerRuntime CpuPointCloudVoxelizer::DoVoxelizePointClouds(
    const CollisionMap& static_environment, const double step_size_multiplier,
    const PointCloudVoxelizationFilterOptions& filter_options,
    const std::vector<PointCloudWrapperPtr>& pointclouds,
    CollisionMap& output_environment) const
{
  const std::chrono::time_point<std::chrono::steady_clock> start_time =
      std::chrono::steady_clock::now();
  // Pose of grid G in world W.
  const Eigen::Isometry3d& X_WG = static_environment.GetOriginTransform();
  // Get grid resolution and size parameters.
  const auto& grid_size = static_environment.GetGridSizes();
  const double cell_size = static_environment.GetResolution();
  const double step_size = cell_size * step_size_multiplier;
  // For each cloud, raycast it into its own "tracking grid"
  VectorCpuVoxelizationTrackingGrid tracking_grids(
          pointclouds.size(),
          CpuVoxelizationTrackingGrid(
              X_WG, grid_size, CpuVoxelizationTrackingCell()));
  for (size_t idx = 0; idx < pointclouds.size(); idx++)
  {
    const PointCloudWrapperPtr& cloud_ptr = pointclouds.at(idx);
    CpuVoxelizationTrackingGrid& tracking_grid = tracking_grids.at(idx);
    RaycastPointCloud(*cloud_ptr, step_size, tracking_grid);
  }
  const std::chrono::time_point<std::chrono::steady_clock> raycasted_time =
      std::chrono::steady_clock::now();
  // Combine & filter
  CombineAndFilterGrids(filter_options, tracking_grids, output_environment);
  const std::chrono::time_point<std::chrono::steady_clock> done_time =
      std::chrono::steady_clock::now();
  return VoxelizerRuntime(
      std::chrono::duration<double>(raycasted_time - start_time).count(),
      std::chrono::duration<double>(done_time - raycasted_time).count());
}

void CpuPointCloudVoxelizer::RaycastPointCloud(
    const PointCloudWrapper& cloud, const double step_size,
    CpuVoxelizationTrackingGrid& tracking_grid) const
{
  // Get X_GW, the transform from grid origin to world
  const Eigen::Isometry3d& X_GW = tracking_grid.GetInverseOriginTransform();
  // Get X_WC, the transform from world to the origin of the pointcloud
  const Eigen::Isometry3d& X_WC = cloud.GetPointCloudOriginTransform();
  // Transform X_GC, transform from grid origin to the origin of the pointcloud
  const Eigen::Isometry3d X_GC = X_GW * X_WC;
  // Get the pointcloud origin in grid frame
  const Eigen::Vector4d p_GCo = X_GC * Eigen::Vector4d(0.0, 0.0, 0.0, 1.0);
  // Get the max range
  const double max_range = cloud.MaxRange();
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (int64_t idx = 0; idx < cloud.Size(); idx++)
  {
    // Location of point P in frame of camera C
    const Eigen::Vector4d p_CP = cloud.GetPointLocationVector4d(idx);
    // Skip invalid points marked with NaN or infinity
    if (std::isfinite(p_CP(0)) && std::isfinite(p_CP(1)) &&
        std::isfinite(p_CP(2)))
    {
      // Location of point P in grid G
      const Eigen::Vector4d p_GP = X_GC * p_CP;
      // Ray from camera to point
      const Eigen::Vector4d ray = p_GP - p_GCo;
      const double ray_length = ray.norm();
      // Step along ray
      const int32_t num_steps =
          std::max(1, static_cast<int32_t>(std::floor(ray_length / step_size)));
      common_robotics_utilities::voxel_grid::GridIndex last_index(-1, -1, -1);
      bool ray_crossed_grid = false;
      for (int32_t step = 0; step < num_steps; step++)
      {
        const double ratio =
            static_cast<double>(step) / static_cast<double>(num_steps);
        if ((ratio * ray_length) > max_range)
        {
          // We've gone beyond max range of the sensor
          break;
        }
        const Eigen::Vector4d p_GQ = p_GCo + (ray * ratio);
        const common_robotics_utilities::voxel_grid::GridIndex index =
            tracking_grid.LocationInGridFrameToGridIndex4d(p_GQ);
        // We don't want to double count in the same cell multiple times
        if (!(index == last_index))
        {
          auto query = tracking_grid.GetMutable(index);
          // We must check to see if the query is within bounds.
          if (query)
          {
            ray_crossed_grid = true;
            query.Value().seen_free_count.fetch_add(1);
          }
          else if (ray_crossed_grid)
          {
            // We've left the grid and there's no reason to keep going.
            break;
          }
        }
        last_index = index;
      }
      // Set the point itself as filled, if it is in range
      if (ray_length <= max_range)
      {
        const common_robotics_utilities::voxel_grid::GridIndex index =
              tracking_grid.LocationInGridFrameToGridIndex4d(p_GP);
        auto query = tracking_grid.GetMutable(index);
        // We must check to see if the query is within bounds.
        if (query)
        {
          query.Value().seen_filled_count.fetch_add(1);
        }
      }
    }
  }
}

void CpuPointCloudVoxelizer::CombineAndFilterGrids(
    const PointCloudVoxelizationFilterOptions& filter_options,
    const VectorCpuVoxelizationTrackingGrid& tracking_grids,
    CollisionMap& filtered_grid) const
{
  // Because we want to improve performance and don't need to know where in the
  // grid we are, we can take advantage of the dense backing vector to iterate
  // through the grid data, rather than the grid cells.
  auto& filtered_grid_backing_store = filtered_grid.GetMutableRawData();
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t voxel = 0; voxel < filtered_grid_backing_store.size(); voxel++)
  {
    auto& current_cell = filtered_grid_backing_store.at(voxel);
    // Filled cells stay filled, we don't work with them.
    // We only change cells that are unknown or empty.
    if (current_cell.Occupancy() <= 0.5)
    {
      int32_t seen_filled = 0;
      int32_t seen_free = 0;
      for (size_t idx = 0; idx < tracking_grids.size(); idx++)
      {
        const CpuVoxelizationTrackingCell& grid_cell =
            tracking_grids.at(idx).GetImmutableRawData().at(voxel);
        const int32_t free_count = grid_cell.seen_free_count.load();
        const int32_t filled_count = grid_cell.seen_filled_count.load();
        const SeenAs seen_as =
            filter_options.CountsSeenAs(free_count, filled_count);
        if (seen_as == SeenAs::FREE)
        {
          seen_free += 1;
        }
        else if (seen_as == SeenAs::FILLED)
        {
          seen_filled += 1;
        }
      }
      if (seen_filled > 0)
      {
        // If any camera saw something here, it is filled.
        current_cell.Occupancy() = 1.0;
      }
      else if (seen_free >= filter_options.NumCamerasSeenFree())
      {
        // Did enough cameras see this empty?
        current_cell.Occupancy() = 0.0;
      }
      else
      {
        // Otherwise, it is unknown.
        current_cell.Occupancy() = 0.5;
      }
    }
  }
}
}  // namespace pointcloud_voxelization
}  // namespace voxelized_geometry_tools


// Copyright (c) 2020 Vsevolod Vlaskine

/// @author vsevolod vlaskine

#include <cmath>
#include <deque>
#include <fstream>
#include <sstream>
#include <tuple>
#include <Eigen/Core>
#include <comma/base/exception.h>
#include <comma/base/types.h>
#include <comma/csv/stream.h>
#include <comma/csv/traits.h>
#include <comma/io/stream.h>
#include <comma/name_value/parser.h>
#include <comma/string/string.h>
#include "../../../point_cloud/voxel_map.h"
#include "../../../visiting/eigen.h"
#include "visit.h"

namespace snark { namespace points_calc { namespace visit {

std::string traits::usage()
{
    std::ostringstream oss;
    oss << "    visit\n"
        << "        default input fields: \"x,y,z\" or \"point/x,point/y,point/z\" ('x' and 'point/x' both ok)\n"
        << "        options\n"
        << "            --at=<filename>[;<csv-options>]; seed points from which visiting starts\n"
        << "            --field-of-view,--fov=<radians>[,relative]; default=3.141592653589793; do not visit input points outside of field of view\n"
        << "                relative: angle calculated between normal at the current input point and direction from the current\n"
        << "                          input point to next-searched input point (\"search direction\")\n"
        << "                          limitation: relative works for input points with normals only for now\n"
        << "                default (\"absolute\"): angle calculated between normal at the last 'at' point and \"search direction\"\n"
        << "            --radius=[<meters>]; how far to visit\n"
        << "            --resolution=<meters>; ignore points farther away the current search point than <meters>\n"
        << "            --unvisited-id,--unvisited=<id>; default=0\n"
        << "            --visited-id,--visited=<id>; default=1\n"
        << std::endl
        << "        examples\n"
        << "            csv-paste 'line-number;size=100' 'line-number;size=100;index' value=0 --head=10000 \\\n"
        << "                | points-calc visit --at <( echo 50,50,0,1,0,0; echo 60,50,0,0,1,0 )';fields=x,y,z,normal' \\\n"
        << "                                    --resolution 1 \\\n"
        << "                                    --fov $( math-deg2rad 20 ) \\\n"
        << "                                    --radius 20  \\\n"
        << "                | view-points '-;fields=x,y,z,id' \\\n"
        << std::endl;
    return oss.str();
}

struct input
{
    Eigen::Vector3d point;
    Eigen::Vector3d normal;
    comma::uint32 block;
    comma::uint32 id;
    input( const Eigen::Vector3d& point = Eigen::Vector3d( 0, 0, 0 ), const Eigen::Vector3d& normal = Eigen::Vector3d( 0, 0, 0 ), comma::uint32 block = 0, comma::uint32 id = 0 ): point( point ), normal( normal ), block( block ), id( id ) {}
    input( comma::uint32 id ): point( 0, 0, 0 ), normal( 0, 0, 0 ), block( 0 ), id( id ) {}
};

struct record
{
    visit::input input;
    std::string buffer;
    comma::uint32 visited_id;
    comma::uint32 visited_by;

    record( const visit::input& input = visit::input(), const std::string& buffer = std::string(), comma::uint32 visited_id = 0 ): input( input ), buffer( buffer ), visited_id( visited_id ), visited_by( 0 ) {}
};

} } } // namespace snark { namespace points_calc { namespace visit {

namespace comma { namespace visiting {

template <> struct traits< snark::points_calc::visit::input >
{
    template < typename K, typename V > static void visit( const K&, snark::points_calc::visit::input& p, V& v )
    {
        v.apply( "point", p.point );
        v.apply( "normal", p.normal );
        v.apply( "block", p.block );
        v.apply( "id", p.id );
    }

    template < typename K, typename V > static void visit( const K&, const snark::points_calc::visit::input& p, V& v )
    {
        v.apply( "point", p.point );
        v.apply( "normal", p.normal );
        v.apply( "block", p.block );
        v.apply( "id", p.id );
    }
};

} } // namespace comma { namespace visiting {

namespace snark { namespace points_calc { namespace visit {

std::string traits::input_fields() { return comma::join( comma::csv::names< snark::points_calc::visit::input >( true ), ',' ); }

std::string traits::input_format() { return comma::csv::format::value< snark::points_calc::visit::input >(); }

std::string traits::output_fields() { return "visited/id"; }

std::string traits::output_format() { return "ui"; }

typedef std::deque< visit::record* > voxel_t; // todo: watch performance

int traits::run( const comma::command_line_options& options )
{
    comma::uint32 unvisited_id = options.value( "--unvisited-id,--unvisited", 0 );
    comma::uint32 visited_id = options.value( "--visited-id,--visited", 1 );
    auto handle_fields = [&]( const std::string& fields ) -> std::pair< std::string, bool >
    {
        if( fields.empty() ) { return std::make_pair( std::string( "point/x,point/y,point/z" ), false ); }
        auto v = comma::split( fields, ',' );
        bool has_normal = false;
        for( auto& f: v )
        {
            if( f == "normal" || f == "normal/x" || f == "normal/y" || f == "normal/z" ) { has_normal = true; }
            else if( f == "x" || f == "y" || f == "z" ) { f = "point/" + f; }
        }
        return std::make_pair( comma::join( v, ',' ), has_normal );
    };
    comma::csv::options csv( options );
    bool stdin_has_normal = false;
    std::tie( csv.fields, stdin_has_normal ) = handle_fields( csv.fields );
    comma::csv::input_stream< input > istream( std::cin, csv, input( unvisited_id ) );
    boost::optional< double > radius = options.optional< double >( "--radius" );
    double squared_radius = radius ? *radius * *radius : 0;
    double resolution = options.value< double >( "--resolution" );
    double squared_resolution = resolution * resolution;
    std::deque< record > records;
    typedef snark::voxel_map< voxel_t, 3 > voxels_t;
    voxels_t voxels( Eigen::Vector3d( resolution, resolution, resolution ) );
    std::deque< const input* > queue; // todo
    bool match_id = csv.has_field( "id" );
    std::string at_options = options.value< std::string >( "--at" );
    comma::csv::options at_csv = comma::name_value::parser( "filename", ';', '=', false ).get< comma::csv::options >( at_options );
    bool at_has_normal = false;
    std::tie( at_csv.fields, at_has_normal ) = handle_fields( at_csv.fields );
    comma::io::istream is( at_csv.filename, at_csv.binary() ? comma::io::mode::binary : comma::io::mode::ascii );
    comma::csv::input_stream< input > at_stream( *is, at_csv, input( visited_id ) );
    const auto& s = comma::split( options.value( "--field-of-view,--fov", boost::lexical_cast< std::string >( M_PI ) ), ',', true );
    double fov = boost::lexical_cast< double >( s[0] );
    double fov_threshold = std::cos( fov / 2 );
    bool fov_relative = false;
    switch( s.size() )
    {
        case 1:
            fov_relative = false;
            break;
        case 2:
            if( s[1] != "relative" ) { COMMA_THROW( comma::exception, "visit: expected: --field-of-view=<angle>[,relative]; got: '" << options.value< std::string >( "--field-of-view,--fov" ) << "'" ); }
            fov_relative = true;
            if( at_has_normal ) { break; }
            COMMA_THROW( comma::exception, "visit: if field of view relative, input fields should have normal" );
        default:
            COMMA_THROW( comma::exception, "visit: expected: --field-of-view=<angle>[,relative]; got: '" << options.value< std::string >( "--field-of-view,--fov" ) << "'" );
    }
    if( fov_relative && !stdin_has_normal ) { COMMA_THROW( comma::exception, "visit: relative field of view implemented only for input points with normals for now" ); }
    input origin;
    unsigned int visited_by = 0; // uber-quick and dirty
    auto output_block = [&]()
    {
        for( const auto& r: records )
        {
            std::cout.write( &r.buffer[0], r.buffer.size() );
            if( csv.binary() ) { std::cout.write( reinterpret_cast< const char* >( &r.visited_id ), 4 ); }
            else { std::cout << csv.delimiter << r.visited_id << std::endl; }
        }
        if( csv.flush ) { std::cout.flush(); }
    };
    auto visit_record = [&]( record& r, const visit::input& i )
    {
        if( r.visited_by == visited_by && r.visited_id != unvisited_id ) { return; }
        if( match_id && r.input.id != i.id ) { return; }
        if( radius && ( r.input.point - origin.point ).squaredNorm() > squared_radius ) { return; }
        const auto& d = r.input.point - i.point;
        if( d.squaredNorm() > squared_resolution ) { return; }
        if( fov_relative ) { if( ( stdin_has_normal ? i : origin ).normal.normalized().dot( d.normalized() ) < fov_threshold ) { return; } }
        else { if( at_has_normal && origin.normal.normalized().dot( ( r.input.point - origin.point ).normalized() ) <= fov_threshold ) { return; } }
        r.input.id = r.visited_id = i.id;
        r.visited_by = visited_by;
        queue.push_back( &r.input );
    };
    auto visit_at = [&]( const visit::input& input )
    {
        auto it = voxels.touch_at( input.point ); // quick and dirty
        voxels_t::index_type i;
        for( i[0] = it->first[0] - 1; i[0] < it->first[0] + 2; ++i[0] )
        {
            for( i[1] = it->first[1] - 1; i[1] < it->first[1] + 2; ++i[1] )
            {
                for( i[2] = it->first[2] - 1; i[2] < it->first[2] + 2; ++i[2] )
                {
                    auto git = voxels.find( i );
                    if( git != voxels.end() ) { for( auto& r: git->second ) { visit_record( *r, input ); } }
                }
            }
        }
    };
    auto handle_block = [&]()
    {
        static boost::optional< input > last;
        auto visit_all_at = [&]( const visit::input& input )
        {
            visit_at( input );
            while( !queue.empty() ) { visit_at( *queue.front() ); queue.pop_front(); }
        };
        visited_by = 0; // uber-quick and dirty
        if( last ) { visit_all_at( *last ); }
        while( at_stream.ready() || is->good() )
        {
            const input* p = at_stream.read();
            if( !p ) { break; }
            origin = *p; // quick and dirty
            if( p->block != records[0].input.block ) { last = *p; break; } // todo: matching block management
            visit_all_at( *p );
            ++visited_by; // uber-quick and dirty
        }
        output_block();
        voxels.clear();
        records.clear();
    };
    while( istream.ready() || std::cin.good() )
    {
        const input* p = istream.read();
        if( !p || ( !records.empty() && records[0].input.block != p->block ) ) { handle_block(); }
        if( !p ) { break; }
        records.push_back( record( *p, istream.last(), unvisited_id ) );
        auto voxel = voxels.touch_at( records.back().input.point );
        voxel->second.push_back( &records.back() );
    }
    return 0;
}

} } } // namespace snark { namespace points_calc { namespace visit {


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
///
/// Code implementing isotopic scattering according to Tamura's
/// formula:
/// S.-I. Tamura, Isotope scattering of dispersive phonons in Ge,
///    Phys. Rev. B 27 (1983) 858–866
/// A. Kundu, N. Mingo, D.A. Broido, D.A. Stewart, Role of light and
///    heavy embedded nanoparticles on the thermal conductivity of SiGe
///    alloys, Phys. Rev. B 84 (2011) 125426.
/// The same method is used for alloys in the virtual crystal
/// approximation.
/// The classes and functions in this file are modelled after those
/// declared in processes.hpp. There are, however, some differences.
/// The most important aspects are:
/// - Conservation of momentum is not enforced for two-phonon
/// processes.
/// - The Gaussian factor of each process is computed when the object
/// is built.
/// - The matrix element of each process is not stored in the object.

#include <cstddef>
#include <cmath>
#include <array>
#include <vector>
#include <boost/mpi.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/serialization/array.hpp>
#include <structures.hpp>
#include <qpoint_grid.hpp>
#include <boost/math/distributions/normal.hpp>

// Forward declarations of elements documented later on.
// Note that serialization of std::vectors of objects without
// a default constructor is broken in Boost version 1.58.0. See:
// http://stackoverflow.com/a/30437359/85371
// https://svn.boost.org/trac/boost/ticket/11342
// This will prevent ALMA from compiling. We check for that
// specific version in our cmake configuration.

namespace alma {
/// Compute the 25th and 75th percentiles of log(sigma)
///
/// @param[in] sigma an Eigen array with all broadening parameters
/// @return an array containing the 25th and 75th percentiles
std::array<double, 2> calc_percentiles_log(
    const Eigen::Ref<const Eigen::ArrayXXd>& sigma);
/// Representation of a elastic two-phonon process.
class Twoph_process {
private:
    friend class boost::serialization::access;

    friend void save_bulk_hdf5(const char* filename,
                               const std::string& description,
                               const Crystal_structure& cell,
                               const Symmetry_operations& symmetries,
                               const Gamma_grid& grid,
                               const std::vector<Threeph_process>& processes,
                               const boost::mpi::communicator& comm);

    friend std::tuple<std::string,
                      std::unique_ptr<Crystal_structure>,
                      std::unique_ptr<Symmetry_operations>,
                      std::unique_ptr<Gamma_grid>,
                      std::unique_ptr<std::vector<Threeph_process>>>
    load_bulk_hdf5(const char* filename, const boost::mpi::communicator& comm);

    /// Deviation from the conservation of energy.
    double domega;
    /// Standard deviation of the Gaussian.
    double sigma;
    /// Gaussian factor of the process, coming from the
    /// regularized Dirac delta.
    double gaussian;

    /// serialization function
    template <class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & this->domega;
        ar & this->sigma;
        ar & this->gaussian;
        ar & this->c;
        ar & this->q;
        ar & this->alpha;
    }

public:
    /// Equivalence class of the first phonon.
    std::size_t c;
    /// q point indices of each of the two phonons involved.
    std::array<std::size_t, 2> q;
    /// Mode indices of the two phonons involved.
    std::array<std::size_t, 2> alpha;
    /// Default constructor
    Twoph_process(){};
    /// Basic constructor.
    Twoph_process(std::size_t _c,
                  const std::array<std::size_t, 2>& _q,
                  const std::array<std::size_t, 2>& _alpha,
                  double _domega,
                  double _sigma)
        : domega(_domega), sigma(_sigma),
          gaussian(boost::math::pdf(boost::math::normal(0., sigma), domega)),
          c(_c), q(std::move(_q)), alpha(std::move(_alpha)) {
    }

    /// Copy constructor.
    Twoph_process(const Twoph_process& original)
        : domega(original.domega), sigma(original.sigma),
          gaussian(original.gaussian), c(original.c), q(original.q),
          alpha(original.alpha) {
    }
    /// Copy assignement operator
    Twoph_process& operator=(const Twoph_process& rhs) {
        using std::swap;
        this->domega = rhs.domega;
        this->sigma = rhs.sigma;
        this->gaussian = rhs.gaussian;
        this->c = rhs.c;
        this->q = rhs.q;
        this->alpha = rhs.alpha;
        return *this;
    }

    /// Move constructor:
    Twoph_process(Twoph_process&& rhs) {
        using std::swap;
        swap(this->domega, rhs.domega);
        swap(this->sigma, rhs.sigma);
        swap(this->gaussian, rhs.gaussian);
        swap(this->c, rhs.c);
        swap(this->q, rhs.q);
        swap(this->alpha, rhs.alpha);
    }

    /// Swap to enable swap semmantics
    void swap(Twoph_process& lhs, Twoph_process& rhs) {
        using std::swap;
        swap(this->domega, rhs.domega);
        swap(this->sigma, rhs.sigma);
        swap(this->gaussian, rhs.gaussian);
        swap(this->c, rhs.c);
        swap(this->q, rhs.q);
        swap(this->alpha, rhs.alpha);
    }

    /// Move assignement operator
    Twoph_process& operator=(Twoph_process&& rhs) {
        using std::swap;
        swap(this->domega, rhs.domega);
        swap(this->sigma, rhs.sigma);
        swap(this->gaussian, rhs.gaussian);
        swap(this->c, rhs.c);
        swap(this->q, rhs.q);
        swap(this->alpha, rhs.alpha);
        return *this;
    }

    /// Explicit constructor used when deserializing objects of
    /// this class. Save time by avoiding the calculatio of
    /// this->gaussian.
    Twoph_process(std::size_t _c,
                  const std::array<std::size_t, 2>& _q,
                  const std::array<std::size_t, 2>& _alpha,
                  double _domega,
                  double _sigma,
                  double _gaussian)
        : domega(_domega), sigma(_sigma), gaussian(_gaussian), c(_c),
          q(std::move(_q)), alpha(std::move(_alpha)) {
    }

    /// Get the "partial scattering rate" Gamma for this process.
    ///
    /// @param[in] cell - description of the unit cell
    /// @param[in] grid - regular grid with phonon spectrum
    /// @return Gamma, the partial scattering rate
    double compute_gamma(const Crystal_structure& cell,
                         const Gamma_grid& grid) const;

    /// Get the "partial scattering rate" Gamma for this process.
    /// using a custom set of g factors.
    ///
    /// @param[in] cell - description of the unit cell
    /// @param[in] gfactors - Pearson deviation coefficient of the mass
    /// at each site.
    /// @param[in] grid - regular grid with phonon spectrum
    /// @return Gamma, the partial scattering rate
    double compute_gamma(const Crystal_structure& cell,
                         const Eigen::Ref<const Eigen::VectorXd>& gfactors,
                         const Gamma_grid& grid) const;
};
/// Look for allowed two-phonon processes in a regular grid.
///
/// Iterate over part of the irreducible q points in the grid
/// (trying to evenly split the equivalence classes over processes)
/// and look for allowed two-phonon processes involving one phonon
/// from that part and another phonon from anywhere in the grid.
/// @param[in] grid - a regular grid containing Gamma
/// @param[in] communicator - MPI communicator to use
/// @param[in] scalebroad - factor modulating all the broadenings
/// @return a vector of Twoph_process objects
std::vector<Twoph_process> find_allowed_twoph(
    const Gamma_grid& grid,
    const boost::mpi::communicator& communicator,
    double scalebroad = 1.0);

/// Compute and the two-phonon contribution to the RTA
/// scattering rates for all vibrational modes on a grid.
///
/// @param[in] cell - a description of the crystal structure
/// @param[in] grid - phonon spectrum on a regular grid
/// @param[in] processes - a vector of allowed two-phonon processes
/// @param[in] comm - an mpi communicator
/// @return a set of scattering rates
Eigen::ArrayXXd calc_w0_twoph(const Crystal_structure& cell,
                              const alma::Gamma_grid& grid,
                              const std::vector<alma::Twoph_process>& processes,
                              const boost::mpi::communicator& comm);

/// Compute and the two-phonon contribution to the RTA
/// scattering rates for all vibrational modes on a grid, with a custom
/// set of g factors.
///
/// @param[in] cell - a description of the crystal structure
/// @param[in] grid - phonon spectrum on a regular grid
/// @param[in] gfactors - Pearson deviation coefficient of the mass
/// at each site.
/// @param[in] processes - a vector of allowed two-phonon processes
/// @param[in] comm - an mpi communicator
/// @return a set of scattering rates
Eigen::ArrayXXd calc_w0_twoph(const Crystal_structure& cell,
                              const Eigen::Ref<const Eigen::VectorXd>& gfactors,
                              const alma::Gamma_grid& grid,
                              const std::vector<alma::Twoph_process>& processes,
                              const boost::mpi::communicator& comm);
} // namespace alma


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

#include "rbm_multival.hpp"

#include <Eigen/Dense>
#include <iostream>
#include <map>
#include <vector>
#include "Utils/all_utils.hpp"
#include "Utils/lookup.hpp"
#include "abstract_machine.hpp"
#include "rbm_spin.hpp"

namespace netket {

RbmMultival::RbmMultival(std::shared_ptr<const AbstractHilbert> hilbert,
                         int nhidden, int alpha, bool usea, bool useb)
    : AbstractMachine(hilbert),
      nv_(hilbert->Size()),
      ls_(hilbert->LocalSize()),
      usea_(usea),
      useb_(useb) {
  nh_ = std::max(nhidden, alpha * nv_);
  Init();
}

void RbmMultival::Init() {
  W_.resize(nv_ * ls_, nh_);
  a_.resize(nv_ * ls_);
  b_.resize(nh_);

  thetas_.resize(nh_);
  lnthetas_.resize(nh_);
  thetasnew_.resize(nh_);
  lnthetasnew_.resize(nh_);

  npar_ = nv_ * nh_ * ls_;

  if (usea_) {
    npar_ += nv_ * ls_;
  } else {
    a_.setZero();
  }

  if (useb_) {
    npar_ += nh_;
  } else {
    b_.setZero();
  }

  auto localstates = GetHilbert().LocalStates();

  localconfs_.resize(nv_ * ls_);
  for (int i = 0; i < nv_ * ls_; i += ls_) {
    for (int j = 0; j < ls_; j++) {
      localconfs_(i + j) = localstates[j];
    }
  }

  mask_.resize(nv_ * ls_, nv_);
  mask_.setZero();

  for (int i = 0; i < nv_ * ls_; i++) {
    mask_(i, i / ls_) = 1;
  }

  for (int i = 0; i < ls_; i++) {
    confindex_[localstates[i]] = i;
  }

  vtilde_.resize(nv_ * ls_);

  InfoMessage() << "RBM Multival Initizialized with nvisible = " << nv_
                << " and nhidden = " << nh_ << std::endl;
  InfoMessage() << "Using visible bias = " << usea_ << std::endl;
  InfoMessage() << "Using hidden bias  = " << useb_ << std::endl;
  InfoMessage() << "Local size is      = " << ls_ << std::endl;
}

int RbmMultival::Nvisible() const { return nv_; }

int RbmMultival::Npar() const { return npar_; }

void RbmMultival::InitRandomPars(int seed, double sigma) {
  VectorType par(npar_);

  netket::RandomGaussian(par, seed, sigma);

  SetParameters(par);
}

void RbmMultival::InitLookup(VisibleConstType v, LookupType &lt) {
  if (lt.VectorSize() == 0) {
    lt.AddVector(b_.size());
  }
  if (lt.V(0).size() != b_.size()) {
    lt.V(0).resize(b_.size());
  }
  ComputeTheta(v, lt.V(0));
}

void RbmMultival::UpdateLookup(VisibleConstType v,
                               const std::vector<int> &tochange,
                               const std::vector<double> &newconf,
                               LookupType &lt) {
  if (tochange.size() != 0) {
    for (std::size_t s = 0; s < tochange.size(); s++) {
      const int sf = tochange[s];
      const int oldtilde = confindex_[v[sf]];
      const int newtilde = confindex_[newconf[s]];

      lt.V(0) -= W_.row(ls_ * sf + oldtilde);
      lt.V(0) += W_.row(ls_ * sf + newtilde);
    }
  }
}

RbmMultival::VectorType RbmMultival::DerLog(VisibleConstType v) {
  LookupType ltnew;
  InitLookup(v, ltnew);
  return DerLog(v, ltnew);
}

RbmMultival::VectorType RbmMultival::DerLog(VisibleConstType v,
                                            const LookupType &lt) {
  VectorType der(npar_);
  der.setZero();

  ComputeVtilde(v, vtilde_);

  int k = 0;

  if (usea_) {
    for (; k < nv_ * ls_; k++) {
      der(k) = vtilde_(k);
    }
  }

  RbmSpin::tanh(lt.V(0), lnthetas_);

  if (useb_) {
    for (int p = 0; p < nh_; p++) {
      der(k) = lnthetas_(p);
      k++;
    }
  }

  for (int i = 0; i < nv_ * ls_; i++) {
    for (int j = 0; j < nh_; j++) {
      der(k) = lnthetas_(j) * vtilde_(i);
      k++;
    }
  }
  return der;
}

RbmMultival::VectorType RbmMultival::GetParameters() {
  VectorType pars(npar_);

  int k = 0;

  if (usea_) {
    for (; k < nv_ * ls_; k++) {
      pars(k) = a_(k);
    }
  }

  if (useb_) {
    for (int p = 0; p < nh_; p++) {
      pars(k) = b_(p);
      k++;
    }
  }

  for (int i = 0; i < nv_ * ls_; i++) {
    for (int j = 0; j < nh_; j++) {
      pars(k) = W_(i, j);
      k++;
    }
  }

  return pars;
}

void RbmMultival::SetParameters(VectorConstRefType pars) {
  int k = 0;

  if (usea_) {
    for (; k < nv_ * ls_; k++) {
      a_(k) = pars(k);
    }
  }

  if (useb_) {
    for (int p = 0; p < nh_; p++) {
      b_(p) = pars(k);
      k++;
    }
  }

  for (int i = 0; i < nv_ * ls_; i++) {
    for (int j = 0; j < nh_; j++) {
      W_(i, j) = pars(k);
      k++;
    }
  }
}

// Value of the logarithm of the wave-function
Complex RbmMultival::LogVal(VisibleConstType v) {
  ComputeTheta(v, thetas_);
  RbmSpin::lncosh(thetas_, lnthetas_);

  return (vtilde_.dot(a_) + lnthetas_.sum());
}

// Value of the logarithm of the wave-function
// using pre-computed look-up tables for efficiency
Complex RbmMultival::LogVal(VisibleConstType v, const LookupType &lt) {
  RbmSpin::lncosh(lt.V(0), lnthetas_);

  ComputeVtilde(v, vtilde_);
  return (vtilde_.dot(a_) + lnthetas_.sum());
}

// Difference between logarithms of values, when one or more visible variables
// are being changed
RbmMultival::VectorType RbmMultival::LogValDiff(
    VisibleConstType v, const std::vector<std::vector<int>> &tochange,
    const std::vector<std::vector<double>> &newconf) {
  const std::size_t nconn = tochange.size();
  VectorType logvaldiffs = VectorType::Zero(nconn);

  ComputeTheta(v, thetas_);
  RbmSpin::lncosh(thetas_, lnthetas_);

  Complex logtsum = lnthetas_.sum();

  for (std::size_t k = 0; k < nconn; k++) {
    if (tochange[k].size() != 0) {
      thetasnew_ = thetas_;

      for (std::size_t s = 0; s < tochange[k].size(); s++) {
        const int sf = tochange[k][s];
        const int oldtilde = confindex_[v[sf]];
        const int newtilde = confindex_[newconf[k][s]];

        logvaldiffs(k) -= a_(ls_ * sf + oldtilde);
        logvaldiffs(k) += a_(ls_ * sf + newtilde);

        thetasnew_ -= W_.row(ls_ * sf + oldtilde);
        thetasnew_ += W_.row(ls_ * sf + newtilde);
      }

      RbmSpin::lncosh(thetasnew_, lnthetasnew_);
      logvaldiffs(k) += lnthetasnew_.sum() - logtsum;
    }
  }
  return logvaldiffs;
}

// Difference between logarithms of values, when one or more visible variables
// are being changed Version using pre-computed look-up tables for efficiency
// on a small number of local changes
Complex RbmMultival::LogValDiff(VisibleConstType v,
                                const std::vector<int> &tochange,
                                const std::vector<double> &newconf,
                                const LookupType &lt) {
  Complex logvaldiff = 0.;

  if (tochange.size() != 0) {
    RbmSpin::lncosh(lt.V(0), lnthetas_);

    thetasnew_ = lt.V(0);

    for (std::size_t s = 0; s < tochange.size(); s++) {
      const int sf = tochange[s];
      const int oldtilde = confindex_[v[sf]];
      const int newtilde = confindex_[newconf[s]];

      logvaldiff -= a_(ls_ * sf + oldtilde);
      logvaldiff += a_(ls_ * sf + newtilde);

      thetasnew_ -= W_.row(ls_ * sf + oldtilde);
      thetasnew_ += W_.row(ls_ * sf + newtilde);
    }

    RbmSpin::lncosh(thetasnew_, lnthetasnew_);
    logvaldiff += (lnthetasnew_.sum() - lnthetas_.sum());
  }
  return logvaldiff;
}

#if 0
// Computhes the values of the theta pseudo-angles
inline void RbmMultival::ComputeTheta(VisibleConstType v, VectorType &theta) {
  ComputeVtilde(v, vtilde_);
  theta = (W_.transpose() * vtilde_ + b_);
}

inline void RbmMultival::ComputeVtilde(VisibleConstType v,
                                       Eigen::VectorXd &vtilde) {
  auto t = (localconfs_.array() == (mask_ * v).array());
  vtilde = t.template cast<double>();
}
#endif

void RbmMultival::Save(const std::string &filename) const {
  json state;
  state["Name"] = "RbmMultival";
  state["Nvisible"] = nv_;
  state["Nhidden"] = nh_;
  state["LocalSize"] = ls_;
  state["UseVisibleBias"] = usea_;
  state["UseHiddenBias"] = useb_;
  state["a"] = a_;
  state["b"] = b_;
  state["W"] = W_;
  WriteJsonToFile(state, filename);
}

void RbmMultival::Load(const std::string &filename) {
  auto const pars = ReadJsonFromFile(filename);
  if (pars.at("Name") != "RbmMultival") {
    throw InvalidInputError(
        "Error while constructing RbmMultival from Json input");
  }

  if (FieldExists(pars, "Nvisible")) {
    nv_ = pars["Nvisible"];
  }

  if (nv_ != GetHilbert().Size()) {
    throw InvalidInputError(
        "Loaded wave-function has incompatible Hilbert space");
  }

  if (FieldExists(pars, "LocalSize")) {
    ls_ = pars["LocalSize"];
  }
  if (ls_ != GetHilbert().LocalSize()) {
    throw InvalidInputError(
        "Loaded wave-function has incompatible Hilbert space");
  }

  if (FieldExists(pars, "Nhidden")) {
    nh_ = FieldVal(pars, "Nhidden");
  } else {
    nh_ = nv_ * double(FieldVal(pars, "Alpha"));
  }

  usea_ = FieldOrDefaultVal(pars, "UseVisibleBias", true);
  useb_ = FieldOrDefaultVal(pars, "UseHiddenBias", true);

  Init();

  // Loading parameters, if defined in the input
  if (FieldExists(pars, "a")) {
    a_ = pars["a"];
  } else {
    a_.setZero();
  }

  if (FieldExists(pars, "b")) {
    b_ = pars["b"];
  } else {
    b_.setZero();
  }
  if (FieldExists(pars, "W")) {
    W_ = pars["W"];
  }
}

bool RbmMultival::IsHolomorphic() const noexcept { return true; }

}  // namespace netket


#ifndef CGTOSHELLPAIR_HH
#define CGTOSHELLPAIR_HH

/*!
 * \file CGTOShellPair.hh
 * \brief Definition of the CGTOShellPair class
 */

#include <Eigen/Core>
#include "CGTOShell.hh"

class CGTOShellPair
{
public:
	/*!
	 * \brief Constructor
	 *
	 * Create a new pair of shells \a shA and \a shB
	 */
	CGTOShellPair(int index, const CGTOShell& shA, const CGTOShell& shB);

	//! Return the number of primitive combinations in this pair
	int size() const { return widthsSum().size(); }

	//! Return the index of this shell pair in the CGTOShellList
	int index() const { return _index; }
	//! Return the total angular momentum of the swo shells combined
	int lsum() const { return _lsum; }
	//! Return the unnormalized weights of the primitive combinations
	const Eigen::ArrayXXd& weights() const { return _weights; }
	/*!
	 * \brief Return the widths of the primitives in the first shell,
	 *    for all primitives in the second shell.
	 */
	const Eigen::ArrayXXd& widthsA() const { return _widthsA; }
	/*!
	 * \brief Return the widths of the primitives in the second shell,
	 *    for all primitives in the first shell.
	 */
	const Eigen::ArrayXXd& widthsB() const { return _widthsB; }
	//! The sums of primitive widths, equivalent to widths() + widthB()
	const Eigen::ArrayXXd& widthsSum() const { return _widths_sum; }
	//! The "reduced" primitive widths \f$\xi = \alpha\beta / (\alpha+\beta)\f$
	const Eigen::ArrayXXd& widthsReduced() const { return _widths_red; }
	/*!
	 * \brief \f\frac{$\exp(-\xi r^2)}{\alpha+\beta}\f$ with \f$r\f$ the
	 *    distance between the centers of the two orbitals.
	 */
	const Eigen::ArrayXXd& gaussReduced() const { return _gauss_red; }
	//! Return half the inverse widths sums, \f$\frac{1}{2(\alpha+\beta)}\f$
	const Eigen::ArrayXXd& hInvWidths() const { return _hinv_widths; }

	//! Return the \a i coordinate of the first shell in this pair
	double centerA(int i) const { return _shA.center(i); }
	//! Return the \a i coordinate of the second shell in this pair
	double centerB(int i) const { return _shB.center(i); }
	//! Return the distance between the two centers in the \a i dimension
	double dAB(int i) const { return _shB.center(i) - _shA.center(i); }

	/*!
	 * \brief Return the weighted average \a i coordinate, for each
	 *    combination of primitive weights.
	 */
	const Eigen::ArrayXXd& P(int i) const
	{
		return _P[i];
	}

	//! Return the position index of the first shell
	int positionIdA() const
	{
		return _shA.positionId();
	}
	//! Return the position index of the first shell
	int positionIdB() const
	{
		return _shB.positionId();
	}
	//! Return \c true if both shells are located on the same center
	bool samePositionId() const
	{
		return positionIdA() == positionIdB();
	}

	/*!
	 * Multiply the contributions to an integral \a C of each primitive pair
	 * with the weights of the primitives, and sum the results to compute
	 * the integral over the contraction.
	 * \param C the primitve integrals
	 */
	template <typename Derived>
	double mulWeights(const Eigen::ArrayBase<Derived>& C) const
	{
		return ((C.colwise() * _shA.weights()).colwise().sum().transpose()
			* _shB.weights()).sum();
	}

private:
	//! Index of this pair in the CGTOShellList
	int _index;
	//! First shell in this pair
	const CGTOShell& _shA;
	//! Second shell in this pair
	const CGTOShell& _shB;
	//! Unnormalized weights of the primitive combinations
	Eigen::ArrayXXd _weights;
	//! Primitive widths of the first orbital
	Eigen::ArrayXXd _widthsA;
	//! Primitive widths of the second orbital
	Eigen::ArrayXXd _widthsB;
	//! Sum of primitive widths, for all combinations of primitives
	Eigen::ArrayXXd _widths_sum;
	//! Reduced primitive widths, for all combinations of primitives
	Eigen::ArrayXXd _widths_red;
	//! \f$\frac{\exp(-\xi r^2)}{\alpha+\beta}\f$
	Eigen::ArrayXXd _gauss_red;
	//! Half of inverse width sum
	Eigen::ArrayXXd _hinv_widths;
	//! Weighted average coordinates
	Eigen::ArrayXXd _P[3];
	//! The total angular momentum of the two shells
	int _lsum;
};

#endif // CGTOSHELLPAIR_HH


//
// $Id: DemuxTypes.hpp 10236 2016-11-30 21:31:54Z atkeller $
//
//
// Original author: Jarrett Egertson <jegertso .@. uw.edu>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef _DEMUXTYPES_HPP
#define _DEMUXTYPES_HPP

#include "pwiz/data/msdata/MSData.hpp"
#include <Eigen>

namespace pwiz {
namespace msdata {
    typedef boost::shared_ptr<const msdata::SpectrumList> SpectrumList_const_ptr;
    typedef boost::shared_ptr<const msdata::Spectrum> Spectrum_const_ptr;
    typedef boost::shared_ptr<const BinaryDataArray> BinaryDataArray_const_ptr;
} // namespace msdata
} // namespace pwiz

namespace DemuxTypes
{
    using namespace Eigen;
    typedef double DemuxScalar;
    typedef Matrix<DemuxScalar, Dynamic, Dynamic> MatrixType;
    typedef boost::shared_ptr<MatrixType> MatrixPtr;

    static const std::string kDEMUX_NAME = "Demultiplexing"; ///< This is a flag used by SpectrumWorkerThreads to handle demultiplexing
} // namespace DemuxTypes
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_POINT_SQUARE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_POINT_SQUARE_HPP

#include <cstddef>

#include <boost/range/value_type.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Create a squared form buffer around a point
\ingroup strategies
\details This strategy can be used as PointStrategy for the buffer algorithm.
    It creates a square from each point, where the point lies in the center.
    It can be applied for points and multi_points, but also for a linestring (if it is degenerate,
    so consisting of only one point) and for polygons (if it is degenerate).
    This strategy is only applicable for Cartesian coordinate systems.

\qbk{
[heading Example]
[buffer_point_square]
[heading Output]
[$img/strategies/buffer_point_square.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_point_circle point_circle]
\* [link geometry.reference.strategies.strategy_buffer_geographic_point_circle geographic_point_circle]
}
 */
class point_square
{
    template
    <
        typename Point,
        typename DistanceType,
        typename MultiplierType,
        typename OutputRange
    >
    inline void add_point(Point const& point,
                DistanceType const& distance,
                MultiplierType const& x,
                MultiplierType const& y,
                OutputRange& output_range) const
    {
        typename boost::range_value<OutputRange>::type p;
        set<0>(p, get<0>(point) + x * distance);
        set<1>(p, get<1>(point) + y * distance);
        output_range.push_back(p);
    }

    template
    <
        typename Point,
        typename DistanceType,
        typename OutputRange
    >
    inline void add_points(Point const& point,
                DistanceType const& distance,
                OutputRange& output_range) const
    {
        add_point(point, distance, -1.0, -1.0, output_range);
        add_point(point, distance, -1.0, +1.0, output_range);
        add_point(point, distance, +1.0, +1.0, output_range);
        add_point(point, distance, +1.0, -1.0, output_range);

        // Close it:
        output_range.push_back(output_range.front());
    }

public :

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Fills output_range with a square around point using distance_strategy
    template
    <
        typename Point,
        typename DistanceStrategy,
        typename OutputRange
    >
    inline void apply(Point const& point,
                DistanceStrategy const& distance_strategy,
                OutputRange& output_range) const
    {
        add_points(point, distance_strategy.apply(point, point,
                        strategy::buffer::buffer_side_left), output_range);
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS

};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_POINT_SQUARE_HPP


//
//  Created by Simon Schreiberhuber on 01.04.15.
//  Copyright (c) 2015 Simon Schreiberhuber. All rights reserved.
//

#include <iostream>
#include <string>
#include <sstream>
#include <limits>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <boost/filesystem.hpp>

#include <assimp/Importer.hpp>
#include <assimp/scene.h>
#include <assimp/postprocess.h>

#include <v4r/io/filesystem.h>
#include <v4r/rendering/depthmapRenderer.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <glog/logging.h>

namespace po = boost::program_options;

int main(int argc, const char * argv[]) {
    std::string input, out_dir;
    bool visualize = false;
    size_t subdivisions = 0, width = 640, height = 480;
    float radius = 3.0, fx = 535.4, fy = 539.2, cx = 320.1, cy = 247.6;

    google::InitGoogleLogging(argv[0]);

    po::options_description desc("Depth-map and point cloud Rendering from mesh file\n======================================\n**Allowed options");
    desc.add_options()
            ("help,h", "produce help message")
            ("input,i", po::value<std::string>(&input)->required(), "input model (.ply)")
            ("output,o", po::value<std::string>(&out_dir)->default_value("/tmp/rendered_pointclouds/"), "output directory to store the point cloud (.pcd) file")
            ("subdivisions,s", po::value<size_t>(&subdivisions)->default_value(subdivisions), "defines the number of subdivsions used for rendering")
            ("radius,r", po::value<float>(&radius)->default_value(radius, boost::str(boost::format("%.2e") % radius)), "defines the radius used for rendering")
            ("width", po::value<size_t>(&width)->default_value(width), "defines the image width")
            ("height", po::value<size_t>(&height)->default_value(height), "defines the image height")
            ("fx", po::value<float>(&fx)->default_value(fx, boost::str(boost::format("%.2e") % fx)), "defines the focal length in x direction used for rendering")
            ("fy", po::value<float>(&fy)->default_value(fy, boost::str(boost::format("%.2e") % fy)), "defines the focal length in y direction used for rendering")
            ("cx", po::value<float>(&cx)->default_value(cx, boost::str(boost::format("%.2e") % cx)), "defines the central point of projection in x direction used for rendering")
            ("cy", po::value<float>(&cy)->default_value(cy, boost::str(boost::format("%.2e") % cy)), "defines the central point of projection in y direction used for rendering")
            ("visualize,v", po::bool_switch(&visualize), "visualize the rendered depth and color map")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return false;
    }

    try
    {
        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
        return false;
    }

    CHECK( (cx < width) && (cy < height) && (cx > 0) && (cy > 0)) << "Parameters not valid!";

    v4r::DepthmapRenderer renderer(width, height);
    renderer.setIntrinsics(fx,fy,cx,cy);

    v4r::DepthmapRendererModel model(input);

    //test if the model has colored elements(note! no textures supported yet.... only colored polygons)
    if(model.hasColor()){
        LOG(INFO) << "Model file has color.";
    }

    renderer.setModel(&model);

    std::vector<Eigen::Vector3f> sphere = renderer.createSphere(3, subdivisions);

    LOG(INFO) <<"Rendering file " << input;

    if (!sphere.empty())
        v4r::io::createDirIfNotExist(out_dir);

    for(size_t i=0; i<sphere.size(); i++){
        //get point from list
        Eigen::Vector3f point = sphere[i];
        //get a camera pose looking at the center:
        Eigen::Matrix4f orientation = renderer.getPoseLookingToCenterFrom(point);

        renderer.setCamPose(orientation);

        float visible;
        cv::Mat color;
        cv::Mat depthmap = renderer.renderDepthmap(visible, color);

        //create and save the according pcd files
        std::stringstream ss; ss << out_dir << "/cloud_" << i << ".pcd";
        std::string file = ss.str();
        if(model.hasColor()) {
            pcl::PointCloud<pcl::PointXYZRGB> cloud = renderer.renderPointcloudColor(visible);
            pcl::io::savePCDFileBinary(file, cloud);

            if (visualize)
                cv::imshow("color", color);
        }
        else {
            pcl::PointCloud<pcl::PointXYZ> cloud = renderer.renderPointcloud(visible);
            pcl::io::savePCDFileBinary(file, cloud);
        }

        LOG(INFO) << "Saved data points to " << file << ".";

        if(visualize) {
            LOG(INFO) << visible << "% visible.";
            cv::imshow("depthmap", depthmap*0.25);
            cv::waitKey();
        }

    }

    return 0;
}