// MIT License
//
// Copyright (c) 2021 Oliver Schick
//
// 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.

#include <iostream>
#include <mpo19/point.hpp>

#define BOOST_TEST_MODULE point_test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

namespace bdata = boost::unit_test::data;

BOOST_AUTO_TEST_CASE(apply_on_elements_test)
{
    using namespace boost;
    point<uint32_t> uut1{4, 5, 6}, uut2{2, 3, 4}, uut3{9, 8, 7};
    point<double> res1(uut1.dimension());
    apply_on_elements(res1
        , [](auto const& a, auto const& b)
          {
            return (double) a * b;
          }
        , uut1, uut2);
    BOOST_TEST(range::equal(res1.elements()
               , std::vector<double>{4*2, 5*3, 6*4}));

    point<std::string> res2 = apply_on_elements(
        [](auto const& a, auto const& b, auto const& c)
        {
            std::string res{};
            res += std::to_string(a);
            res += std::string{" * "};
            res += std::to_string(b);
            res += std::string{" + "};
            res += std::to_string(c);
            res += std::string{" = "};
            res += std::to_string(a*b+c);
            return res;
        }, uut1, uut2, uut3);

    BOOST_TEST(range::equal(res2.elements(), std::vector<std::string>
    {
        std::string{"4 * 2 + 9 = "} + std::to_string(4*2+9),
        std::string{"5 * 3 + 8 = "} + std::to_string(5*3+8),
        std::string{"6 * 4 + 7 = "} + std::to_string(6*4+7)
    }));
}

BOOST_AUTO_TEST_CASE(point_test_examples)
{
    using namespace boost;
    point<uint32_t> uut1(3);
    BOOST_TEST(uut1.dimension() == 3);

    std::vector<uint32_t> v(3);
    point<uint32_t> uut2(v.begin(), v.end());
    BOOST_TEST(uut2.dimension() == 3);

    BOOST_TEST( (point<double>{4.0, 5.0, 6.0}) == (point<uint32_t>{4, 5, 6}) );

    BOOST_TEST( (point<double>{1.2, 1.3, 1.4} + point<int>{3, 5, 4})
                    ==
                (point<double>{3+1.2, 5+1.3, 4+1.4}) );
    BOOST_TEST( (point<double>{1.2, 1.3, 1.4} + point<int>{3, 5, 4})
                    !=
                 (point<int>{4, 6, 5}) );

    BOOST_TEST( (point<double>{1.2, -1.3, -1.4} - point<int>{-2, 1, -14})
                    ==
                (point<double>{1.2-(-2), -1.3-1, -1.4-(-14)}) );

    BOOST_TEST( (point<double>{1.2, 1.3, 1.4} - point<int>{-2, 1, -14})
                    !=
                (point<int>{3, 0, 15}) );

    BOOST_TEST( (point<double>{1.2, -1.3, -1.4} * 3)
                    ==
                (point<double>{1.2*3, -1.3*3, -1.4*3}) );

    BOOST_TEST( (point<double>{1.2, 1.3, 1.4} * 3) != (point<int>{3, 3, 3}) );

    BOOST_TEST( (-point<int>{1, -2, 3}) == (point<int>{-1, 2, -3}) );

    BOOST_TEST( (point<int>{3, 4, 5}.fold()) == 12 );

    BOOST_TEST( (point<int>{2, 5, 7}.fold(std::multiplies<>{})) == 70);

    struct implicit_int_string : public std::string{
        using base = std::string;
        implicit_int_string(int i) : base{std::to_string(i)}{}
        implicit_int_string(base const& b) : base{b}{}
        implicit_int_string(base&& b) : base{std::forward<base>(b)}{}
    };

    BOOST_TEST( (point<int>{2, 5, 7}.fold([](implicit_int_string&& res, int arg){
                     using namespace std::string_literals;
                     return implicit_int_string("f("s + std::move(res) + ", "s + std::to_string(arg) + ")"s);
                 }))
                 ==
                 "f(f(2, 5), 7)");

    point<uint32_t> uut3{4, 5, 6};

    BOOST_TEST( (uut3.fold()) == 15 );

    point<uint32_t> const uut4{10, 9, 8, 7, 6, 5};

    BOOST_TEST( (uut4.fold(std::multiplies<>{})) == (10*9*8*7*6*5) );
}

constexpr int m = 1000;
auto random_3d_point =
    bdata::random(0, m) ^ bdata::random(0, m) ^ bdata::random(0, m);

BOOST_DATA_TEST_CASE(operator_consistency_test
                     , random_3d_point ^ random_3d_point ^ random_3d_point
                       ^ bdata::random(0, m) ^ bdata::random(0, m)
                       ^ bdata::xrange(100)
                     , x1, y1, z1
                     , x2, y2, z2
                     , x3, y3, z3
                     , s1, s2, idx)
{
    (void) idx;
    point<int> p1{x1, y1, z1}, p2{x2, y2, z2}, p3{x3, y3, z3};

    //Associativity
    BOOST_TEST( ((p1 + p2) + p3) == (p1 + (p2 + p3)) );

    BOOST_TEST( ((s1 * p1) * s2) == (s1 * (p1 * s2)) );
    BOOST_TEST( (p1 * (s1 * s2)) == ((p1 * s1) * s2) );

    //Commutativity
    BOOST_TEST( (p1 + p2) == (p2 + p1) );
    BOOST_TEST( (s1 * p1) == (p1 * s1) );

    //Neutral Element
    BOOST_TEST( (p1 + point<int>{0, 0, 0}) == p1 );
    BOOST_TEST( (p1 * 1) == p1 );

    //Distributivity
    BOOST_TEST( (s1 * (p1 + p2)) == (s1 * p1 + s1 * p2) );

    //Plus is consistent
    BOOST_TEST( (p1 + p1) == (2*p1) );
    BOOST_TEST( (p1 + p1 + p1) == (3*p1) );

    //Subtraction and Negation are consistent
    BOOST_TEST( (-p1) == (p1*(-1)) );
    BOOST_TEST( (p1 + (-p2)) == (p1 - p2) );
    BOOST_TEST( (p1 - p2) == (-(p2 - p1)) );
    BOOST_TEST( (-(p1 - p2)) == ((-p1) + p2) );
    BOOST_TEST( (p1 - p1) == (point<int>{0, 0, 0}) );
    BOOST_TEST( (point<int>{0, 0, 0} - p1) == (-p1) );
    BOOST_TEST( ((-p1) - p1) == ((-2) * p1) );
    BOOST_TEST( ((-p1) - p1 - p1) == ((-3) * p1) );

    //Consistency with =, +=, -=, *=
    point<int> a_p1 = p1, a_p2 = p2, a_p3{p3};
     BOOST_TEST( (a_p1) == (p1) );
    BOOST_TEST( (a_p2) == (p2) );
    BOOST_TEST( (a_p3) == (p3) );
    a_p1 += p2;
    BOOST_TEST( a_p1 == (p1 + p2) );
    BOOST_TEST( (a_p1 += a_p1 += p3) == (2*(p1 + p2 + p3)) );
    a_p2 *= s1;
    BOOST_TEST( a_p2 == (p2 * s1) );
    BOOST_TEST( (a_p2 *= s2) == (p2 * s1 * s2) );
    a_p3 -= p1;
    BOOST_TEST( a_p3 == (p3 - p1) );
    BOOST_TEST( (a_p3 -= p2) == (p3 - p1 - p2) );
    BOOST_TEST( (a_p3 -= a_p3 -= p3) == (point<int>{0, 0, 0}) );

}


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

#ifndef SIMULATORMPI_HPP
#define SIMULATORMPI_HPP

#include <boost/container/vector.hpp>
#include <boost/mpi.hpp>
#include <chrono>
#include <complex>
#include <functional>
#include <random>
#include <vector>

#include "simulator-mpi/SwapArrays.hpp"
#include "simulator-mpi/alignedallocator.hpp"
#include "simulator-mpi/fusion_mpi.hpp"

namespace mpi = boost::mpi;
namespace bc = boost::container;

#ifdef _WIN32
#     ifdef SIMULATOR_LIBRARY_EXPORT
#          define EXPORT_API __declspec(dllexport)
#     else
#          define EXPORT_API __declspec(dllimport)
#     endif  // SIMULATOR_LIBRARY_EXPORT
#else
#     define EXPORT_API
#endif  // _WIN32

class EXPORT_API SimulatorMPI
{
public:
     using Index = int64_t;
     using Float = double;
     using Complex = std::complex<Float>;
     using Matrix
         = std::vector<std::vector<Complex, aligned_allocator<Complex, 64>>>;
     using StateVector = bc::vector<Complex, aligned_allocator<Complex, 64>>;
     using RndEngine = std::mt19937;
     using Duration = std::chrono::duration<Float>;
     using Clock = std::chrono::high_resolution_clock;

     static constexpr size_t kNotFound_ = static_cast<size_t>(-1);
     //! Constructor
     /*!
      * \param seed Seed for pseudo-random number generator
      * \param max_local Maximum number of local qubits
      * \param max_cluster_size Maximum number of qubits in fused multi-qubit gate
      */
     SimulatorMPI(uint64_t seed, size_t max_local, size_t max_cluster_size);

     //! Constructor
     /*!
      * \brief With this constructor you can define your own MPI communicator which
               must be able to work with `2^N` processes, where N is the number of qubits.
      * \param aWorld A MPI communicator
      * \param seed Seed for pseudo-random number generator
      * \param max_local Maximum number of local qubits
      * \param max_cluster_size Maximum number of qubits in fused multi-qubit gate
      */
     SimulatorMPI(mpi::communicator aWorld, uint64_t seed, size_t max_local,
                  size_t max_cluster_size);

     //! Copy constructor
     /*!
      * \note Actual declaration is `SimulatorMPI(const SimulatorMPI &other) = delete`.\n
      * Copying of the simulator is prohibited.
      */
     SimulatorMPI(const SimulatorMPI &other) = delete;

     //! Destructor
     ~SimulatorMPI();

     //! Allocate one qubit with a given ID
     /*!
      * \param id ID of the qubit to allocate
      */
     void AllocateQubit(Index id);

     //! Allocate the quantum register qureg with given qubits IDs
     /*!
      * \param ids Array of qubit IDs
      * \param init Value to be assigned to all amplitudes
      * \throw std::runtime_error if this is not the first allocation and init is not equal 0.0
      */
     void AllocateQureg(const std::vector<Index> &ids, Complex init = 0);

     //! De-allocate one qubit with a given ID
     /*!
      * \param id ID of the qubit to de-allocate
      */
     void DeallocateQubit(Index id);

     /*!
      * \brief Actually make calculations:
               + fuse applied (used ApplyGate()) gates into a single multi-qubit gate
               + calculate the product of the resulting gate by state vector
      * \throw std::runtime_error if gate cannot be applied
      */
     void Run();

     //! Save gate and than apply it to function Run()
     /*!
      * \param m Matrix of the gate
      * \param ids Array of qubit IDs
      * \param ctrl Array of control qubits
      * \throw std::runtime_error if gate is non-diagonal
      */
     void ApplyGate(Matrix m, std::vector<Index> ids, std::vector<Index> ctrl);

     //! Measure qubits state
     /*!
      * \param ids Array of qubit IDs to measure
      * \return Bit array of measurement results (containing either True or False)
      */
     std::vector<bool> MeasureQubits(std::vector<Index> const &ids);

     //! See SwapQubits()
     /*!
      * \brief Print logs: duration, qubits, bandwidth
      */
     void SwapQubitsWrapper(const std::vector<Index> &swap_pairs_ids);

     //! Swap global and local qubits in pairs
     /*!
      * \param swap_pairs_ids Array of qubit pairs IDs (there is a global qubit
               ID on the first place, a local - on the second)
      * \throw std::runtime_error if qubits are not unique
      */
     void SwapQubits(const std::vector<Index> &swap_pairs_ids);

     /*!
      * \brief Return the amplitude of the supplied amplitude index
      * \param bit_string Index amplitude
      * \param ids Array of qubit IDs
      * \return Amplitude of the provided bit_string
      * \throw std::runtime_error if the number of IDs does not match the number
               of qubits or if any of the desired qubits are not already allocated
      */
     Complex GetAmplitude(const std::vector<bool> &bit_string,
                          const std::vector<Index> &ids) const;

     /*!
      * \brief Return the probability of the outcome bit_string when measuring
      *        the quantum register qureg.
      * \param bit_string Measurement outcome
      * \param ids Array of qubit IDs
      * \return Probability of measuring the provided bit string
      * \throw std::runtime_error if the number of IDs does not match the length of bit_string
      * \note Doesn't actually make the calculations
      */
     Float GetProbability(const std::vector<bool> &bit_string,
                          const std::vector<Index> &ids);

     /*!
      * \brief Return the entropy of quantum state.
      */
     Float Entropy();

     /*!
      * \brief Get permutation of qubits.
      * \return Array of qubit IDs
      */
     std::vector<Index> GetQubitsPermutation() const;

     /*!
      * \brief Get permutation of local qubits.
      * \return Array of qubit IDs
      */
     std::vector<Index> GetLocalQubitsPermutation();

     /*!
      * \brief Get permutation of global qubits.
      * \return Array of qubit IDs
      */
     std::vector<Index> GetGlobalQubitsPermutation();

     //! Set permutation to all qubits without actually reordering quantum state vector
     /*!
      * \param p Array of qubit IDs
      */
     void SetQubitsPermutation(std::vector<Index> p);

     /*!
      * \brief Access the ordering of the qubits and this MPI process's part of
        state vector directly.
      * \return A tuple where the first entry is a dictionary mapping qubit
                indices to bit-locations (all qubits) and the second entry is the local part
                of state vector.
      */
     std::tuple<std::map<int, int>, StateVector &> cheat_local();

     //! Collapse a quantum register into a classical basis state
     /*!
      * \param ids Array of qubit IDs
      * \param values Measurement outcome for each of the qubits in `qureg`.
      * \throw std::runtime_error if the number of IDs does not match the number of values
               or measured probability less than 1.e-12
      */
     void collapseWaveFunction(const std::vector<Index> &ids,
                               const std::vector<bool> &values);

     //! This function is not supported
     template <class F, class QuReg>
     void emulate_math(F const &, QuReg, const std::vector<Index> &,
                       unsigned = 1)
     {
          world_.barrier();
          throw std::runtime_error(
              "SimulatorMPI::emulate_math() is not supported");
     }

     /*!
      * \return Number of local qubits
      */
     size_t LocalQubitsCount() const
     {
          return locals_.size();
     }

     /*!
      * \return Number of global qubits
      */
     size_t GlobalQubitsCount() const
     {
          return globals_.size()
                 - std::count(globals_.begin(), globals_.end(), -1);
     }

     /*!
      * \return Number of all qubits
      */
     size_t TotalQubitsCount() const
     {
          return LocalQubitsCount() + GlobalQubitsCount();
     }

     mpi::environment env_;
     mpi::communicator world_;
     StateVector vec_;

private:
     const Float kMaxFloatError_;
     const size_t kMinLocal_;
     const size_t kMaxLocal_;
     const size_t kMaxGlobal_;
     size_t kMaxClusterSize_;
     std::vector<Index> locals_;
     std::vector<Index> globals_;

     Fusion fused_gates_;
     RndEngine rnd_eng_;
     std::function<double()> rng_;

     int rank_;
     SwapBuffers<Complex> buffs_;

     int run_gates = 0;
     int stage_gates = 0;
     int total_gates = 0;
     int stage_runs = 0;
     int total_runs = 0;
     int total_stages = 0;
     Float total_runs_duration = 0.0;
     Float total_swap_duration = 0.0;
     Float total_measure_duration = 0.0;
     Float total_alloc_duration = 0.0;
     Float total_dealloc_duration = 0.0;
     Clock::time_point start_stage_time;
     Clock::time_point start_time;

     void AllocateLocalQubit(Index id);
     void AllocateGlobalQubit(Index id);
     void DeallocateLocalQubit(Index id);
     void DeallocateGlobalQubit(Index id);
     void CheckNorm();
     size_t ArrayFind(const std::vector<Index> &v, Index val) const;
     size_t ArrayFindSure(const std::vector<Index> &v, Index val) const;
     uint64_t IdsToBits(const std::vector<Index> &ids,
                        const std::vector<Index> &perm) const;
     std::vector<Index> ExtractLocalCtrls(const std::vector<Index> &ctrl) const;
     void StartStage();
     void EndStage();

     bc::vector<Float> block_distribution;
     bc::vector<Float> total_block_distribution;

     void calcLocalApproxDistribution(size_t n);

     Float getProbability_internal(uint64_t local_msk, uint64_t local_val,
                                   uint64_t global_msk, uint64_t global_val);
     void normalize(Float norm, uint64_t local_msk, uint64_t local_val,
                    uint64_t global_msk, uint64_t global_val);
};

#endif  // SIMULATORMPI_HPP


#pragma once

#include <cstdlib>
#include <utility>
#include <tuple>
#include <numeric>

#include <boost/unordered_map.hpp>
#include "types.hh"

/*!
  @brief Generate a region automaton from a timed automaton
  @tparam NVar the number of variable in TA

  TA to RA adds states with BFS. Initial configuration is the initial states of RA. The RA contain only the states reachable from initial states.
 */
template <int NVar>
void ta_to_ra (const TimedAutomaton<NVar> &TA,RegionAutomaton &RA)
{
  //! number of states of RA
  State numOfStates;
  Region initialRegion;
  initialRegion.integer_parts.resize (NVar);
  fill(initialRegion.integer_parts.begin(),
       initialRegion.integer_parts.end(),
       std::make_pair(0,0));
  initialRegion.max_constraints = TA.max_constraints;

  // make initial state
  numOfStates = TA.initialStates.size();
  RA.edges.resize(numOfStates);
  RA.initialStates.resize (numOfStates);
  iota (RA.initialStates.begin(), RA.initialStates.end(),0);

  RA.abstractedStates.reserve (numOfStates);
  for (const auto &conf : TA.initialStates ) {
    RA.abstractedStates.push_back (std::make_pair(conf,initialRegion));
  }

  /*!
    @brief Current configuration of BFS

    A configuration consistes of a tuple ((conf,\alpha),conf,alpha). Remark (conf,\alpha) is not a pair but just a number representing a state of RA.
   */
  std::vector<std::tuple<RAState,TAState,Region> > nextConf;
  nextConf.resize (RA.abstractedStates.size());
  for (std::size_t i = 0; i < nextConf.size();i++) {
    nextConf[i] = std::make_tuple(i,RA.abstractedStates[i].first,RA.abstractedStates[i].second);
  }

  /*!
    @brief translater from TAState and Region to its corresponding state in RA.

    The type is like this.
    (TAState,([Int,Int],[[Int]],[Int])) -> RAState
  */
  boost::unordered_map<std::pair<TAState,std::tuple<std::vector< std::pair<int,int> >, std::list<std::list<int> >, std::vector<int> > >,RAState> regions_in_ra;
  while (!nextConf.empty ()) {
    std::vector<std::tuple<State,State,Region> > currentConf = nextConf;
    nextConf.clear();
    for (const auto &conf : currentConf) {
      Region nowRegion,nextRegion;
      bool self_successor;
      nowRegion = std::get<2>(conf);
      const bool existsNextRegion = nowRegion.nextRegion (nextRegion,self_successor);

      nextRegion = self_successor ? nowRegion : nextRegion;
      if (!existsNextRegion && !self_successor)
        break;
      do {
        nowRegion = std::move(nextRegion);
        bool tooLarge = true;
#ifdef DEBUG
        cout << "====================="<< endl;
#endif
        const Region::Interpretation val = nowRegion;
        for (const auto &edge : TA.edges.at(std::get<1>(conf))) {
#ifdef DEBUG
          cout << (int)get<1>(conf) << ":"
               << (int)edge.source << "->"
               << (int)edge.target << " "
               << (edge.guard(val) ? "true" : "false")
               << endl;
#endif
          if (std::all_of(edge.guard.begin(),edge.guard.end(),
                          [&val] (const Constraint &delta){return delta(val) == Order::EQ;})) {
            tooLarge = false;
            Region targetRegion = nowRegion;
            for (auto x : edge.resetVars) {
              targetRegion.integer_parts[x] = {0,0};
              for (auto it = targetRegion.frac_order.begin();
                   it != targetRegion.frac_order.end(); it++) {
                const auto initialNum = it->size();
                it->remove(x);
                if (initialNum != it->size()) {
                  if (it->empty() ) {
                    targetRegion.frac_order.erase(it);
                  }
                  break;
                }
              }
            }

#ifdef DEBUG
            cout << "nowRegion" << endl;
            cout << nowRegion << endl;

            cout << "targetRegion" << endl;
            cout << targetRegion << endl;
#endif
            // nextRegion state is new
            const auto targetRegionState = std::make_pair(edge.target,std::make_tuple(targetRegion.integer_parts,targetRegion.frac_order,targetRegion.max_constraints));
            const auto targetStateInRA = regions_in_ra.find(targetRegionState);
#ifdef DEBUG
            cout << (targetStateInRA == RA.abstractedStates.end()) << endl;
            cout << (int)get<0>(conf) << "-" << edge.c << ">" << static_cast<int>(targetStateInRA - RA.abstractedStates.begin()) << endl;
#endif
            // targetRegionState is already added
            if (targetStateInRA != regions_in_ra.end()) {
              RA.edges[std::get<0>(conf)].push_back ({std::get<0>(conf),targetStateInRA->second,edge.c});
            } else {
#ifdef DEBUG
              cout << "added " << (int)numOfStates << " = " << targetStateInRA - RA.abstractedStates.begin()<< endl;
#endif
              RA.edges[std::get<0>(conf)].push_back ({std::get<0>(conf),static_cast<State>(numOfStates),edge.c});
              if (binary_search (TA.acceptingStates.begin(),
                                 TA.acceptingStates.end (),edge.target)) {
                RA.acceptingStates.push_back (numOfStates);
              }
              regions_in_ra[targetRegionState] = numOfStates;
              RA.abstractedStates.push_back (std::make_pair (edge.target,targetRegion));
              nextConf.push_back (std::make_tuple (numOfStates,edge.target,targetRegion));
              numOfStates++;
              RA.edges.resize(numOfStates);
            }
          } else if (std::none_of(edge.guard.begin(), edge.guard.end(),
                                  [val](const Constraint &delta){return delta (val) == Order::GT;})) {
            tooLarge = false;
          }
        }
        if (tooLarge) break;
      } while(nowRegion.nextRegion (nextRegion,self_successor));
    }
    //    std::cout << numOfStates << std::endl;
  }

}


///////////////////////////////////////////////////////////////////////////////
// rolling_mean.hpp
// Copyright (C) 2008 Eric Niebler.
// Copyright (C) 2012 Pieter Bastiaan Ober (Integricom).
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_ROLLING_MEAN_HPP_EAN_26_12_2008
#define BOOST_ACCUMULATORS_STATISTICS_ROLLING_MEAN_HPP_EAN_26_12_2008

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/rolling_sum.hpp>
#include <boost/accumulators/statistics/rolling_count.hpp>

namespace boost { namespace accumulators
{
   namespace impl
   {
      ///////////////////////////////////////////////////////////////////////////////
      // lazy_rolling_mean_impl
      //    returns the mean over the rolling window and is calculated only
      //    when the result is requested
      template<typename Sample>
      struct lazy_rolling_mean_impl
         : accumulator_base
      {
         // for boost::result_of
         typedef typename numeric::functional::fdiv<Sample, std::size_t, void, void>::result_type result_type;

         lazy_rolling_mean_impl(dont_care)
         {
         }

         template<typename Args>
         result_type result(Args const &args) const
         {
            return numeric::fdiv(rolling_sum(args), rolling_count(args));
         }
      };

      ///////////////////////////////////////////////////////////////////////////////
      // immediate_rolling_mean_impl
      //     The non-lazy version computes the rolling mean recursively when a new
      //     sample is added
      template<typename Sample>
      struct immediate_rolling_mean_impl
         : accumulator_base
      {
         // for boost::result_of
         typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

         template<typename Args>
         immediate_rolling_mean_impl(Args const &args)
            : mean_(numeric::fdiv(args[sample | Sample()],numeric::one<std::size_t>::value))
         {
         }

         template<typename Args>
         void operator()(Args const &args)
         {
            if(is_rolling_window_plus1_full(args))
            {
               mean_ += numeric::fdiv(args[sample]-rolling_window_plus1(args).front(),rolling_count(args));
            }
            else
            {
               result_type prev_mean = mean_;
               mean_ += numeric::fdiv(args[sample]-prev_mean,rolling_count(args));
            }
         }

         template<typename Args>
         result_type result(Args const &) const
         {
            return mean_;
         }

      private:

         result_type mean_;
      };
   } // namespace impl

   ///////////////////////////////////////////////////////////////////////////////
   // tag::lazy_rolling_mean
   // tag::immediate_rolling_mean
   // tag::rolling_mean
   //
   namespace tag
   {
      struct lazy_rolling_mean
         : depends_on< rolling_sum, rolling_count >
      {
         /// INTERNAL ONLY
         ///
         typedef accumulators::impl::lazy_rolling_mean_impl< mpl::_1 > impl;

#ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
         /// tag::rolling_window::window_size named parameter
         static boost::parameter::keyword<tag::rolling_window_size> const window_size;
#endif
      };

      struct immediate_rolling_mean
         : depends_on< rolling_window_plus1, rolling_count>
      {
         /// INTERNAL ONLY
         ///
         typedef accumulators::impl::immediate_rolling_mean_impl< mpl::_1> impl;

#ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
         /// tag::rolling_window::window_size named parameter
         static boost::parameter::keyword<tag::rolling_window_size> const window_size;
#endif
      };

      // make immediate_rolling_mean the default implementation
      struct rolling_mean : immediate_rolling_mean {};
   } // namespace tag

   ///////////////////////////////////////////////////////////////////////////////
   // extract::lazy_rolling_mean
   // extract::immediate_rolling_mean
   // extract::rolling_mean
   //
   namespace extract
   {
      extractor<tag::lazy_rolling_mean> const lazy_rolling_mean = {};
      extractor<tag::immediate_rolling_mean> const immediate_rolling_mean = {};
      extractor<tag::rolling_mean> const rolling_mean = {};

      BOOST_ACCUMULATORS_IGNORE_GLOBAL(lazy_rolling_mean)
         BOOST_ACCUMULATORS_IGNORE_GLOBAL(immediate_rolling_mean)
         BOOST_ACCUMULATORS_IGNORE_GLOBAL(rolling_mean)
   }

   using extract::lazy_rolling_mean;
   using extract::immediate_rolling_mean;
   using extract::rolling_mean;

   // rolling_mean(lazy) -> lazy_rolling_mean
   template<>
   struct as_feature<tag::rolling_mean(lazy)>
   {
      typedef tag::lazy_rolling_mean type;
   };

   // rolling_mean(immediate) -> immediate_rolling_mean
   template<>
   struct as_feature<tag::rolling_mean(immediate)>
   {
      typedef tag::immediate_rolling_mean type;
   };

   // for the purposes of feature-based dependency resolution,
   // immediate_rolling_mean provides the same feature as rolling_mean
   template<>
   struct feature_of<tag::immediate_rolling_mean>
      : feature_of<tag::rolling_mean>
   {
   };

   // for the purposes of feature-based dependency resolution,
   // lazy_rolling_mean provides the same feature as rolling_mean
   template<>
   struct feature_of<tag::lazy_rolling_mean>
      : feature_of<tag::rolling_mean>
   {
   };
}} // namespace boost::accumulators

#endif


// Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic
//
// This source code is part of DopeVector header library
// and it is subject to Apache 2.0 License.
//
// Author: Giorgio Marcias
// email: marcias.giorgio@gmail.com
// Author: Maurizio Kovacic
// email: maurizio.kovacic@gmail.com

#ifndef EigenExpression_hpp
#define EigenExpression_hpp

#ifdef DOPE_USE_EIGEN

#include <DopeVector/internal/Expression.hpp>
#include <Eigen/Core>

namespace dope {

	namespace internal {

		template < class Derived, class Er, typename T, SizeType Dimension, typename Op >
		class EigenStaticArrayBinaryExpression : public StaticArrayExpression<EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, Op>, T, Dimension> {
			static_assert((Eigen::MatrixBase<Derived>::RowsAtCompileTime == Dimension && Eigen::MatrixBase<Derived>::ColsAtCompileTime == 1) ||
						  (Eigen::MatrixBase<Derived>::RowsAtCompileTime == 1 && Eigen::MatrixBase<Derived>::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector.");
		private:
			const Eigen::MatrixBase<Derived>                  &_el;
			const Er	                                          &_er;
			mutable std::array<std::function<T()>, Dimension>  _values;
			static const Op                                    _op;

		public:
			inline EigenStaticArrayBinaryExpression(const Eigen::MatrixBase<Derived> &el, const Er &er);

			inline T operator[](const SizeType i) const;
		};

		template < class El, typename T, SizeType Dimension, class Derived, typename Op >
		class StaticArrayBinaryEigenExpression : public StaticArrayExpression<StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, Op>, T, Dimension> {
			static_assert((Eigen::MatrixBase<Derived>::RowsAtCompileTime == Dimension && Eigen::MatrixBase<Derived>::ColsAtCompileTime == 1) ||
						  (Eigen::MatrixBase<Derived>::RowsAtCompileTime == 1 && Eigen::MatrixBase<Derived>::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector.");
		private:
			const El                                          &_el;
			const Eigen::MatrixBase<Derived>                  &_er;
			mutable std::array<std::function<T()>, Dimension>  _values;
			static const Op                                    _op;

		public:
			inline StaticArrayBinaryEigenExpression(const El &el, const Eigen::MatrixBase<Derived> &er);

			inline T operator[](const SizeType i) const;
		};

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::plus<T>> operator+ (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::minus<T>> operator- (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::multiplies<T>> operator* (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::divides<T>> operator/ (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::modulus<T>> operator% (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::plus<T>> operator+ (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::minus<T>> operator- (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::multiplies<T>> operator* (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::divides<T>> operator/ (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::modulus<T>> operator% (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

	}

}

#include <DopeVector/internal/inlines/eigen_support/EigenExpression.inl>

#endif

#endif


#include <iostream>
#include <string>
#include <random>
#include <cmath>
#include <array>
#include <random>
#include <algorithm>
#include <adept.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <cppunit/ui/text/TestRunner.h>
#include <cppunit/XmlOutputter.h>
#include <cppunit/TestCase.h>
#include <cppunit/TestSuite.h>
#include <cppunit/TestCaller.h>
#include <cppunit/TestRunner.h>

#include "model.hpp"
#include "reaction.hpp"

using namespace std;

constexpr array<double, 4> tps = {{0.1, 10.0, 1000.0, 10000.0}};

class interfaceTestCase : public CppUnit::TestCase {
public:
	/// Setup method
	void setUp() {
		random_device rd;
		gen = new mt19937(rd());
		rxnRatesIn = getParams();
	}

	/// Teardown method
	void tearDown() {
		delete gen;
	}

	// method to create a suite of tests
	static CppUnit::Test *suite() {
		CppUnit::TestSuite *suiteOfTests = new CppUnit::TestSuite("interfaceTestCase");

		suiteOfTests->addTest(new CppUnit::TestCaller<interfaceTestCase>("testrunCkine",
			&interfaceTestCase::testrunCkine));
		suiteOfTests->addTest(new CppUnit::TestCaller<interfaceTestCase>("testrunCkineS",
			&interfaceTestCase::testrunCkineS));
		suiteOfTests->addTest(new CppUnit::TestCaller<interfaceTestCase>("testrunCkinePretreat",
			&interfaceTestCase::testrunCkinePretreat));
		suiteOfTests->addTest(new CppUnit::TestCaller<interfaceTestCase>("testrunCkineSPretreat",
			&interfaceTestCase::testrunCkineSPretreat));
		return suiteOfTests;
	}

	mt19937 *gen;
	array<double, Nparams> rxnRatesIn;

protected:
	void checkRetVal(int retVal, array<double, Nparams> &rxnRatesIn) {
		if (retVal < 0) {
			for (auto i = rxnRatesIn.begin(); i != rxnRatesIn.end(); ++i)
				std::cout << *i << ' ';

			cout << std::endl;
		}

		CPPUNIT_ASSERT(retVal >= 0);
	}

	array<double, Nparams> getParams() {
		array<double, Nparams> rxnRatesIn;
		lognormal_distribution<> dis(0.1, 0.25);

		generate(rxnRatesIn.begin(), rxnRatesIn.end(), [this, &dis]() { return dis(*this->gen); });

		rxnRatesIn[49] = tanh(rxnRatesIn[49])*0.9;

		return rxnRatesIn;
	}

	void testrunCkine() {
		array<double, Nspecies*tps.size()> output;
		array<double, Nspecies*tps.size()> output2;

		int retVal = runCkine(tps.data(), tps.size(), output.data(), rxnRatesIn.data(), 0.0, nullptr);

		// Run a second time to make sure we get the same thing
		int retVal2 = runCkine(tps.data(), tps.size(), output2.data(), rxnRatesIn.data(), 0.0, nullptr);

		checkRetVal(retVal, rxnRatesIn);
		checkRetVal(retVal2, rxnRatesIn);

		CPPUNIT_ASSERT(std::equal(output.begin(), output.end(), output2.begin()));
	}

	void testrunCkineS() {
		array<double, tps.size()> output;
		array<double, tps.size()> output2;
		array<double, Nparams*tps.size()> soutput;
		array<double, Nparams*tps.size()> soutput2;
		array<double, Nspecies> actV;
		fill(actV.begin(), actV.end(), 0.0);
		actV[10] = 1.0;

		checkRetVal(runCkineS(tps.data(), tps.size(), output.data(), soutput.data(), actV.data(), rxnRatesIn.data(), 0.0, nullptr), rxnRatesIn);

		// Run a second time to make sure we get the same thing
		checkRetVal(runCkineS(tps.data(), tps.size(), output2.data(), soutput2.data(), actV.data(), rxnRatesIn.data(), 0.0, nullptr), rxnRatesIn);

		CPPUNIT_ASSERT(std::equal(output.begin(), output.end(), output2.begin()));

		// Check sensitivities reproducibility
		for (size_t ii = 0; ii < soutput.size(); ii++) {
			CPPUNIT_ASSERT_EQUAL_MESSAGE(std::string("runCkineS sensitivity at pos ") + std::to_string(ii), soutput[ii], soutput2[ii]);
		}
	}

	void testrunCkinePretreat() {
		array<double, Nspecies*tps.size()> output;
		array<double, Nspecies*tps.size()> output2;
		array<double, Nlig> preL = {{0.1, 0.2, 0.1, 0.01, 0.3, 0.6}};

		int retVal = runCkine(tps.data(), tps.size(), output.data(), rxnRatesIn.data(), 10.0, preL.data());

		// Run a second time to make sure we get the same thing
		int retVal2 = runCkine(tps.data(), tps.size(), output2.data(), rxnRatesIn.data(), 10.0, preL.data());

		checkRetVal(retVal, rxnRatesIn);
		checkRetVal(retVal2, rxnRatesIn);

		CPPUNIT_ASSERT(std::equal(output.begin(), output.end(), output2.begin()));
	}

	void testrunCkineSPretreat() {
		array<double, tps.size()> output;
		array<double, tps.size()> output2;
		array<double, Nparams*tps.size()> soutput;
		array<double, Nparams*tps.size()> soutput2;
		array<double, Nspecies> actV;
		array<double, Nlig> preL = {{0.1, 0.2, 0.1, 0.01, 0.3, 0.6}};
		fill(actV.begin(), actV.end(), 0.0);
		actV[10] = 1.0;

		checkRetVal(runCkineS(tps.data(), tps.size(), output.data(), soutput.data(), actV.data(), rxnRatesIn.data(), 10.0, preL.data()), rxnRatesIn);

		// Run a second time to make sure we get the same thing
		checkRetVal(runCkineS(tps.data(), tps.size(), output2.data(), soutput2.data(), actV.data(), rxnRatesIn.data(), 10.0, preL.data()), rxnRatesIn);

		CPPUNIT_ASSERT(std::equal(output.begin(), output.end(), output2.begin()));

		// Check sensitivities reproducibility
		for (size_t ii = 0; ii < soutput.size(); ii++) {
			CPPUNIT_ASSERT_EQUAL_MESSAGE(std::string("Pretreat sensitivity at pos ") + std::to_string(ii), soutput[ii], soutput2[ii]);
		}
	}
};

// the main method
int main () {
	CppUnit::TextUi::TestRunner runner;

	ofstream outputFile("testResults.xml");
	CppUnit::XmlOutputter* outputter = new CppUnit::XmlOutputter(&runner.result(), outputFile);
	runner.setOutputter(outputter);

	runner.addTest(interfaceTestCase::suite());

	runner.run();

	outputFile.close();

	return 0;
}


#ifndef KILLGEN_FIXPOINT_ITERATOR_HPP
#define KILLGEN_FIXPOINT_ITERATOR_HPP

/**
  * Specialized fixpoint iterators and domains for kill-gen problems.
  */

#include <crab/common/stats.hpp>
#include <crab/common/debug.hpp>
#include <crab/cfg/cfg_bgl.hpp>
#include <crab/analysis/graphs/sccg.hpp>
#include <crab/analysis/graphs/topo_order.hpp>

#include <crab/domains/discrete_domains.hpp>
#include <crab/domains/patricia_trees.hpp>

#include <boost/optional.hpp>

namespace crab {

  namespace domains {

    // A wrapper for discrete_domain (i.e,. set of Element)
    template<class Element>
    class flat_killgen_domain: public ikos::writeable {

     private:

      typedef flat_killgen_domain<Element> flat_killgen_domain_t;
      typedef ikos::discrete_domain<Element> discrete_domain_t;

     public:

      typedef typename discrete_domain_t::iterator iterator;
      typedef Element element_t;

     private:

      discrete_domain_t _inv;

     public:

      flat_killgen_domain(discrete_domain_t inv)
          : ikos::writeable(), _inv(inv){ }

      static flat_killgen_domain_t top() {
        return flat_killgen_domain(discrete_domain_t::top());
      }

      static flat_killgen_domain_t bottom() {
        return flat_killgen_domain(discrete_domain_t::bottom());
      }

      flat_killgen_domain()
          : ikos::writeable(), _inv(discrete_domain_t::bottom()){ }

      flat_killgen_domain(Element e)
          : ikos::writeable(), _inv(e) { }

      flat_killgen_domain(const flat_killgen_domain_t &o)
          : ikos::writeable(), _inv(o._inv) { }

      flat_killgen_domain(flat_killgen_domain_t &&o)
          : ikos::writeable(), _inv(std::move(o._inv)) { }

      flat_killgen_domain_t& operator=(const flat_killgen_domain_t &other) {
        if (this != &other)
          _inv = other._inv;
        return *this;
      }

      flat_killgen_domain_t& operator=(flat_killgen_domain_t &&other) {
        _inv = std::move(other._inv);
        return *this;
      }

      iterator begin() { return _inv.begin(); }

      iterator end() { return _inv.end(); }

      unsigned size() { return _inv.size(); }

      bool is_bottom() { return _inv.is_bottom(); }

      bool is_top() { return _inv.is_top(); }

      bool operator==(flat_killgen_domain_t other) {
	return *this <= other && other <= *this;
      }

      bool operator<=(flat_killgen_domain_t other) {
        if (is_bottom ())
          return true;
        else if (other.is_top ())
          return true;
        else
          return (_inv <= other._inv);
      }

      void operator-=(Element x) {
        if (is_bottom ())
          return;
        _inv -= x;
      }

      void operator-=(flat_killgen_domain_t other) {
        if (is_bottom () || other.is_bottom ())
          return;

        if (!other._inv.is_top()) {
          for (auto v : other)
            _inv -= v;
        }
      }

      void operator+=(Element x) {
        if (is_top ())
          return;
        _inv += x;
      }

      void operator+=(flat_killgen_domain_t other) {
        if (is_top () || other.is_bottom ()) {
          return;
        } else if (other.is_top ()) {
          _inv = discrete_domain_t::top();
        } else {
          _inv = (_inv | other._inv);
        }
      }

      flat_killgen_domain_t operator|(flat_killgen_domain_t other) {
        return (_inv | other._inv);
      }

      flat_killgen_domain_t operator&(flat_killgen_domain_t other) {
        return (_inv & other._inv);
      }

      void write(crab_os& o) { _inv.write(o); }

    };

    // To represent sets of pairs (Key,Value).
    // Bottom means empty set rather than failure.
    template <typename Key, typename Value>
    class separate_killgen_domain: public ikos::writeable {

    private:
      typedef ikos::patricia_tree<Key,Value> patricia_tree_t;
      typedef typename patricia_tree_t::unary_op_t unary_op_t;
      typedef typename patricia_tree_t::binary_op_t binary_op_t;
      typedef typename patricia_tree_t::partial_order_t partial_order_t;

    public:
      typedef separate_killgen_domain<Key,Value > separate_killgen_domain_t;
      typedef typename patricia_tree_t::iterator iterator;
      typedef Key key_type;
      typedef Value value_type;

    private:
      bool _is_top;
      patricia_tree_t _tree;

    public:
      class bottom_found { };

      class join_op: public binary_op_t {
	boost::optional< Value > apply(Value x, Value y) {
	  Value z = x.operator|(y);
	  if (z.is_top()) {
	    return boost::optional<Value>();
	  } else {
	    return boost::optional<Value>(z);
	  }
	}
	bool default_is_absorbing() { return false; }
      }; // class join_op

      class meet_op: public binary_op_t {
	boost::optional< Value > apply(Value x, Value y) {
	  Value z = x.operator&(y);
	  if (z.is_bottom()) {
	    throw bottom_found();
	  } else {
	    return boost::optional<Value>(z);
	  }
	};
	bool default_is_absorbing() { return true; }
      }; // class meet_op

      class domain_po: public partial_order_t {
	bool leq(Value x, Value y) { return x.operator<=(y); }
	bool default_is_top() { return false; }
      }; // class domain_po

   public:

      static separate_killgen_domain_t top() {
	return separate_killgen_domain_t (true);
      }

      static separate_killgen_domain_t bottom() {
	return separate_killgen_domain_t (false);
      }

   private:

      static patricia_tree_t apply_operation(binary_op_t& o,
					     patricia_tree_t t1,
					     patricia_tree_t t2) {
	t1.merge_with(t2, o);
	return t1;
      }

      separate_killgen_domain(patricia_tree_t t)
	: _is_top(false), _tree(t) { }

      separate_killgen_domain(bool b)
	: _is_top(b) { }

    public:

      separate_killgen_domain()
	: _is_top(false), _tree (patricia_tree_t()) { }

      separate_killgen_domain(const separate_killgen_domain_t& o)
	: _is_top(o._is_top), _tree(o._tree) { }

      separate_killgen_domain_t& operator=(separate_killgen_domain_t o) {
	this->_is_top = o._is_top;
	this->_tree = o._tree;
	return *this;
      }

      iterator begin() const {
	if (this->is_top()) {
	  CRAB_ERROR("Separate killgen domain: trying to invoke iterator on top");
	} else {
	  return this->_tree.begin();
	}
      }

      iterator end() const {
	if (this->is_top()) {
	  CRAB_ERROR("Separate killgen domain: trying to invoke iterator on top");
	} else {
	  return this->_tree.end();
	}
      }

      bool is_top() const {
	return _is_top;
      }

      bool is_bottom() const {
	return (!is_top () && _tree.empty ());
      }

      bool operator<=(separate_killgen_domain_t o) {
	domain_po po;
	return (o.is_top() || (!is_top() && (_tree.leq (o._tree, po))));
      }

      separate_killgen_domain_t operator|(separate_killgen_domain_t o) {
	if (is_top() || o.is_top ()) {
	  return separate_killgen_domain_t::top();
	} else {
	  join_op op;
	  return separate_killgen_domain_t(apply_operation(op, _tree, o._tree));
	}
      }

      separate_killgen_domain_t operator&(separate_killgen_domain_t o) {
	if (is_top ()) {
	  return o;
	} else if (o.is_top()) {
	  return *this;
	} else {
	  try {
	    meet_op op;
	    return separate_killgen_domain_t(apply_operation(op, _tree, o._tree));
	  }
	  catch (bottom_found& exc) {
	    return separate_killgen_domain_t::bottom ();
	  }
	}
      }

      void set(Key k, Value v) {
	if (!is_top ()) {
	  // if (v.is_bottom()) {
	  //   this->_tree.remove(k);
	  // } else {
	  //   this->_tree.insert(k, v);
	  // }
	  this->_tree.insert(k, v);
	}
      }

      separate_killgen_domain_t& operator-=(Key k) {
	if (!is_top ()) {
	  _tree.remove(k);
	}
	return *this;
      }

      Value operator[](Key k) {
	if (is_top ())
	  return Value::top ();
	else {
	  boost::optional< Value > v = _tree.lookup(k);
	  if (v) {
	    return *v;
	  } else {
	    return Value::bottom();
	  }
	}
      }

      void write(crab::crab_os& o) {
	if (this->is_top()) {
	  o << "{...}";
	} if (_tree.empty ()) {
	  o << "_|_";
	}
	else {
	  o << "{";
	  for (typename patricia_tree_t::iterator it = this->_tree.begin();
	       it != this->_tree.end(); ) {
	    Key k = it->first;
	    k.write(o);
	    o << " -> ";
          Value v = it->second;
          v.write(o);
          ++it;
          if (it != this->_tree.end()) {
            o << "; ";
	  }
	  }
	  o << "}";
	}
      }
    }; // class separate_killgen_domain

  } // end namespace domains

  namespace iterators {

    // API for a kill-gen analysis operations
    template<class CFG, class Dom>
    class killgen_operations_api {

     public:

      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef Dom killgen_domain_t;

     protected:

      CFG _cfg;

     public:

      killgen_operations_api (CFG cfg): _cfg(cfg) { }

      virtual ~killgen_operations_api() { }

      // whether forward or backward analysis
      virtual bool is_forward () = 0;

      // initial state
      virtual Dom entry() = 0;

      // (optional) initialization for the fixpoint
      virtual void init_fixpoint () = 0;

      // confluence operator
      virtual Dom merge(Dom, Dom) = 0;

      // analyze a basic block
      virtual Dom analyze (basic_block_label_t, Dom) = 0;

      // analysis name
      virtual std::string name () = 0;
    };

    // A simple fixpoint for a killgen analysis
    template<class CFG, class KgAnalysisOps>
    class killgen_fixpoint_iterator {

     public:

      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef typename KgAnalysisOps::killgen_domain_t killgen_domain_t;
      typedef boost::unordered_map<basic_block_label_t,killgen_domain_t> inv_map_t;
      typedef typename inv_map_t::iterator iterator;
      typedef typename inv_map_t::const_iterator const_iterator;

     protected:

      CFG _cfg;
      inv_map_t _in_map;
      inv_map_t _out_map;

     private:

      KgAnalysisOps _analysis;

      /// XXX: run_bwd_fixpo(G) is equivalent to run_fwd_fixpo(reverse(G)).
      ///      However, weak_rev_topo_sort(G) != weak_topo_sort(reverse(G))
      /// For instance, for a G=(V,E) where
      ///   V= {v1,v2, v3, v4, v5},
      ///   E= {(v1,v2), (v1,v3), (v2,v4), (v4,v1), (v3,v5)}
      /// (1) weak_rev_topo_sort(cfg)=[v5,v3,v4,v2,v1]
      /// (2) weak_topo_sort(reverse(cfg))=[v5,v3,v2,v4,v1] or even
      ///     worse [v5,v3,v1,v4,v2] if vertices in the same scc are
      ///     traversed in preorder.
      /// For a backward analysis, (1) will converge faster.
      /// For all of this, we decide not to reverse graphs and have
      /// two dual versions for the forward and backward analyses.

      void run_fwd_fixpo (std::vector<typename CFG::node_t> &order,
                          unsigned &iterations){

        order = crab::analyzer::graph_algo::weak_topo_sort(_cfg);
        assert ((int)order.size () == std::distance(_cfg.begin(), _cfg.end()));
        bool change = true;
        iterations = 0;
        while (change) {
          change = false;
          ++iterations;
          for (auto &n: order) {
            auto in = _analysis.entry();
            for (auto p: _cfg.prev_nodes (n))
              in = _analysis.merge(in, _out_map[p]);
            auto old_out = _out_map[n];
            auto out = _analysis.analyze(n, in);
            if (!(out <= old_out)) {
              _out_map[n] = _analysis.merge(out, old_out);
              change = true;
            } else
              _in_map[n] = in;
          }
        }
      }

      void run_bwd_fixpo (std::vector<typename CFG::node_t> &order,
                          unsigned &iterations){

        order = crab::analyzer::graph_algo::weak_rev_topo_sort(_cfg);
        assert ((int)order.size () == std::distance(_cfg.begin(), _cfg.end()));
        bool change = true;
        iterations = 0;
        while (change) {
          change = false;
          ++iterations;
          for (auto &n: order) {
            auto out = _analysis.entry();
            for (auto p: _cfg.next_nodes (n))
              out = _analysis.merge(out, _in_map[p]);
            auto old_in = _in_map[n];
            auto in = _analysis.analyze(n, out);
            if (!(in <= old_in)) {
              _in_map[n] = _analysis.merge(in, old_in);
              change = true;
            } else
              _out_map[n] = out;
          }
        }
      }

     public:

      killgen_fixpoint_iterator (CFG cfg)
	: _cfg (cfg), _analysis (_cfg) { }

      void release_memory () {
        _in_map.clear();
        _out_map.clear();
      }

      void run() {
        crab::ScopedCrabStats __st__(_analysis.name());

        _analysis.init_fixpoint();

        std::vector<typename CFG::node_t> order;
        unsigned iterations = 0;
        if (_analysis.is_forward())
          run_fwd_fixpo(order, iterations);
        else
          run_bwd_fixpo(order, iterations);

        CRAB_LOG(_analysis.name(),
                 crab::outs()  << "fixpoint ordering={";
                 bool first=true;
                 for (auto &v : order) {
                   if (!first) crab::outs() << ",";
                   first=false;
                   crab::outs() << cfg_impl::get_label_str(v);
                 }
                 crab::outs() << "}\n";);

        CRAB_LOG(_analysis.name(),
                 crab::outs() << _analysis.name() << ": "
                              << "fixpoint reached in " << iterations << " iterations.\n");

        CRAB_LOG(_analysis.name(),
                 crab::outs() << _analysis.name() << " sets:\n";
                 for (auto n: boost::make_iterator_range (_cfg.label_begin (),
                                                          _cfg.label_end ())) {
                   crab::outs() << cfg_impl::get_label_str(n) << " "
                                << "IN="  << _in_map[n]  << " "
                                << "OUT=" << _out_map[n] << "\n";
                 }
                 crab::outs() << "\n";);
      }

      iterator in_begin() { return _in_map.begin(); }
      iterator in_end() { return _in_map.end(); }
      const_iterator in_begin() const { return _in_map.begin(); }
      const_iterator in_end() const { return _in_map.end(); }

      iterator out_begin() { return _out_map.begin(); }
      iterator out_end() { return _out_map.end(); }
      const_iterator out_begin() const { return _out_map.begin(); }
      const_iterator out_end() const { return _out_map.end(); }

   };

  } // end namespace iterators
} // end namespace crab

#endif


//
// Created by erik on 1/3/17.
//

#include "components.h"
#include "core/ecs/EntityManager.h"
#include <boost/algorithm/clamp.hpp>
#include <boost/throw_exception.hpp>

using namespace spatacs;
using namespace game;
using namespace components;

double Health::setCurrent(double new_health)
{
    mCurrent = boost::algorithm::clamp(new_health, 0.0, mMaximum);
    return mCurrent;
}

void Health::setMaximum(double new_maximum)
{
    if(new_maximum < 0)
        BOOST_THROW_EXCEPTION(std::logic_error("negative maximum health!"));

    mMaximum = new_maximum;
    setCurrent(mCurrent);
}

double Health::setHealthStatus(double percentage)
{
    return setCurrent(percentage * maximum());
}

PhysicsData::PhysicsData(physics::ObjectID id, mass_t mass, length_vec pos, velocity_vec vel) :
    mPhysicsID(id), mMass(mass), mPosition(pos), mVelocity(vel)
{

}

PhysicsData& PhysicsData::setPosition(const length_vec& pos)
{
    mPosition = pos;
    return *this;
}

PhysicsData& PhysicsData::setVelocity(const velocity_vec& vel)
{
    mVelocity = vel;
    return *this;
}

PhysicsData& PhysicsData::setMass(mass_t m)
{
    mMass = m;
    return *this;
}

physics::ObjectID PhysicsData::id() const
{
    return mPhysicsID;
}

PhysicsData& PhysicsData::setForce(const force_vec& force)
{
    mForce = force;
    return *this;
}

mass_t PhysicsData::mass() const
{
    return mMass;
}

const length_vec& PhysicsData::position() const
{
    return mPosition;
}

const velocity_vec& PhysicsData::velocity() const
{
    return mVelocity;
}

const force_vec& PhysicsData::force() const
{
    return mForce;
}

// ---------------------------------------------------------------------------------------------------------------------
//                                              Propulsion Control
// ---------------------------------------------------------------------------------------------------------------------

void PropulsionControl::setDesiredAcceleration(const accel_vec& desired)
{
    mDesiredAcceleration = desired;
}

const accel_vec& PropulsionControl::desired_acceleration() const
{
    return mDesiredAcceleration;
}

void PropulsionControl::setMaximumAcceleration(accel_t macc)
{
    mMaximumAcceleration = macc;
}

accel_t PropulsionControl::maximum_acceleration() const
{
    return mMaximumAcceleration;
}

// ---------------------------------------------------------------------------------------------------------------------
//                                                  Fuel Tank
// ---------------------------------------------------------------------------------------------------------------------

FuelTank::FuelTank(mass_t v) : mCapacity(v)
{
}

mass_t FuelTank::request(mass_t desire)
{
    if(desire < 0.0_kg)
        BOOST_THROW_EXCEPTION( std::logic_error("Negative mass specified!") );
    auto m = std::min(desire, mCurrent);
    mCurrent -= m;
    return m;
}

mass_t FuelTank::fill(mass_t amount)
{
    if(amount < 0.0_kg)
        BOOST_THROW_EXCEPTION( std::logic_error("Negative mass specified!") );

    auto space = mCapacity - mCurrent;
    auto m = std::min(amount, space);
    mCurrent += m;
    return amount - m;
}

mass_t FuelTank::capacity() const
{
    return mCapacity;
}


/*
 * Copyright 2016 C. Brett Witherspoon
 */

#ifndef SIGNUM_OPENCL_FFT_HPP_
#define SIGNUM_OPENCL_FFT_HPP_

#include <complex>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/kernel.hpp>
#include <boost/compute/program.hpp>

namespace signum
{
namespace opencl
{

//! Compute the DFT using the FFT algorithm with OpenCL
class fft
{
public:
    fft(boost::compute::command_queue& queue, std::size_t length);

    fft(const fft&) = delete;

    ~fft() = default;

    fft& operator=(const fft&) = delete;

    std::complex<float>* map(cl_mem_flags flags, const boost::compute::wait_list &events = boost::compute::wait_list());

    boost::compute::event unmap();

    boost::compute::wait_list operator()();

private:
    void *m_host_ptr;
    const std::size_t m_length;
    const std::size_t m_stages;
    boost::compute::command_queue m_queue;
    boost::compute::program m_program;
    std::vector<boost::compute::kernel> m_kernels;
    boost::compute::buffer m_buffer;
    boost::compute::wait_list m_events;
};

} // end namespace opencl
} // end namespace signum

#endif /* SIGNUM_OPENCL_FFT_HPP_ */


#include <waypoint_server/map.h>

#include <fstream>
#include <stdexcept>
#include <fstream>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <yaml-cpp/yaml.h>

namespace {
    // YAML node keys
    static const std::string ns = "waypoint_server",
                             waypoints_ns = "waypoints",
                             scale = "scale",
                             position = "position",
                             euler_angles = "euler_angles",
                             x = "x",
                             y = "y",
                             z = "z",
                             id = "id",
                             connections = "connections",
                             properties = "properties",
                             key = "key",
                             value = "value";
}

namespace waypoint_server {
    void geometricConverter(Map &),
         geometricDeconverter(Map &);

    Map::Map(const bool &debug) : debug(debug) {
        position = Eigen::Vector3f::Zero();
        euler_angles = Eigen::Vector3f::Zero();

        scale = 1.0;

        is_converted = false;
    }

    void Map::load(const std::string &filename) {
        std::lock_guard<std::mutex> lock(waypoints_mutex);

        if(filename.empty()) {
            throw std::runtime_error("Load failed: empty waypoint_server::Map::load()");
        }
        std::ifstream input_file{filename};

        if(!input_file.is_open()) {
            std::cout
                << "Does not find "
                << filename
                << " waypoint_server::Map::load()"
                << std::endl;
            return;
        }

        YAML::Node yaml,
                   waypoints_yaml;

        yaml = YAML::LoadFile(filename);

        named_waypoints.clear();

        scale = yaml[::ns][::scale].as<float>();
        position.x() = yaml[::ns][::position][::x].as<float>();
        position.y() = yaml[::ns][::position][::y].as<float>();
        position.z() = yaml[::ns][::position][::z].as<float>();
        euler_angles.x() = yaml[::ns][::euler_angles][::x].as<float>();
        euler_angles.y() = yaml[::ns][::euler_angles][::y].as<float>();
        euler_angles.z() = yaml[::ns][::euler_angles][::z].as<float>();

        waypoints_yaml = yaml[::ns][::waypoints_ns];

        for(unsigned int i = 0; i < waypoints_yaml.size(); i ++) {
            YAML::Node data = waypoints_yaml[i];

            if(data[::id].IsNull()) {
                continue;
            }
            std::string index = data[::id].as<std::string>();

            if(data[::position].IsNull()) {
                named_waypoints[index].enable_position = false;
            }
            else {
                named_waypoints[index].goal(0) = data[::position][::x].as<float>();
                named_waypoints[index].goal(1) = data[::position][::y].as<float>();
                named_waypoints[index].goal(2) = data[::position][::z].as<float>();
            }

            if(data[::euler_angles].IsNull()) {
                named_waypoints[index].enable_orientation = false;
            }
            else {
                named_waypoints[index].goal(3) = data[::euler_angles][::x].as<float>();
                named_waypoints[index].goal(4) = data[::euler_angles][::y].as<float>();
                named_waypoints[index].goal(5) = data[::euler_angles][::z].as<float>();
            }

            if(!data[::connections].IsNull()) {
                for(unsigned int i = 0; i < data[::connections].size(); i ++) {
                    named_waypoints[index].connections.push_back(data[::connections][i].as<std::string>());
                }
            }
            if(!data[::properties].IsNull()) {
                for(const auto &propertiy : data[::properties]) {
                    named_waypoints[index].properties[propertiy[::key].as<std::string>()]
                        = propertiy[::value].as<std::string>();
                }
            }
        }

        setConvertStatus(false);
        geometricConverter(*this);

        if(!debug) {
            return;
        }
        std::cout << "scale: " << scale << std::endl;
        std::cout << "position: " << position.transpose() << std::endl;
        std::cout << "euler_angles: " << euler_angles.transpose() << std::endl;
        for(const auto &[key, waypoint] : named_waypoints) {
            std::cout << "id: " << key << std::endl;
            std::cout << "  goal: " << waypoint.goal.transpose() << std::endl;
            std::cout << "  enable_position: " << std::boolalpha << waypoint.enable_position << std::endl;
            std::cout << "  enable_orientation: " << std::boolalpha << waypoint.enable_orientation << std::endl;

            for(const auto &connect : waypoint.connections) {
                std::cout << "  connect: " << connect << std::endl;
            }
            for(const auto &[key, value] : waypoint.properties) {
                std::cout << "  key: " << key << ", value: " << value << std::endl;
            }
        }
    }

    void Map::save(const std::string &filename) {
        std::lock_guard<std::mutex> lock(waypoints_mutex);

        if(filename.empty()) {
            throw std::runtime_error("Save failed: empty filename waypoint_server::Map::save()");
        }
        YAML::Node yaml,
                   waypoints_yaml;

        geometricDeconverter(*this);

        waypoints_yaml[::scale] = scale;
        waypoints_yaml[::position][::x] = position.x();
        waypoints_yaml[::position][::y] = position.y();
        waypoints_yaml[::position][::z] = position.z();
        waypoints_yaml[::euler_angles][::x] = euler_angles.x();
        waypoints_yaml[::euler_angles][::y] = euler_angles.y();
        waypoints_yaml[::euler_angles][::z] = euler_angles.z();

        unsigned int count_of_waypoints = 0;

        for(const auto &[key, data] : named_waypoints) {
            YAML::Node waypoint_yaml;

            waypoint_yaml[::id] = key;
            waypoint_yaml[::position][::x] = data.goal(0);
            waypoint_yaml[::position][::y] = data.goal(1);
            waypoint_yaml[::position][::z] = data.goal(2);
            waypoint_yaml[::euler_angles][::x] = data.goal(3);
            waypoint_yaml[::euler_angles][::y] = data.goal(4);
            waypoint_yaml[::euler_angles][::z] = data.goal(5);

            unsigned int count_of_properties = 0;

            for(const auto &[pkey, pdata] : data.properties) {
                YAML::Node properties_yaml;

                properties_yaml[::key] = pkey;
                properties_yaml[::value] = pdata;

                waypoint_yaml[::properties][count_of_properties] = properties_yaml;
                count_of_properties ++;
            }
            unsigned int count_of_connections = 0;

            for(const auto &connect : data.connections) {
                waypoint_yaml[::connections][count_of_connections] = connect;
                count_of_connections ++;
            }

            waypoints_yaml[::waypoints_ns][count_of_waypoints] = waypoint_yaml;
            count_of_waypoints ++;
        }
        yaml[::ns] = waypoints_yaml;

        std::ofstream output_file{filename};

        if(!output_file.is_open()) {
            throw std::runtime_error("Failed open " + filename + " waypoint_server::Map::save()");
        }
        output_file << yaml << std::endl;;
        output_file.close();

        setConvertStatus(false);
        geometricConverter(*this);
    }

    Map::NamedWaypoints &Map::data() noexcept {
        return named_waypoints;
    }

    void Map::setPosition(const Eigen::Vector3f &position) {
        this->position = position;
    }

    void Map::setEulerAngle(const Eigen::Vector3f &euler_angles) {
        this->euler_angles = euler_angles;
    }

    const float &Map::getScale() const {
        return scale;
    }

    const Eigen::Vector3f &Map::getPosition() const {
        return position;
    }

    const Eigen::Vector3f &Map::getEulerAngle() const {
        return euler_angles;
    }

    void Map::setQuaternion(const Key &key, const Waypoint::Quaternion &quaternion) {
        {
            std::lock_guard<std::mutex> lock(waypoints_mutex);
            named_waypoints[key].quaternion = quaternion;
        }
        setQuaternion(key);
    }

    void Map::setQuaternion(const Key &key) {
        std::lock_guard<std::mutex> lock(waypoints_mutex);

        named_waypoints[key].goal.block(3, 0, 3, 1) = named_waypoints[key].quaternion.toRotationMatrix().eulerAngles(0, 1, 2);
    }

    void Map::updateQuaternion(const Key &key) {
        if(named_waypoints.count(key) == 0) {
            throw std::runtime_error(
                "Update failed because not found waypoint waypoint_server::Map::updateQuaternion()"
            );
        }
        Waypoint::Quaternion quaternion;
        const Eigen::Vector3f waypoint_euler_angles{named_waypoints[key].goal.block(3, 0, 3, 1)};

        quaternion = Eigen::AngleAxisf(waypoint_euler_angles.z(), waypoint_euler_angles.UnitZ());
        quaternion = Eigen::AngleAxisf(waypoint_euler_angles.y(), waypoint_euler_angles.UnitY()) * quaternion;
        quaternion = Eigen::AngleAxisf(waypoint_euler_angles.x(), waypoint_euler_angles.UnitX()) * quaternion;
    }

    const bool &Map::isConverted() const {
        return is_converted;
    }

    void Map::setConvertStatus(const bool &status) {
        is_converted = status;
    }

    void Map::append(
        const Key &key,
        const Eigen::Vector3f &position,
        const Eigen::Quaternionf &quaternion
    ) {
        std::lock_guard<std::mutex> lock(waypoints_mutex);
        named_waypoints[key].goal.block(0, 0, 3, 1) = position;
        setQuaternion(key, quaternion);
    }

    void Map::erase() {
        std::lock_guard<std::mutex> lock(waypoints_mutex);
        named_waypoints.clear();
    }

    void Map::erase(const Key &key) {
        if(!hasKey(key)) {
            return;
        }
        std::lock_guard<std::mutex> lock(waypoints_mutex);
        named_waypoints.erase(key);
    }

    const bool Map::hasKey(const Key &key) {
        std::lock_guard<std::mutex> lock(waypoints_mutex);

        if(named_waypoints.count(key) == 0) {
            return false;
        }
        return true;
    }

    Waypoint &Map::operator [] (const Key &key) {
        std::lock_guard<std::mutex> lock(waypoints_mutex);

        return named_waypoints[key];
    }

    void geometricConverter(Map &map) {
        if(map.isConverted()) {
            return;
        }
        if(map.debug) {
            std::cout << "Start geometricConverter" << std::endl;
        }

        Eigen::Vector3f map_position,
                        map_euler_angles;

        map_position = map.getPosition();
        map_euler_angles = map.getEulerAngle();

        Waypoint::Quaternion map_quaternion;

        map_quaternion = Eigen::AngleAxisf(map_euler_angles.z(), map_euler_angles.UnitZ());
        map_quaternion = Eigen::AngleAxisf(map_euler_angles.y(), map_euler_angles.UnitY()) * map_quaternion;
        map_quaternion = Eigen::AngleAxisf(map_euler_angles.x(), map_euler_angles.UnitX()) * map_quaternion;

        for(auto &&[key, waypoint] : map.data()) {
            Eigen::Vector3f position = waypoint.goal.block(0, 0, 3, 1);
            Eigen::Vector3f euler_angles = waypoint.goal.block(3, 0, 3, 1);

            Waypoint::Quaternion quaternion;

            quaternion = Eigen::AngleAxisf(euler_angles.z(), euler_angles.UnitZ());
            quaternion = Eigen::AngleAxisf(euler_angles.y(), euler_angles.UnitY()) * quaternion;
            quaternion = Eigen::AngleAxisf(euler_angles.x(), euler_angles.UnitX()) * quaternion;

            quaternion = quaternion * map_quaternion;
            waypoint.quaternion = quaternion;
            position = map_quaternion * (position + map_position);
            position = position * map.getScale();

            waypoint.goal.block(0, 0, 3, 1) = position;
            waypoint.goal.block(3, 0, 3, 1) = quaternion.toRotationMatrix().eulerAngles(0, 1, 2);
            waypoint.quaternion = quaternion;
        }

        map.setConvertStatus(true);
    }

    void geometricDeconverter(Map &map) {
        if(!map.isConverted()) {
            return;
        }
        if(map.debug) {
            std::cout << "Start geometricDeconverter" << std::endl;
        }

        Eigen::Vector3f map_position,
                        map_euler_angles;

        map_position = map.getPosition();
        map_euler_angles = map.getEulerAngle();

        Waypoint::Quaternion map_quaternion;

        map_quaternion = Eigen::AngleAxisf(map_euler_angles.z(), map_euler_angles.UnitZ());
        map_quaternion = Eigen::AngleAxisf(map_euler_angles.y(), map_euler_angles.UnitY()) * map_quaternion;
        map_quaternion = Eigen::AngleAxisf(map_euler_angles.x(), map_euler_angles.UnitX()) * map_quaternion;

        for(auto &&[key, waypoint] : map.data()) {
            Eigen::Vector3f position = waypoint.goal.block(0, 0, 3, 1);
            Eigen::Vector3f euler_angles = waypoint.goal.block(3, 0, 3, 1);

            Waypoint::Quaternion quaternion;

            quaternion = Eigen::AngleAxisf(euler_angles.z(), euler_angles.UnitZ());
            quaternion = Eigen::AngleAxisf(euler_angles.y(), euler_angles.UnitY()) * quaternion;
            quaternion = Eigen::AngleAxisf(euler_angles.x(), euler_angles.UnitX()) * quaternion;
            quaternion = quaternion * map_quaternion.inverse();

            position = position / map.getScale();
            position = map_quaternion.inverse() * position - map_position;

            waypoint.goal.block(0, 0, 3, 1) = position;
            waypoint.goal.block(3, 0, 3, 1) = quaternion.toRotationMatrix().eulerAngles(0, 1, 2);
            waypoint.quaternion = quaternion;
        }

        map.setConvertStatus(false);
    }
}


/* Copyright Institue of Sound and Vibration Research - All rights reserved. */

#include <librbbl/multichannel_convolver_uniform.hpp>

#include <libefl/basic_matrix.hpp>
#include <libvisr/constants.hpp>
#include <libpml/matrix_parameter.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>

#include <ciso646>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <vector>

namespace visr
{
namespace rbbl
{
namespace test
{

BOOST_AUTO_TEST_CASE( MultichannelConvolver )
{
  static const std::size_t alignment = 8; // element
  using SampleType = float;
  using Conv = MultichannelConvolverUniform<SampleType>;
  boost::filesystem::path const baseDir = CMAKE_CURRENT_SOURCE_DIR;
  pml::MatrixParameter<SampleType> const filters = pml::MatrixParameter<SampleType>::fromAudioFile( (baseDir/boost::filesystem::path("signals/filters_4ch.wav")).string(), alignment );
  pml::MatrixParameter<SampleType> const inputSignal = pml::MatrixParameter<SampleType>::fromAudioFile( (baseDir / boost::filesystem::path( "signals/input_4ch.wav" )).string( ), alignment );
  pml::MatrixParameter<SampleType> const outputSignalReference = pml::MatrixParameter<SampleType>::fromAudioFile( (baseDir / boost::filesystem::path( "signals/output_4ch.wav" )).string( ), alignment );

  std::size_t const cNumberOfInputs = inputSignal.numberOfRows();
  std::size_t const cNumberOfOutputs = outputSignalReference.numberOfRows( );
  std::size_t const cNumFilters = filters.numberOfRows();
  std::size_t const cSignalLength = inputSignal.numberOfColumns();

  std::size_t const cFilterLength = filters.numberOfColumns();

  std::size_t const cBlockLength = 32;
  BOOST_CHECK( cBlockLength % alignment == 0 );

  std::size_t const cMaxRoutings = 12;
  rbbl::FilterRoutingList routings = {{ 0, 0, 0, 1.0f }, { 1, 1, 1, 1.0f } };

  efl::BasicMatrix<SampleType> outputSignal( cNumberOfOutputs, cSignalLength, alignment );

  Conv convolver( cNumberOfInputs, cNumberOfOutputs, cBlockLength, cFilterLength, cMaxRoutings,
    cNumFilters, routings, filters, alignment, "kissfft" );

  BOOST_CHECK_MESSAGE( cSignalLength % cBlockLength == 0 , "The signal length must be an integral multiple of the block size." );
  std::size_t const cNumBlocks = cSignalLength / cBlockLength;
  for( std::size_t blockIdx( 0 ); blockIdx < cNumBlocks; ++blockIdx )
  {
    const std::size_t signalIdx = blockIdx * cBlockLength;
    convolver.process( inputSignal.data() + signalIdx, inputSignal.stride(),
                       outputSignal.data() + signalIdx, outputSignal.stride() );
  }

  for( std::size_t chIdx( 0 ); chIdx < cNumberOfOutputs; ++chIdx )
  {
    SampleType const * outSig = outputSignal.row( chIdx );
    SampleType const * outSigRef = outputSignalReference.row( chIdx );

    efl::BasicVector<SampleType> diff( cSignalLength, alignment );
    std::transform( outSig, outSig + cSignalLength, outSigRef, diff.data(), []( SampleType lhs, SampleType rhs ) { return std::abs( lhs - rhs ); } );

    SampleType * res = diff.data();

    SampleType const * maxErr = std::max_element( res, res + diff.size() );
    std::cout << "Max. error for channel " << chIdx << ": " << *res << std::endl;
    BOOST_CHECK_MESSAGE( *maxErr <= static_cast<SampleType>(4.0) * std::numeric_limits<SampleType>::epsilon(), "Convolution error exceeds 4*epsilon() for this datatype." );
  }
}

} // namespace test
} // namespace rbbl
} // namespace visr


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Regression

#include <fstream>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <xolotl/core/advection/W111AdvectionHandler.h>
#include <xolotl/core/network/PSIReactionNetwork.h>
#include <xolotl/options/Options.h>
#include <xolotl/test/CommandLine.h>
#include <xolotl/util/MPIUtils.h>

using namespace std;
using namespace xolotl;
using namespace core;
using namespace advection;

using Kokkos::ScopeGuard;
BOOST_GLOBAL_FIXTURE(ScopeGuard);

/**
 * This suite is responsible for testing the W111AdvectionHandler.
 */
BOOST_AUTO_TEST_SUITE(W111AdvectionHandler_testSuite)

/**
 * Method checking the initialization and the compute advection methods.
 */
BOOST_AUTO_TEST_CASE(checkAdvection)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=8 0 0 1 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	util::mpiInit(cl.argc, cl.argv);
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	std::vector<double> temperatures;
	for (int l = 0; l < 5; l++) {
		grid.push_back((double)l);
		temperatures.push_back(1000.0);
	}

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create ofill
	network::IReactionNetwork::SparseFillMap ofill;

	// Create a collection of advection handlers
	std::vector<IAdvectionHandler*> advectionHandlers;

	// Create the advection handler and initialize it
	W111AdvectionHandler advectionHandler;
	advectionHandler.initialize(network, ofill);
	advectionHandler.initializeAdvectionGrid(advectionHandlers, grid, 3, 0);

	// Check the total number of advecting clusters
	BOOST_REQUIRE_EQUAL(advectionHandler.getNumberOfAdvecting(), 7);

	// Check the clusters in ofill
	BOOST_REQUIRE_EQUAL(ofill[1][0], 1);
	BOOST_REQUIRE_EQUAL(ofill[3][0], 3);
	BOOST_REQUIRE_EQUAL(ofill[5][0], 5);
	BOOST_REQUIRE_EQUAL(ofill[7][0], 7);
	BOOST_REQUIRE_EQUAL(ofill[9][0], 9);
	BOOST_REQUIRE_EQUAL(ofill[11][0], 11);
	BOOST_REQUIRE_EQUAL(ofill[13][0], 13);

	// Set the size parameter in the x direction
	double hx = 1.0;

	// Create the arrays of concentration
	double concentration[3 * dof];
	double newConcentration[3 * dof];

	// Initialize their values
	for (int i = 0; i < 3 * dof; i++) {
		concentration[i] = (double)i * i;
		newConcentration[i] = 0.0;
	}

	// Set the temperature to 1000K to initialize the diffusion coefficients
	network.setTemperatures(temperatures);
	network.syncClusterDataOnHost();

	// Get pointers
	double* conc = &concentration[0];
	double* updatedConc = &newConcentration[0];

	// Get the offset for the grid point in the middle
	double* concOffset = conc + dof;
	double* updatedConcOffset = updatedConc + dof;

	// Fill the concVector with the pointer to the middle, left, and right grid
	// points
	double* concVector[3]{};
	concVector[0] = concOffset; // middle
	concVector[1] = conc; // left
	concVector[2] = conc + 2 * dof; // right

	// Set the grid position
	plsm::SpaceVector<double, 3> gridPosition{hx, 0.0, 0.0};

	// Compute the advection at this grid point
	advectionHandler.computeAdvection(
		network, gridPosition, concVector, updatedConcOffset, hx, hx, 0);

	// Check the new values of updatedConcOffset
	BOOST_REQUIRE_CLOSE(updatedConcOffset[1], -2.0171e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[3], -2.2012e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[5], -2.8037e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[7], -2.6928e+11, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[9], -1.0761e+12, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[11], -3.6701e+10, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[13], -1.0429e+10, 0.01);
	BOOST_REQUIRE_CLOSE(updatedConcOffset[0], 0.0, 0.01); // Does not advect
	BOOST_REQUIRE_CLOSE(updatedConcOffset[15], 0.0, 0.01); // Does not advect

	// Initialize the rows, columns, and values to set in the Jacobian
	int nAdvec = advectionHandler.getNumberOfAdvecting();
	IdType indices[nAdvec];
	double val[2 * nAdvec];
	// Get the pointer on them for the compute advection method
	IdType* indicesPointer = &indices[0];
	double* valPointer = &val[0];

	// Compute the partial derivatives for the advection a the grid point 1
	advectionHandler.computePartialsForAdvection(
		network, valPointer, indicesPointer, gridPosition, hx, hx, 0);

	// Check the values for the indices
	BOOST_REQUIRE_EQUAL(indices[0], 1);
	BOOST_REQUIRE_EQUAL(indices[1], 3);
	BOOST_REQUIRE_EQUAL(indices[2], 5);
	BOOST_REQUIRE_EQUAL(indices[3], 7);
	BOOST_REQUIRE_EQUAL(indices[4], 9);
	BOOST_REQUIRE_EQUAL(indices[5], 11);
	BOOST_REQUIRE_EQUAL(indices[6], 13);

	// Check values
	BOOST_REQUIRE_CLOSE(val[0], -815207266.0, 0.01);
	BOOST_REQUIRE_CLOSE(val[1], 50950454.0, 0.01);
	BOOST_REQUIRE_CLOSE(val[2], -700039625.0, 0.01);
	BOOST_REQUIRE_CLOSE(val[3], 43752477.0, 0.01);

	// Get the stencil
	auto stencil = advectionHandler.getStencilForAdvection(gridPosition);

	// Check the value of the stencil
	BOOST_REQUIRE_EQUAL(stencil[0], 1); // x
	BOOST_REQUIRE_EQUAL(stencil[1], 0);
	BOOST_REQUIRE_EQUAL(stencil[2], 0);

	// Finalize MPI
	MPI_Finalize();
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/thread/mutex.hpp>
#include "DistanceThreader.hpp"
#include <iostream>
#include <stdexcept>
#include <boost/thread/thread.hpp>

#include <cfloat>

zhang::DistanceThreader::DistanceThreader (std::vector<double> * ksdl,
                                           std::vector<double> * ksda,
                                           std::vector<double> * ksdb,
                                           std::vector<double> * ksdx,
                                           std::vector<double> * ksdy,
                                           std::vector<double> * kdist,
                                           int * klbl,
                                           double* lvec,
                                           double* avec,
                                           double* bvec,
                                           int offset,
                                           double invwt,
                                           const int width,
                                           const int height,
                                           int p,
                                           int numk)
{
    std::cout << "Creating DistanceThreader... (" << p << " threads)" << std::endl;

    _kseedsl = ksdl;
    _kseedsa = ksda;
    _kseedsb = ksdb;
    _kseedsx = ksdx;
    _kseedsy = ksdy;
    _kdist   = kdist;
    _klbl    = klbl;
    _lvec    = lvec;
    _avec    = avec;
    _bvec    = bvec;

    _offset  = offset;
    _invwt   = invwt;
    _width   = width;
    _height  = height;
    _p       = p;
    _numk    = numk;

    _mutex = new boost::mutex();

}

zhang::DistanceThreader::~DistanceThreader ()
{
    std::cout << "Destructing DistanceThreader" << std::endl;

    delete _mutex;
}

void zhang::DistanceThreader::distvec_reset(){
    (*_kdist).assign(_width*_height, DBL_MAX);
}

void zhang::DistanceThreader::update(std::vector<double> * ksdl,
            std::vector<double> * ksda,
            std::vector<double> * ksdb,
            std::vector<double> * ksdx,
            std::vector<double> * ksdy,
            std::vector<double> * kdist,
                          int * klbl)
{
//     ksdl = _kseedsl;
//     ksda = _kseedsa;
//     ksdb = _kseedsb ;
//     ksdx = _kseedsx;
//     ksdy = _kseedsy;
//     kdist = _kdist;
//     klbl = _klbl;
    _kseedsl = ksdl;
    _kseedsa = ksda;
    _kseedsb = ksdb;
    _kseedsx = ksdx;
    _kseedsy = ksdy;
    _kdist   = kdist;
    _klbl    = klbl;
}

void zhang::DistanceThreader::operator()(int myIdx)
{
    std::cout << "in operator - (" << myIdx << ")" << std::endl;

    for (int n = 0; n < _numk; n++)
    {

        if ( n % _p == myIdx)
        {
//             boost::mutex::scoped_lock lock(*(_mutex));
            int x1, y1, x2, y2;
            double l, a, b;
            double dist;
            double distxy;

            y1 = std::max(0.0,              (*_kseedsy)[n]-_offset);
            y2 = std::min((double)_height,  (*_kseedsy)[n]+_offset);
            x1 = std::max(0.0,              (*_kseedsx)[n]-_offset);
            x2 = std::min((double)_width,   (*_kseedsx)[n]+_offset);

            std::cout << "offset::" << _offset << "  kseedsx::" << (*_kseedsx)[n] << "  kseedsy::" << (*_kseedsy)[n] << "  y1::" << y1 << "  y2::" << y2 << "  x1::" << x1 << "  x2::" << x2 << std::endl;
            for( int y = y1; y < y2; y++ )
            {
                for( int x = x1; x < x2; x++ )
                {
                    int i = y*_width + x;
                    // get the value of l a b in the pixel
                    l = _lvec[i];
                    a = _avec[i];
                    b = _bvec[i];
                    dist =          (l - (*_kseedsl)[n])*(l - (*_kseedsl)[n]) +
                                    (a - (*_kseedsa)[n])*(a - (*_kseedsa)[n]) +
                                    (b - (*_kseedsb)[n])*(b - (*_kseedsb)[n]);
                    distxy =        (x - (*_kseedsx)[n])*(x - (*_kseedsx)[n]) +
                                    (y - (*_kseedsy)[n])*(y - (*_kseedsy)[n]);

                    //--------------------------------------------------------------
                    //dist = sqrt(dist) + sqrt(distxy*invwt);//this is more exact
                    dist += distxy*_invwt;
                    //--------------------------------------------------------------

//                         boost::mutex::scoped_lock lock(*(_mutex));
                    if( dist < (*_kdist).at(i) )
                    {
//                         std::cout << "kdist before::" << (*_kdist).at(i) << " dist::" << dist;
                        (*_kdist).at(i) = dist;
                        _klbl[i]  = n;

//                         std::cout << " kdist after::" << (*_kdist)[i] << std::endl;
                    }

                }
            }
        }

    }
}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/less_equal.hpp>
#include <boost/hana/not.hpp>
namespace hana = boost::hana;

static_assert(hana::less_equal(1, 4), "");
static_assert(hana::less_equal(1, 1), "");
BOOST_HANA_CONSTANT_CHECK(!hana::less_equal(hana::int_c<3>, hana::int_c<2>));

int main() { }


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE multidispatch
#include <boost/test/unit_test.hpp>

#include <type_traits>
#include "map.hpp"

template<typename T> struct add_one;

template<int N>
struct add_one<std::integral_constant<int, N>>
{
	using type = std::integral_constant<int, N+1>;
};

template<typename...> struct list;

BOOST_AUTO_TEST_CASE(map_test)
{
	using in_list =
		list<
			std::integral_constant<int, 1>,
			std::integral_constant<int, 2>,
			std::integral_constant<int, 3>,
			std::integral_constant<int, 4>,
			std::integral_constant<int, 5>
		>;

	using out_list =
		list<
			std::integral_constant<int, 2>,
			std::integral_constant<int, 3>,
			std::integral_constant<int, 4>,
			std::integral_constant<int, 5>,
			std::integral_constant<int, 6>
		>;

	BOOST_CHECK( (std::is_same<meta::map<add_one, in_list>::type, out_list>::value) );
}


// Copyright (C) 2004-2006 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

// Example usage of dijkstra_shortest_paths algorithm

// Enable PBGL interfaces to BGL algorithms
#include <boost/graph/use_mpi.hpp>

// Communication via MPI
#include <boost/graph/distributed/mpi_process_group.hpp>

// Dijkstra's single-source shortest paths algorithm
#include <boost/graph/dijkstra_shortest_paths.hpp>

// Distributed adjacency list
#include <boost/graph/distributed/adjacency_list.hpp>

// METIS Input
#include <boost/graph/metis.hpp>

// Graphviz Output
#include <boost/graph/distributed/graphviz.hpp>

// Standard Library includes
#include <fstream>
#include <string>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

using namespace boost;
using boost::graph::distributed::mpi_process_group;

/* An undirected, weighted graph with distance values stored on the
   vertices. */
typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>, undirectedS,
                       /*Vertex properties=*/property<vertex_distance_t, float>,
                       /*Edge properties=*/property<edge_weight_t, float> >
  Graph;

int main(int argc, char* argv[])
{
  boost::mpi::environment env(argc,argv);

  // Parse command-line options
  const char* filename = "weighted_graph.gr";
  if (argc > 1) filename = argv[1];

  // Open the METIS input file
  std::ifstream in(filename);
  graph::metis_reader reader(in);

  // Load the graph using the default distribution
  Graph g(reader.begin(), reader.end(), reader.weight_begin(),
          reader.num_vertices());

  // Get vertex 0 in the graph
  graph_traits<Graph>::vertex_descriptor start = vertex(0, g);

  // Compute shortest paths from vertex 0
  dijkstra_shortest_paths(g, start,
                          distance_map(get(vertex_distance, g)));

  // Output a Graphviz DOT file
  std::string outfile = filename;
  if (argc > 2)
    outfile = argv[2];
  else {
    size_t i = outfile.rfind('.');
    if (i != std::string::npos)
      outfile.erase(outfile.begin() + i, outfile.end());
    outfile += "-dijkstra.dot";
  }

  if (process_id(process_group(g)) == 0) {
    std::cout << "Writing GraphViz output to " << outfile << "... ";
    std::cout.flush();
  }
  write_graphviz(outfile, g,
                 make_label_writer(get(vertex_distance, g)),
                 make_label_writer(get(edge_weight, g)));
  if (process_id(process_group(g)) == 0)
    std::cout << "Done." << std::endl;

  return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/maxlog10.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check maxlog10 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxlog10;
  using boost::simd::Maxlog10;
  T ref = T(0);
  STF_TYPE_IS(decltype(Maxlog10<T>()), T);
  STF_EQUAL(Maxlog10<T>(), ref);
  STF_EQUAL(maxlog10( as(T{}) ),ref);
}

STF_CASE_TPL( "Check maxlog10 behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxlog10;
  using boost::simd::Maxlog10;
  T ref = T(308.2547155599167);

  STF_TYPE_IS(decltype(Maxlog10<T>()), T);
  STF_IEEE_EQUAL(Maxlog10<T>(), ref);
  STF_IEEE_EQUAL(maxlog10( as(T{}) ), ref);
}

STF_CASE_TPL( "Check maxlog10 behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxlog10;
  using boost::simd::Maxlog10;
  T ref = T(38.23080825805664);
  STF_TYPE_IS(decltype(Maxlog10<T>()), T);
  STF_IEEE_EQUAL(Maxlog10<T>(), ref);
  STF_IEEE_EQUAL(maxlog10( as(T{}) ), ref);
}