/**
 * @file   NormalModeMeanSquareFluctuationCalculator.hpp
 * @author see AUTHORS
 * @brief  NormalModeMeanSquareFluctuationCalculator header file.
 */

#ifndef NORMALMODEMEANSQUAREFLUCTUATION_HPP
#define NORMALMODEMEANSQUAREFLUCTUATION_HPP

#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include <vector>
#include <iostream>

#include <plog/Log.h>

#include "Residuum.hpp"
#include "Protein.hpp"
#include "ProteinSegment.hpp"
#include "StructureType.hpp"
#include "ForceConstantSelector.hpp"
#include "ModelReduction.hpp"

#define NMODE 1

/**
 * @class NormalModeMeanSquareFluctuationCalculator
 * @brief used for calculation of NMA mean square fluctuation
 */
class NormalModeMeanSquareFluctuationCalculator {
    public:
        /**
         * @param residues
         */
        NormalModeMeanSquareFluctuationCalculator(const std::vector<Residuum> & residues);

        Eigen::VectorXd get_mean_square_fluctuation() const;
        Eigen::VectorXd get_weighted_eigenvalues() const;
        Eigen::VectorXd get_weighted_eigenvector() const;
        Eigen::VectorXd get_eigenvalues() const;

        /**
         * @return the eigenvectors
         */
        Eigen::MatrixXd get_eigenvectors() const;

        /**
         * @return the hessian matrix
         */
        Eigen::MatrixXd get_hessian_matrix() const;

        /**
         * @param protein
         * @param temperature
         * @param force_constant_selector
         * @param protein_segment
         */
        void calculate(const Protein & protein,
                       const double & temperature,
                       const ForceConstantSelector & force_constant_selector,
                       const std::shared_ptr<ProteinSegment> & protein_segment,
                       const ModelReduction & model_reduction);

        /**
         * @param protein
         * @param temperature
         * @param force_constant_selector
         * @param complete_protein_segment
         */
        void calculate(const Protein & protein,
                       const double & temperature,
                       const ForceConstantSelector & force_constant_selector,
                       const std::shared_ptr<ProteinSegment> &protein_segment);

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    protected:
        /**
         * @brief reduces the model according to model reduction input
         * @aram model_reduction
         */
        void calculate_model_reduction(const ModelReduction & model_reduction);

        /**
         * @brief calculates the eigenvalues with the magic number 53.0516 which was also
         * used in the original REACH programm for unknown reasons
         */
        void calculate_weighted_eigenvalues();

        /**
         * @brief calculates the weighted eigenvector depending on NMODE and the mass
         */
        void calculate_weighted_eigenvector();

        /**
         * @brief calculates the mean square fluctuation
         */
        void calculate_mean_square_fluctuation(const double &temperature);

        /*
         * @brief calculate the hessian matrix for the protein segment
         */
        void calculate_hessian_matrix(const Protein & protein,
                                      const ForceConstantSelector & kk_selector,
                                      const std::shared_ptr<ProteinSegment> & protein_segment);

        /**
         * @brief calculate both, the eigenvalues and the eigenvectors
         */
        void calculate_eigenvalues_and_eigenvectors();

        bool done;
        size_t residue_count;

        Eigen::VectorXd mass;
        Eigen::MatrixXd hessian_matrix;
        Eigen::VectorXd mean_square_fluctuation;
        Eigen::VectorXd weighted_eigenvalues;
        Eigen::VectorXd weighted_eigenvector;
        Eigen::VectorXd eigenvalues;
        Eigen::MatrixXd eigenvectors;
};

#endif

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MATH_EXPRESSION_EVALUATOR_HPP
#define CRYPTO3_MATH_EXPRESSION_EVALUATOR_HPP

#ifndef CRYPTO3_MATH_EXPRESSION_HPP
#error "evaluator.hpp must not be included directly!"
#endif

#include <map>
#include <string>

#include <boost/fusion/include/adapt_struct.hpp>
#include <nil/crypto3/math/expressions/ast.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {
            namespace expressions {
                namespace detail {
                    namespace ast {

                        // Optimizer

                        template <typename T>
                        struct holds_alternative_impl {
                            using result_type = bool;

                            template <typename U>
                            bool operator()(U const &) const {
                                return std::is_same<U, T>::value;
                            }
                        };

                        template <typename T, typename... Ts>
                        bool holds_alternative(x3::variant<Ts...> const &v) {
                            return boost::apply_visitor(holds_alternative_impl<T>(), v);
                        }

                        struct ConstantFolder {
                            using result_type = operand;

                            result_type operator()(nil) const {
                                return result_type{0};
                            }

                            result_type operator()(double n) const {
                                return result_type{n};
                            }

                            result_type operator()(std::string const &c) const {
                                return result_type{c};
                            }

                            result_type operator()(operation const &x, operand const &lhs) const {
                                auto rhs = boost::apply_visitor(*this, x.rhs);

                                if (holds_alternative<double>(lhs) && holds_alternative<double>(rhs)) {
                                    return result_type{
                                        x.op(boost::get<double>(lhs), boost::get<double>(rhs))};
                                }
                                return result_type{binary_op{x.op, lhs, rhs}};
                            }

                            result_type operator()(unary_op const &x) const {
                                auto rhs = boost::apply_visitor(*this, x.rhs);

                                /// If the operand is known, we can directly evaluate the function.
                                if (holds_alternative<double>(rhs)) {
                                    return result_type{x.op(boost::get<double>(rhs))};
                                }
                                return result_type{unary_op{x.op, rhs}};
                            }

                            result_type operator()(binary_op const &x) const {
                                auto lhs = boost::apply_visitor(*this, x.lhs);
                                auto rhs = boost::apply_visitor(*this, x.rhs);

                                /// If both operands are known, we can directly evaluate the function,
                                /// else we just update the children with the new expressions.
                                if (holds_alternative<double>(lhs) && holds_alternative<double>(rhs)) {
                                    return result_type{
                                        x.op(boost::get<double>(lhs), boost::get<double>(rhs))};
                                }
                                return result_type{binary_op{x.op, lhs, rhs}};
                            }

                            result_type operator()(expression const &x) const {
                                auto state = boost::apply_visitor(*this, x.lhs);
                                for (operation const &oper : x.rhs) {
                                    state = (*this)(oper, state);
                                }
                                return result_type{state};
                            }
                        };

                        struct eval {
                            using result_type = double;

                            explicit eval(std::map<std::string, double> sym) : st(std::move(sym)) {}

                            double operator()(nil) const {
                                BOOST_ASSERT(0);
                                return 0;
                            }

                            double operator()(double n) const { return n; }

                            double operator()(std::string const &c) const {
                                auto it = st.find(c);
                                if (it == st.end()) {
                                    throw std::invalid_argument("Unknown variable " + c); // NOLINT
                                }
                                return it->second;
                            }

                            double operator()(operation const &x, double lhs) const {
                                double rhs = boost::apply_visitor(*this, x.rhs);
                                return x.op(lhs, rhs);
                            }

                            double operator()(unary_op const &x) const {
                                double rhs = boost::apply_visitor(*this, x.rhs);
                                return x.op(rhs);
                            }

                            double operator()(binary_op const &x) const {
                                double lhs = boost::apply_visitor(*this, x.lhs);
                                double rhs = boost::apply_visitor(*this, x.rhs);
                                return x.op(lhs, rhs);
                            }

                            double operator()(expression const &x) const {
                                double state = boost::apply_visitor(*this, x.lhs);
                                for (operation const &oper : x.rhs) {
                                    state = (*this)(oper, state);
                                }
                                return state;
                            }

                        private:
                            std::map<std::string, double> st;
                        };

                    } // namespace ast
                }    // namespace detail
            }    // namespace expressions
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_MATH_EXPRESSION_EVALUATOR_HPP


#include <iostream>
#include <boost/sort/spreadsort/float_sort.hpp>
#include <vector>
#include <deque>
#include <mpi.h>
#define CAST_TYPE int
#define DATA_TYPE float

using namespace std;
using namespace boost::sort::spreadsort;

float* data;
float* temp_buffer;
float* put_buffer;

// make float sort in acending order
struct rightshift{
inline CAST_TYPE operator()(const DATA_TYPE &x, const unsigned offset) const {
    return float_mem_cast<DATA_TYPE, CAST_TYPE>(x) >> offset;
  }
};

// odd-even sort
void exchange(int flag, int neighbor_total, int n, int rank) {
  int self, neighbor, run, neighbor_run;
  bool tmp = false;
  float num;
  MPI_Status status;
  self = neighbor = run = neighbor_run = 0;

  // check if no need to change
  if (data[0] >= temp_buffer[neighbor_total-1]) {
    tmp = true;
    MPI_Send(&tmp, 1, MPI_C_BOOL, rank - 1, 0, MPI_COMM_WORLD);
    return;
  } else {
    tmp = false;
    MPI_Send(&tmp, 1, MPI_C_BOOL, rank - 1, 0, MPI_COMM_WORLD);
  }

  // start to sort element
  while (self < n || neighbor < neighbor_total) {
    if (neighbor_run < neighbor_total) {
      if (data[self] <= temp_buffer[neighbor]) {
        num = data[self];
        self += 1;
      } else {
        num = temp_buffer[neighbor];
        neighbor += 1;
      }
      put_buffer[neighbor_run] = num;
      neighbor_run += 1;
    } else {
      if (self == n) {
        num = temp_buffer[neighbor];
        neighbor += 1;
      } else if (neighbor == neighbor_total) {
        num = data[self];
        self += 1;
      } else if (data[self] <= temp_buffer[neighbor]) {
        num = data[self];
        self += 1;
      } else {
        num = temp_buffer[neighbor];
        neighbor += 1;
      }
      data[run] = num;
      run += 1;
    }
  }
  MPI_Send(put_buffer, neighbor_total, MPI_FLOAT, rank - 1, 0, MPI_COMM_WORLD);
}

int main(int argc, char** argv) {
  int rank, size, n, flag;
  bool odd = false;
  bool even = false;
  bool result = false;
  double starttime, endtime;
  n = atoll(argv[1]);

  MPI_Init(&argc, &argv);
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
  MPI_Comm_size(MPI_COMM_WORLD, &size);
  MPI_Status status;
  MPI_File f;
  int start;
  int end;
  int total;
  unsigned long long offset;
  int neighbor_total;

  // start time
  double start_time = MPI_Wtime();

  start = (n / size) * rank;
  end = start + n / size;
  if (rank == size-1) end = n;
  total = end - start;
  offset = start * sizeof(float);

  data = new float[n / size + size];
  temp_buffer = new float[n / size + size];
  put_buffer = new float[n/ size + size];

  // read input
  int check = MPI_File_open(MPI_COMM_WORLD, argv[2], MPI_MODE_RDONLY, MPI_INFO_NULL, &f);
  if (check != MPI_SUCCESS) {
    cout << "open file error" << endl;
    MPI_Abort(MPI_COMM_WORLD, check);
  }
  MPI_File_read_at(f, offset, data, total, MPI_FLOAT, MPI_STATUS_IGNORE);
  MPI_File_close(&f);

  // float_sort
  float_sort(data, data+total, rightshift());

  // start odd-even sort
  flag = 0;
  for (int j = 0; j < size+1; j++) {
    if (flag == 0) { // even sort
      if (rank % 2 == 0 && rank == size -1) {
        odd = true;
      } else if (rank % 2 == 1) {
        MPI_Recv(&neighbor_total, 1, MPI_INT, rank - 1, 0, MPI_COMM_WORLD, &status);
        MPI_Recv(temp_buffer, neighbor_total, MPI_FLOAT, rank - 1, 0, MPI_COMM_WORLD, &status);
        exchange(flag, neighbor_total, total, rank);
      } else {
        MPI_Send(&total, 1, MPI_INT, rank + 1, 0, MPI_COMM_WORLD);
        MPI_Send(data, total, MPI_FLOAT, rank + 1, 0, MPI_COMM_WORLD);
        bool tmp;
        MPI_Recv(&tmp, 1, MPI_C_BOOL, rank + 1, 0, MPI_COMM_WORLD, &status);
        if (!tmp) {
          MPI_Recv(data, total, MPI_FLOAT, rank + 1, 0, MPI_COMM_WORLD, &status);
        }
      }
    } else { // odd sort
      if (rank == 0 || (rank == size - 1 && rank % 2 == 1)) {
        even = true;
      } else if (rank % 2 == 0 && rank != 0) {
        MPI_Recv(&neighbor_total, 1, MPI_INT, rank - 1, 0, MPI_COMM_WORLD, &status);
        MPI_Recv(temp_buffer, neighbor_total, MPI_FLOAT, rank - 1, 0, MPI_COMM_WORLD, &status);
        exchange(flag, neighbor_total, total, rank);
      } else {
        MPI_Send(&total, 1, MPI_INT, rank + 1, 0, MPI_COMM_WORLD);
        MPI_Send(data, total, MPI_FLOAT, rank + 1, 0, MPI_COMM_WORLD);
        bool tmp;
        MPI_Recv(&tmp, 1, MPI_C_BOOL, rank + 1, 0, MPI_COMM_WORLD, &status);
        if (!tmp) {
          MPI_Recv(data, total, MPI_FLOAT, rank + 1, 0, MPI_COMM_WORLD, &status);
        }
      }
    }

    flag = (flag == 0) ? 1 : 0;
  }

  // write back
  check = MPI_File_open(MPI_COMM_WORLD, argv[3], MPI_MODE_CREATE | MPI_MODE_WRONLY, MPI_INFO_NULL, &f);
  if (check != MPI_SUCCESS) {
    cout << "open file error" << endl;
    MPI_Abort(MPI_COMM_WORLD, check);
  }
  MPI_File_write_at(f, offset, data, total, MPI_FLOAT, MPI_STATUS_IGNORE);
  MPI_File_close(&f);

  // calculate total time
  // if (rank == 0) {
  //   cout << "total time: " << MPI_Wtime() - start_time << endl;
  // }
  MPI_Finalize();

  return 0;
}


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

#include <iostream>
#include <memory>
#include <ctime>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"
#include "MatrixIO.h"

using namespace gpr;

typedef GaussianKernel<double>		DPKernelType;
typedef std::shared_ptr<DPKernelType> DPKernelTypePointer;
typedef GaussianProcess<double> DPGaussianProcessType;
typedef std::shared_ptr<DPGaussianProcessType> DPGaussianProcessTypePointer;

typedef GaussianKernel<float>		KernelType;
typedef std::shared_ptr<KernelType> KernelTypePointer;
typedef GaussianProcess<float> GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType> GaussianProcessTypePointer;

typedef DPGaussianProcessType::VectorType DPVectorType;
typedef DPGaussianProcessType::MatrixType DPMatrixType;
typedef GaussianProcessType::VectorType VectorType;
typedef GaussianProcessType::MatrixType MatrixType;

void Test1(){
    /*
     * Test 1: write and read matrix
     * - generate random matrix and look if the Write/Read function works
     */
    std::cout << "Test 1: write/read random matrix ..." << std::flush;

    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, 1);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    unsigned N = 100;
    unsigned K = 50;

    // generate double precision random matrix
    DPMatrixType dp_m(N, K);
    for (unsigned i =0; i < N ; i++) {
        for (unsigned j = 0; j < K ; j++) {
            dp_m(i,j) = r();
        }
    }
    WriteMatrix<DPMatrixType>(dp_m, "/tmp/gpr_matrix_io_test_dp.txt");
    DPMatrixType dp_m_read = ReadMatrix<DPMatrixType>("/tmp/gpr_matrix_io_test_dp.txt");

    double dp_err = (dp_m - dp_m_read).norm();

    // generate double precision random matrix
    MatrixType sp_m(K, N);
    for (unsigned i =0; i < K ; i++) {
        for (unsigned j = 0; j < N ; j++) {
            sp_m(i,j) = r();
        }
    }

    WriteMatrix<MatrixType>(sp_m, "/tmp/gpr_matrix_io_test_sp.txt");
    MatrixType sp_m_read = ReadMatrix<MatrixType>("/tmp/gpr_matrix_io_test_sp.txt");

    double sp_err = (sp_m - sp_m_read).norm();

    if(sp_err == 0 && dp_err == 0){
        std::cout << "\t\t\t [passed]." << std::endl;
    }
    else{
        std::stringstream ss; ss<<sp_err <<", "<< dp_err; throw ss.str();
    }
}

void Test2(){
    /*
     * Test 2: gaussian process save and load
     */
    std::cout << "Test 2: save/load gaussian process... " << std::flush;
    KernelTypePointer k(new KernelType(std::sqrt(2)));
    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(0);

    unsigned number_of_samples = 10;

    // add training samples
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(2);
        x(0) = x(1) = i * 2*M_PI/number_of_samples;

        VectorType y(2);
        y(0) = std::sin(x(0));
        y(1) = std::cos(x(1));

        gp->AddSample(x,y);
    }
    gp->Initialize();

    gp->Save("/tmp/gp_io_test-");

    KernelTypePointer k_dummy(new KernelType(1));
    GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));
    gp_read->Load("/tmp/gp_io_test-");

    if(*gp.get() == *gp_read.get()){
        std::cout << "\t\t\t [passed]." << std::endl;
    }
    else{
        throw std::string("read/write");
    }
}

void Test3(){
    /*
     * Test 3: gaussian process save and load with efficient setting
     *         (core matrix is not saved)
     */
    std::cout << "Test 3.1: (efficient) save/load gaussian process... " << std::flush;
    KernelTypePointer k(new KernelType(std::sqrt(2)));
    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(0);
    if(gp->GetEfficientStorage()){
        std::cout << "\t [failed]. Efficient storage setting has to be turned off by default." << std::endl;
        return;
    }
    gp->SetEfficientStorage(true);

    unsigned number_of_samples = 10;

    // add training samples
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(2);
        x(0) = x(1) = i * 2*M_PI/number_of_samples;

        VectorType y(2);
        y(0) = std::sin(x(0));
        y(1) = std::cos(x(1));

        gp->AddSample(x,y);
    }
    gp->Initialize();

    // predict a point to compare later
    VectorType x(2);
    x(0) = x(1) = 2.56 * 2*M_PI/number_of_samples;

    VectorType yt(2);
    yt= gp->Predict(x);

    gp->Save("/tmp/gp_io_test-");

    {
        KernelTypePointer k_dummy(new KernelType(1));
        GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));
        gp_read->Load("/tmp/gp_io_test-");

        if(*gp.get() == *gp_read.get() && (yt-gp_read->Predict(x)).norm() < 1e-06){
            std::cout << "\t [passed]." << std::endl;
        }
        else{
            std::stringstream ss; ss<<(yt-gp_read->Predict(x)).norm(); throw ss.str();
        }
    }

    std::cout << "Test 3.2: (efficient) save/load gaussian process... " << std::flush;
    {
        // testing if compare operator works find with the core matrix
        KernelTypePointer k_dummy(new KernelType(1));
        GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));
        gp_read->Load("/tmp/gp_io_test-");

        float c = (*gp_read)(x, x); // core matrix should be built
        if(*gp.get() == *gp_read.get()){
            throw std::string("Comparison with late core matrix construction not right.");
            return;
        }
        c = (*gp)(x, x); // now the core matrix of gp should be built as well
        if(*gp.get() != *gp_read.get()){
            throw std::string("Comparison with late core matrix construction not right.");
            return;
        }

        if((yt-gp_read->Predict(x)).norm() < 1e-06){
            std::cout << " \t [passed]." << std::endl;
        }
        else{
            std::stringstream ss; ss<<(yt-gp_read->Predict(x)).norm(); throw ss.str();
        }
    }

    std::cout << "Test 3.3: (efficient) save/load gaussian process... " << std::flush;
    {
        float c = (*gp)(x, x); // ensure that core matrix is built
        gp->Save("/tmp/gp_io_test-");

        KernelTypePointer k_dummy(new KernelType(1));
        GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));
        gp_read->Load("/tmp/gp_io_test-");

        if(*gp.get() == *gp_read.get() && (yt-gp_read->Predict(x)).norm() < 1e-06){
            std::cout << " \t [passed]." << std::endl;
        }
        else{
            std::stringstream ss; ss<<(yt-gp_read->Predict(x)).norm(); throw ss.str();
        }
    }
}

int main (int argc, char *argv[]){
    std::cout << "Input/Output test:" << std::endl;
    try{
        Test1();
        Test2();
        Test3();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/lazy.hpp>

#include <cassert>
#include <functional>
#include <iostream>
#include <sstream>
using namespace boost::hana;

template <typename T>
auto read = lazy([](std::istream& stream) {
    T x;
    stream >> x;
    std::cout << "read " << x << " from the stream\n";
    return x;
});

int main() {
    //! [main]
    std::stringstream ss;
    int in = 123;

    std::cout << "creating the monadic chain...\n";
    auto out = read<int>(std::ref(ss))
        | [](auto x) {
            std::cout << "performing x + 1...\n";
            return lazy(x + 1);
        }
        | [](auto x) {
            std::cout << "performing x / 2...\n";
            return lazy(x / 2);
        };

    std::cout << "putting " << in << " in the stream...\n";
    ss << in; // nothing is evaluated yet

    std::cout << "evaluating the monadic chain...\n";
    auto eout = eval(out);

    std::cout << "the result of the monadic chain is " << eout << "\n";
    assert(eout == (in + 1) / 2);
    //! [main]
}


//#define BOOST_TEST_DYN_LINK
#ifdef UNIX
	#define BOOST_TEST_DYN_LINK
#endif
#define BOOST_TEST_MODULE BsplineTest
#include <boost/test/unit_test.hpp>
#include "MTUSplineInterface.h"
#include "DataFromFile.h"

#ifdef WINDOWS
#include <direct.h>
#else
#include <unistd.h>
#endif

BOOST_AUTO_TEST_CASE(Bspline)
{
	std::string subjectSpecificXml = "cfg/TestData/gait2392Left.xml";
	std::string subjectName = "TestData";
	MTUSplineInterface test(subjectSpecificXml, subjectName);
	test.initialisationFromXML();
	std::vector<std::vector<double> > position, OpenSimMA, OpenSimLmt, ma;
	std::vector<double> lmt;

	// Read IK, LMT and ma from filesmake test
	DataFromFile ikData("cfg/TestData/Data2/angles_0.sto");
	DataFromFile maData("cfg/TestData/3DGaitModel2392-scaled_MuscleAnalysis_MomentArm_ankle_angle_l.sto");
	DataFromFile lmtData("cfg/TestData/3DGaitModel2392-scaled_MuscleAnalysis_Length.sto");

	std::vector<std::string> musclesNameMAOpensim = maData.getColumnNames();
	std::vector<std::string> musclesNameLMTOpenSim = lmtData.getColumnNames();
	std::vector<std::string> dofNameIK = ikData.getColumnNames();

	std::vector<std::string> DOFName = test.getDOFName();
	std::vector<std::string> MuscleName = test.getMuscleName();
	std::vector<std::vector<std::string> > musclesNamesOnDof = test.getMusclesNamesOnDof();

	std::vector<double> itReorganizeLMT;
	for (std::vector<std::string>::iterator it = MuscleName.begin(); it != MuscleName.end(); it++)
	{
		std::vector<std::string>::iterator temp = std::find(musclesNameLMTOpenSim.begin(), musclesNameLMTOpenSim.end(), *it);
		itReorganizeLMT.push_back(std::distance(musclesNameLMTOpenSim.begin(), temp));
	}

	std::vector<std::string>::iterator itMA = std::find(DOFName.begin(), DOFName.end(), "ankle_angle_l");
	if (itMA == DOFName.end())
	{
		std::cout << "Error ankle_angle_l not find in MA from BSpline" << std::endl;
		exit(0);
	}

	int itMaDist = std::distance(DOFName.begin(), itMA);

	std::vector<double> itReorganizeMA;
	for (std::vector<std::string>::iterator it = musclesNamesOnDof.at(itMaDist).begin(); it != musclesNamesOnDof.at(itMaDist).end(); it++)
	{
		std::vector<std::string>::iterator temp = std::find(musclesNameMAOpensim.begin(), musclesNameMAOpensim.end(), *it);
		itReorganizeMA.push_back(std::distance(musclesNameMAOpensim.begin(), temp));
	}

	while (ikData.areStillData())
	{
		std::vector<double> temp;
		ikData.readNextData();
		temp = ikData.getCurrentData();
		position.push_back(temp);
	}

	while (maData.areStillData())
	{
		// Get the Data
		std::vector<double> temp, maStateReorg;
		maData.readNextData();
		temp = maData.getCurrentData();
		// The Data order and number of muscle is not the same
		// so it need to be reorganize
		for (std::vector<double>::const_iterator it = itReorganizeMA.begin(); it != itReorganizeMA.end(); it++)
			maStateReorg.push_back(temp.at(*it));
		OpenSimMA.push_back(maStateReorg);
	}

	while (lmtData.areStillData())
	{
		// Get the Data
		std::vector<double> temp, lmtStateReorg;
		lmtData.readNextData();
		temp = lmtData.getCurrentData();
		// The Data order and number of muscle is not the same
		// so it need to be reorganize
		for (std::vector<double>::const_iterator it = itReorganizeLMT.begin(); it != itReorganizeLMT.end(); it++)
			lmtStateReorg.push_back(temp.at(*it));
		OpenSimLmt.push_back(lmtStateReorg);
	}

	for (std::vector<std::vector<double> >::const_iterator it1 = position.begin(); it1 != position.end(); it1++)
	{
		//std::cout << it1->at(0) << std::endl;
		//std::cout << it1->at(1) << std::endl;
		//std::cout << it1->at(2) << std::endl;
		test.setPosition(*it1);
		lmt = test.getLMT();
		ma = test.getMA();
		int cpt = std::distance<std::vector<std::vector<double> >::const_iterator >(position.begin(), it1);

		//std::cout << lmt.size() << std::endl;

		BOOST_REQUIRE_EQUAL(lmt.size(), OpenSimLmt.at(cpt).size());
		BOOST_REQUIRE_EQUAL(ma.at(itMaDist).size(), OpenSimMA.at(cpt).size());
		for (std::vector<double>::const_iterator it2 = lmt.begin(); it2 != lmt.end(); it2++)
		{
			//std::cout << MuscleName.at(std::distance<std::vector<double>::const_iterator>(lmt.begin(), it2)) << ":" << *it2 << "\t";
			BOOST_CHECK(abs(*it2 - OpenSimLmt.at(cpt).at(std::distance<std::vector<double>::const_iterator>(lmt.begin(), it2))) < 0.001);
			//break;
		}

		for (std::vector<double>::const_iterator it2 = ma.at(itMaDist).begin(); it2 != ma[itMaDist].end(); it2++)
		{
			//std::cout << musclesNamesOnDof.at(itMaDist).at(std::distance<std::vector<double>::const_iterator>(ma.at(itMaDist).begin(), it2)) << ":" << *it2 << "\t";
			BOOST_CHECK(abs(*it2 - OpenSimMA.at(cpt).at(std::distance<std::vector<double>::const_iterator>(ma.at(itMaDist).begin(), it2))) < 0.005);
		}
		//std::cout << std::endl;
	}
}


//Webots headers
#include <webots/Robot.hpp>
#include <webots/Emitter.hpp>
#include <webots/Receiver.hpp>
#include <webots/Motor.hpp>

//My headers
#include "../../libraries/PID.h"
#include "../../libraries/tracking.h"
#include "../../libraries/epuck_controller.h"
#include "../../libraries/greyWolf.h"
#include "../../libraries/Obstacle.h"

//Standard Headers
#include<iostream>
#include <vector>
#include<fstream>
#include<limits>
#include <cstdlib>
#include <boost/algorithm/string.hpp>
#include <time.h>
#define _USE_MATH_DEFINES

using namespace webots;

bool inObstacleRegion(std::vector<Obstacle> ObstacleArray, robot_tracking::Position givenPosition, int robot_index){
  bool flag = false;
  for(std::vector<Obstacle>::iterator it = ObstacleArray.begin() ; it != ObstacleArray.end(); ++it){
    if(it->checkIfInside(givenPosition.x,givenPosition.z,robot_index))
      flag = true;
  }
  return flag;
}

int main(int argc, char **argv) {
  //Get robot and configuration parameters from custom data
  Robot *robot = new Robot();
  std::vector<Obstacle> ObstacleList;
  // Obstacle horizontalCylinder(-2.76,-2.26,2,1.4,M_PI);
  ObstacleList.emplace_back(1.85,-0.38,0.6);
  ObstacleList.emplace_back(-2.76,3.42,0.9);
  ObstacleList.emplace_back(-1.27,-1.25,0.6,0.6,0);
  ObstacleList.emplace_back(1.36,3.31,0.75,0.3,0);
  ObstacleList.emplace_back(1.59,-2.82,1,1,0);
  std::string robotName = robot->getName();
  std::cout<<robotName+".csv"<<"\n";
  std::ofstream myfile(robotName+".csv");
  int timeStep = (int)robot->getBasicTimeStep();
  std::string customData = robot->getCustomData();
  std::vector<std::string> data;
  boost::split(data, customData, boost::is_any_of(","));
  srand(time(0)*(std::stoi(data[0])+1));
  //Get Motors
  Motor *left_motor = robot->getMotor("left wheel motor");
  Motor *right_motor = robot->getMotor("right wheel motor");
  //Get GPS and IMU
  GPS *gps = robot->getGPS("gps");
  InertialUnit *imu = robot->getInertialUnit("inertial unit");
  //Setup controller
  Epuck_Controller epuck(robot,left_motor,right_motor,gps,imu);
  epuck.setup_HeadingControl(1,0,0,0.01);
  epuck.setup_DistanceControl(10,0,0,0.05);
  //Emitter and Receiver setup
  Emitter *emitter = robot->getEmitter("emitter");
  Receiver *receiver = robot->getReceiver("receiver");
  //Setup the GreyWolf Object
  robot->step(timeStep);
  GreyWolf wolf(emitter, receiver, timeStep,epuck.get_currentPosition());
  robot->step(timeStep);
  wolf.set_boundaries(5,-5,5,-5);
  wolf.set_swarmSettings(std::stoi(data[0]),std::stoi(data[1]),std::stoi(data[2]));
  wolf.set_individualSettings(std::stof(data[3]),std::stof(data[4]),std::stoi(data[5]));
  //Setup operation
  robot->step(timeStep);
  wolf.update_position(epuck.get_currentPosition());
  robot->step(timeStep);
  //Share the starting positions with all the robots.
  std::cout<<"Sending out init status update\n";
  wolf.init_StatusUpdate(epuck.get_currentPosition());
  epuck.wait(3);
  robot->step(timeStep);
  wolf.get_allEvents(0);
  // std::cout<<"Finished initialization\n";
  //Setup target
  robot_tracking::Position targetPosition = {2.32,2.32, M_PI/2};
  wolf.set_targetPosition(targetPosition);
  int num_iter = std::stoi(data[2]);
  for(int i=0;i<num_iter;i++){
    // if(std::stoi(data[0])==3){
      // std::cout<<"_________Iteration Number___________:"<<i<<"\n";
    // }
    myfile<<i<<",";
    robot_tracking::Position currentPos = epuck.get_currentPosition();
    myfile<<currentPos.x<<","<<currentPos.z<<","<<robot->getTime()<<"\n";
    myfile.flush();
    wolf.update_position(currentPos);
    robot->step(timeStep);
    wolf.clear_eventMessage();
    robot->step(timeStep);
    std::cout<<"_________ROBOT INDEX___________:"<<std::stoi(data[0])<<"\n";
    std::cout<<"\nIteration Number"<<i<<"\n";
    wolf.emit_currentPosition(i);
    robot->step(timeStep);
    wolf.get_allEvents(i);
    robot->step(timeStep);
    robot_tracking::Position nextPosition = wolf.calculateNextPosition(i);
    robot->step(timeStep);
    // nextPosition = wolf.calculateNextPosition(i);
    // bool checkInside = powerSupply.checkIfInside(nextPosition.x,nextPosition.z,std::stoi(data[0]));
    bool checkInside = inObstacleRegion(ObstacleList,nextPosition,std::stoi(data[0]));
    while(checkInside){
      std::cout<<"_________ROBOT INDEX___________:"<<std::stoi(data[0])<<"\tCalled obstacle check"<<"\n";
      nextPosition = wolf.generateRandomPosition();
      checkInside = inObstacleRegion(ObstacleList,nextPosition,std::stoi(data[0]));
    }
    robot->step(timeStep);
    epuck.setTargetPosition(nextPosition);
    robot->step(timeStep);
    epuck.moveForward_BugAlgorithm(std::stoi(data[0]));
    robot->step(timeStep);
    currentPos = epuck.get_currentPosition();
    if(!wolf.check_insideBoundary(currentPos)){
      std::cout<<"_________ROBOT INDEX___________:"<<std::stoi(data[0])<<"\n";
      std::cout<<"Called out of boundary correction. Generating random position\n";
      //If the robot is outside the boundary, the robot returns to the previous position and calculates the next iteration again.
      nextPosition = wolf.generateRandomPosition();
      checkInside = inObstacleRegion(ObstacleList,nextPosition,std::stoi(data[0]));
      while(checkInside){
        std::cout<<"_________ROBOT INDEX___________:"<<std::stoi(data[0])<<"\tCalled obstacle check"<<"\n";
        nextPosition = wolf.generateRandomPosition();
        checkInside = inObstacleRegion(ObstacleList,nextPosition,std::stoi(data[0]));
      }
      epuck.setTargetPosition(nextPosition);
      robot->step(timeStep);
      epuck.moveForward_BugAlgorithm(std::stoi(data[0]));
      robot->step(timeStep);
      i-=1;
    }
    // currentPos = epuck.get_currentPosition();
    // while(powerSupply.checkIfInside(nextPosition.x,nextPosition.z)){
      // std::cout<<"WHILEn";
      // std::cout<<"_________ROBOT INDEX___________:"<<std::stoi(data[0])<<"\tCalled obstacle check"<<"\n";
      // std::cout<<"Called obstacle check"<<"\n";
      // nextPosition = wolf.generateRandomPosition();
      // epuck.setTargetPosition(nextPosition);
    // }

  }
  myfile.close();
  delete robot;
  return 0;
}


#ifndef TYPE_DEFINITIONS_H
#define TYPE_DEFINITIONS_H

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <boost/multi_array.hpp>

struct PointXYZIStamped : public pcl::PointXYZI
{
	ros::Time stamp;
};

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIStamped,
                                   (float, x, x)
                                   (float, y, y)
                                   (float, z, z)
                                   (float, intensity, intensity)
                                   (ros::Time, stamp, stamp))

typedef enum {WALL, FLOOR, BUOY} BlockType;

class Block {
public:
    Block(){nPoints_ = 0; height_ = 0; depth_ = 0;}
    Block (pcl::PointXYZI mean,pcl::PointXYZI variance,double nPoints,double height,double depth, BlockType type){
		mean_ = mean;
        variance_ = variance;
		nPoints_ = nPoints;
		height_ = height;
		depth_ = depth;
        type_ = type;
	}
    ~Block(){}

    BlockType type_;

    pcl::PointXYZI mean_;
    pcl::PointXYZI variance_;

	double nPoints_;
	//Highest Z
	double height_;
	//Lowest Z
	double depth_;

    void setMean(pcl::PointXYZI newMean) {mean_ = newMean;}
    void increaseNPoints(int n) {nPoints_+=n;}

};

typedef std::vector <boost::shared_ptr<Block> > cell;

typedef pcl::PointCloud<PointXYZIStamped> intensityCloud;

typedef boost::multi_array<cell, 2> grid_type;
typedef grid_type::index gridIndex;

#endif