#ifndef STAN_MATH_REV_MAT_FUN_COV_EXP_QUAD_HPP
#define STAN_MATH_REV_MAT_FUN_COV_EXP_QUAD_HPP

#include <stan/math/rev/core.hpp>
#include <stan/math/rev/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/squared_distance.hpp>
#include <stan/math/prim/scal/fun/exp.hpp>
#include <stan/math/prim/scal/meta/scalar_type.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <vector>
#include <cmath>

namespace stan {
namespace math {

/**
 * This is a subclass of the vari class for precomputed
 * gradients of cov_exp_quad.
 *
 * The class stores the double values for the distance
 * matrix, pointers to the varis for the covariance
 * matrix, along with a pointer to the vari for sigma,
 * and the vari for l.
 *
 * @tparam T_x type of std::vector of elements
 * @tparam T_sigma type of sigma
 * @tparam T_l type of length scale
 */
template <typename T_x, typename T_sigma, typename T_l>
class cov_exp_quad_vari : public vari {
 public:
  const size_t size_;
  const size_t size_ltri_;
  const double l_d_;
  const double sigma_d_;
  const double sigma_sq_d_;
  double* dist_;
  vari* l_vari_;
  vari* sigma_vari_;
  vari** cov_lower_;
  vari** cov_diag_;

  /**
   * Constructor for cov_exp_quad.
   *
   * All memory allocated in
   * ChainableStack's stack_alloc arena.
   *
   * It is critical for the efficiency of this object
   * that the constructor create new varis that aren't
   * popped onto the var_stack_, but rather are
   * popped onto the var_nochain_stack_. This is
   * controlled to the second argument to
   * vari's constructor.
   *
   * @param x std::vector input that can be used in square distance
   *    Assumes each element of x is the same size
   * @param sigma standard deviation
   * @param l length scale
   */
  cov_exp_quad_vari(const std::vector<T_x>& x, const T_sigma& sigma,
                    const T_l& l)
      : vari(0.0),
        size_(x.size()),
        size_ltri_(size_ * (size_ - 1) / 2),
        l_d_(value_of(l)),
        sigma_d_(value_of(sigma)),
        sigma_sq_d_(sigma_d_ * sigma_d_),
        dist_(ChainableStack::instance().memalloc_.alloc_array<double>(
            size_ltri_)),
        l_vari_(l.vi_),
        sigma_vari_(sigma.vi_),
        cov_lower_(ChainableStack::instance().memalloc_.alloc_array<vari*>(
            size_ltri_)),
        cov_diag_(
            ChainableStack::instance().memalloc_.alloc_array<vari*>(size_)) {
    double inv_half_sq_l_d = 0.5 / (l_d_ * l_d_);
    size_t pos = 0;
    for (size_t j = 0; j < size_ - 1; ++j) {
      for (size_t i = j + 1; i < size_; ++i) {
        double dist_sq = squared_distance(x[i], x[j]);
        dist_[pos] = dist_sq;
        cov_lower_[pos] = new vari(
            sigma_sq_d_ * std::exp(-dist_sq * inv_half_sq_l_d), false);
        ++pos;
      }
    }
    for (size_t i = 0; i < size_; ++i)
      cov_diag_[i] = new vari(sigma_sq_d_, false);
  }

  virtual void chain() {
    double adjl = 0;
    double adjsigma = 0;

    for (size_t i = 0; i < size_ltri_; ++i) {
      vari* el_low = cov_lower_[i];
      double prod_add = el_low->adj_ * el_low->val_;
      adjl += prod_add * dist_[i];
      adjsigma += prod_add;
    }
    for (size_t i = 0; i < size_; ++i) {
      vari* el = cov_diag_[i];
      adjsigma += el->adj_ * el->val_;
    }
    l_vari_->adj_ += adjl / (l_d_ * l_d_ * l_d_);
    sigma_vari_->adj_ += adjsigma * 2 / sigma_d_;
  }
};

/**
 * This is a subclass of the vari class for precomputed
 * gradients of cov_exp_quad.
 *
 * The class stores the double values for the distance
 * matrix, pointers to the varis for the covariance
 * matrix, along with a pointer to the vari for sigma,
 * and the vari for l.
 *
 * @tparam T_x type of std::vector of elements
 * @tparam T_l type of length scale
 */
template <typename T_x, typename T_l>
class cov_exp_quad_vari<T_x, double, T_l> : public vari {
 public:
  const size_t size_;
  const size_t size_ltri_;
  const double l_d_;
  const double sigma_d_;
  const double sigma_sq_d_;
  double* dist_;
  vari* l_vari_;
  vari** cov_lower_;
  vari** cov_diag_;

  /**
   * Constructor for cov_exp_quad.
   *
   * All memory allocated in
   * ChainableStack's stack_alloc arena.
   *
   * It is critical for the efficiency of this object
   * that the constructor create new varis that aren't
   * popped onto the var_stack_, but rather are
   * popped onto the var_nochain_stack_. This is
   * controlled to the second argument to
   * vari's constructor.
   *
   * @param x std::vector input that can be used in square distance
   *    Assumes each element of x is the same size
   * @param sigma standard deviation
   * @param l length scale
   */
  cov_exp_quad_vari(const std::vector<T_x>& x, double sigma, const T_l& l)
      : vari(0.0),
        size_(x.size()),
        size_ltri_(size_ * (size_ - 1) / 2),
        l_d_(value_of(l)),
        sigma_d_(value_of(sigma)),
        sigma_sq_d_(sigma_d_ * sigma_d_),
        dist_(ChainableStack::instance().memalloc_.alloc_array<double>(
            size_ltri_)),
        l_vari_(l.vi_),
        cov_lower_(ChainableStack::instance().memalloc_.alloc_array<vari*>(
            size_ltri_)),
        cov_diag_(
            ChainableStack::instance().memalloc_.alloc_array<vari*>(size_)) {
    double inv_half_sq_l_d = 0.5 / (l_d_ * l_d_);
    size_t pos = 0;
    for (size_t j = 0; j < size_ - 1; ++j) {
      for (size_t i = j + 1; i < size_; ++i) {
        double dist_sq = squared_distance(x[i], x[j]);
        dist_[pos] = dist_sq;
        cov_lower_[pos] = new vari(
            sigma_sq_d_ * std::exp(-dist_sq * inv_half_sq_l_d), false);
        ++pos;
      }
    }
    for (size_t i = 0; i < size_; ++i)
      cov_diag_[i] = new vari(sigma_sq_d_, false);
  }

  virtual void chain() {
    double adjl = 0;

    for (size_t i = 0; i < size_ltri_; ++i) {
      vari* el_low = cov_lower_[i];
      adjl += el_low->adj_ * el_low->val_ * dist_[i];
    }
    l_vari_->adj_ += adjl / (l_d_ * l_d_ * l_d_);
  }
};

/**
 * Returns a squared exponential kernel.
 *
 * @param x std::vector input that can be used in square distance
 *    Assumes each element of x is the same size
 * @param sigma standard deviation
 * @param l length scale
 * @return squared distance
 * @throw std::domain_error if sigma <= 0, l <= 0, or
 *   x is nan or infinite
 */
template <typename T_x>
inline typename boost::enable_if_c<
    boost::is_same<typename scalar_type<T_x>::type, double>::value,
    Eigen::Matrix<var, -1, -1> >::type
cov_exp_quad(const std::vector<T_x>& x, const var& sigma, const var& l) {
  check_positive("cov_exp_quad", "sigma", sigma);
  check_positive("cov_exp_quad", "l", l);
  size_t x_size = x.size();
  for (size_t i = 0; i < x_size; ++i)
    check_not_nan("cov_exp_quad", "x", x[i]);

  Eigen::Matrix<var, -1, -1> cov(x_size, x_size);
  if (x_size == 0)
    return cov;

  cov_exp_quad_vari<T_x, var, var>* baseVari
      = new cov_exp_quad_vari<T_x, var, var>(x, sigma, l);

  size_t pos = 0;
  for (size_t j = 0; j < x_size - 1; ++j) {
    for (size_t i = (j + 1); i < x_size; ++i) {
      cov.coeffRef(i, j).vi_ = baseVari->cov_lower_[pos];
      cov.coeffRef(j, i).vi_ = cov.coeffRef(i, j).vi_;
      ++pos;
    }
    cov.coeffRef(j, j).vi_ = baseVari->cov_diag_[j];
  }
  cov.coeffRef(x_size - 1, x_size - 1).vi_ = baseVari->cov_diag_[x_size - 1];
  return cov;
}

/**
 * Returns a squared exponential kernel.
 *
 * @param x std::vector input that can be used in square distance
 *    Assumes each element of x is the same size
 * @param sigma standard deviation
 * @param l length scale
 * @return squared distance
 * @throw std::domain_error if sigma <= 0, l <= 0, or
 *   x is nan or infinite
 */
template <typename T_x>
inline typename boost::enable_if_c<
    boost::is_same<typename scalar_type<T_x>::type, double>::value,
    Eigen::Matrix<var, -1, -1> >::type
cov_exp_quad(const std::vector<T_x>& x, double sigma, const var& l) {
  check_positive("cov_exp_quad", "marginal variance", sigma);
  check_positive("cov_exp_quad", "length-scale", l);
  size_t x_size = x.size();
  for (size_t i = 0; i < x_size; ++i)
    check_not_nan("cov_exp_quad", "x", x[i]);

  Eigen::Matrix<var, -1, -1> cov(x_size, x_size);
  if (x_size == 0)
    return cov;

  cov_exp_quad_vari<T_x, double, var>* baseVari
      = new cov_exp_quad_vari<T_x, double, var>(x, sigma, l);

  size_t pos = 0;
  for (size_t j = 0; j < x_size - 1; ++j) {
    for (size_t i = (j + 1); i < x_size; ++i) {
      cov.coeffRef(i, j).vi_ = baseVari->cov_lower_[pos];
      cov.coeffRef(j, i).vi_ = cov.coeffRef(i, j).vi_;
      ++pos;
    }
    cov.coeffRef(j, j).vi_ = baseVari->cov_diag_[j];
  }
  cov.coeffRef(x_size - 1, x_size - 1).vi_ = baseVari->cov_diag_[x_size - 1];
  return cov;
}

}  // namespace math
}  // namespace stan
#endif


/*=========================================================================
 *
 *  Copyright David Doria 2011 daviddoria@gmail.com
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

// Pixel descriptors
#include "PixelDescriptors/ImagePatchPixelDescriptor.h"

// Visitors
//#include "Visitors/DefaultInpaintingVisitor.hpp"
#include "Visitors/RandomDescriptorCreatorVisitor.hpp"

// Nearest neighbors
#include "NearestNeighbor/metric_space_search.hpp"

// Initializers
#include "Initializers/SimpleInitializer.hpp"

// ITK
#include "itkImageFileReader.h"

// Boost
#include <boost/graph/grid_graph.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost
{
  enum vertex_data_t { vertex_data };
  BOOST_INSTALL_PROPERTY(vertex, data);
};

int main(int argc, char *argv[])
{
  // Create the graph
  typedef boost::grid_graph<2> VertexListGraphType;
  boost::array<std::size_t, 2> graphSideLengths = {{300, 300}};
  VertexListGraphType graph(graphSideLengths);
  typedef boost::graph_traits<VertexListGraphType>::vertex_descriptor VertexDescriptorType;

  // Create the topology
  const unsigned int descriptorDimension = 100;
  typedef boost::hypercube_topology<descriptorDimension, boost::minstd_rand> TopologyType;
  TopologyType space;

  // Get the index map
  typedef boost::property_map<VertexListGraphType, boost::vertex_index_t>::const_type IndexMapType;
  IndexMapType indexMap(get(boost::vertex_index, graph));

  // Create the descriptor map. This is where the data for each pixel is stored. The Topology
  typedef boost::vector_property_map<TopologyType::point_type, IndexMapType> DescriptorMapType;
  DescriptorMapType descriptorMap(num_vertices(graph), indexMap);

  //DefaultInpaintingVisitor visitor;
  RandomDescriptorCreatorVisitor<DescriptorMapType> visitor(descriptorMap, descriptorDimension);

  // Initialize
  SimpleInitializer(&visitor, graph);

  // Create the tree
  std::cout << "Creating tree..." << std::endl;
  typedef dvp_tree<VertexDescriptorType, TopologyType, DescriptorMapType > TreeType;
  // TreeType tree(graph, space, descriptorMap); // Create from all positions on the graph_traits - don't want to do this unless all positions on the topology are valid.

  // std::vector<TopologyType::point_type> validPositions; // Don't store the positions, but rather store the vertex_descriptors
  std::vector<VertexDescriptorType> validPositions;
  typename boost::graph_traits<VertexListGraphType>::vertex_iterator vi,vi_end;

  for (tie(vi,vi_end) = vertices(graph); vi != vi_end; ++vi)
  {
    validPositions.push_back(*vi);
  }

  std::cout << "There are " << validPositions.size() << " validPositions." << std::endl;

  std::cout << "Creating tree..." << std::endl;
  TreeType tree(validPositions.begin(), validPositions.end(), space, descriptorMap);
  std::cout << "Finished creating tree." << std::endl;

  // Create the nearest neighbor finder
  typedef multi_dvp_tree_search<VertexListGraphType, TreeType> SearchType;
  SearchType nearestNeighborFinder;
  nearestNeighborFinder.graph_tree_map[&graph] = &tree;

  return EXIT_SUCCESS;
}


#include "wrapper.hpp"
#include "piano.hpp"
#include "python_common.hpp"
#include <Eigen/Dense>
#include <numpy/ndarrayobject.h>
#include <portaudio.h>

static PyModuleDef Module = {
    PyModuleDef_HEAD_INIT,
    "spectrum_analyzer",
    "",
    -1,
    nullptr,
    nullptr,
    nullptr,
    nullptr,
    nullptr
};

int PaInit(PyObject*, PyObject*, PyObject*) {
    // We don't acquire GIL here becuase portaudio won't access Python objects.
    Pa_Initialize();
    return 0;
}

void PaEnd(PyObject*) {
    Pa_Terminate();
}

PyTypeObject PaInitializerType {
    PyVarObject_HEAD_INIT(NULL, 0)
    "_PaInitializer",          /* tp_name */
    sizeof(PyObject),          /* tp_basicsize */
    0,                         /* tp_itemsize */
    PaEnd,                     /* tp_dealloc */
    0,                         /* tp_print */
    0,                         /* tp_getattr */
    0,                         /* tp_setattr */
    0,                         /* tp_reserved */
    0,                         /* tp_repr */
    0,                         /* tp_as_number */
    0,                         /* tp_as_sequence */
    0,                         /* tp_as_mapping */
    0,                         /* tp_hash  */
    0,                         /* tp_call */
    0,                         /* tp_str */
    0,                         /* tp_getattro */
    0,                         /* tp_setattro */
    0,                         /* tp_as_buffer */
    Py_TPFLAGS_DEFAULT,        /* tp_flags */
    0,                         /* tp_doc */
    0,                         /* tp_traverse */
    0,                         /* tp_clear */
    0,                         /* tp_richcompare */
    0,                         /* tp_weaklistoffset */
    0,                         /* tp_iter */
    0,                         /* tp_iternext */
    0,                         /* tp_methods */
    0,                         /* tp_members */
    0,                         /* tp_getset */
    0,                         /* tp_base */
    0,                         /* tp_dict */
    0,                         /* tp_descr_get */
    0,                         /* tp_descr_set */
    0,                         /* tp_dictoffset */
    PaInit,                    /* tp_init */
    0,                         /* tp_alloc */
    PyType_GenericNew          /* tp_new */
};

PyObject* get_pitch(PyObject*, PyObject* args) {
    PyObject* py_wav;
    unsigned interval;
    if (!PyArg_ParseTuple(args, "OI", &py_wav, &interval))
        return nullptr;
    wav_file& wav = *static_cast<py_wav_handle*>(py_wav);
    const std::size_t len = wav.size_of(interval);
    Eigen::VectorXd ref(len, 1);
    if (wav.get(len, ref.data())) {
        PyErr_SetString(PyExc_IndexError, "File length exceeded");
        return nullptr;
    }
    // Fitting form x[i] = sum(a[j] * cos(omega[j] * n) + b[j] * sin(omega[j] * n), j)
    // where i and n are time, namely n = i / sample_rate
    //Eigen::MatrixXd cos_sin_consts(len, 88 * 2);
    //for (int i = 0; i != len; ++i) {
    //    // first 88 cos, second 88 sin
    //    for (int j = 0; j != 88; ++j) {
    //        double tmp = i * piano_omegas[j] / wav.sample_rate;
    //        cos_sin_consts(i, j) = std::cos(tmp);
    //        cos_sin_consts(i, 88 + j) = std::sin(tmp);
    //    }
    //}
    Eigen::MatrixXd tmp(2 * 88, 2 * 88); // = cos_sin_consts.transpose() * cos_sin_consts
    //for (int i = 0; i < 88; ++i) {
    //    for (int j = 0; j < 88; ++j) {
    //        double acc1 = 0, acc2 = 0, acc3 = 0, acc4 = 0;
    //        for (int k = 0; k != len; ++k) {
    //            double tmp = k * piano_omegas[i] / wav.sample_rate;
    //            double tmp2 = k * piano_omegas[j] / wav.sample_rate;
    //            acc1 += std::cos(tmp) * std::cos(tmp2);
    //            acc2 += std::cos(tmp) * std::sin(tmp2);
    //            acc3 += std::sin(tmp) * std::cos(tmp2);
    //            acc4 += std::sin(tmp) * std::sin(tmp2);
    //        }
    //        tmp(i, j) = acc1;
    //        tmp(i, 88 + j) = acc2;
    //        tmp(88 + i, j) = acc3;
    //        tmp(88 + i, 88 + j) = acc4;
    //    }
    //}
    // Lagrange's formulae applied and optimized for symmetricity
    for (int i = 0; i != 88; ++i) {
        double htmp3 = piano_omegas[i] / wav.sample_rate;
        double tmp3 = 2 * htmp3;
        tmp(i, i) = len - 0.5 + std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3));
        tmp(i, 88 + i) = 0.5 * (std::cos(htmp3) - std::cos(len * tmp3 + htmp3)) / std::sin(htmp3);
        tmp(88 + i, 88 + i) = len + 0.5 - std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3));
        for (int j = i + 1; j != 88; ++j) {
            double tmp1 = piano_omegas[i] / wav.sample_rate;
            double tmp2 = piano_omegas[j] / wav.sample_rate;
            double tmp3 = tmp1 + tmp2, tmp4 = tmp1 - tmp2;
            double htmp3 = tmp3 * 0.5, htmp4 = tmp4 * 0.5;
            tmp(i, j) = std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3)) + std::sin(len * tmp4 + htmp4) / (2 * std::sin(htmp4)) - 1;
            tmp(i, 88 + j) = 0.5 * (std::cos(htmp3) - std::cos(len * tmp3 + htmp3)) / std::sin(htmp3) - 0.5 * (std::cos(htmp4) - std::cos(len * tmp4 + htmp4)) / std::sin(htmp4);
            /* tmp(88 + i, j) */ tmp(j, 88 + i) = 0.5 * (std::cos(htmp3) - std::cos(len * tmp3 + htmp3)) / std::sin(htmp3) + 0.5 * (std::cos(htmp4) - std::cos(len * tmp4 + htmp4)) / std::sin(htmp4);
            tmp(88 + i, 88 + j) = std::sin(len * tmp4 + htmp4) / (2 * std::sin(htmp4)) - std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3));
        }
    }
    tmp /= 2;
    Eigen::Matrix<double, 88 * 2, 1> rhs; // = cos_sin_consts.tranpose() * ref
#pragma omp parallel for
    for (int i = 0; i < 88; ++i) {
        double acc0 = 0;
        double acc1 = 0, acc2 = 0;
        for (int j = 0; j != len; ++j) {
            acc1 += std::cos(acc0) * ref[j];
            acc2 += std::sin(acc0) * ref[j];
            acc0 += piano_omegas[i] / wav.sample_rate;
        }
        rhs[i] = acc1;
        rhs[88 + i] = acc2;
    }
    Eigen::Matrix<double, 88 * 2, 1> sol = tmp.selfadjointView<Eigen::Upper>().llt().solve(rhs);
    Eigen::Map<Eigen::Array<double, 88, 1>> a(sol.data());
    Eigen::Map<Eigen::Array<double, 88, 1>> b(sol.data() + 88);
    npy_intp dim[] = { 88 };
    PyObject* ret = PyArray_SimpleNew(1, dim, NPY_DOUBLE);
    if (!ret)
        return nullptr;
    Eigen::Map<Eigen::Array<double, 88, 1>> power(static_cast<double*>(PyArray_GETPTR1(ret, 0)));
    power = (a.cwiseAbs2() + b.cwiseAbs2());
    return ret;
}

static PyMethodDef Methods[] = {
    { "get_pitch", get_pitch, METH_VARARGS, "" },
    { nullptr }
};

PyMODINIT_FUNC
PyInit_spectrum_analyzer() {
    PyObject* m = PyExc(PyModule_Create(&Module), nullptr);
    import_array();
    PyModule_AddFunctions(m, Methods);
    PyOnly(PyType_Ready(&py_wav_handle_type), 0);
    Py_INCREF(&py_wav_handle_type);
    PyModule_AddObject(m, "wav_file", reinterpret_cast<PyObject*>(&py_wav_handle_type));
    PyOnly(PyType_Ready(&PaInitializerType), 0);
    Py_INCREF(&PaInitializerType);
    PyOnly(PyType_Ready(&pa_stream_handle_type), 0);
    Py_INCREF(&pa_stream_handle_type);
    auto empty_arg = PyTuple_New(0);
    auto init = PyObject_CallObject(reinterpret_cast<PyObject*>(&PaInitializerType), empty_arg);
    Py_DECREF(empty_arg);
    PyModule_AddObject(m, "__Do_NOT_TouchThis", init);
    return m;
}


/*
*  Copyright 2017 Richard Forster
*
*  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
*/

#include "community.h"
#include "Graph.h"
#include "Status.h"
#include "StatusCUDA.h"
#include "CUDAGraph.h"
#include "partition.h"
#include "cached_allocator.h"
//#include "IntroSort.h"
//#include "vectorSummation.h"
#include "utils.h"
#include <set>
#include <map>
#include <utility>
#include <time.h>
#define _USE_MATH_DEFINES
#include <math.h>

#include <iostream>
#include <fstream>
#include <string>
#include <thread>
#include <mutex>
#include <atomic>
#include <list>
#include <algorithm>

#include <cuda.h>
#include <cuda_runtime.h>
#include <cuda_profiler_api.h>
#include <thrust/reduce.h>
#include <thrust/unique.h>
#include <thrust/iterator/counting_iterator.h>
#include <thrust/execution_policy.h>

//#include <cudpp_hash.h>

#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/class.hpp>

using namespace std;

#define __PASS_MAX -1
#define __MIN 0.0000001

cached_allocator alloc;

float modularity(Status status, long localWeight)
{
	float links = status.totalWeight;

	float result = 0.0;

	for (int i = 0; i < status.keys; ++i)
	{
        if (status.node[i] == -1)
            continue;

		float inDegree = status.internals[i];

		float degree = status.degrees[i];

		if (links > 0)
			// result += inDegree / (float)localWeight - pow(degree, 2) / (float)localWeight / (float)localWeight;
			result += inDegree / links - pow((degree / ((float)2.0 * links)), 2);
	}

	return result;
}

Partition partitionAtLevel(std::vector<Partition> &dendogram, int level)
{
	Partition partition = dendogram[0];

	for (int i = 1; i < level + 1; ++i)
	{
		for (int j = 0; j < partition.count; ++j)
		{
			int node = partition.partition.node[j];
			int com = partition.partition.community[j];

			int newCom = -1;

			for (int k = 0; k < dendogram[i].count; ++k)
			{
				if (dendogram[i].partition.node[k] == com)
					newCom = dendogram[i].partition.community[k];
			}

			partition.partition.community[j] = newCom;
		}
	}

	return partition;
}

Partition getPartitions(std::vector<Partition> &dendogram, int level = 0)
{
	if ((level < 0) || (level > dendogram.size() - 1))
		throw "Invalid argument: level is not between 1 and len(dendogram) - 1 included.";

	return partitionAtLevel(dendogram, level || dendogram.size() - 1);
}

int countPartitions(Partition partitions)
{
	int max = 0;

	for (int i = 0; i < partitions.count; ++i)
	{
		if (partitions.partition.community[i] > max)
			max = partitions.partition.community[i];
	}

	return 1 + max;
}

extern float modularityCUDA(StatusCUDA &status, int nodeCount);
extern float oneLevelCUDA(StatusCUDA &status, int nodeCount, int neighboursCount, int neighbourComputeCount, float &lower);
extern void CalculateNeighbour(StatusCUDA &status, CUDAGraph &graph);
extern void initCUDA(StatusCUDA &status, CUDAGraph &graph);
extern Partition renumberCUDA(CUDPPHandle &theCudpp, StatusCUDA &status);
extern void inducedGraphCUDA(CUDPPHandle &theCudpp, StatusCUDA &status, CUDAGraph &cuGraph, Partition &d_partition);

void AllocateMemory(StatusCUDA &d_status, Graph &graph, CUDAGraph &cuGraph)
{
	cudaMalloc(&d_status.uint_memory, sizeof(unsigned int) * (graph.getNodeSize() + (graph.getEdgeSize() * 4)));

	cout << cudaGetErrorName(cudaGetLastError()) << endl;
	cout << sizeof(unsigned int) * (graph.getNodeSize() + (graph.getEdgeSize() * 4)) / 1024 / 1024 << endl;

	cudaMalloc(&d_status.int_memory, sizeof(int) * (graph.getNodeSize() * 12 + (graph.getEdgeSize() * 2) * 5));

	cout << cudaGetErrorName(cudaGetLastError()) << endl;
	cout << sizeof(int) * (graph.getNodeSize() * 12 + (graph.getEdgeSize() * 2) * 5) / 1024 / 1024 << endl;

	cudaMalloc(&d_status.float_memory, sizeof(float) * (graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2) * 2));

	cout << cudaGetErrorName(cudaGetLastError()) << endl;
	cout << sizeof(float) * (graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2) * 2) / 1024 / 1024 << endl;

	d_status.node = d_status.uint_memory;
	d_status.hash_idx = d_status.uint_memory + graph.getNodeSize();
	d_status.hash_idx_source = d_status.uint_memory + graph.getNodeSize() + (graph.getEdgeSize() * 2);
	d_status.hash_idx_target = d_status.hash_idx;// d_status.uint_memory + graph.getNodeSize() + (graph.getEdgeSize() * 3);

	d_status.nodesToCom = d_status.int_memory;
	d_status.nodesToComPrev = d_status.int_memory + graph.getNodeSize();
	d_status.nodesToComNext = d_status.int_memory + graph.getNodeSize() * 2;
	d_status.internals = d_status.int_memory + graph.getNodeSize() * 3;
	d_status.degrees = d_status.int_memory + graph.getNodeSize() * 4;

	d_status.bestCom = d_status.int_memory + graph.getNodeSize() * 5;

	d_status.comSize = d_status.int_memory + graph.getNodeSize() * 6;
	d_status.comSizeUpd = d_status.int_memory + graph.getNodeSize() * 7;
	d_status.degreeUpd = d_status.int_memory + graph.getNodeSize() * 8;
	d_status.internalUpd = d_status.int_memory + graph.getNodeSize() * 9;

	d_status.nrOfNeighbours = d_status.int_memory + graph.getNodeSize() * 10;
	d_status.startOfNeighbours = d_status.int_memory + graph.getNodeSize() * 11;

	cuGraph.node = d_status.node;// d_status.int_memory + graph.getNodeSize() * 12;

	d_status.neighbourSource_local = d_status.int_memory + graph.getNodeSize() * 12;
	d_status.neighbourCounts = d_status.int_memory + graph.getNodeSize() * 12 + (graph.getEdgeSize() * 2);
	d_status.neighbourCommunities_local = d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 2);
	d_status.neighbourCommunities = d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 3);
	d_status.neighbourSource = d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 4);
	d_status.neighbourTarget = d_status.neighbourCommunities_local;// d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 5);
	//d_status.temp_count = d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 6);
	d_status.temp_source = d_status.neighbourCommunities;// d_status.int_memory + graph.getNodeSize() * 13 + ((graph.getEdgeSize() * 2) * 7);
	//cuGraph.edgeSource = d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 5);
	//cuGraph.edgeTarget = d_status.int_memory + graph.getNodeSize() * 12 + ((graph.getEdgeSize() * 2) * 5 + graph.getEdgeSize());
	cuGraph.edgeSource_temp = d_status.neighbourSource_local;// d_status.int_memory + graph.getNodeSize() * 13 + ((graph.getEdgeSize() * 2) * 7 + graph.getEdgeSize() * 2);
	cuGraph.edgeTarget_temp = d_status.neighbourSource_local + graph.getEdgeSize();// d_status.int_memory + graph.getNodeSize() * 13 + ((graph.getEdgeSize() * 2) * 7 + graph.getEdgeSize() * 3);

	d_status.gdegrees = d_status.float_memory;
	d_status.loops = d_status.float_memory + graph.getNodeSize();
	d_status.result = d_status.float_memory + graph.getNodeSize() * 2;
	d_status.neighbourWeights = d_status.float_memory + graph.getNodeSize() * 3;
	d_status.neighbourWeights_local = d_status.float_memory + graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2);
	//d_status.neighbourWeights_store = d_status.float_memory + graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2) * 2;
	//d_status.communityIncrease = d_status.float_memory + graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2) * 3;
	//cuGraph.edgeWeight = d_status.float_memory + graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2) * 3;
	cuGraph.edgeWeight_temp = d_status.neighbourWeights_local;// d_status.float_memory + graph.getNodeSize() * 3 + (graph.getEdgeSize() * 2) * 4 + graph.getEdgeSize();
}

void DeAllocateMemory(StatusCUDA &d_status)
{
	cudaFree(d_status.uint_memory);
	cudaFree(d_status.int_memory);
	cudaFree(d_status.float_memory);
}

int first = 0;

std::vector<Partition> generate_dendogram(Graph &original_graph, map<int,pair<int,float>> &part_init)
{
	if (original_graph.getEdgeSize() == 0)
	{
		Community part;
		part.community = new int[original_graph.getNodeSize()];
		part.node = new int[original_graph.getNodeSize()];

		for (int i = 0; i < original_graph.getEdgeSize(); ++i)
		{
			part.community[i] = part.node[i] = original_graph.getNodeAt(i);
		}

		Partition p;
		p.partition = part;
		p.count = original_graph.getNodeSize();

		std::vector<Partition> ret;
		ret.push_back(p);

		return ret;
	}

	std::vector<Partition> status_list;
	StatusCUDA d_status;
	CUDAGraph cuGraph;

	//CUDPPHandle theCudpp;

	//CUDPPResult result = cudppCreate(&theCudpp);

	//d_status.hash_table_config.type = CUDPP_BASIC_HASH_TABLE;
	//d_status.hash_table_config.kInputSize = 10;// original_graph.getNodeSize();
	//d_status.hash_table_config.space_usage = 1.05;

	//clock_t begin = clock();

	//result = cudppHashTable(theCudpp, &d_status.hash_table_handle, &d_status.hash_table_config);
	//if (result != CUDPP_SUCCESS)
	//{
	//	fprintf(stderr, "Error in cudppHashTable call in"
	//		"testHashTable (make sure your device is at"
	//		"least compute version 2.0\n");
	//}

	AllocateMemory(d_status, original_graph, cuGraph);

	cuGraph.nodesSize = original_graph.getNodeSize();
	cuGraph.edgeSize = original_graph.getEdgeSize();

	cout << "nodes: " << cuGraph.nodesSize << endl;
	cout << "edges: " << cuGraph.edgeSize << endl;

	double time = 0;

	//cudaDeviceSetCacheConfig(cudaFuncCachePreferL1);

	int *neighbourSource = new int[original_graph.getEdgeSize()];
	int *neighbourTarget = new int[original_graph.getEdgeSize()];
	float *neighbourWeights = new float[original_graph.getEdgeSize()];

	double memcpy = 0;

	for (int round = 0; round < 1; ++round)
	{
		first = 1;

		clock_t test1 = clock();

		clock_t t1 = clock();

		cuGraph.nodesSize = original_graph.getNodeSize();
		cuGraph.edgeSize = original_graph.getEdgeSize();

		cudaMemcpy(cuGraph.node, original_graph.getNodes(), sizeof(unsigned int) * cuGraph.nodesSize, cudaMemcpyHostToDevice);
		cudaMemcpy(d_status.neighbourSource, original_graph.getEdgeContainer().source, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);
		cudaMemcpy(d_status.neighbourTarget, original_graph.getEdgeContainer().target, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

		cudaMemcpy(d_status.neighbourWeights, original_graph.getEdgeContainer().weight, sizeof(float) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

		//cudaMemcpy(cuGraph.edgeSource, original_graph.getEdgeContainer().source, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);
		//cudaMemcpy(cuGraph.edgeTarget, original_graph.getEdgeContainer().target, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

		//cudaMemcpy(cuGraph.edgeWeight, original_graph.getEdgeContainer().weight, sizeof(float) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

		clock_t t2 = clock();

		memcpy += (double)(t2 - t1) / CLOCKS_PER_SEC;

		//cout << "cuGraph init memcpy time: " << (double)(t2 - t1) / CLOCKS_PER_SEC << endl;

		CUDPPHandle theCudpp;

		t1 = clock();

		CUDPPResult result = cudppCreate(&theCudpp);

		if (result != CUDPP_SUCCESS)
		{
			fprintf(stderr, "Error initializing CUDPP Library.\n");
			unsigned int retval = 1;
			//return retval;
		}

		d_status.hash_table_config.type = CUDPP_BASIC_HASH_TABLE;
		d_status.hash_table_config.kInputSize = original_graph.getNodeSize();
		d_status.hash_table_config.space_usage = 1.05;

		clock_t begin = clock();

		result = cudppHashTable(theCudpp, &d_status.hash_table_handle, &d_status.hash_table_config);
		if (result != CUDPP_SUCCESS)
		{
			fprintf(stderr, "Error in cudppHashTable call in"
				"testHashTable (make sure your device is at"
				"least compute version 2.0\n");
		}
		t2 = clock();

		//cout << "cudpp creation: " << (double)(t2 - t1) / CLOCKS_PER_SEC << endl;

		double neighbour = 0, init = 0, onelevel = 0, renumber = 0, induce = 0;

		t1 = clock();

		CalculateNeighbour(d_status, cuGraph);

		t2 = clock();

		neighbour += (double)(t2 - t1) / CLOCKS_PER_SEC;

		//cout << "cuGraph CalculateNeighbour time: " << (double)(t2 - t1) / CLOCKS_PER_SEC << endl;

		clock_t start = clock();

		initCUDA(d_status, cuGraph);

		clock_t end = clock();

		init += (double)(end - start) / CLOCKS_PER_SEC;

		//cout << "init time cuda: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

		map<int, pair<int, float>> empty_map;

		start = clock();

		float mod = modularityCUDA(d_status, original_graph.getNodeSize());// modularity(status, localWeight);

		end = clock();

		//cout << "modularity time: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

		//cout << "modularity: " << mod << endl;

		start = clock();

		float new_mod =  oneLevelCUDA(d_status, cuGraph.nodesSize, cuGraph.nrOfAllNeighbours, cuGraph.edgeSize * 2, mod);

		end = clock();

		onelevel += (double)(end - start) / CLOCKS_PER_SEC;

		//cout << "modularity with onelevel time: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

		begin = clock();

		Partition p2 = renumberCUDA(theCudpp, d_status);

		end = clock();

		renumber += (double)(end - start) / CLOCKS_PER_SEC;

		//cout << "renumberCUDA time: " << (double)(end - begin) / CLOCKS_PER_SEC << endl;

		status_list.push_back(p2);

		mod = new_mod;

		start = clock();

		cudaMemcpy(d_status.neighbourSource, original_graph.getEdgeContainer().source, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);
		cudaMemcpy(d_status.neighbourTarget, original_graph.getEdgeContainer().target, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

		cudaMemcpy(d_status.neighbourWeights, original_graph.getEdgeContainer().weight, sizeof(float) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

		end = clock();

		memcpy += (double)(end - start) / CLOCKS_PER_SEC;

		start = clock();

		inducedGraphCUDA(theCudpp, d_status, cuGraph, p2);

		end = clock();

		induce += (double)(end - start) / CLOCKS_PER_SEC;

		start = clock();

		cudaMemcpy(neighbourSource, d_status.neighbourSource, sizeof(int) * cuGraph.edgeSize, cudaMemcpyDeviceToHost);
		cudaMemcpy(neighbourTarget, d_status.neighbourTarget, sizeof(int) * cuGraph.edgeSize, cudaMemcpyDeviceToHost);

		cudaMemcpy(neighbourWeights, d_status.neighbourWeights, sizeof(float) * cuGraph.edgeSize, cudaMemcpyDeviceToHost);

		end = clock();

		memcpy += (double)(end - start) / CLOCKS_PER_SEC;

		//cout << "inducedCUDA time: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

		while (true)
		{
			//break;

			t1 = clock();

			CalculateNeighbour(d_status, cuGraph);

			t2 = clock();

			neighbour += (double)(t1 - t1) / CLOCKS_PER_SEC;

			//cout << "cuGraph CalculateNeighbour time: " << (double)(t2 - t1) / CLOCKS_PER_SEC << endl;

			start = clock();

			initCUDA(d_status, cuGraph);

			end = clock();

			init += (double)(end - start) / CLOCKS_PER_SEC;

			//cout << "init time cuda: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

			start = clock();

			new_mod = oneLevelCUDA(d_status, cuGraph.nodesSize, cuGraph.nrOfAllNeighbours, cuGraph.edgeSize * 2, mod);

			end = clock();

			//cout << "onelevel time: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

			////cout << "after onelevel" << endl;

			begin = clock();

			Partition p2 = renumberCUDA(theCudpp, d_status);

			end = clock();

			renumber += (double)(end - start) / CLOCKS_PER_SEC;

			//cout << "renumberCUDA time: " << (double)(end - begin) / CLOCKS_PER_SEC << endl;

			status_list.push_back(p2);

			if ((new_mod - mod) < __MIN)
			{
				cout << "modularity: " << mod << endl;

				break;
			}

			mod = new_mod;

			start = clock();

			cudaMemcpy(d_status.neighbourSource, neighbourSource, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);
			cudaMemcpy(d_status.neighbourTarget, neighbourTarget, sizeof(int) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

			cudaMemcpy(d_status.neighbourWeights, neighbourWeights, sizeof(float) * cuGraph.edgeSize, cudaMemcpyHostToDevice);

			end = clock();

			memcpy += (double)(end - start) / CLOCKS_PER_SEC;

			start = clock();

			inducedGraphCUDA(theCudpp, d_status, cuGraph, p2);

			end = clock();

			induce += (double)(end - start) / CLOCKS_PER_SEC;

			start = clock();

			cudaMemcpy(neighbourSource, d_status.neighbourSource, sizeof(int) * cuGraph.edgeSize, cudaMemcpyDeviceToHost);
			cudaMemcpy(neighbourTarget, d_status.neighbourTarget, sizeof(int) * cuGraph.edgeSize, cudaMemcpyDeviceToHost);

			cudaMemcpy(neighbourWeights, d_status.neighbourWeights, sizeof(float) * cuGraph.edgeSize, cudaMemcpyDeviceToHost);

			end = clock();

			memcpy += (double)(end - start) / CLOCKS_PER_SEC;

			//cout << "inducedCUDA time: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

		}

		cout << "Memcpy: " << memcpy << endl;
		cout << "neighbour: " << neighbour << endl;
		cout << "init: " << init << endl;
		cout << "onelevel: " << onelevel << endl;
		cout << "renumber: " << renumber << endl;
		cout << "induce: " << induce << endl;

		t1 = clock();

		cudppDestroyHashTable(theCudpp, d_status.hash_table_handle);
		//cudppDestroyHashTable(theCudpp, d_status.commMap_hash_table_handle);
		//cudppDestroyHashTable(theCudpp, d_status.nodeCommMap_hash_table_handle);

		result = cudppDestroy(theCudpp);
		if (result != CUDPP_SUCCESS)
		{
			fprintf(stderr, "Error shutting down CUDPP Library.\n");
		}
		//cout << "############################################################" << endl;

		t2 = clock();

		//cout << "cudpp destroy" << (double)(t2 - t1) / CLOCKS_PER_SEC << endl;

		clock_t test2 = clock();

		//for (int i = 0; i < status_list.size(); ++i)
		//	status_list[i].Deallocate();

		//status_list.clear();

		if (round > 0)
			time += (double)(test2 - test1) / CLOCKS_PER_SEC;

		cout << (double)(test2 - test1) / CLOCKS_PER_SEC << endl;
	}

	DeAllocateMemory(d_status);

	cout << "time in avg 100 runs: " << time / double(100) << endl;

	cudaProfilerStop();

	return status_list;
}

int* calculateClusterSize(Graph &graph, Partition partitions, int numberOfClusters)
{
    int *clusterSize = new int[numberOfClusters];

    for (int i = 0; i < numberOfClusters; ++i)
        clusterSize[i] = 0;

    for (int i = 0; i < graph.getNodeSize(); ++i)
    {
        int cluster;
        for (int j = 0; j < partitions.count; ++j)
        {
            if (partitions.partition.node[j] == graph.getNodeAt(i))
            {
                cluster = partitions.partition.community[j];
                break;
            }
        }

        graph.setCluster(i, cluster);
        graph.setIndexInCluster(i,clusterSize[cluster]);
        clusterSize[cluster] += 1;
    }

    for (int i = 1; i < numberOfClusters; ++i)
        clusterSize[i] += clusterSize[i-1];

    return clusterSize;
}

void circleLayout(float *x, float *y, int i, int N)
{
    *x = (50 * cos(2 * M_PI * i / N));
    *y = (50 + 50 * sin(2 * M_PI * i / N));
}

struct LayoutData
{
    int begin, end;
    int *clusterSize;
    int nrPartitions;
    int nodeCount;
    Graph *graph;
};

void layoutProcessThread(LayoutData data)
{
    int metr = 0;
    for (int j = 0; j < data.nrPartitions; ++j)
    {
        if (j > 0)
            metr = data.clusterSize[j-1];
        for (int i = data.begin; i < data.end; ++i)
        {
            if (data.graph->getCluster(i) == j)
            {
                circleLayout(data.graph->getXPtr(i), data.graph->getYPtr(i), metr+data.graph->getIndexInCluster(i), data.nodeCount);
            }
        }
    }
}

#define numberOfProc 8

void layoutProcess(int *clusterSize, Graph &graph, int nrPartitions)
{
    LayoutData data[numberOfProc];
    thread *threads[numberOfProc];

    int nodeCount = graph.getNodeSize();

    for (int i = 0; i < numberOfProc; ++i)
    {
        int parts = nodeCount / numberOfProc;
        if (nodeCount % numberOfProc != 0)
            ++parts;

        data[i].begin = i * parts;
        if ((data[i].begin + parts) > nodeCount)
            data[i].end = nodeCount;
        else
            data[i].end = data[i].begin + parts;

        data[i].clusterSize = clusterSize;
        data[i].nodeCount = nodeCount;
        data[i].graph = &graph;
        data[i].nrPartitions = nrPartitions;

        threads[i] = new thread(layoutProcessThread, data[i]);
    }

    for (int i = 0; i < numberOfProc; ++i)
    {
        threads[i]->join();
        delete threads[i];
    }
}

int nodesCount;
int edgesCount;

#include <set>

set<int> nodeSet;

int *loadNodes()
{
	ifstream nodes("nodes.txt");
	int *loadedNodes;

	////////cout << nodes.fail() << endl;
	if (!nodes.fail())
	{
		int count = 0;

		string input;
		nodes >> input;
		//////cout << input << endl;
		loadedNodes = new int[atoi(input.c_str())];

		nodes >> input;
		int i = 0;

		while (!nodes.eof())
		{
			////////cout << i << endl;
			loadedNodes[i] = atoi(input.c_str());

			pair<set<int>::iterator, bool> result = nodeSet.insert(loadedNodes[i]);

			if (result.second == true)
				++count;

			++i;

			nodes >> input;
			////////cout << "in" << endl;
		}

		nodesCount = i;
		//cout << "newly added nodes after node load: " << count << endl;
	}

	//	//////cout << "out" << endl;

	return loadedNodes;
}

Edge *loadEdges()
{
	ifstream edges("edges.txt");
	Edge *loadedEdges;
	if (!edges.fail())
	{
		string input;
		edges >> input;
		loadedEdges = new Edge[atoi(input.c_str())];

		edges >> input;
		int i = 0;

		while (!edges.eof())
		{
			loadedEdges[i].source = atoi(input.c_str());
			edges >> input;
			loadedEdges[i].target = atoi(input.c_str());
			edges >> input;
			loadedEdges[i].weight = atof(input.c_str());
			edges >> input;

			nodeSet.insert(loadedEdges[i].source);
			nodeSet.insert(loadedEdges[i].target);

			++i;
		}

		edgesCount = i;
	}

	//cout << "number of nodes after edge load: " << nodeSet.size() << endl;

	return loadedEdges;
}

int main(int argc, char* argv[])
{
	Graph graph;

	clock_t load1 = clock();

	Edge *edges = loadEdges();

	clock_t load2 = clock();

	cout << "loadedges time: " << (double)(load2 - load1) / CLOCKS_PER_SEC << endl;

	load1 = clock();

	int *nodes = loadNodes();

	load2 = clock();

	cout << "loadnodes time: " << (double)(load2 - load1) / CLOCKS_PER_SEC << endl;

	//cout << "after load" << endl;

	//cout << nodesCount << endl;
	//cout << edgesCount << endl;

	graph.setNodes(nodes, nodesCount);

	cout << "after node set" << endl;

	graph.setEdges(edges, edgesCount);

	cout << "after edge set" << endl;

	clock_t gpu1 = clock();

	cudaSetDevice(0);

	clock_t gpu2 = clock();

	//cout << "GPU set time: " << (double)(gpu2 - gpu1) / CLOCKS_PER_SEC << endl;

	gpu1 = clock();

	int *d_warmup;
	cudaMalloc(&d_warmup, sizeof(int)*1024*1024*200);
	cudaFree(d_warmup);

	gpu2 = clock();

	//cout << "GPU meminit time: " << (double)(gpu2 - gpu1) / CLOCKS_PER_SEC << endl;

	clock_t start = clock();

	map<int, pair<int, float>> init_state;

	std::vector<Partition> dendogram = generate_dendogram(graph, init_state);

	clock_t end = clock();

	cout << "generate time: " << (double)(end - start) / CLOCKS_PER_SEC << endl;

	cout << countPartitions(getPartitions(dendogram)) << endl;

	//for (int i = 0; i < dendogram.size(); ++i)
	//	dendogram[i].Deallocate();

	return 0;
}

boost::python::dict start(boost::python::list nodeList, boost::python::list edgeList, boost::python::list part_init)
{
	bool tryToRecoverFromException = false;
	boost::python::dict partitionDictionary;

	do
	{
		try
		{
			try
			{
				tryToRecoverFromException = false;

				Graph original_graph(nodeList, edgeList);
				//original_graph.makeAssocMat();

				map<int, pair<int, float>> init_state;
				map<int, int> reComm;
				int newComm = 0;

				for (int i = 0; i < boost::python::len(part_init); ++i)
				{
					boost::python::dict partDict = boost::python::extract<boost::python::dict>(part_init[i]);
					pair<int, float> partition;

					int comm = boost::python::extract<int>(partDict["community"]);

					if (reComm.find(comm) != reComm.end())
						comm = reComm[comm];
					else
					{
						reComm[comm] = newComm;
						comm = newComm;
						++newComm;
					}

					float weight = boost::python::extract<float>(partDict["weight"]);

					partition.first = comm;
					partition.second = weight;
					init_state[boost::python::extract<int>(partDict["node"])] = partition;
				}

				clock_t start = clock();

				std::vector<Partition> dendogram = generate_dendogram(original_graph, init_state);

				clock_t end = clock();

				//cout << "louvain in " << (double)(end-start) / CLOCKS_PER_SEC << endl;

				start = clock();

				Partition partitions = getPartitions(dendogram);

				end = clock();

				cout << "get partitions " << (double)(end-start) / CLOCKS_PER_SEC << endl;

				start = clock();

				int nrPartitions = countPartitions(partitions);

				end = clock();

				cout << "count partitions " << (double)(end-start) / CLOCKS_PER_SEC << " " << nrPartitions << endl;

				start = clock();

				int *clusterSize = calculateClusterSize(original_graph, partitions, nrPartitions);

				end = clock();

				cout << "cluster size time " << (double)(end-start) / CLOCKS_PER_SEC << endl;

				start = clock();

				layoutProcess(clusterSize, original_graph, nrPartitions);

				delete[] clusterSize;

				end = clock();

				cout << "layout process time " << (double)(end - start) / CLOCKS_PER_SEC << endl;

				start = clock();

				partitionDictionary["size"] = nrPartitions;
				for (int i = 0; i < partitions.count; ++i)
				{
					partitionDictionary[partitions.partition.node[i]] = partitions.partition.community[i];
				}

				end = clock();

				cout << "dictionary done" << endl;

				for (int i = 0; i < dendogram.size(); ++i)
					dendogram[i].Deallocate();

				cout << "deallocate done" << endl;

				for (int i = 0; i < original_graph.getNodeSize(); ++i)
				{
					boost::python::dict nodeDict = boost::python::extract<boost::python::dict>(nodeList[i]);
					nodeDict["cluster"] = original_graph.getCluster(i);
					nodeDict["my_community"] = original_graph.getCluster(i);
					nodeDict["x"] = original_graph.getX(i);
					nodeDict["y"] = original_graph.getY(i);
				}

				cout << "ready to return" << endl;
			}
			catch (/*bad_alloc exception*/ exception e)
			{
				//cout << "bad alloc?" << endl;
				cout << e.what() << endl;
				tryToRecoverFromException = false;
			}

			cout << "ready to return 2" << endl;
		}
		catch (...)
		{
			tryToRecoverFromException = true;
		}

		cout << "ready to return 3" << endl;
	} while (tryToRecoverFromException);

	cout << "before return" << endl;

	return partitionDictionary;
}

#if defined(_WIN32)
BOOST_PYTHON_MODULE(Louvain_ext_Windows)
#elif defined(__linux__)
BOOST_PYTHON_MODULE(Louvain_ext_CentOS)
#endif
{
	using namespace boost::python;
	def("start", start);
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
  Copyright (C) 2013 Peter Caspers

  This file is part of QuantLib, a free-software/open-source library
  for financial quantitative analysts and developers - http://quantlib.org/

  QuantLib is free software: you can redistribute it and/or modify it
  under the terms of the QuantLib license.  You should have received a
  copy of the license along with this program; if not, please email
  <quantlib-dev@lists.sf.net>. The license is also available online at
  <http://quantlib.org/license.shtml>.

  This program is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  or FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file cmsreplicationpricer.cpp
  \brief
*/

#include <ql/cashflows/cmsreplicationpricer.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/cmscoupon.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/time/schedule.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/pricingengines/blackformula.hpp>

#include <boost/make_shared.hpp>
#include <ql/termstructures/volatility/atmsmilesection.hpp>

namespace QuantLib {

    CmsReplicationPricer::CmsReplicationPricer(const Handle<SwaptionVolatilityStructure>& swaptionVol,
                                               const Handle<Quote>& meanReversion,
                                               const Handle<YieldTermStructure>& couponDiscountCurve,
                                               const Settings& settings) :
        CmsCouponPricer(swaptionVol), meanReversion_(meanReversion),
        couponDiscountCurve_(couponDiscountCurve), settings_(settings) {

        registerWith(meanReversion_);
        if(!couponDiscountCurve_.empty())
            registerWith(couponDiscountCurve_);
    }

    void CmsReplicationPricer::initialize(const FloatingRateCoupon& coupon){

        coupon_ =  dynamic_cast<const CmsCoupon*>(&coupon);
        QL_REQUIRE(coupon_, "CMS coupon needed");
        gearing_ = coupon_->gearing();
        spread_ = coupon_->spread();
        Time accrualPeriod = coupon_->accrualPeriod();
        QL_REQUIRE(accrualPeriod != 0.0, "null accrual period");

        fixingDate_ = coupon_->fixingDate();
        paymentDate_ = coupon_->date();
        swapIndex_ = coupon_->swapIndex();

        forwardCurve_ = swapIndex_->forwardingTermStructure();
        if(swapIndex_->exogenousDiscount())
            discountCurve_ = swapIndex_->discountingTermStructure();
        else
            discountCurve_ = forwardCurve_;

        // if no coupon discount curve is given just use the discounting curve from the swap index.
        // for rate calculation this curve cancels out in the computation, so e.g. the discounting
        // swap engine will produce correct results, even if the couponDiscountCurve is not set here.
        // only the price member function in this class will be dependent on the coupon discount curve.

        if(couponDiscountCurve_.empty())
            couponDiscountCurve_ = discountCurve_;

        today_ = QuantLib::Settings::instance().evaluationDate();

        if(paymentDate_ > today_)
            discount_ = couponDiscountCurve_->discount(paymentDate_);
        else discount_= 1.;

        spreadLegValue_ = spread_ * accrualPeriod * discount_;

        if (fixingDate_ > today_) {

            swapTenor_ = swapIndex_->tenor();
            swap_ = swapIndex_->underlyingSwap(fixingDate_);

            swapRateValue_ = swap_->fairRate();
            annuity_ = 1.0E4 * std::fabs(swap_->fixedLegBPS());

            boost::shared_ptr<SmileSection> sectionTmp =
                swaptionVolatility()->smileSection(fixingDate_, swapTenor_);

            // if the section does not provide an atm level, we enhance it to
            // have one, no need to exit with an exception ...

            if (sectionTmp->atmLevel() == Null<Real>())
                smileSection_ = boost::make_shared<AtmSmileSection>(
                    sectionTmp, swapRateValue_);
            else
                smileSection_ = sectionTmp;

            const Leg& fixedCoupons = swap_->fixedLeg();
            fixedLegPaymentDates_ = std::vector<Date>(fixedCoupons.size());
            fixedLegYearFractions_ = std::vector<Real>(fixedCoupons.size());
            for(Size i=0; i<fixedCoupons.size(); i++) {
                boost::shared_ptr<FixedRateCoupon> coupon =
                    boost::dynamic_pointer_cast<FixedRateCoupon>(fixedCoupons[i]);
                fixedLegPaymentDates_[i] = coupon->date();
                fixedLegYearFractions_[i] = coupon->accrualPeriod();
            }

            const Leg& floatingCoupons = swap_->floatingLeg();
            floatingLegStartDates_ = floatingLegEndDates_ = floatingLegPaymentDates_ =
                std::vector<Date>(floatingCoupons.size());
            for(Size i=0; i<floatingCoupons.size(); i++) {
                boost::shared_ptr<IborCoupon> coupon =
                    boost::dynamic_pointer_cast<IborCoupon>(floatingCoupons[i]);
                floatingLegStartDates_[i] = coupon->accrualStartDate();
                floatingLegEndDates_[i] = coupon->accrualEndDate();
                floatingLegPaymentDates_[i] = coupon->date();
            }

        }
    }

    Real CmsReplicationPricer::hullWhiteScenario(Real dt, Real h) const {

        Real e;

        if(close(meanReversion_->value(),0.0))
            e = dt;
        else
            e = (1.0 - std::exp( -dt * meanReversion_->value() ) ) / meanReversion_->value();

        return std::exp( -h * e );

    }

    Real CmsReplicationPricer::annuity(Real h) const {

        Real annuity=0.0;
        for(Size i=0; i<fixedLegYearFractions_.size(); i++) {
            Real dt = discountCurve_->dayCounter().yearFraction(fixingDate_,fixedLegPaymentDates_[i]);
            annuity += fixedLegYearFractions_[i] * discountCurve_->discount(fixedLegPaymentDates_[i])
                / discountCurve_->discount(fixingDate_) * hullWhiteScenario(dt,h);
        }

        return annuity;

    }

    Real CmsReplicationPricer::floatingLegNpv(Real h) const {

        if(settings_.simplifiedFloatingLeg_) {
            Real dt1 = discountCurve_->dayCounter().yearFraction(fixingDate_,floatingLegStartDates_.front());
            Real dt2 = discountCurve_->dayCounter().yearFraction(fixingDate_,floatingLegPaymentDates_.back());
            return (discountCurve_->discount(floatingLegStartDates_.front()) * hullWhiteScenario(dt1,h) -
                    discountCurve_->discount(floatingLegPaymentDates_.back()) * hullWhiteScenario(dt2,h)) /
                discountCurve_->discount(fixingDate_);
        }
        else {
            Real npv=0.0;
            for(Size i=0; i<floatingLegStartDates_.size();i++) {
                Real dt1 = forwardCurve_->dayCounter().yearFraction(fixingDate_,floatingLegStartDates_[i]);
                Real dt2 = forwardCurve_->dayCounter().yearFraction(fixingDate_,floatingLegEndDates_[i]);
                Real dt3 = discountCurve_->dayCounter().yearFraction(fixingDate_,floatingLegPaymentDates_[i]);
                // we use that the day counter of the floating leg is equal to that of the float index always for
                // swapIndex underlying swaps
                npv += (forwardCurve_->discount(floatingLegStartDates_[i]) *
                            hullWhiteScenario(dt1, h) /
                            (forwardCurve_->discount(floatingLegEndDates_[i]) *
                             hullWhiteScenario(dt2, h)) -
                        1.0) *
                       discountCurve_->discount(floatingLegPaymentDates_[i]) /
                       discountCurve_->discount(fixingDate_) *
                       hullWhiteScenario(dt3, h);
            }
            return npv;
        }

    }

    Real CmsReplicationPricer::swapRate(Real h) const {

        return floatingLegNpv(h) / annuity(h);

    }

    Real CmsReplicationPricer::h(Real rate) const {

        Real a = -10.0, b = 10.0;
        HHelper h(this,rate);
        Brent solver;

        Real c;

        try {
            c = solver.solve(h,1.0E-6,0.0,a,b);
        } catch(QuantLib::Error e) {
            QL_FAIL("can not imply h from rate (" << rate << "):" << e.what());
        }

        return c;

    }

    Real CmsReplicationPricer::strikeFromVegaRatio(Real ratio, Option::Type optionType, Real referenceStrike) const {

        Real a,b,min,max,k;
        if(optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k = std::min(smileSection_->maxStrike(), settings_.upperRateBound_);
        }
        else {
            a = min = k = std::max(smileSection_->minStrike(), settings_.lowerRateBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        VegaRatioHelper h(&*smileSection_,smileSection_->vega(swapRateValue_)*ratio);
        Brent solver;

        try {
            k = solver.solve(h,1.0E-5,(a+b)/2.0,a,b);
        } catch(...) {
            // use default value set above
        }

        return std::min(std::max(k,min),max);

    }

    Real CmsReplicationPricer::strikeFromPrice(Real price, Option::Type optionType, Real referenceStrike) const {

        Real a,b,min,max,k;
        if(optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k = std::min(smileSection_->maxStrike(), settings_.upperRateBound_);
        }
        else {
            a = min = k = std::max(smileSection_->minStrike(), settings_.lowerRateBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        PriceHelper h(&*smileSection_, optionType, price);
        Brent solver;

        try {
            k = solver.solve(h,1.0E-5,swapRateValue_,a,b);
        } catch(...) {
            // use default value set above
        }

        return std::min(std::max(k,min),max);

    }

    Real CmsReplicationPricer::optionletPrice(Option::Type optionType, Real strike) const {

        Real phi = optionType == Option::Call ? 1.0 : -1.0;

        if(optionType == Option::Call && strike >= settings_.upperRateBound_) return 0.0;
        if(optionType == Option::Put && strike <= settings_.lowerRateBound_) return 0.0;

        // determine vector of hs corresponding to scenarios;

        std::vector<Real> hs;
        switch(settings_.strategy_) {

        case Settings::DiscreteStrikeSpreads : {
            for(Size i=0;i<settings_.n_;i++) {
                Real k = strike + phi*settings_.discreteStrikeSpreads_[i];
                if(k>=smileSection_->minStrike() && k<=smileSection_->maxStrike())
                    hs.push_back( h(k) );
            }
            break;
        }

        case Settings::RateBound : {
            Real h1 = h(strike);
            Real h2 = h(optionType == Option::Call ? settings_.upperRateBound_ : settings_.lowerRateBound_);
            for(Size i=0;i<settings_.n_;i++) {
                hs.push_back(h1+((Real)(i+1)/(settings_.n_-1))*(h2-h1));
            }
            break;
        }

        case Settings::VegaRatio : {
            // strikeFromVegaRatio ensures that returned strike is on the expected side of strike
            Real effectiveBound = optionType == Option::Call ?
                std::min(settings_.upperRateBound_ ,
                         strikeFromVegaRatio(settings_.vegaRatio_,optionType,strike)) :
                std::max(settings_.lowerRateBound_,
                         strikeFromVegaRatio(settings_.vegaRatio_,optionType,strike));
            if(close(fabs(effectiveBound-strike),0.0)) return 0.0;
            Real h1 = h(strike);
            Real h2 = h(effectiveBound);
            for(Size i=0;i<settings_.n_;i++) {
                hs.push_back(h1+((Real)(i+1)/(settings_.n_-1))*(h2-h1));
            }
            break;
        }

        case Settings::PriceThreshold : {
            // strikeFromPrice ensures that returned strike is on the expected side of strike
            Real effectiveBound = optionType == Option::Call ?
                std::min(settings_.upperRateBound_,
                         strikeFromPrice(settings_.priceThreshold_,optionType,strike)) :
                std::max(settings_.lowerRateBound_,
                         strikeFromPrice(settings_.priceThreshold_,optionType,strike));
            if(close(fabs(effectiveBound-strike),0.0)) return 0.0;
            Real h1 = h(strike);
            Real h2 = h(effectiveBound);
            for(Size i=0;i<settings_.n_;i++) {
                hs.push_back(h1+((Real)(i+1)/(settings_.n_-1))*(h2-h1));
            }
            break;
        }

        default:
            QL_FAIL("Unknown strategy (" << settings_.strategy_ << ")");

        }

        // compute the hedge basket and price it
        std::vector<Real> weights, strikes;

        Real rate = strike, lastRate;
        Real price = QL_MAX_REAL, lastPrice, basketPrice = 0.0;

        for(Size i=0;i<hs.size();i++) {
            lastPrice = price;
            lastRate = rate;
            rate = swapRate(hs[i]);
            strikes.push_back(lastRate);
            Real ann = annuity(hs[i]);
            Real npv=0.0;
            for(Size j=0;j<i;j++) { // this can be done _much_ more efficiently ...
                npv += weights[j] * phi * (rate - strikes[j]) * ann;
            }
            Real dt = discountCurve_->dayCounter().yearFraction(fixingDate_,paymentDate_);
            weights.push_back( ( phi * discountCurve_->discount(paymentDate_) /
                                 discountCurve_->discount(fixingDate_) *
                                 hullWhiteScenario(dt,hs[i]) * (rate - strike) - npv ) /
                               ( phi * ann * (rate - lastRate) ) );
            price = blackFormula(optionType,lastRate,swapRateValue_,
                                 std::sqrt(smileSection_->variance(lastRate)),annuity_);
            if(settings_.enforceMonotonicPrices_)
                price = std::min( lastPrice, price );
            basketPrice += weights.back()*price;
        }

        // note that fixedLegBPS() is computed w.r.t. discountCurve_, but the coupon discount curve
        // may be different. We have to take this into account here.

        basketPrice *=  coupon_->accrualPeriod() * discount_ / discountCurve_->discount(paymentDate_);

        return basketPrice;

    }

    Real CmsReplicationPricer::meanReversion() const { return meanReversion_->value();}

    Rate CmsReplicationPricer::swapletRate() const {
        return swapletPrice()/(coupon_->accrualPeriod()*discount_);
    }

    Real CmsReplicationPricer::capletPrice(Rate effectiveCap) const {
        // caplet is equivalent to call option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs =
                std::max(coupon_->swapIndex()->fixing(fixingDate_)-effectiveCap, 0.);
            Rate price = (gearing_*Rs)*(coupon_->accrualPeriod()*discount_);
            return price;
        } else {
            Real capletPrice = optionletPrice(Option::Call, effectiveCap);
            return gearing_ * capletPrice;
        }
    }

    Rate CmsReplicationPricer::capletRate(Rate effectiveCap) const {
        return capletPrice(effectiveCap)/(coupon_->accrualPeriod()*discount_);
    }

    Real CmsReplicationPricer::floorletPrice(Rate effectiveFloor) const {
        // floorlet is equivalent to put option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs =
                std::max(effectiveFloor-coupon_->swapIndex()->fixing(fixingDate_),0.);
            Rate price = (gearing_*Rs)*(coupon_->accrualPeriod()*discount_);
            return price;
        } else {
            Real floorletPrice = optionletPrice(Option::Put, effectiveFloor);
            return gearing_ * floorletPrice;
        }
    }

    Rate CmsReplicationPricer::floorletRate(Rate effectiveFloor) const {
        return floorletPrice(effectiveFloor)/(coupon_->accrualPeriod()*discount_);
    }

    Real CmsReplicationPricer::swapletPrice() const {

        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = coupon_->swapIndex()->fixing(fixingDate_);
            Rate price = (gearing_*Rs + spread_)*(coupon_->accrualPeriod()*discount_);
            return price;
        } else {
            Real atmCapletPrice = optionletPrice(Option::Call, swapRateValue_);
            Real atmFloorletPrice = optionletPrice(Option::Put, swapRateValue_);
            return gearing_ *(coupon_->accrualPeriod()* discount_ * swapRateValue_
                              + atmCapletPrice - atmFloorletPrice)
                + spreadLegValue_;
        }
    }

}


// Copyright 2019 Netherlands eScience Center and ASTRON
// Licensed under the Apache License, version 2.0. See LICENSE for details.
#include <complex>

#include <boost/multi_array.hpp>

namespace correlator
{
constexpr int NR_INPUTS = 2 * 576;
constexpr int NR_CHANNELS = 64;
constexpr int NR_SAMPLES_PER_CHANNEL = 3072;
constexpr double SUBBAND_BANDWIDTH = 195312.5;
constexpr int NR_TAPS = 16;
constexpr int NR_BASELINES = NR_INPUTS * (NR_INPUTS + 1) / 2;
constexpr int NR_SAMPLES_PER_MINOR_LOOP = 64;

typedef boost::multi_array<std::complex<float>, 3> InputDataType;
const auto InputDataDims = boost::extents[NR_INPUTS][NR_CHANNELS][NR_SAMPLES_PER_CHANNEL + NR_TAPS - 1];
typedef boost::multi_array<std::complex<float>, 3> FilteredDataType;
const auto FilteredDataDims = boost::extents[NR_INPUTS][NR_SAMPLES_PER_CHANNEL][NR_CHANNELS];
typedef boost::multi_array<float, 2> FilterWeightsType;
const auto FilterWeightsDims = boost::extents[NR_CHANNELS][NR_TAPS];
typedef boost::multi_array<float, 1> BandPassCorrectionWeights;
const auto BandPassCorrectionWeightsDims = boost::extents[NR_CHANNELS];
typedef boost::multi_array<double, 1> DelaysType;
const auto DelaysDims = boost::extents[NR_INPUTS];
typedef boost::multi_array<std::complex<float>, 3> CorrectedDataType;
const auto CorrectedDataDims = boost::extents[NR_CHANNELS][NR_INPUTS][NR_SAMPLES_PER_CHANNEL];
typedef boost::multi_array<std::complex<float>, 2> VisibilitiesType;
const auto VisibilitiesDims = boost::extents[NR_CHANNELS][NR_BASELINES];

typedef boost::multi_array<std::complex<float>, 2> FusedFilterType;
const auto FusedFilterDims = boost::extents[NR_SAMPLES_PER_MINOR_LOOP][NR_CHANNELS];
typedef boost::multi_array<std::complex<float>, 1> ComplexChannelType;
const auto ComplexChannelDims = boost::extents[NR_CHANNELS];

extern const bool delay_compensation;
extern const bool bandpass_correction;

void FFT(FilteredDataType&);

FilteredDataType
FIR_filter(const InputDataType&, const FilterWeightsType&);

CorrectedDataType
transpose(const FilteredDataType&, const BandPassCorrectionWeights&);

void
applyDelays(CorrectedDataType&, const DelaysType&, const DelaysType&, double);

CorrectedDataType
pipeline
( const InputDataType&
, const FilterWeightsType&
, const BandPassCorrectionWeights&
, const DelaysType&
, const DelaysType&
, double
);

VisibilitiesType
correlate(const CorrectedDataType&);
} // namespace correlator

namespace correlator::fused
{
void
FFT(boost::multi_array_ref<std::complex<float>, 2>::reference);

void
FIR_filter
( const InputDataType&
, const FilterWeightsType&
, FusedFilterType&
, unsigned
, unsigned
);

void
transposeInit
( ComplexChannelType&
, ComplexChannelType&
, const BandPassCorrectionWeights&
, const DelaysType&
, const DelaysType&
, double
, unsigned
);

void
transpose
( CorrectedDataType&
, const BandPassCorrectionWeights&
, FusedFilterType&
, ComplexChannelType&
, ComplexChannelType&
, unsigned
, unsigned
);

CorrectedDataType
pipeline
( const InputDataType&
, const FilterWeightsType&
, const BandPassCorrectionWeights&
, const DelaysType&
, const DelaysType&
, double
);
} // namespace correlator::fused


#pragma once
#include <ros/ros.h>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <fake_scan.hpp>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <particle.hpp>
#include <point.hpp>
#include <pose_estimation_icp.hpp>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <time.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <visualization_msgs/MarkerArray.h>

typedef boost::normal_distribution<> GaussianDistribution;
typedef boost::mt19937 RandomGenerator;
typedef boost::variate_generator<RandomGenerator, GaussianDistribution> GaussianGenerator;

namespace Turtlebot
{

class Localization
{
public:
    Localization(ros::NodeHandle &nh, ros::NodeHandle &pnh);
    ~Localization();

    void Localize();

private:
    void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);
    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg);
    void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
    void initializeLocalization();
    const std::deque<Particle> sampleParticles();
    const bool checkForCollision(const Point &pt) const;
    const int getMapLocation(const Point &pt) const;
    void takeActionParticles(std::deque<Particle> &particles);
    void calcParticleWeights(std::deque<Particle> &particles);
    const double calcDistanceScore(const tf::Point &particle_pt, const tf::Point &sensor_pt);
    const double calcRotationScore(const tf::Quaternion &particle_q, const tf::Quaternion &sensor_q);
    void pruneAndNormalizeParticles();
    const tf::Pose calcFinalPose();
    void pubParticles();
    void integratePoseToCurrentTime(tf::Pose &pose);
    void setPreviousPose(const tf::Pose &pose);
    void pubFinalPose();
    std::vector<double> calcCovarianceMatrix();
    void integrateOdomToScanTime();
    void integrateOdomToCurrentTime();

    ros::Subscriber m_scan_sub;
    ros::Subscriber m_odom_sub;
    ros::Subscriber m_map_sub;
    ros::Subscriber m_pose_sub;
    ros::Publisher m_particle_pub;
    ros::Publisher m_pose_pub;

    PoseEstimationICP *m_pose_icp;
    FakeScan *m_fake_scan;

    std::deque<Particle> m_particles;
    geometry_msgs::PoseWithCovarianceStamped m_prev_pose;

    sensor_msgs::LaserScan::ConstPtr m_scan;
    sensor_msgs::LaserScan m_prev_scan;
    bool m_have_scan = false;
    nav_msgs::Odometry::ConstPtr m_odom;
    nav_msgs::Odometry m_odom_at_cur_time;
    nav_msgs::Odometry::ConstPtr m_prev_odom;
    bool m_have_odom = false;
    nav_msgs::Odometry m_odom_at_scan;
    nav_msgs::Odometry m_odom_at_last_scan;
    bool m_have_map = false;
    nav_msgs::OccupancyGrid::ConstPtr m_map;
    bool m_initialized = false;
    bool m_have_pose_estimate = false;

    RandomGenerator m_rng;
    GaussianGenerator *m_gen_s;
    GaussianGenerator *m_gen_st;
    GaussianGenerator *m_gen_sens_x;
    GaussianGenerator *m_gen_sens_y;
    GaussianGenerator *m_gen_sens_yaw;

    double m_sensor_var_x;
    double m_sensor_var_y;
    double m_sensor_var_yaw;

    int m_num_particles;
    double m_percent_to_drop;
    double m_percent_to_average;

    bool m_compensate;

};

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_NEGATIVE_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_NEGATIVE_HPP_INCLUDED
#include <boost/simd/predicates/functions/is_negative.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/bitofsign.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/false.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         (is_negative_, tag::cpu_,
                              (A0)(X),
                              ((simd_<arithmetic_<A0>,X>))
                             )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return is_ltz(a0);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is unsigned_
/////////////////////////////////////////////////////////////////////////////

  BOOST_DISPATCH_IMPLEMENT         (is_negative_, tag::cpu_,
                              (A0)(X),
                              ((simd_<unsigned_<A0>,X>))
                             )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(const A0&)const
    {
      return boost::simd::False<result_type>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT         (is_negative_, tag::cpu_,
                              (A0)(X),
                              ((simd_<floating_<A0>,X>))
                             )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return is_equal(b_or(bitofsign(a0), One<A0>()), Mone<A0>());
    }
  };
} } }
#endif


#include "scene.h"

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/box.hpp>

#include <set>
#include <tuple>
#include <new>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

BOOST_GEOMETRY_REGISTER_POINT_2D(corner, float, bg::cs::cartesian, x, y)

BOOST_GEOMETRY_REGISTER_BOX(aabb, corner, min_corner, max_corner)

template<typename Box> struct ptr
{
    Box const* p;
};

namespace boost
{
namespace geometry
{
namespace index
{
template <typename Box>
struct indexable< ptr<Box> >
{
    typedef ptr<Box> V;

    typedef Box const& result_type;
    result_type operator()(V const& v) const
    {
        return *(v.p);
    }
};
}
}
}

typedef ptr<aabb> AABB;

static bool operator==(AABB const& l, AABB const& r)
{
    return l.p == r.p;
}

static bool operator<(AABB const& l, AABB const& r)
{
    return l.p->z < r.p->z;
}

struct scene
{
    bgi::rtree<AABB, bgi::quadratic<16>> t;
};

struct scene_list
{
    std::set<AABB> s;
    std::set<AABB>::iterator i;
};

int scene_add(scene* s, aabb const* r)
{
    try
    {
        AABB R;
        R.p = r;
        s->t.insert(R);
        return 0;
    }
    catch(...)
    {
        return -1;
    }
}

int scene_remove(scene* s, aabb const* r)
{
    try
    {
        AABB R;
        R.p = r;
        s->t.remove(R);
        return 0;
    }
    catch(...)
    {
        return -1;
    }
}

scene_list* scene_intersect(scene* s, aabb const* r)
{
    scene_list* l = nullptr;
    try
    {
        l = new scene_list;
        s->t.query(bgi::intersects(r), std::inserter(l->s, l->s.end()));
        l->i = l->s.begin();
        return l;
    }
    catch(...)
    {
        delete l;
        return nullptr;
    }
}

aabb const* scene_list_next(scene_list* l)
{
    try
    {
        if(l->i != l->s.end())
            return l->i++->p;
    }
    catch(...)
    {
    }
    return nullptr;
}

void scene_list_destroy(scene_list* l)
{
    delete l;
}

scene* scene_init()
{
    try
    {
        return new scene;
    }
    catch(...)
    {
        return nullptr;
    }
}

void scene_destroy(scene* s)
{
    delete s;
}


#include <lwr/lwr.h>
#include <boost/chrono.hpp>
#include <boost/lexical_cast.hpp>

TEST(LWRTest,1DRegression)
{

  lwr::lwr_options lwr_opts;

  lwr_opts.D.resize(dim);
  for(int i = 0; i < dim;i++){
      lwr_opts.D[i] = 1*1;
  }

  lwr_opts.k_bias = 1;
  lwr_opts.y_bias = 0;
  lwr_opts.bUseKDT = true;
  lwr::LWR Lwr(lwr_opts);
  Lwr.mtype = conversion::ROW_M;

  // Generate data
  arma::mat    X(num_data,dim,arma::fill::randu);

  X = X.st();

  X = X * 10;
  arma::colvec y(num_data,arma::fill::randu);
  arma::mat Xtest(num_test,dim,arma::fill::randu);
  Xtest = Xtest.st();
  double *ptr_y = new double[num_test];

  Lwr.set_X(X);
  Lwr.set_Y(y);

  auto start = boost::chrono::steady_clock::now();

  Lwr.f(ptr_y,Xtest);

  auto end = boost::chrono::steady_clock::now();

  auto diff = end - start;

  return boost::chrono::duration <double, boost::milli> (diff).count() / 1000.0;

}

int main(int argc,char** argv){

    std::cout<< "=== LWR (test run) ===" << std::endl;

    std::vector<int> dims(15);
    for(std::size_t i = 0; i < dims.size();i++){
        dims[i] = i+1;
    }
    std::vector<int> num_data = {{1000,10000,100000,100000}};

    int num_runs = 10;

    arma::mat time(num_data.size(),dims.size());
    time.zeros();

    for(int r = 0; r < num_runs;r++){

        std::cout<< "run(" << r+1 << ")" << std::endl;

        for(std::size_t i = 0; i < num_data.size();i++){
            std::cout<< i << "/" << num_data.size() << std::endl;
            for(std::size_t j = 0; j < dims.size();j++){
                time(i,j) = evaluate_lwr(dims[j],num_data[i],1);
            }
        }

        time.save("time_ " + boost::lexical_cast<std::string>(r+1) + ".txt",arma::raw_ascii);

    }

    return 0;
}


//
// Created by samarth on 7/25/18.
//

#ifndef DART_HAND_FITTER_HPP
#define DART_HAND_FITTER_HPP

#include <pangolin/pangolin.h>
#include <pangolin/gl/glcuda.h>
#include <boost/filesystem.hpp>

#include <string>
#include <thread>
#include <condition_variable>
#include <unordered_map>

// DART includes
#include "tracker_no_obs.h"
#include "util/gl_dart.h"
#include "optimization/priors.h"
#include "hand_fitter_config.h"

class HandFitter {
 public:
  HandFitter(const std::string &data_dir_=MODELS_DIR);
  ~HandFitter();

  void show_axes(bool show) { _show_axes = show; }
  void close() { pangolin::Quit(); }
  void set_targets(const std::unordered_map<std::string, float3>
    &joint_locations, const std::unordered_map<std::string, bool>
    &inner_surface);
  Eigen::Matrix4f init();
  dart::Pose fit(float &residue);

  void set_max_iterations(int m);
  void set_attract_weight(float w);
  void set_intra_model_intersection(float w);
  void set_lm_damping(float w);
  void set_log10_regularization(float w);
  void set_params(float aw, float iw, float lmd, float reg) {
    set_attract_weight(aw);
    set_intra_model_intersection(iw);
    set_lm_damping(lmd);
    set_log10_regularization(reg);
  }

 private:
  bool _init, _fit, _show_axes, _show_hand, _calc_error, _print_grasp;
  int _max_iterations;
  float _attract_weight, _intra_model_intersection, _lm_damping,
      _log10_regularization;
  std::shared_ptr<std::thread> pango_thread;
  boost::filesystem::path data_dir;

  std::shared_ptr<dart::TrackerNoObs> tracker;
  bool tracker_ready;
  std::mutex tracker_mutex;
  std::condition_variable tracker_cv;

  std::vector<int> graspit2dartdofs;

  // stores the tacker priors
  std::unordered_map<std::string, std::shared_ptr<dart::Point3D3DPrior> >
      contact_priors;
  // stores whether joint was observed on outer/inner (0/1) hand surface
  std::unordered_map<std::string, size_t> joint2inout;
  // stores 3D point locations for each joint
  // each joint has 2 3D points: outer/inner hand surface
  std::unordered_map<std::string, std::vector<float3> > joint2framept;
  // stores the frame ID of each joint
  std::unordered_map<std::string, size_t> joint2frameid;
  // stores joint target points (in camera coordinate frame)
  std::unordered_map<std::string, float3> joint_targets;
  // randomly sampled DOF initializations
  std::vector<std::vector<float> > dof_inits;

  Eigen::Matrix4f fit_rigid_joints();
  void pangolin_loop();
  void create_pangolin_vars_callbacks();
  void create_tracker();
  void show_3d_3d_prior(const std::string &joint_name, float size=10.f);
  float3 src_frame_point_in_camera(const std::string &joint_name);
  float calculate_error();
  Eigen::Matrix4f estimate_srt(Eigen::MatrixXf src_pts, Eigen::MatrixXf dst_pts);
};

#endif //DART_HAND_FITTER_HPP


#include <fstream>
#include <iostream>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/algorithm/string.hpp>

using Wire = std::string;
using Signal = std::uint16_t;
using Command = Signal (*) (Signal, Signal);

struct Operation {
	Command cmd;
	std::string lhs_operand;
	std::string rhs_operand;
};

template<typename T>
T AND(const T lhs, const T rhs) { return lhs & rhs; }

template<typename T>
T OR(const T lhs, const T rhs) { return lhs | rhs; }

template<typename T>
T LSHIFT(const T lhs, const T rhs) { return lhs << rhs; }

template<typename T>
T RSHIFT(const T lhs, const T rhs) { return lhs >> rhs; }

template<typename T>
T NOT(const T lhs, const T) { return ~lhs; }

template<typename T>
T SIGNAL(const T lhs, const T) { return lhs; }

int main() {

	auto filename = std::string{"circuit.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		auto signal = std::string{};

		auto commands = std::unordered_map<std::string, Command>{
			{"AND", AND},
			{"OR", OR},
			{"LSHIFT", LSHIFT},
			{"RSHIFT", RSHIFT},
			{"NOT", NOT},
			{signal, SIGNAL}
		};

		auto operations = std::unordered_map<Wire, Operation>{};
		auto wires = std::unordered_map<Wire, Signal>{};

		/*
		 * map each line to a set of operations on a single wire
		 */
		const auto max_tokens = 5U;
		auto tokens = std::vector<std::string>{max_tokens};

		auto line = std::string{};

		while(std::getline(file, line)) {

			boost::split(tokens, line, [] (auto c) { return (c == ' '); });

			// last element is always a wire
			auto& wire = tokens.back();

			// 4 tokens means a binary operation, 3 tokens is an unary operation,
			// and 2 tokens is a simple assignment
			if(tokens.size() == 4) {
				operations.emplace(wire, Operation{commands[tokens[1]], tokens[0], tokens[2]});
			} else if(tokens.size() == 3) {
				operations.emplace(wire, Operation{commands[tokens[0]], tokens[1], {}});
			} else {
				operations.emplace(wire, Operation{commands[signal], tokens[0], {}});
			}
		}

		auto get_signal = [] (auto is_operand_signal, auto& operand, auto is_wire_mapped, auto& wire) {
			return (is_operand_signal ? std::stoul(operand) : (is_wire_mapped ? wire->second : 0));
		};

		//execute operations
		for(auto operation = operations.begin(); !operations.empty();) {

			auto& lhs_str = operation->second.lhs_operand;
			auto& rhs_str = operation->second.rhs_operand;

			auto command = operation->second.cmd;

			auto is_lhs_signal = std::isdigit(lhs_str.front());
			auto is_rhs_signal = std::isdigit(rhs_str.front());

			auto lhs_wire = wires.find(lhs_str);
			auto rhs_wire = wires.find(rhs_str);

			auto is_lhs_wire_mapped = (lhs_wire != wires.end());
			auto is_rhs_wire_mapped = (rhs_wire != wires.end());

			auto is_lhs_operand_ok = (is_lhs_signal || is_lhs_wire_mapped);
			auto is_rhs_operand_ok = (is_rhs_signal || rhs_str.empty() || is_rhs_wire_mapped);

			// operation has either signals, mapped wires or a mix of both
			if(is_lhs_operand_ok && is_rhs_operand_ok) {

				auto lhs_signal = get_signal(is_lhs_signal, lhs_str, is_lhs_wire_mapped, lhs_wire);
				auto rhs_signal = get_signal(is_rhs_signal, rhs_str, is_rhs_wire_mapped, rhs_wire);

				wires[operation->first] = command(lhs_signal, rhs_signal);

				operation = operations.erase(operation);

			} else {
				// no operation possible right now, so move on
				++operation;
			}

			// if there are still operations left at the end
			// of the map, restart the whole process
			if(operation == operations.end() && !operations.empty()) {
				operation = operations.begin();
			}
		}

		std::cout << wires["a"] << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


/*
 ISC License

 Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder

 Permission to use, copy, modify, and/or distribute this software for any
 purpose with or without fee is hereby granted, provided that the above
 copyright notice and this permission notice appear in all copies.

 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

 */
#include <Eigen/Dense>
#include <vector>
#include "environment/spice/spice_interface.h"
#include <cstring>
#include <iostream>
#include <cmath>

#include "exponentialAtmosphere.h"
#include "architecture/messaging/system_messaging.h"
#include "utilities/linearAlgebra.h"
#include "utilities/astroConstants.h"
#include "utilities/bsk_Print.h"
#include "../../dynamics/_GeneralModuleFiles/stateData.h"
#include "../../_GeneralModuleFiles/sys_model.h"

/*! Initializer - sets the default properties to those of Earth.
*/
ExponentialAtmosphere::ExponentialAtmosphere()
{
    this->planetName = "Earth";
    this->planetPosInMsgName = "spice_planet_output_data";
    this->OutputBufferCount = 2;
    //! Set the default atmospheric properties to those of Earth
    this->atmosphereProps.baseDensity = 1.217;
    this->atmosphereProps.scaleHeight = 8500.0;
    this->atmosphereProps.planetRadius = 6371.008 * 1000.0;
    this->localAtmoTemp = 293.0; //! Placeholder value from http://nssdc.gsfc.nasa.gov/planetary/factsheet/earthfact.html
    this->relativePos.fill(0.0);
    this->scStateInMsgNames.clear();
    this->tmpAtmo.neutralDensity = this->atmosphereProps.baseDensity;
    this->tmpAtmo.localTemp = this->localAtmoTemp;
    this->planetPosInMsgId = -1;
    memset(&this->bodyState, 0x0, sizeof(SpicePlanetStateSimMsg));

    return;
}

/*! The destructor.*/
ExponentialAtmosphere::~ExponentialAtmosphere()
{
    return;
}

/*! This method is used to add additional spacecraft to the model density update. This allows
the use of a single exponentialAtmosphere instance for multiple spacecraft about a planet.
 @return void
 */
void ExponentialAtmosphere::addSpacecraftToModel(std::string tmpScMsgName){
  std::string tmpAtmoMsgName;
  this->scStateInMsgNames.push_back(tmpScMsgName);
  tmpAtmoMsgName = "atmo_dens"+ std::to_string(this->scStateInMsgNames.size()-1)+"_data";
  this->atmoDensOutMsgNames.push_back(tmpAtmoMsgName);
  return;
}

/*! SelfInit for this method creates a seperate density message for each of the spacecraft
that were added using AddSpacecraftToModel.
*/
void ExponentialAtmosphere::SelfInit()
{
    std::string tmpAtmoMsgName;
    uint64_t tmpAtmoMsgId;
    //! Begin method steps
    std::vector<std::string>::iterator it;
    std::vector<std::string>::iterator nameIt;

    for(it = this->atmoDensOutMsgNames.begin(); it!=this->atmoDensOutMsgNames.end(); it++){
      tmpAtmoMsgId = SystemMessaging::GetInstance()->CreateNewMessage(*it, sizeof(atmoPropsSimMsg), this->OutputBufferCount, "atmoPropsSimMsg", moduleID);

      this->atmoDensOutMsgIds.push_back(tmpAtmoMsgId);
    }

    return;
}

/*! Private "setter" method for changing the base density.*/
void ExponentialAtmosphere::setBaseDensity(double BaseDens){
  this->atmosphereProps.baseDensity = BaseDens;
  return;
}

/*! Private "setter" method for changing the scale height.*/
void ExponentialAtmosphere::setScaleHeight(double ScaleHeight){
  this->atmosphereProps.scaleHeight = ScaleHeight;
}

/*! Private "setter" method for changing the planet radius.*/
void ExponentialAtmosphere::setPlanetRadius(double PlanetRadius){
  this->atmosphereProps.planetRadius = PlanetRadius;
}

/*! Public "setter" method to change the planet referenced by the model.*/
void ExponentialAtmosphere::setPlanet(std::string PlanetName){
  this->planetName = PlanetName;
  double BaseDens = 0; // In kg/m^3
  double ScaleHeight = 0; // In meters

  if(PlanetName.compare("earth") == 0){
    BaseDens = 1.217;
    ScaleHeight = 8500.0;
    setBaseDensity(BaseDens);
    setScaleHeight(ScaleHeight);
} else if(PlanetName.compare("mars")==0){
    BaseDens = 0.020;
    ScaleHeight = 11100.0;
    setPlanetRadius(3389.5 * 1000.0);
    setBaseDensity(BaseDens);
    setScaleHeight(ScaleHeight);
} else if (PlanetName.compare("venus")==0){
    setPlanetRadius(6051.8 * 1000.0);
    BaseDens = 65.0;
    ScaleHeight = 15900.0;
    setBaseDensity(BaseDens);
    setScaleHeight(ScaleHeight);
  } else{
      BSK_PRINT(MSG_WARNING, "Planet %s not found. Either undefined or non-atmospheric. Please define other atmospheric parameters.", PlanetName.c_str());
  }
  return;
}
/*! This method is used to connect the input command message to the thrusters.
 It sets the message ID based on what it finds for the input string.  If the
 message is not successfully linked, it will warn the user.
 @return void
 */
void ExponentialAtmosphere::CrossInit()
{
  this->planetPosInMsgId = SystemMessaging::GetInstance()->subscribeToMessage(
  this->planetPosInMsgName, sizeof(SpicePlanetStateSimMsg), moduleID);

  std::vector<std::string>::iterator it;
  for(it = scStateInMsgNames.begin(); it!=scStateInMsgNames.end(); it++){
    this->scStateInMsgIds.push_back(SystemMessaging::GetInstance()->subscribeToMessage(*it, sizeof(SCPlusStatesSimMsg), moduleID));
  }
  return;
}

/*! This method is used to write the output densities whose names are established in AddSpacecraftToModel.
 @param CurrentClock The current time used for time-stamping the message
 @return void
 */
void ExponentialAtmosphere::WriteOutputMessages(uint64_t CurrentClock)
{
    atmoPropsSimMsg tmpAtmo;
    std::vector<int64_t>::iterator it;
    std::vector<atmoPropsSimMsg>::iterator atmoIt;
    atmoIt = atmoOutBuffer.begin();
    for(it = atmoDensOutMsgIds.begin(); it!= atmoDensOutMsgIds.end(); it++, atmoIt++){
        tmpAtmo = *atmoIt;
//        BSK_PRINT(MSG_DEBUG, "Neutral Density %f",tmpAtmo.neutralDensity);
      SystemMessaging::GetInstance()->WriteMessage(*it,
                                                  CurrentClock,
                                                  sizeof(atmoPropsSimMsg),
                                                  reinterpret_cast<uint8_t*>(&tmpAtmo),
                                                  moduleID);
    }
}
    /*SystemMessaging::GetInstance()->WriteMessage(this->thrusterOutMsgIds.at(idx),
                                                CurrentClock,
                                                sizeof(ThrusterOutputData),
                                                reinterpret_cast<uint8_t*>(&tmpThruster),
                                                moduleID);*/

/*! This method is used to read the incoming command message and set the
 associated spacecraft positions for computing the atmosphere.
 */
bool ExponentialAtmosphere::ReadInputs()
{
  SCPlusStatesSimMsg tmpState;
  //! Begin method steps
  SingleMessageHeader localHeader;
  memset(&this->bodyState, 0x0, sizeof(SpicePlanetStateSimMsg));
  memset(&tmpState, 0x0, sizeof(SCPlusStatesSimMsg));
  scStates.clear();
  if(this->scStateInMsgIds[0] >= 0)
  {
    //! Iterate over spacecraft message ids
    std::vector<int64_t>::iterator it;
    for(it = scStateInMsgIds.begin(); it!= scStateInMsgIds.end(); it++){
      SystemMessaging::GetInstance()->ReadMessage(*it, &localHeader,
                                                  sizeof(SCPlusStatesSimMsg),
                                                  reinterpret_cast<uint8_t*>(&tmpState),
                                                  moduleID);
    this->scStates.push_back(tmpState);
      }
  }
  if(planetPosInMsgId >= 0)
  {
      SystemMessaging::GetInstance()->ReadMessage(this->planetPosInMsgId , &localHeader,
                                                  sizeof(SpicePlanetStateSimMsg), reinterpret_cast<uint8_t*>(&this->bodyState), moduleID);
  }
  return(true);

}

/*! This method is used to update the internal density variables based on each spacecraft's position
and the pre-set atmospheric density properties.
 */
void ExponentialAtmosphere::updateLocalAtmo(double currentTime)
{
    double tmpDensity = 0.0;
    double tmpAltitude = 0.0;
    std::vector<SCPlusStatesSimMsg>::iterator it;
    atmoPropsSimMsg tmpData;
    uint64_t atmoInd = 0;
    this->atmoOutBuffer.clear();
    for(it = scStates.begin(); it != scStates.end(); it++, atmoInd++){
      this->updateRelativePos(this->bodyState, *it);
      tmpPosMag = this->relativePos.norm();
      tmpAltitude = tmpPosMag - this->atmosphereProps.planetRadius;

      tmpDensity = this->atmosphereProps.baseDensity * exp(-1.0 * tmpAltitude / this->atmosphereProps.scaleHeight);
//        BSK_PRINT(MSG_DEBUG, "Altitude %f, Density %f", tmpAltitude, tmpDensity);
      tmpData.neutralDensity = tmpDensity;
      tmpData.localTemp = 300.0;

      this->atmoOutBuffer.push_back(tmpData);
    }
      return;
}

void ExponentialAtmosphere::updateRelativePos(SpicePlanetStateSimMsg& planetState, SCPlusStatesSimMsg& scState)
{
    uint64_t iter = 0;
    if(planetPosInMsgId >= 0)
    {
      for(iter = 0; iter < 3; iter++)
      {
        //std::cout<<"Planet State: "<<planetState.PositionVector[iter]<<std::endl;
        //std::cout<<"SC State:"<<scState.r_BN_N[iter]<<std::endl;
        this->relativePos(iter,0) = scState.r_BN_N[iter] - planetState.PositionVector[iter];

        //std::cout<<relativePos[iter,0]<<std::endl;
      }
    }
    else{
      this->relativePos(iter,0) = scState.r_BN_N[iter];
    }
    //std::cout<<"Relative Pos: "<<this->relativePos <<std::endl;
    return;
}

/*! This method prompts the exponential atmosphere model to update the state message as part of the cyclic simulation run.
 @return void
 @param CurrentSimNanos The current simulation time in nanoseconds
 */
void ExponentialAtmosphere::UpdateState(uint64_t CurrentSimNanos)
{
    //! Begin method steps
    //! - Read the inputs and then call ConfigureThrustRequests to set up dynamics
    if(this->ReadInputs())
    {
        updateLocalAtmo(CurrentSimNanos*1.0E-9);
    }
    WriteOutputMessages(CurrentSimNanos);

}