#ifndef STAN_MATH_REV_MAT_FUN_QUAD_FORM_HPP
#define STAN_MATH_REV_MAT_FUN_QUAD_FORM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/quad_form.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>

namespace stan {
namespace math {

namespace {
template <typename Ta, int Ra, int Ca, typename Tb, int Rb, int Cb>
class quad_form_vari_alloc : public chainable_alloc {
 private:
  inline void compute(const Eigen::Matrix<double, Ra, Ca>& A,
                      const Eigen::Matrix<double, Rb, Cb>& B) {
    Eigen::Matrix<double, Cb, Cb> Cd(B.transpose() * A * B);
    for (int j = 0; j < C_.cols(); j++) {
      for (int i = 0; i < C_.rows(); i++) {
        if (sym_) {
          C_(i, j) = var(new vari(0.5 * (Cd(i, j) + Cd(j, i)), false));
        } else {
          C_(i, j) = var(new vari(Cd(i, j), false));
        }
      }
    }
  }

 public:
  quad_form_vari_alloc(const Eigen::Matrix<Ta, Ra, Ca>& A,
                       const Eigen::Matrix<Tb, Rb, Cb>& B,
                       bool symmetric = false)
      : A_(A), B_(B), C_(B_.cols(), B_.cols()), sym_(symmetric) {
    compute(value_of(A), value_of(B));
  }

  Eigen::Matrix<Ta, Ra, Ca> A_;
  Eigen::Matrix<Tb, Rb, Cb> B_;
  Eigen::Matrix<var, Cb, Cb> C_;
  bool sym_;
};

template <typename Ta, int Ra, int Ca, typename Tb, int Rb, int Cb>
class quad_form_vari : public vari {
 protected:
  inline void chainA(Eigen::Matrix<double, Ra, Ca>& A,
                     const Eigen::Matrix<double, Rb, Cb>& Bd,
                     const Eigen::Matrix<double, Cb, Cb>& adjC) {}
  inline void chainB(Eigen::Matrix<double, Rb, Cb>& B,
                     const Eigen::Matrix<double, Ra, Ca>& Ad,
                     const Eigen::Matrix<double, Rb, Cb>& Bd,
                     const Eigen::Matrix<double, Cb, Cb>& adjC) {}

  inline void chainA(Eigen::Matrix<var, Ra, Ca>& A,
                     const Eigen::Matrix<double, Rb, Cb>& Bd,
                     const Eigen::Matrix<double, Cb, Cb>& adjC) {
    Eigen::Matrix<double, Ra, Ca> adjA(Bd * adjC * Bd.transpose());
    for (int j = 0; j < A.cols(); j++) {
      for (int i = 0; i < A.rows(); i++) {
        A(i, j).vi_->adj_ += adjA(i, j);
      }
    }
  }
  inline void chainB(Eigen::Matrix<var, Rb, Cb>& B,
                     const Eigen::Matrix<double, Ra, Ca>& Ad,
                     const Eigen::Matrix<double, Rb, Cb>& Bd,
                     const Eigen::Matrix<double, Cb, Cb>& adjC) {
    Eigen::Matrix<double, Ra, Ca> adjB(Ad * Bd * adjC.transpose()
                                       + Ad.transpose() * Bd * adjC);
    for (int j = 0; j < B.cols(); j++)
      for (int i = 0; i < B.rows(); i++)
        B(i, j).vi_->adj_ += adjB(i, j);
  }

  inline void chainAB(Eigen::Matrix<Ta, Ra, Ca>& A,
                      Eigen::Matrix<Tb, Rb, Cb>& B,
                      const Eigen::Matrix<double, Ra, Ca>& Ad,
                      const Eigen::Matrix<double, Rb, Cb>& Bd,
                      const Eigen::Matrix<double, Cb, Cb>& adjC) {
    chainA(A, Bd, adjC);
    chainB(B, Ad, Bd, adjC);
  }

 public:
  quad_form_vari(const Eigen::Matrix<Ta, Ra, Ca>& A,
                 const Eigen::Matrix<Tb, Rb, Cb>& B, bool symmetric = false)
      : vari(0.0) {
    impl_ = new quad_form_vari_alloc<Ta, Ra, Ca, Tb, Rb, Cb>(A, B, symmetric);
  }

  virtual void chain() {
    Eigen::Matrix<double, Cb, Cb> adjC(impl_->C_.rows(), impl_->C_.cols());

    for (int j = 0; j < impl_->C_.cols(); j++)
      for (int i = 0; i < impl_->C_.rows(); i++)
        adjC(i, j) = impl_->C_(i, j).vi_->adj_;

    chainAB(impl_->A_, impl_->B_, value_of(impl_->A_), value_of(impl_->B_),
            adjC);
  }

  quad_form_vari_alloc<Ta, Ra, Ca, Tb, Rb, Cb>* impl_;
};
}  // namespace

template <typename Ta, int Ra, int Ca, typename Tb, int Rb, int Cb>
inline typename boost::enable_if_c<boost::is_same<Ta, var>::value
                                       || boost::is_same<Tb, var>::value,
                                   Eigen::Matrix<var, Cb, Cb> >::type
quad_form(const Eigen::Matrix<Ta, Ra, Ca>& A,
          const Eigen::Matrix<Tb, Rb, Cb>& B) {
  check_square("quad_form", "A", A);
  check_multiplicable("quad_form", "A", A, "B", B);

  quad_form_vari<Ta, Ra, Ca, Tb, Rb, Cb>* baseVari
      = new quad_form_vari<Ta, Ra, Ca, Tb, Rb, Cb>(A, B);

  return baseVari->impl_->C_;
}

template <typename Ta, int Ra, int Ca, typename Tb, int Rb>
inline typename boost::enable_if_c<
    boost::is_same<Ta, var>::value || boost::is_same<Tb, var>::value, var>::type
quad_form(const Eigen::Matrix<Ta, Ra, Ca>& A,
          const Eigen::Matrix<Tb, Rb, 1>& B) {
  check_square("quad_form", "A", A);
  check_multiplicable("quad_form", "A", A, "B", B);

  quad_form_vari<Ta, Ra, Ca, Tb, Rb, 1>* baseVari
      = new quad_form_vari<Ta, Ra, Ca, Tb, Rb, 1>(A, B);

  return baseVari->impl_->C_(0, 0);
}

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


#pragma once

#include <supereight/shared/commons.h>
#include <supereight/voxel_traits.hpp>

#include <Eigen/Dense>

#ifdef SE_CUDA_VF
#include <cuda_runtime.h>
#endif

namespace se {

struct OFusion {};

struct bfusion_update {
    const float* depth;

    Eigen::Vector2i depthSize;

    float noiseFactor;
    float timestamp;
    float voxelsize;

    SE_DEVICE_FUNC
    bfusion_update(const float* d, const Eigen::Vector2i& framesize, float n,
        float t, float vs);

    template<typename DataHandlerT>
    SE_DEVICE_ONLY_FUNC void operator()(DataHandlerT& handler,
        const Eigen::Vector3i&, const Eigen::Vector3f& pos,
        const Eigen::Vector2f& pixel);
};

template<>
struct voxel_traits<OFusion> {
#ifdef SE_CUDA_VF
    using value_type = float2;
#else
    using value_type = struct {
        float x;
        float y;
    };
#endif

    using update_func_type = bfusion_update;

    static constexpr bool invert_normals = false;

    SE_DEVICE_FUNC
    static inline value_type empty() { return {0.f, 0.f}; }

    SE_DEVICE_FUNC
    static inline value_type initValue() { return {0.f, 0.f}; }

    template<typename OctreeT, typename HashType, typename IncF>
    SE_DEVICE_FUNC static void buildAllocationList(HashType* allocation_list,
        int reserved, IncF get_idx, const OctreeT& octree,
        const Eigen::Vector3f& world_vertex, const Eigen::Vector3f& direction,
        const Eigen::Vector3f& camera_pos, float depth_sample,
        float noise_factor);

    template<typename OctreeT>
    SE_DEVICE_FUNC static Eigen::Vector4f raycast(const OctreeT& octree,
        const Eigen::Vector3f& origin, const Eigen::Vector3f& direction,
        const float tnear, const float tfar, const float, const float step,
        const float);
};

} // namespace se


/*  $Id: adaptivepathdropcutter.cpp 758 2011-06-07 14:00:45Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "millingcutter.h"
#include "clpoint.h"
#include "pointdropcutter.h"
#include "adaptivepathdropcutter.h"

namespace ocl
{

//********   ********************** */

AdaptivePathDropCutter::AdaptivePathDropCutter() {
    cutter = NULL;
    surf = NULL;
    path = NULL;
    minimumZ = 0.0;
    subOp.clear();
    subOp.push_back( new PointDropCutter() ); // we delegate to PointDropCutter, who does the heavy lifting
    sampling = 0.1;
    min_sampling = 0.01;
    cosLimit = 0.999;
}

AdaptivePathDropCutter::~AdaptivePathDropCutter() {
    std::cout << " ~AdaptivePathDropCutter() \n";
    delete subOp[0];
}

void AdaptivePathDropCutter::run() {
    adaptive_sampling_run();
}

void AdaptivePathDropCutter::adaptive_sampling_run() {
    std::cout << " apdc::adaptive_sampling_run()... ";

    clpoints.clear();
    BOOST_FOREACH( const Span* span, path->span_list ) {  // this loop could run in parallel, since spans don't depend on eachother
        CLPoint start = span->getPoint(0.0);
        CLPoint stop = span->getPoint(1.0);
        subOp[0]->run(start);
        subOp[0]->run(stop);
        clpoints.push_back(start);
        adaptive_sample( span, 0.0, 1.0, start, stop);
    }
    std::cout << " DONE clpoints.size()=" << clpoints.size() << "\n";
}

void AdaptivePathDropCutter::adaptive_sample(const Span* span, double start_t, double stop_t, CLPoint start_cl, CLPoint stop_cl) {
    const double mid_t = start_t + (stop_t-start_t)/2.0; // mid point sample
    assert( mid_t > start_t );  assert( mid_t < stop_t );
    CLPoint mid_cl = span->getPoint(mid_t);
    //std::cout << " apdc sampling at " << mid_t << "\n";
    subOp[0]->run( mid_cl );
    double fw_step = (stop_cl-start_cl).xyNorm();
    if ( (fw_step > sampling) || // above minimum step-forward, need to sample more
          ( (!flat(start_cl,mid_cl,stop_cl)) && (fw_step > min_sampling) ) ) { // OR not flat, and not max sampling
        adaptive_sample( span, start_t, mid_t , start_cl, mid_cl  );
        adaptive_sample( span, mid_t  , stop_t, mid_cl  , stop_cl );
    } else {
        clpoints.push_back(stop_cl);
    }
}

bool AdaptivePathDropCutter::flat(CLPoint& start_cl, CLPoint& mid_cl, CLPoint& stop_cl)  {
    CLPoint v1 = mid_cl-start_cl;
    CLPoint v2 = stop_cl-mid_cl;
    v1.normalize();
    v2.normalize();
    return (v1.dot(v2) > cosLimit);
}

} // end namespace


//---------------------------------------------------------------------------------------------------------------------
//  AR MICO plugin
//---------------------------------------------------------------------------------------------------------------------
//  Copyright 2020 Pablo Ramon Soria (a.k.a. Bardo91) pabramsor@gmail.com
//---------------------------------------------------------------------------------------------------------------------
//  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 <mico/ar/flow/BlockMultiplyTransform.h>
#include <flow/Outpipe.h>
#include <flow/Policy.h>

#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>

#include <opencv2/aruco.hpp>

namespace mico{
    namespace ar {
        BlockMultiplyTransform::BlockMultiplyTransform(){
            createPipe<Eigen::Matrix4f>("T1*T2");

            createPolicy({  flow::makeInput<Eigen::Matrix4f>("T1"),
                            flow::makeInput<Eigen::Matrix4f>("T2") });

            registerCallback({"T1", "T2"},
                [&](flow::DataFlow _data){

                    if (!idle_) return;

                    idle_ = false;
                    if (getPipe("T1*T2")->registrations()) {
                        auto T1 = _data.get<Eigen::Matrix4f>("T1");
                        auto T2 = _data.get<Eigen::Matrix4f>("T2");
                        Eigen::Matrix4f mul = T1 * T2;
                        getPipe("T1*T2")->flush(mul);
                    }
                    idle_ = true;

                }
            );
        }
    }
}


#include "hsm/hsm.h"

#include <gtest/gtest.h>

#include <boost/hana.hpp>

namespace {

namespace bh {
using namespace boost::hana;
}

// States
struct S1 {
};
struct S2 {
};
struct S3 {
};
struct S4 {
};
struct S5 {
};
struct S6 {
};
struct S7 {
};

// Events
struct e1 {
};
struct e2 {
};
struct e3 {
};
struct e4 {
};
struct e5 {
};
struct e6 {
};

// Guards
const auto trueGuard = [](auto /*event*/) { return true; };
const auto falseGuard = [](auto /*event*/) { return false; };

// Actions
const auto a1 = [](auto /*event*/) {};

using namespace ::testing;

struct SubState {
    constexpr auto make_transition_table()
    {
        // clang-format off
        return hsm::transition_table(
            // Region 0
            hsm::transition(S1 {}, hsm::event<e1> {}, trueGuard, a1, S2 {})
        );
        // clang-format on
    }

    constexpr auto initial_state()
    {
        return hsm::initial(S1 {});
    }
};

struct MainState {
    constexpr auto make_transition_table()
    {
        // clang-format off
        return hsm::transition_table(
            // Region 0
            hsm::transition(S1 {}, hsm::event<e1> {}, trueGuard , a1, S2 {}),
            hsm::transition(S1 {}, hsm::event<e2> {}, trueGuard , a1, S4 {}),
            // Guard tests
            hsm::transition(S1 {}, hsm::event<e3> {}, falseGuard, a1, S2 {}),
            hsm::transition(S1 {}, hsm::event<e4> {}, trueGuard , a1, S2 {}),
            hsm::transition(S1 {}, hsm::event<e5> {}, falseGuard, a1, S2 {}),
            hsm::transition(S1 {}, hsm::event<e6> {}, trueGuard , a1, S6 {}),
            hsm::transition(S6 {}, hsm::none {}     , falseGuard, a1, S1 {}),

            hsm::transition(S2 {}, hsm::event<e1> {}, trueGuard , a1, SubState {}),
            hsm::transition(S4 {}, hsm::none {}     , trueGuard , a1, S1 {}),
            // Region 1
            hsm::transition(S3 {}, hsm::event<e1> {}, trueGuard , a1, S4 {}),
            hsm::transition(S3 {}, hsm::event<e2> {}, trueGuard , a1, S5 {}),
            // Guard tests
            hsm::transition(S3 {}, hsm::event<e3> {}, falseGuard, a1, S2 {}),
            hsm::transition(S3 {}, hsm::event<e4> {}, falseGuard, a1, S2 {}),
            hsm::transition(S3 {}, hsm::event<e5> {}, trueGuard , a1, S2 {}),
            hsm::transition(S3 {}, hsm::event<e6> {}, trueGuard , a1, S7 {}),
            hsm::transition(S7 {}, hsm::none {}     , trueGuard , a1, S1 {}),

            hsm::transition(S5 {}, hsm::none {}     , trueGuard , a1, S1 {})
        );
        // clang-format on
    }

    constexpr auto initial_state()
    {
        return hsm::initial(S1 {}, S3 {});
    }
};

}

class OrthogonalRegionsTests : public Test {
    protected:
        hsm::Sm<MainState> sm;
};

TEST_F(OrthogonalRegionsTests, should_start_in_region_initial_states)
{
    ASSERT_TRUE(sm.is(0, MainState{}, S1 {}));
    ASSERT_TRUE(sm.is(1, MainState{}, S3 {}));
}

TEST_F(OrthogonalRegionsTests, should_transit_in_all_regions)
{
    sm.process_event(e1{});
    ASSERT_TRUE(sm.is(0, MainState{}, S2 {}));
    ASSERT_TRUE(sm.is(1, MainState{}, S4 {}));
}

TEST_F(OrthogonalRegionsTests, should_anonymous_transit_in_all_regions)
{
    sm.process_event(e2{});
    ASSERT_TRUE(sm.is(0, MainState{}, S1 {}));
    ASSERT_TRUE(sm.is(1, MainState{}, S1 {}));
}

TEST_F(OrthogonalRegionsTests, should_guard_in_all_regions)
{
    sm.process_event(e3 {});
    ASSERT_TRUE(sm.is(0, MainState {}, S1 {}));
    ASSERT_TRUE(sm.is(1, MainState {}, S3 {}));
}

TEST_F(OrthogonalRegionsTests, should_guard_in_a_single_regions)
{
    sm.process_event(e4 {});
    ASSERT_TRUE(sm.is(0, MainState {}, S2 {}));
    ASSERT_TRUE(sm.is(1, MainState {}, S3 {}));
}

TEST_F(OrthogonalRegionsTests, should_guard_in_a_single_regions_inverse)
{
    sm.process_event(e5 {});
    ASSERT_TRUE(sm.is(0, MainState {}, S1 {}));
    ASSERT_TRUE(sm.is(1, MainState {}, S2 {}));
}

TEST_F(OrthogonalRegionsTests, should_anonymous_guard_in_a_single_regions)
{
    sm.process_event(e6 {});
    ASSERT_TRUE(sm.is(0, MainState {}, S6 {}));
    ASSERT_TRUE(sm.is(1, MainState {}, S1 {}));
}


#include <igl/AABB.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

#include <cstring>
#include <iostream>
void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & Ele,
  Eigen::MatrixXd & bb_mins,
  Eigen::MatrixXd & bb_maxs,
  Eigen::VectorXi & elements)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3 || dim == 2,
    "Mesh vertex list must be #V by 2 or 3 list of vertex positions");

  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,Ele);
  if(nrhs > 2)
  {
    mexErrMsgTxt(nrhs == 2, "The number of input arguments must be 2.");
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd V,bb_mins,bb_maxs;
  MatrixXi Ele;
  VectorXi elements;
  VectorXi I;
  parse_rhs(nrhs,prhs,V,Ele,bb_mins,bb_maxs,elements);
  bool was_serialized = bb_mins.size()>0;

  switch(V.cols())
  {
    default:
      mexErrMsgTxt(false,"Un-supported dimension.");
      break;
    case 3:
    {
      AABB<MatrixXd,3> aabb;
      aabb.init(V,Ele,bb_mins,bb_maxs,elements);
      aabb.serialize(bb_mins,bb_maxs,elements);
      break;
    }
    case 2:
    {
      AABB<MatrixXd,2> aabb;
      aabb.init(V,Ele,bb_mins,bb_maxs,elements);
      aabb.serialize(bb_mins,bb_maxs,elements);
      break;
    }
  }

  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 3:
    {
      prepare_lhs_index(elements,plhs+2);
      //fallthrough
    }
    case 2:
    {
      prepare_lhs_double(bb_maxs,plhs+1);
      //fallthrough
    }
    case 1:
    {
      prepare_lhs_double(bb_mins,plhs+0);
      //fallthrough
    }
    case 0: break;
  }

  std::cout.rdbuf(outbuf);
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick
// Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "modules/commons/params/default_params.hpp"
#include "modules/commons/params/setter_params.hpp"
#include "modules/geometry/commons.hpp"
#include "modules/geometry/line.hpp"
#include "modules/geometry/polygon.hpp"
#include "modules/models/behavior/motion_primitives/continuous_actions.hpp"
#include "modules/models/behavior/motion_primitives/macro_actions.hpp"
#include "modules/models/behavior/motion_primitives/primitives.hpp"
#include "modules/models/dynamic/single_track.hpp"
#include "modules/world/observed_world.hpp"

using namespace modules::models::dynamic;
using namespace modules::models::execution;
using namespace modules::commons;
using namespace modules::models::behavior;
using namespace modules::models::dynamic;
using namespace modules::world;
using namespace modules::geometry;

class DummyObservedWorld : public ObservedWorld {
 public:
  DummyObservedWorld(const State& init_state, const ParamsPtr& params)
      : ObservedWorld(std::make_shared<World>(params), AgentId()),
        init_state_(init_state) {}

  virtual State CurrentEgoState() const { return init_state_; }

  virtual double GetWorldTime() const { return 0.0f; }

 private:
  State init_state_;
};

TEST(behavior_motion_primitives_add, behavior_test) {
  auto params = std::make_shared<DefaultParams>();
  DynamicModelPtr dynamics(new SingleTrackModel(params));
  BehaviorMPContinuousActions behavior(dynamics, params);
  Input u(2);
  u << 0, 0;
  behavior.AddMotionPrimitive(u);
}

TEST(behavior_motion_primitives_plan, behavior_test) {
  auto params = std::make_shared<SetterParams>();
  params->SetReal("integration_time_delta", 0.01);
  DynamicModelPtr dynamics(new SingleTrackModel(params));

  BehaviorMPContinuousActions behavior(dynamics, params);
  Input u1(2);
  u1 << 2, 0;
  BehaviorMPContinuousActions::MotionIdx idx1 = behavior.AddMotionPrimitive(u1);
  Input u2(2);
  u2 << 0, 1;
  BehaviorMPContinuousActions::MotionIdx idx2 = behavior.AddMotionPrimitive(u2);
  Input u3(2);
  u3 << 0, 0;
  BehaviorMPContinuousActions::MotionIdx idx3 = behavior.AddMotionPrimitive(u3);

  // X Longitudinal with zero velocity
  State init_state(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state << 0.0, 0.0, 0.0, 0.0, 0.0;
  DummyObservedWorld world(init_state, params);

  behavior.ActionToBehavior(idx1);
  Trajectory traj1 = behavior.Plan(0.5, world);
  EXPECT_NEAR(traj1(traj1.rows() - 1, StateDefinition::X_POSITION),
              2 / 2 * 0.5 * 0.5, 0.05);

  // X Longitudinal with nonzero velocity
  init_state << 0.0, 0.0, 0.0, 0.0, 5.0;
  DummyObservedWorld world1(init_state, params);
  behavior.ActionToBehavior(idx1);
  traj1 = behavior.Plan(0.5, world1);
  EXPECT_NEAR(traj1(traj1.rows() - 1, StateDefinition::X_POSITION),
              5.0 * 0.5 + 2 / 2 * 0.5 * 0.5, 0.1);

  // Y Longitudinal
  init_state << 0.0, 0.0, 0.0, B_PI_2, 0.0;
  DummyObservedWorld world2(init_state, params);
  behavior.ActionToBehavior(idx1);
  traj1 = behavior.Plan(0.5, world2);
  EXPECT_NEAR(traj1(traj1.rows() - 1, StateDefinition::Y_POSITION),
              2 / 2 * 0.5 * 0.5, 0.05);

  // X Constant motion
  init_state << 0.0, 0.0, 0.0, 0.0, 2.0;
  DummyObservedWorld world3(init_state, params);
  behavior.ActionToBehavior(idx3);
  Trajectory traj3 = behavior.Plan(0.5, world3);
  EXPECT_NEAR(traj3(traj3.rows() - 1, StateDefinition::X_POSITION), 0.5 * 2,
              0.005);
}

TEST(primitive_constant_acceleration, behavior_test) {
  using modules::models::behavior::primitives::PrimitiveConstAccStayLane;
  auto params = std::make_shared<DefaultParams>();
  DynamicModelPtr dynamics(new SingleTrackModel(params));
  PrimitiveConstAccStayLane primitive(params, dynamics, 0, 0.1);

  State init_state(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state << 0.0, 0.0, 0.0, 0.0, 5.0;
  auto world1 = std::make_shared<DummyObservedWorld>(init_state, params);
  EXPECT_TRUE(primitive.IsPreConditionSatisfied(world1));
  // auto traj = primitive.Plan(0.5, world1);
}

TEST(primitive_change_left, behavior_test) {
  using modules::models::behavior::primitives::PrimitiveConstAccChangeToLeft;
  auto params = std::make_shared<DefaultParams>();
  DynamicModelPtr dynamics(new SingleTrackModel(params));
  PrimitiveConstAccChangeToLeft primitive(params, dynamics, 0.1);

  State init_state(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state << 0.0, 0.0, 0.0, 0.0, 5.0;
  auto world1 = std::make_shared<DummyObservedWorld>(init_state, params);
  // EXPECT_FALSE(primitive.IsPreConditionSatisfied(world1));
  // auto traj = primitive.Plan(0.5, world1);
}

TEST(primitive_gap_keeping, behavior_test) {
  using modules::models::behavior::primitives::PrimitiveGapKeeping;
  auto params = std::make_shared<DefaultParams>();
  DynamicModelPtr dynamics(new SingleTrackModel(params));
  PrimitiveGapKeeping primitive(params, dynamics);

  State init_state(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state << 0.0, 0.0, 0.0, 0.0, 5.0;
  auto world1 = std::make_shared<DummyObservedWorld>(init_state, params);
  // EXPECT_FALSE(primitive.IsPreConditionSatisfied(world1));
  // auto traj = primitive.Plan(0.5, world1);
}

TEST(macro_actions, behavior_test) {
  using namespace modules::models::behavior::primitives;
  using modules::models::behavior::primitives::PrimitiveConstAccStayLane;

  auto params = std::make_shared<DefaultParams>();
  params->SetReal("integration_time_delta", 0.01);
  DynamicModelPtr dynamics(new SingleTrackModel(params));

  std::vector<std::shared_ptr<Primitive>> prim_vec;

  auto primitive =
      std::make_shared<PrimitiveConstAccStayLane>(params, dynamics, 0, 0.1);
  prim_vec.push_back(primitive);

  auto primitive_left =
      std::make_shared<PrimitiveConstAccChangeToLeft>(params, dynamics, 0.1);
  prim_vec.push_back(primitive_left);

  auto primitive_right =
      std::make_shared<PrimitiveConstAccChangeToRight>(params, dynamics, 0.1);
  prim_vec.push_back(primitive_right);

  BehaviorMPMacroActions behavior(dynamics, params);
  for (auto& p : prim_vec) {
    auto idx = behavior.AddMotionPrimitive(primitive);
  }
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/*
  Tencent is pleased to support the open source community by making
  Plato available.
  Copyright (C) 2019 THL A29 Limited, a Tencent company.
  All rights reserved.

  Licensed under the BSD 3-Clause License (the "License"); you may
  not use this file except in compliance with the License. You may
  obtain a copy of the License at

  https://opensource.org/licenses/BSD-3-Clause

  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.

  See the AUTHORS file for names of contributors.
*/

#ifndef __PLATO_ALGO_BNC_BADER_HPP__
#define __PLATO_ALGO_BNC_BADER_HPP__

#include <cstdint>
#include <cstdlib>
#include <unordered_map>
#include <algorithm>
#include <utility>
#include <vector>
#include <unordered_set>
#include <boost/format.hpp>
#include <boost/lockfree/queue.hpp>

#include "glog/logging.h"

#include "plato/util/perf.hpp"
#include "plato/util/atomic.hpp"
#include "plato/graph/graph.hpp"
#include "plato/engine/dualmode.hpp"

#include "cgm.hpp"

namespace plato { namespace algo {
/**
 * @brief bader option
 */
struct bader_opts_t {
  vid_t max_iteration_ = 0;
  int chosen_ = -1;
  float constant_ = 2;
};

/**
 * @brief
 * @tparam INCOMING
 * @tparam OUTGOING
 * @tparam T
 */
template <typename INCOMING, typename OUTGOING, typename T>
class bader_betweenness_t {
public:
  using partition_t = typename dualmode_detail::partition_traits<INCOMING, OUTGOING>::type;
  using betweenness_state_t  = dense_state_t<T, partition_t>;
  using label_state_t = dense_state_t<vid_t, partition_t>;
  using active_subset_t = bitmap_t<>;
  struct bader_msg_type_t {
    vid_t src_;
    T value_;
  };

  /**
   * @brief
   * @param engine
   * @param graph_info
   * @param opts
   */
  explicit bader_betweenness_t (
    dualmode_engine_t<INCOMING, OUTGOING> * engine,
    const graph_info_t &graph_info, const bader_opts_t& opts = bader_opts_t());
  /// \brief
  ~bader_betweenness_t();

  // return the betweenness centrality of a given vertex
  T get_betweenness_of(vid_t v_i) const;

  // compute the betweenness of all vertices
  void compute();
  /**
   * @brief
   * @tparam Callback
   * @param streams
   */
  template <typename Callback>
  void save(Callback&& callback);

  /**
   * @brief
   * @return
   */
  vid_t get_chosen() const { return chosen_; }

  /**
   * @brief
   * @return
   */
  std::unordered_set<vid_t>* get_samples() { return &samples_; }

  /**
   * @brief
   * @return
   */
  vid_t get_major_componnent_vertices() const { return major_component_vertices_; }

private:
  /**
   * @brief
   * @param root
   * @param post_proc
   */
  void epoch(vid_t root, std::function<void(const betweenness_state_t*, vid_t)> post_proc);

private:
  dualmode_engine_t<INCOMING, OUTGOING> * engine_;
  graph_info_t graph_info_;
  betweenness_state_t betweenness_;
  betweenness_state_t num_paths_;
  betweenness_state_t dependencies_;
  T sum_dependence_;
  T sum_dependence_max_;
  vid_t chosen_;
  float constant_;
  vid_t max_iteration_;
  std::unordered_set<vid_t> samples_;
  connected_component_t<INCOMING, OUTGOING> *cc_;
  label_state_t global_labels_;
  vid_t major_component_label_;
  vid_t major_component_vertices_;
  active_subset_t active_all_;
};

template <typename INCOMING, typename OUTGOING, typename T>
bader_betweenness_t<INCOMING, OUTGOING, T>::bader_betweenness_t(
  dualmode_engine_t<INCOMING, OUTGOING>* engine, const graph_info_t &graph_info,
  const bader_opts_t& opts)
  : engine_(engine),
    graph_info_(graph_info),
    betweenness_(graph_info.max_v_i_, engine->in_edges()->partitioner()),
    num_paths_(graph_info.max_v_i_, engine->in_edges()->partitioner()),
    dependencies_(graph_info.max_v_i_, engine->in_edges()->partitioner()),
    sum_dependence_(0),
    global_labels_(graph_info.max_v_i_, engine->in_edges()->partitioner()),
    active_all_(graph_info.max_v_i_ + 1) {
  cc_ = new connected_component_t<INCOMING, OUTGOING>(engine, graph_info);
  cc_->compute();
  major_component_label_ = cc_->get_major_label();
  major_component_vertices_ = cc_->get_major_vertices();
  constant_ = opts.constant_;
  sum_dependence_max_ = constant_ * major_component_vertices_;
  if (opts.max_iteration_ == 0) {
    max_iteration_ = major_component_vertices_;
  } else {
    max_iteration_ = opts.max_iteration_;
  }

  active_all_.fill();

  //init label
  auto local_label = cc_->get_labels();

  auto active_view_all = plato::create_active_v_view(engine_->out_edges()->partitioner()->self_v_view(), active_all_);
  active_view_all.template foreach<vid_t>([&](vid_t v_i){
    global_labels_[v_i] = (*local_label)[v_i];
    return 0;
  });

  auto& cluster_info = plato::cluster_info_t::get_instance();
  if (cluster_info.partition_id_ != 0) {
    vid_t v_begin = engine_->out_edges()->partitioner()->offset_[cluster_info.partition_id_];
    vid_t v_end = engine_->out_edges()->partitioner()->offset_[cluster_info.partition_id_+1];
    MPI_Send(&(global_labels_[v_begin]), v_end - v_begin, get_mpi_data_type<vid_t>(), 0, 0, MPI_COMM_WORLD);
  }
  else {
    for (int i = 1; i < cluster_info.partitions_; ++i ) {
      MPI_Status recv_status;
      vid_t v_begin = engine_->out_edges()->partitioner()->offset_[i];
      vid_t v_end = engine_->out_edges()->partitioner()->offset_[i+1];
      MPI_Recv(&(global_labels_[v_begin]), v_end - v_begin, get_mpi_data_type<vid_t>(), i, 0, MPI_COMM_WORLD, &recv_status);
    }
  }

  //get chosen vertex
  if (opts.chosen_ == -1) {
    if (cluster_info.partition_id_ == 0) {
      do {
        chosen_ = rand() % graph_info.vertices_;
      } while (global_labels_[chosen_] != major_component_label_);
    }
    MPI_Bcast(&chosen_, 1, get_mpi_data_type<vid_t>(), 0, MPI_COMM_WORLD);
    LOG(INFO) << "chosen: "  << chosen_ << std::endl;
  }
  else {
    chosen_ = opts.chosen_;
  }

}

template <typename INCOMING, typename OUTGOING, typename T>
bader_betweenness_t<INCOMING, OUTGOING, T>::~bader_betweenness_t() {
  delete cc_;
}

template <typename INCOMING, typename OUTGOING, typename T>
void bader_betweenness_t<INCOMING, OUTGOING, T>::compute() {
  auto continue_iterating = [&](vid_t iter) {
    return iter < max_iteration_ && sum_dependence_ < sum_dependence_max_;
  };

  // pick a new random vertex that is not used before
  // we use a set to record the vertices that are used.
  auto& cluster_info = plato::cluster_info_t::get_instance();
  auto gen_next_vertex = [&]() {
    vid_t nxt;
    if (cluster_info.partition_id_ == 0) {
      size_t curr_size = samples_.size();
      do {
        nxt = std::rand() % graph_info_.vertices_;
        if (global_labels_[nxt] == major_component_label_) {
          samples_.insert(nxt);
        }
      } while (curr_size == samples_.size());
    }
    MPI_Bcast(&nxt, 1, get_mpi_data_type<vid_t>(), 0, MPI_COMM_WORLD);
    if (cluster_info.partition_id_ != 0) samples_.insert(nxt);
    return nxt;
  };
  auto active_view_all = plato::create_active_v_view(engine_->out_edges()->partitioner()->self_v_view(), active_all_);
  auto accumulate = [&](const betweenness_state_t* dependencies, vid_t root) {
    active_view_all.template foreach<vid_t>([&](vid_t v_i){
      if (v_i != root) {
        T d = (*dependencies)[v_i];
        betweenness_[v_i] += d;
        if (v_i == chosen_) {
          sum_dependence_ += d;
        }
      }
      return 1;
    });

    int chosen_partition = engine_->out_edges()->partitioner()->get_partition_id(chosen_);
    MPI_Bcast(&sum_dependence_, 1, get_mpi_data_type<T>(), chosen_partition,MPI_COMM_WORLD);
    LOG(INFO) << "sum_deppendece: " << sum_dependence_ << " sum_dependence_max: " << sum_dependence_max_ << std::endl;
  };

  auto update_betweenness = [&](vid_t sample_size) {
    active_view_all.template foreach<vid_t>([&](vid_t v_i) {
      betweenness_[v_i] = major_component_vertices_ * betweenness_[v_i] / sample_size;
      if (std::isnan(betweenness_[v_i])) {
        betweenness_[v_i] = 0;
      }
      return 1;
    });
  };

  for (vid_t iter = 0; continue_iterating(iter); ++iter) {
    auto root = gen_next_vertex();
    LOG(INFO) << "next root: " << root << std::endl;
    epoch(root, accumulate);
    LOG_IF(INFO, !cluster_info.partition_id_ && (iter + 1) % 10 == 0)
    << boost::format("iter=%u/%u, sum_dependence[%u]=%.1f/%.1f\n") % (iter + 1) % max_iteration_ % chosen_ % sum_dependence_ % sum_dependence_max_;
  }

  vid_t sample_size = samples_.size();
  MPI_Bcast(&sample_size, 1, get_mpi_data_type<vid_t>(), 0, MPI_COMM_WORLD);

  update_betweenness(sample_size);
}

template <typename INCOMING, typename OUTGOING, typename T>
T bader_betweenness_t<INCOMING, OUTGOING, T>::get_betweenness_of(vid_t v_i) const {
  auto& cluster_info = plato::cluster_info_t::get_instance();
  int vtx_partition = engine_->out_edges()->partitioner()->get_partition_id(v_i);
  T ret = cluster_info.partition_id_ == vtx_partition ? betweenness_[v_i] : 0;
  MPI_Bcast(&ret, 1, get_mpi_data_type<T>(), vtx_partition, MPI_COMM_WORLD);
  return ret;
}

template <typename INCOMING, typename OUTGOING, typename T>
void bader_betweenness_t<INCOMING, OUTGOING, T>::epoch(
  vid_t root, std::function<void(const betweenness_state_t*, vid_t)> post_proc) {
  std::vector<active_subset_t*> levels;
  auto active_current = new active_subset_t(graph_info_.max_v_i_ + 1);
  auto visited = engine_->alloc_v_subset();
  vid_t actives = 1;
  visited.clear();
  visited.set_bit(root);
  active_current->clear();
  active_current->set_bit(root);
  levels.push_back(active_current);
  num_paths_.fill((T)0.0);
  num_paths_[root] = 1.0;

  using adj_unit_list_spec_t = typename INCOMING::adj_unit_list_spec_t;
  using pull_context_t = plato::template mepa_ag_context_t<bader_msg_type_t>;
  using pull_message_t = plato::template mepa_ag_message_t<bader_msg_type_t>;
  using push_context_t = plato::template mepa_bc_context_t<bader_msg_type_t>;
  for (int epoch_i = 0; actives > 0; ++epoch_i) {
    auto active_next = new active_subset_t(graph_info_.max_v_i_ + 1);
    active_next->clear();
    engine_->template foreach_edges<bader_msg_type_t, vid_t> (
      [&](const push_context_t& context, vid_t v_i) {
        context.send(bader_msg_type_t{ v_i, num_paths_[v_i] });
      },
      [&](int /*p_i*/, bader_msg_type_t& msg) {
        auto neighbours = engine_->out_edges()->neighbours(msg.src_);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          vid_t dst = it->neighbour_;
          if (!visited.get_bit(dst)) {
            if (num_paths_[dst] == 0) {
              active_next->set_bit(dst);
            }
            write_add(&(num_paths_[dst]), msg.value_);
          }
        }
        return 0;
      },
      [&](const pull_context_t& context, vid_t v_i, const adj_unit_list_spec_t& adjs) {
        if (visited.get_bit(v_i)) return;
        T sum = 0;
        for (auto it = adjs.begin_; adjs.end_ != it; ++it) {
          vid_t src = it->neighbour_;
          if (active_current->get_bit(src)) {
            sum += num_paths_[src];
          }
        }
        if (sum > 0) {
          context.send(pull_message_t{ v_i, bader_msg_type_t{ v_i, sum } });
        }
      },
      [&](int, pull_message_t& msg) {
        if (!visited.get_bit(msg.v_i_)) {
          active_next->set_bit(msg.v_i_);
          write_add(&(num_paths_[msg.v_i_]), msg.message_.value_);
        }
        return 0;
      },
      *active_current
    );
    auto active_view = plato::create_active_v_view(engine_->out_edges()->partitioner()->self_v_view(), *active_next);
    actives = active_view.template foreach<vid_t> ([&](vid_t v_i) {
      visited.set_bit(v_i); return 1;
    });
    if (actives > 0) levels.push_back(active_next);
    active_current = active_next;
  }

  dependencies_.fill((T)0.0);
  visited.clear();
  while (levels.size() > 0) {
    auto active_view = plato::create_active_v_view(engine_->out_edges()->partitioner()->self_v_view(), *(levels.back()));
    actives = active_view.template foreach<vid_t> ([&](vid_t v_i) {
      visited.set_bit(v_i); return 1;
    });
    engine_->template foreach_edges<bader_msg_type_t, vid_t> (
      [&](const push_context_t& context, vid_t v_i) {
        T value = (dependencies_[v_i] + 1.0) / num_paths_[v_i];
        context.send(bader_msg_type_t{ v_i, value });
      },
      [&](int /*p_i*/, bader_msg_type_t& msg) {
        auto neighbours = engine_->out_edges()->neighbours(msg.src_);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          vid_t dst = it->neighbour_;
          if (!visited.get_bit(dst)) {
            write_add(&(dependencies_[dst]), msg.value_ * num_paths_[dst]);
          }
        }
        return 0;
      },
      [&](const pull_context_t& context, vid_t v_i, const adj_unit_list_spec_t& adjs) {
        if (visited.get_bit(v_i)) return;
        T sum = 0;
        for (auto it = adjs.begin_; adjs.end_ != it; ++it) {
          vid_t src = it->neighbour_;
          if (levels.back()->get_bit(src)) {
            sum += (dependencies_[src] + 1.0) / num_paths_[src];
          }
        }
        if (sum > 0) {
          context.send(pull_message_t{ v_i, bader_msg_type_t{ v_i, sum } });
        }
      },
      [&](int, pull_message_t& msg) {
        if (!visited.get_bit(msg.v_i_)) {
          write_add(&(dependencies_[msg.v_i_]), msg.message_.value_ * num_paths_[msg.v_i_]);
        }
        return 0;
      },
      *(levels.back())
    );
    delete levels.back();
    levels.pop_back();
  }

  post_proc(&dependencies_, root);
}

template <typename INCOMING, typename OUTGOING, typename T>
template <typename Callback>
void bader_betweenness_t<INCOMING, OUTGOING, T>::save(Callback&& callback) {
  // traverse
  auto active_view_all = plato::create_active_v_view(engine_->out_edges()->partitioner()->self_v_view(), active_all_);
  active_view_all.template foreach<vid_t>([&] (vid_t v_i) {
    callback(v_i, betweenness_[v_i]);
    return 1;
  });
}

}
}
#endif


// COL216 Assignment 4
// Mallika Prabhakar 2019CS50440
// Sayam Sethi 2019CS10399

#include <bits/stdc++.h>
#include <boost/tokenizer.hpp>

using namespace std;

// struct to store the registers and the functions to be executed
struct MIPS_Architecture
{
private:
	// struct to store all information required by DRAM queue to execute the instruction
	struct QElem
	{
		bool id;
		int PCaddr, value, issueCycle, startCycle, remainingCycles;

		/**
		 * initialise the queue element
		 * @param id 0 for sw and 1 for lw
		 * @param PCaddr stores the PC address of corresponding instruction
		 * @param value content to be stored (for sw) / register id (for lw)
		 * @param issueCycle the cycle in which the request was issued
		 * @param startCycle the cycle when the instruction began
		 * @param remainingCycles number of cycles pending to finish execution of request
		*/
		QElem(bool id, int PCaddr, int value, int issueCycle, int startCycle = -1, int remainingCycles = -1)
		{
			this->id = id;
			this->PCaddr = PCaddr;
			this->value = value;
			this->issueCycle = issueCycle;
			this->startCycle = startCycle;
			this->remainingCycles = remainingCycles;
		}
	};

public:
	// constants
	static const int MAX = (1 << 20), ROWS = (1 << 10), DRAM_MAX = (1 << 5);
	// DRAM delays
	int row_access_delay, col_access_delay;
	// instruction set
	unordered_map<string, function<int(MIPS_Architecture &, string, string, string)>> instructions;
	// mapping names to a unique number
	unordered_map<string, int> registerMap, address;
	// vector to store the commands in input program
	vector<vector<string>> commands;
	// "dynamic" vars
	int registers[32], PCcurr, PCnext, delay, currRow, currCol, clockCycles, rowBufferUpdates, DRAMsize;
	pair<int, int> registersAddrDRAM[32];
	// data stored in allocated memory
	vector<vector<int>> data;
	vector<int> buffer, commandCount;
	// data structure to store info about requests sent to the DRAM, key is the row number and value is QElem
	unordered_map<int, unordered_map<int, queue<QElem>>> DRAMbuffer;
	// last location accessed by DRAM is stored
	pair<int, int> lastAddr;
	bool isDRAM;

	// constructor to initialise the instruction set
	MIPS_Architecture(ifstream &file, int row_delay, int col_delay)
	{
		row_access_delay = row_delay, col_access_delay = col_delay;

		instructions = {{"add", &MIPS_Architecture::add}, {"sub", &MIPS_Architecture::sub}, {"mul", &MIPS_Architecture::mul}, {"beq", &MIPS_Architecture::beq}, {"bne", &MIPS_Architecture::bne}, {"slt", &MIPS_Architecture::slt}, {"j", &MIPS_Architecture::j}, {"lw", &MIPS_Architecture::lw}, {"sw", &MIPS_Architecture::sw}, {"addi", &MIPS_Architecture::addi}};

		for (int i = 0; i < 32; ++i)
			registerMap["$" + to_string(i)] = i;
		registerMap["$zero"] = 0;
		registerMap["$at"] = 1;
		registerMap["$v0"] = 2;
		registerMap["$v1"] = 3;
		for (int i = 0; i < 4; ++i)
			registerMap["$a" + to_string(i)] = i + 4;
		for (int i = 0; i < 8; ++i)
			registerMap["$t" + to_string(i)] = i + 8, registerMap["$s" + to_string(i)] = i + 16;
		registerMap["$t8"] = 24;
		registerMap["$t9"] = 25;
		registerMap["$k0"] = 26;
		registerMap["$k1"] = 27;
		registerMap["$gp"] = 28;
		registerMap["$sp"] = 29;
		registerMap["$s8"] = 30;
		registerMap["$ra"] = 31;

		constructCommands(file);
	}

	// perform add immediate operation
	int addi(string r1, string r2, string num)
	{
		if (!checkRegisters({r1, r2}) || registerMap[r1] == 0)
			return 1;
		try
		{
			if (registersAddrDRAM[registerMap[r2]] != make_pair(-1, -1))
				return -registerMap[r2] - 1;
			registers[registerMap[r1]] = registers[registerMap[r2]] + stoi(num);
			registersAddrDRAM[registerMap[r1]] = {-1, -1};
			PCnext = PCcurr + 1;
			return 0;
		}
		catch (exception &e)
		{
			return 4;
		}
	}

	// perform add operation
	int add(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a + b; });
	}

	// perform subtraction operation
	int sub(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a - b; });
	}

	// perform multiplication operation
	int mul(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a * b; });
	}

	// implements slt operation
	int slt(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a < b; });
	}

	// perform the operation
	int op(string r1, string r2, string r3, function<int(int, int)> operation)
	{
		if (!checkRegisters({r1, r2, r3}) || registerMap[r1] == 0)
			return 1;
		if (registersAddrDRAM[registerMap[r2]] != make_pair(-1, -1))
			return -registerMap[r2] - 1;
		if (registersAddrDRAM[registerMap[r3]] != make_pair(-1, -1))
			return -registerMap[r3] - 1;
		registers[registerMap[r1]] = operation(registers[registerMap[r2]], registers[registerMap[r3]]);
		registersAddrDRAM[registerMap[r1]] = {-1, -1};
		PCnext = PCcurr + 1;
		return 0;
	}

	// perform the beq operation
	int beq(string r1, string r2, string label)
	{
		return bOP(r1, r2, label, [](int a, int b) { return a == b; });
	}

	// perform the bne operation
	int bne(string r1, string r2, string label)
	{
		return bOP(r1, r2, label, [](int a, int b) { return a != b; });
	}

	// implements beq and bne by taking the comparator
	int bOP(string r1, string r2, string label, function<bool(int, int)> comp)
	{
		if (!checkLabel(label))
			return 4;
		if (address.find(label) == address.end() || address[label] == -1)
			return 2;
		if (!checkRegisters({r1, r2}))
			return 1;
		if (registersAddrDRAM[registerMap[r1]] != make_pair(-1, -1))
			return -registerMap[r1] - 1;
		if (registersAddrDRAM[registerMap[r2]] != make_pair(-1, -1))
			return -registerMap[r2] - 1;
		PCnext = comp(registers[registerMap[r1]], registers[registerMap[r2]]) ? address[label] : PCcurr + 1;
		return 0;
	}

	// perform the jump operation
	int j(string label, string unused1 = "", string unused2 = "")
	{
		if (!checkLabel(label))
			return 4;
		if (address.find(label) == address.end() || address[label] == -1)
			return 2;
		PCnext = address[label];
		return 0;
	}

	// perform load word operation
	int lw(string r, string location, string unused1 = "")
	{
		if (!checkRegister(r) || registerMap[r] == 0)
			return 1;
		auto address = locateAddress(location);
		if (!address.first)
			return address.second;

		PCnext = PCcurr + 1;
		if (address.second == lastAddr.first)
		{
			registers[registerMap[r]] = lastAddr.second;
			registersAddrDRAM[registerMap[r]] = {-1, -1};
			return 0;
		}

		if (DRAMsize == DRAM_MAX)
			return -33;
		isDRAM = true;
		DRAMbuffer[address.second / ROWS][(address.second % ROWS) / 4].push({1, PCcurr, registerMap[r], clockCycles + 1});
		registersAddrDRAM[registerMap[r]] = {clockCycles + 1, address.second};
		++DRAMsize;
		return 0;
	}

	// perform store word operation
	int sw(string r, string location, string unused1 = "")
	{
		if (!checkRegister(r))
			return 1;
		auto address = locateAddress(location);
		if (!address.first)
			return address.second;
		if (registersAddrDRAM[registerMap[r]] != make_pair(-1, -1))
			return -registerMap[r] - 1;

		if (DRAMsize == DRAM_MAX)
			return -33;
		lastAddr = {address.second, registers[registerMap[r]]};
		isDRAM = true;
		DRAMbuffer[address.second / ROWS][(address.second % ROWS) / 4].push({0, PCcurr, registers[registerMap[r]], clockCycles + 1});
		++rowBufferUpdates, ++DRAMsize;
		PCnext = PCcurr + 1;
		return 0;
	}

	// parse the address and return the actual address or error
	pair<bool, int> locateAddress(string location)
	{
		if (location.back() == ')')
		{
			try
			{
				int lparen = location.find('('), offset = stoi(lparen == 0 ? "0" : location.substr(0, lparen));
				string reg = location.substr(lparen + 1);
				reg.pop_back();
				if (!checkRegister(reg))
					return {false, 3};
				if (registersAddrDRAM[registerMap[reg]] != make_pair(-1, -1))
					return {false, -registerMap[reg] - 1};
				int address = registers[registerMap[reg]] + offset;
				if (address % 4 || address < int(4 * commands.size()) || address >= MAX)
					return {false, 3};
				return {true, address};
			}
			catch (exception &e)
			{
				return {false, 4};
			}
		}
		try
		{
			int address = stoi(location);
			if (address % 4 || address < int(4 * commands.size()) || address >= MAX)
				return {false, 3};
			return {true, address};
		}
		catch (exception &e)
		{
			return {false, 4};
		}
	}

	// checks if label is valid
	inline bool checkLabel(string str)
	{
		return str.size() > 0 && isalpha(str[0]) && all_of(++str.begin(), str.end(), [](char c) { return (bool)isalnum(c); }) && instructions.find(str) == instructions.end();
	}

	// checks if the register is a valid one
	inline bool checkRegister(string r)
	{
		return registerMap.find(r) != registerMap.end();
	}

	// checks if all of the registers are valid or not
	bool checkRegisters(vector<string> regs)
	{
		return all_of(regs.begin(), regs.end(), [&](string r) { return checkRegister(r); });
	}

	// parse the command assuming correctly formatted MIPS instruction (or label)
	void parseCommand(string line)
	{
		// strip until before the comment begins
		line = line.substr(0, line.find('#'));
		vector<string> command;
		boost::tokenizer<boost::char_separator<char>> tokens(line, boost::char_separator<char>(", \t"));
		for (auto &s : tokens)
			command.push_back(s);
		// empty line or a comment only line
		if (command.empty())
			return;
		else if (command.size() == 1)
		{
			string label = command[0].back() == ':' ? command[0].substr(0, command[0].size() - 1) : "?";
			if (address.find(label) == address.end())
				address[label] = commands.size();
			else
				address[label] = -1;
			command.clear();
		}
		else if (command[0].back() == ':')
		{
			string label = command[0].substr(0, command[0].size() - 1);
			if (address.find(label) == address.end())
				address[label] = commands.size();
			else
				address[label] = -1;
			command = vector<string>(command.begin() + 1, command.end());
		}
		else if (command[0].find(':') != string::npos)
		{
			int idx = command[0].find(':');
			string label = command[0].substr(0, idx);
			if (address.find(label) == address.end())
				address[label] = commands.size();
			else
				address[label] = -1;
			command[0] = command[0].substr(idx + 1);
		}
		else if (command[1][0] == ':')
		{
			if (address.find(command[0]) == address.end())
				address[command[0]] = commands.size();
			else
				address[command[0]] = -1;
			command[1] = command[1].substr(1);
			if (command[1] == "")
				command.erase(command.begin(), command.begin() + 2);
			else
				command.erase(command.begin(), command.begin() + 1);
		}
		if (command.empty())
			return;
		if (command.size() > 4)
			for (int i = 4; i < (int)command.size(); ++i)
				command[3] += " " + command[i];
		command.resize(4);
		commands.push_back(command);
	}

	// construct the commands vector from the input file
	void constructCommands(ifstream &file)
	{
		string line;
		while (getline(file, line))
			parseCommand(line);
		file.close();
	}

	// execute the commands sequentially
	void executeCommands()
	{
		if (commands.size() >= MAX / 4)
		{
			handleExit(5);
			return;
		}

		initVars();
		cout << "Cycle info:\n";
		while (PCcurr < (int)commands.size())
		{
			delay = 0, isDRAM = false;
			vector<string> &command = commands[PCcurr];
			if (instructions.find(command[0]) == instructions.end())
			{
				handleExit(4);
				return;
			}
			int ret = instructions[command[0]](*this, command[1], command[2], command[3]);
			if (ret > 0)
			{
				handleExit(ret);
				return;
			}
			if (ret != 0)
			{
				finishCurrDRAM(-ret - 1);
				continue;
			}
			++clockCycles;
			++commandCount[PCcurr];
			PCcurr = PCnext;
			if (!DRAMbuffer.empty())
			{
				// first lw/sw operation after DRAM_buffer emptied
				if (currCol == -1)
					setNextDRAM(DRAMbuffer.begin()->first, DRAMbuffer[DRAMbuffer.begin()->first].begin()->first);
				else if (--DRAMbuffer[currRow][currCol].front().remainingCycles == 0)
					finishCurrDRAM();
			}
			printCycleExecution(command, PCcurr);
		}
		while (!DRAMbuffer.empty())
			finishCurrDRAM();
		bufferUpdate();
		handleExit(0);
	}

	// finish the currently running DRAM instruction and set the next one
	void finishCurrDRAM(int nextRegister = 32)
	{
		auto &Q = DRAMbuffer[currRow][currCol];
		int nextRow = currRow, nextCol = currCol;
		QElem top = DRAMbuffer[currRow][currCol].front();
		popAndUpdate(Q, nextRow, nextCol);

		clockCycles += top.remainingCycles;
		if (top.id && registersAddrDRAM[top.value] != make_pair(-1, -1))
		{
			registers[top.value] = buffer[currCol];
			if (registersAddrDRAM[top.value].first == top.issueCycle)
			{
				registersAddrDRAM[top.value] = {-1, -1};
				if (nextRegister == top.value)
					nextRegister = 32;
			}
			lastAddr.first = currCol * 4 + currRow * ROWS;
			lastAddr.second = buffer[currCol];
			printDRAMCompletion(top.PCaddr, top.startCycle, clockCycles);
		}
		else if (!top.id)
		{
			buffer[currCol] = top.value;
			printDRAMCompletion(top.PCaddr, top.startCycle, clockCycles);
		}
		else
			printDRAMCompletion(top.PCaddr, top.startCycle, clockCycles, "rejected");

		setNextDRAM(nextRow, nextCol, nextRegister);
	}

	// set the next DRAM command to be executed (implements reordering)
	void setNextDRAM(int nextRow, int nextCol, int nextRegister = 32)
	{
		if (DRAMbuffer.empty())
		{
			currCol = -1;
			return;
		}
		if (nextRegister != 32)
		{
			nextRow = registersAddrDRAM[nextRegister].second / ROWS;
			nextCol = (registersAddrDRAM[nextRegister].second % ROWS) / 4;
		}

		QElem top = DRAMbuffer[nextRow][nextCol].front();
		while (top.id && registersAddrDRAM[top.value].first != top.issueCycle && popAndUpdate(DRAMbuffer[nextRow][nextCol], nextRow, nextCol, true))
			top = DRAMbuffer[nextRow][nextCol].front();

		if (DRAMbuffer.empty())
		{
			currCol = -1;
			return;
		}
		bufferUpdate(nextRow, nextCol);
		DRAMbuffer[currRow][currCol].front().startCycle = clockCycles + 1;
		DRAMbuffer[currRow][currCol].front().remainingCycles = delay;
	}

	// pop the queue element and update the row and column if needed (returns false if DRAM empty after pop)
	bool popAndUpdate(queue<QElem> &Q, int &row, int &col, bool skip = false)
	{
		if (skip)
			printDRAMCompletion(Q.front().PCaddr, clockCycles, clockCycles, "skipped");
		Q.pop();
		--DRAMsize;
		if (Q.empty())
		{
			DRAMbuffer[row].erase(col);
			if (DRAMbuffer[row].empty())
			{
				DRAMbuffer.erase(row);
				if (!DRAMbuffer.empty())
					row = DRAMbuffer.begin()->first;
			}
			if (DRAMbuffer.empty())
				return false;
			col = DRAMbuffer[row].begin()->first;
			return true;
		}
		return true;
	}

	// implement buffer update
	void bufferUpdate(int row = -1, int col = -1)
	{
		if (row == -1)
		{
			delay = (currRow != -1) * row_access_delay;
			if (currRow != -1)
				++rowBufferUpdates, data[currRow] = buffer, buffer = vector<int>();
		}
		else if (currRow == -1)
			delay = row_access_delay + col_access_delay, ++rowBufferUpdates, buffer = data[row];
		else if (currRow != row)
			delay = 2 * row_access_delay + col_access_delay, ++rowBufferUpdates, data[currRow] = buffer, buffer = data[row];
		else
			delay = col_access_delay;
		currRow = row, currCol = col;
	}

	// prints the cycle info of DRAM delay
	void printDRAMCompletion(int PCaddr, int begin, int end, string action = "executed")
	{
		cout << begin << '-' << end << " (DRAM call " << action << "): (PC address " << PCaddr << ") ";
		for (auto s : commands[PCaddr])
			cout << s << ' ';
		cout << "\n\n";
	}

	// print cycle info
	void printCycleExecution(vector<string> &command, int PCaddr)
	{
		if (isDRAM)
			cout << clockCycles << ": (DRAM call queued) ";
		else
			cout << clockCycles << ": " << (command[0] == "lw" ? "(used forwarding) " : "");
		cout << "(PC address " << PCaddr << ") ";
		for (auto &s : command)
			cout << s << ' ';
		cout << '\n';
		printRegisters();
		cout << "\n";
	}

	// print the register data in hexadecimal
	void printRegisters()
	{
		cout << hex;
		for (int i = 0; i < 32; ++i)
			cout << registers[i] << ' ';
		cout << dec << "\n";
	}

	/*
		handle all exit codes:
		0: correct execution
		1: register provided is incorrect
		2: invalid label
		3: unaligned or invalid address
		4: syntax error
		5: commands exceed memory limit
	*/
	void handleExit(int code)
	{
		switch (code)
		{
		case 1:
			cerr << "Invalid register provided or syntax error in providing register\n";
			break;
		case 2:
			cerr << "Label used not defined or defined too many times\n";
			break;
		case 3:
			cerr << "Unaligned or invalid memory address specified\n";
			break;
		case 4:
			cerr << "Syntax error encountered\n";
			break;
		case 5:
			cerr << "Memory limit exceeded\n";
			break;
		default:
			break;
		}
		if (code != 0)
		{
			cerr << "Error encountered at:\n";
			for (auto &s : commands[PCcurr])
				cerr << s << ' ';
			cerr << '\n';
		}

		cout << "Exit code: " << code << '\n';

		cout << "\nThe Row Buffer was updated " << rowBufferUpdates << " times.\n";
		cout << "\nFollowing are the non-zero data values:\n";
		for (int i = 0; i < ROWS; ++i)
			for (int j = 0; j < ROWS / 4; ++j)
				if (data[i][j] != 0)
					cout << (ROWS * i + 4 * j) << '-' << (ROWS * i + 4 * j) + 3 << hex << ": " << data[i][j] << '\n'
						 << dec;
		cout << "\nTotal number of cycles: " << clockCycles << " + " << delay << " (cycles taken for code execution + final writeback delay)\n";
		cout << "\nCount of instructions executed:\n";
		for (int i = 0; i < (int)commands.size(); ++i)
		{
			cout << commandCount[i] << " times:\t";
			for (auto &s : commands[i])
				cout << s << ' ';
			cout << '\n';
		}
	}

	// initialize variables before executing commands
	void initVars()
	{
		clockCycles = 0, PCcurr = 0, rowBufferUpdates = 0, DRAMsize = 0, currRow = -1, currCol = -1;
		fill_n(registers, 32, 0);
		fill_n(registersAddrDRAM, 32, make_pair(-1, -1));
		data = vector<vector<int>>(ROWS, vector<int>(ROWS >> 2, 0));
		buffer = vector<int>(ROWS >> 2, 0);
		commandCount.clear();
		commandCount.assign(commands.size(), 0);
		DRAMbuffer.clear();
		lastAddr = {-1, -1};
	}
};

int main(int argc, char *argv[])
{
	if (argc != 4)
	{
		cerr << "Required arguments: file_name ROW_ACCESS_DELAY COL_ACCESS_DELAY\n";
		return 0;
	}
	ifstream file(argv[1]);
	MIPS_Architecture *mips;
	if (file.is_open())
		try
		{
			mips = new MIPS_Architecture(file, stoi(argv[2]), stoi(argv[3]));
		}
		catch (exception &e)
		{
			cerr << "Required arguments: file_name ROW_ACCESS_DELAY COL_ACCESS_DELAY\n";
			return 0;
		}
	else
	{
		cerr << "File could not be opened. Terminating...\n";
		return 0;
	}

	mips->executeCommands();
	return 0;
}


#define BOOST_TEST_MODULE pcraster mldd out_edge_iterator
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "mldd_outedgeiterator.h"

BOOST_AUTO_TEST_CASE(constructor)
{
  using namespace mldd;

 {
  OutEdgeIterator begin(Vertex(1,1),0,0);
  OutEdgeIterator end  (Vertex(1,1));
  size_t i=0;
  for(OutEdgeIterator oi=begin; oi!=end; ++oi) {
    i++;
  }
  BOOST_CHECK(!i);
 }
 {
    OutEdgeIterator begin(Vertex(1,1),(1<<0)|(1<<6),0);
    OutEdgeIterator end  (Vertex(1,1));
    size_t i=0;
    for(OutEdgeIterator oi=begin; oi!=end; ++oi) {
        Edge e=oi.edge();
        BOOST_CHECK(e.source()==Vertex(1,1));
        switch (i) {
          case 0: BOOST_CHECK(e.target()==Vertex(2,0)); break;
          case 1: BOOST_CHECK(e.target()==Vertex(0,1)); break;
        };
        i++;
    }
    BOOST_CHECK(i==2);
 }
 {
    OutEdgeIterator begin(Vertex(1,1),(1<<0)|(1<<1),0);
    OutEdgeIterator end  (Vertex(1,1));
    size_t i=0;
    for(OutEdgeIterator oi=begin; oi!=end; ++oi) {
        Edge e=oi.edge();
        BOOST_CHECK(e.source()==Vertex(1,1));
        switch (i) {
          case 0: BOOST_CHECK(e.target()==Vertex(2,0)); break;
          case 1: BOOST_CHECK(e.target()==Vertex(2,1)); break;
        };
        i++;
    }
    BOOST_CHECK(i==2);
 }
 { // any() must do the same
    OutEdgeIterator begin(Vertex(1,1),(1<<0)|(1<<1),0);
    size_t i=0;
    for(OutEdgeIterator oi=begin; oi.any(); ++oi) {
        Edge e=oi.edge();
        BOOST_CHECK(e.source()==Vertex(1,1));
        switch (i) {
          case 0: BOOST_CHECK(e.target()==Vertex(2,0)); break;
          case 1: BOOST_CHECK(e.target()==Vertex(2,1)); break;
        };
        i++;
    }
    BOOST_CHECK(i==2);
 }
 {
    OutEdgeIterator begin(Vertex(1,1),0xFF,0);
    OutEdgeIterator end  (Vertex(1,1));
    size_t i=0;
    OutEdgeIterator oi=begin;
    for(    ; oi != end /* oi.any() */; ++oi) {
        i++;
        Edge e=oi.edge();
        BOOST_CHECK(e.source()==Vertex(1,1));
    }
    BOOST_CHECK(i==8);
    // keep at past the end
    ++oi;
    BOOST_CHECK(oi==end);
    ++oi;
    BOOST_CHECK(oi==end);
 }
}


//---------------------------------------------------------------------------//
//!
//! \file   rng_timer.cpp
//! \author Alex Robinon
//! \brief  Main function for timing custom random number generator
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <time.h>

// Boost Scoped Pointer
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "Utility_RandomNumberGenerator.hpp"

// Time macro
#define TIME() (clock()/((double)CLOCKS_PER_SEC))

// Generator timing function
void timeGenerator( const int trial_size, const int histories = 1 )
{
  // Raw generator
  Utility::LinearCongruentialGenerator generator;

  double time1 = TIME();

  // Wrapped double generator timing
  for( int i = 0; i < histories; ++i )
  {
    Utility::RandomNumberGenerator::initialize( i );

    for( int j = 0; j < trial_size/histories; ++j )
      Utility::RandomNumberGenerator::getRandomNumber<double>();
  }
  std::cout << std::endl;

  double time2 = TIME();

  // Raw double generator timing
  for( int i = 0; i < histories; ++i )
  {
    for( int j = 0; j < trial_size/histories; ++j )
      generator.getRandomNumber();

    generator.nextHistory();
  }

  double time3 = TIME();

  // Check for valid time intervals
  if( time2 - time1 < 1.0e-15 || time3 - time2 < 1.0e-15 )
  {
    std::cerr << "Timing information not accurate enough for this generator."
	      << std::endl;
  }
  else
  {
    // Calculate the generation speed (Millions/sec)
    double mdbls_per_sec_wrapped = trial_size/(time2-time1)/1e6;
    double mdbls_per_sec_raw = trial_size/(time3-time2)/1e6;

    // Print the last double generated
    std::cout << "Last random number generated: "
	      << generator.getRandomNumber() << " "
	      << Utility::RandomNumberGenerator::getRandomNumber<double>()
	      << std::endl
	      << "Random numbers per history: " << trial_size/histories
	      << std::endl
	      << "User + System time information (NOTE: MRS = Million Random "
	      << "Numbers Per Second)\n" << std::endl
	      << "  Wrapped Double generator:\tTime = " << time2-time1
	      << " seconds " << "=> " << mdbls_per_sec_wrapped
	      << std::endl
	      << "  Raw Double generator:\t\tTime = " << time3-time2
	      << " seconds " << "=> " << mdbls_per_sec_raw
	      << std::endl << std::endl;
  }
}

// Main itming function
int main()
{
  Utility::RandomNumberGenerator::createStreams();

  Utility::RandomNumberGenerator::initialize();

  int trial_size = 10000000;

  std::cout << "Timing generator for single history" << std::endl;
  timeGenerator( trial_size );

  std::cout << "Timing generator for 10 histories" << std::endl;
  timeGenerator( trial_size, 10 );

  std::cout << "Timing generator for 100 histories" << std::endl;
  timeGenerator( trial_size, 100 );

  std::cout << "Timing generator for 1000 histories" << std::endl;
  timeGenerator( trial_size, 1000 );

  return 0;
}

//---------------------------------------------------------------------------//
// end rng_timer.cpp
//---------------------------------------------------------------------------//


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_BOOLEAN_FUNCTIONS_COMPLEX_GENERIC_IF_ELSE_ZERO_HPP_INCLUDED
#define NT2_BOOLEAN_FUNCTIONS_COMPLEX_GENERIC_IF_ELSE_ZERO_HPP_INCLUDED

#include <nt2/boolean/functions/if_else_zero.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT   ( if_else_zero_, tag::cpu_, (A0)(A1)
                             , (generic_< fundamental_<A0> >)
                               (generic_< complex_<floating_<A1> > >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return result_type(if_else_zero(a0, nt2::real(a1)), if_else_zero(a0, nt2::imag(a1)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( if_else_zero_, tag::cpu_, (A0)(A1)
                             , (generic_< complex_<floating_<A0> > >)
                               (generic_< unspecified_<A1> >)
                             )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return if_else_zero(is_nez(a0), a1);
    }
  };

} } }

#endif


// Copyright DEWETRON GmbH 2020

#include "wav_writer.h"

//#include "uni_math_constants.h"
#include <cmath>
#include <boost/test/unit_test.hpp>

#if _MSC_VER > 1920
#pragma warning(disable:4996)
#endif

BOOST_AUTO_TEST_SUITE(uni_wav_writer_test_suite)

namespace
{
    const int CHUNK_SIZE = 8;
    const int RIFFWAVE_SIZE = CHUNK_SIZE + 4;
    const int FORMAT_SIZE = CHUNK_SIZE + 16;
}

BOOST_AUTO_TEST_CASE(WriteHeaderTest)
{
    FILE* tmp = std::tmpfile();
    WavWriter writer(tmp);
    writer.writeHeader(WavFormatTag::WAV_FORMAT_PCM, 16, 44100, 2, 0);

    struct header
    {
        char riff[4];
        uint32_t length;
        char wave[4];
    };
    BOOST_CHECK_EQUAL(sizeof(header), RIFFWAVE_SIZE);

    BOOST_REQUIRE_EQUAL(0, fseek(tmp, 0, SEEK_SET));
    header buffer;
    BOOST_REQUIRE_EQUAL(1, fread(&buffer, sizeof(header), 1, tmp));
    BOOST_CHECK(!std::strncmp(buffer.riff, "RIFF", 4));
    BOOST_CHECK_EQUAL(buffer.length, 4 + FORMAT_SIZE + CHUNK_SIZE);
    BOOST_CHECK(!std::strncmp(buffer.wave, "WAVE", 4));

    struct fmtheader
    {
        char fmt[4];
        uint32_t length;
        uint16_t format_tag;
        uint16_t channels;
        uint32_t sample_rate;
        uint32_t bytes_per_second;
        uint16_t block_align;
        uint16_t bits_per_sample;
    } fmt;

    BOOST_REQUIRE_EQUAL(1, fread(&fmt, sizeof(fmt), 1, tmp));
    BOOST_CHECK(!std::strncmp(fmt.fmt, "fmt ", 4));
    BOOST_CHECK_EQUAL(fmt.length, 16);
    BOOST_CHECK_EQUAL(fmt.format_tag, 1);
    BOOST_CHECK_EQUAL(fmt.channels, 2);
    BOOST_CHECK_EQUAL(fmt.sample_rate, 44100);
    BOOST_CHECK_EQUAL(fmt.bytes_per_second, 2 * 2 * 44100);
    BOOST_CHECK_EQUAL(fmt.block_align, 2 * 2);
    BOOST_CHECK_EQUAL(fmt.bits_per_sample, 16);

    struct dataheader
    {
        char data[4];
        uint32_t length;
    } dataheader;
    BOOST_REQUIRE_EQUAL(1, fread(&dataheader, sizeof(dataheader), 1, tmp));
    BOOST_CHECK(!std::strncmp(dataheader.data, "data ", 4));
    BOOST_CHECK_EQUAL(dataheader.length, 0);
}

BOOST_AUTO_TEST_CASE(WriteSamplesTest)
{
    FILE* tmp = std::tmpfile();
    WavWriter writer(tmp);
    writer.writeHeader(WavFormatTag::WAV_FORMAT_PCM, 16, 44100, 2, 10);
    for (int16_t n = 0; n < 10; ++n)
    {
        writer.appendSamples(&n, sizeof(int16_t));
    }
    BOOST_CHECK_EQUAL(writer.samplesWritten(), 10);

    // batch add samples
    std::vector<int16_t> data(10, 0);
    writer.appendSamples(data.data(), data.size() * sizeof(int16_t));
    BOOST_CHECK_EQUAL(writer.samplesWritten(), 20);

    BOOST_REQUIRE_EQUAL(0, fseek(tmp, RIFFWAVE_SIZE + FORMAT_SIZE + CHUNK_SIZE, SEEK_SET));
    std::vector<uint16_t> samples(2 * 10);
    BOOST_REQUIRE_EQUAL(samples.size(), fread(samples.data(), sizeof(uint16_t), samples.size(), tmp));
    for (uint16_t n = 0; n < 10; ++n)
    {
        BOOST_CHECK_EQUAL(samples[n], n);
    }
}

#if 0
BOOST_AUTO_TEST_CASE(ExternalUsabiliyTest)
{
    // This test is used to generate a WAV file with various settings to test the import into a 3rd party audio program
    WavWriter writer("C:\\Temp\\export.wav");
    const uint32_t Fs = 44100;
    const double f0 = 440;
    const std::size_t num_samples = Fs * 2;
    const std::size_t bit_depth = 24;

    writer.writeHeader(WavFormatTag::WAV_FORMAT_PCM, bit_depth, Fs, 1, num_samples);
    const double w = uni::math::constants::two_pi * f0 / Fs;
    const double lambda = -2.0 / Fs;
    const double conversion_factor = (1 << (bit_depth - 1)) - 1;
    for (std::size_t n = 0; n < num_samples; ++n)
    {
        double scale = std::exp(lambda * n);
        const double a = scale * std::sin(w * n);
        sint64 sample = static_cast<sint64>(a * conversion_factor);
        writer.appendSamples(&sample, bit_depth / 8);
    }
    writer.close();
}

BOOST_AUTO_TEST_CASE(ExternalUsabiliyFloatTest)
{
    // This test is used to generate a WAV file with various settings to test the import into a 3rd party audio program
    WavWriter writer("C:\\Temp\\export_float.wav");
    const uint32_t Fs = 44100;
    const double f0 = 440;
    const std::size_t num_samples = Fs * 2;
    using sample_format = float;
    const std::size_t bit_depth = sizeof(sample_format) * 8;

    writer.writeHeader(WavFormatTag::WAV_FORMAT_FLOAT, bit_depth, Fs, 1, num_samples);
    const double w = uni::math::constants::two_pi * f0 / Fs;
    const double lambda = -2.0 / Fs;
    for (std::size_t n = 0; n < num_samples; ++n)
    {
        double scale = std::exp(lambda * n);
        const double a = scale * std::sin(w * n);
        sample_format sample = static_cast<sample_format>(a);
        writer.appendSamples(&sample, sizeof(sample));
    }
    writer.close();
}
#endif

BOOST_AUTO_TEST_SUITE_END()


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <type_traits>

// sample(type_unification-Type)
template <typename T>
class Type { /* nothing */ };

Type<int> t{};
// end-sample

// sample(type_unification-metafunction)
template <typename T>
constexpr Type<T*> add_pointer(Type<T> const&)
{ return {}; }

template <typename T>
constexpr std::false_type is_pointer(Type<T> const&)
{ return {}; }

template <typename T>
constexpr std::true_type is_pointer(Type<T*> const&)
{ return {}; }
// end-sample

namespace usage {
// sample(type_unification-usage)
Type<int> t{};
auto p = add_pointer(t);
static_assert(is_pointer(p), "");
// end-sample
}

namespace sugar {
// sample(type_unification-sugar)
template <typename T>
constexpr Type<T> type{};

auto t = type<int>;
auto p = add_pointer(t);
static_assert(is_pointer(p), "");
// end-sample
}

namespace citizen {
using namespace boost::hana;
using namespace boost::hana::literals;

// sample(type_unification-first_class)
auto xs = make_tuple(type<int>, type<char>, type<void>);
auto c = xs[1_c];

// sugar:
auto ys = tuple_t<int, char, void>;
// end-sample
}

int main() { }


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/GeometricDerivatives/AdiabaticModeLocalizer.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <boost/optional.hpp>

using namespace Scine::Utils;

void init_adiabatic_mode_localizer(pybind11::module& m) {
  pybind11::class_<AdiabaticModesContainer> adiabaticModesContainer(m, "AdiabaticModesContainer", R"delim(
    A class storing the localized adiabatic properties.
    This class is an extension of the standard normal mode container, with modes being accessed by specifying the
    internal coordinate of interest.
    Note: Currently only interatomic stretching coordinates, e.g., bonds, are supported as internal coordinates.
    )delim");

  adiabaticModesContainer.def("add_mode", &AdiabaticModesContainer::addMode, pybind11::arg("internal_coordinate"),
                              pybind11::arg("mode"), pybind11::arg("force_constant"), R"delim(
    Adds a mode to the container.

    :param internal_coordinate: The internal coordinate to which this mode corresponds.
    :param mode: The mode to be added.
    :param force_constant: The adiabatic force constant of the mode.
    )delim");

  adiabaticModesContainer.def("get_mode", &AdiabaticModesContainer::getMode, pybind11::arg("internal_coordinate"), R"delim(
    Returns the localized vibrational mode corresponding to the given internal coordinate.

    :param: internal_coordinate The internal coordinate of interest.
    :return: The localized mode.
    )delim");

  adiabaticModesContainer.def("get_mode_as_molecular_trajectory", &AdiabaticModesContainer::getModeAsMolecularTrajectory,
                              pybind11::arg("internal_coordinate"), pybind11::arg("atoms"),
                              pybind11::arg("scaling_factor"), R"delim(
    Returns a molecular trajectory corresponding to a vibrational mode.

    :param internal_coordinate: The internal coordinate of interest.
    :param atoms: The atom collection of interest.
    :param scaling_factor: The scaling factor applied to the mode to obtain the maximum displacement.
    :return: The molecular trajectory representing the mode.
    )delim");

  adiabaticModesContainer.def("get_wave_numbers", &AdiabaticModesContainer::getWaveNumbers, R"delim(
    Gets the wave numbers corresponding to the localized modes.

    :return: Mapping between internal coordinates and adiabatic wave numbers.
    )delim");

  adiabaticModesContainer.def("get_force_constants", &AdiabaticModesContainer::getForceConstants, R"delim(
    Gets the force constants corresponding to the localized modes.

    :return: Mapping between internal coordinates and adiabatic force constants.
    )delim");

  pybind11::class_<AdiabaticModeLocalizer> adiabaticModeLocalizer(m, "AdiabaticModeLocalizer", R"delim(
    This class calculates the localized vibrational modes and adiabatic force constants corresponding to internal
    coordinates based on the local vibrational mode theory by Konkoli and Cremer. Adiabatic force constants are
    equivalent to the relaxed force constants derived from Decius' compliance matrix.

    Konkoli, Z.; Cremer, D. Int. J. Quantum Chem. 1998, 67 (1), 1–9.
    https://doi.org/10.1002/(SICI)1097-461X(1998)67:1<1::AID-QUA1>3.0.CO;2-Z.

    Implemented after:
    Kraka, E.; Zou, W.; Tao, Y. Wiley Interdiscip. Rev. Comput. Mol. Sci. 2020, e1480. https://doi.org/10.1002/wcms.1480.

    Note: Currently only interatomic stretching coordinates, e.g., bonds, are supported as internal coordinates.
    )delim");

  adiabaticModeLocalizer.def(pybind11::init<Eigen::MatrixXd, AtomCollection, BondOrderCollection, double>(),
                             pybind11::arg("hessian"), pybind11::arg("atoms"), pybind11::arg("bond_orders"),
                             pybind11::arg("bonding_threshold") = 0.5, R"delim(
    Constructor for the AdiabaticModeLocalizer class processing a bond order collection.
    All bonds with a bond order larger than bonding_threshold will be used as internal coordinates.

    :param hessian: The cartesian, not mass-weighted Hessian matrix.
    :param atoms: The atom collection of interest.
    :param bond_orders: The bond order collection of the structure of interest.
    :param: bonding_threshold The bond order threshold beyond which bonds are analyzed in terms of localized modes. (default: 0.5)
    )delim");

  adiabaticModeLocalizer.def(pybind11::init<Eigen::MatrixXd, AtomCollection, const std::vector<std::pair<int, int>>>(),
                             pybind11::arg("hessian"), pybind11::arg("atoms"), pybind11::arg("internal_coordinates"), R"delim(
    Constructor for the AdiabaticModeLocalizer class processing indices of atom pairs as internal coordinates.

    :param hessian: The cartesian, not mass-weighted Hessian matrix.
    :param atoms: The atom collection of interest.
    :param internal_coordinates:  The internal coordinates of interest.
    )delim");

  adiabaticModeLocalizer.def("localize_modes", &AdiabaticModeLocalizer::localizeModes, R"delim(
    Calculates the adiabatic localized modes corresponding to the given internal coordinates.

    References:
    Konkoli, Z.; Cremer, D. Int. J. Quantum Chem. 1998, 67 (1), 1–9.
    https://doi.org/10.1002/(SICI)1097-461X(1998)67:1<1::AID-QUA1>3.0.CO;2-Z.

    Kraka, E.; Zou, W.; Tao, Y. Wiley Interdiscip. Rev. Comput. Mol. Sci. 2020, e1480.
    https://doi.org/10.1002/wcms.1480.

    :return: A container with the localized adiabatic modes, force constants and wavenumbers.
    )delim");
}


#include <boost/icl/discrete_interval.hpp>


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

#include <boost/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/type.hpp>

#include <cstddef>
#include <type_traits>
using namespace boost::hana;

// operator()
static_assert(size_t<0>() == 0, "");
static_assert(size_t<1>() == 1, "");
static_assert(int_<-3>() == -3, "");

// decltype(operator())
BOOST_HANA_CONSTANT_ASSERT(decltype_(size_t<0>()) == type<std::size_t>);
BOOST_HANA_CONSTANT_ASSERT(decltype_(int_<-3>()) == type<int>);

// conversions
constexpr std::size_t a = size_t<0>, b = size_t<1>;
static_assert(a == 0 && b == 1, "");

constexpr int c = int_<0>, d = int_<-3>;
static_assert(c == 0 && d == -3, "");

// nested ::value
static_assert(decltype(int_<1>)::value == 1, "");

// nested ::type
static_assert(std::is_same<
    decltype(int_<1>)::type,
    std::remove_cv_t<decltype(int_<1>)>
>{}, "");

// nested ::value_type
static_assert(std::is_same<decltype(int_<1>)::value_type, int>{}, "");

int main() { }


#ifndef CoordinateSystem_hpp
#define CoordinateSystem_hpp

#include <sstream>
#include <type_traits>

#include <boost/array.hpp>
#include <boost/multi_array.hpp>
#include <boost/optional.hpp>

#include "Aliases.hpp"
#include "RangeDiscretizers.hpp"
#include "Utils.hpp"

/** @file CoordinateSystem.hpp
 * @brief
 * @author C.D. Clark III
 * @date 06/13/17
 */

/** @class CoordinateSystem
 * @brief
 * @author C.D. Clark III */
template<typename T>
using array1D = boost::multi_array<T, 1>;
template<typename T>
using view1D = boost::detail::multi_array::multi_array_view<T, 1>;

template<typename COORD, size_t NUMDIMS,
         template<typename> class ARRAY = array1D>
class CoordinateSystem
{
 public:
  typedef ARRAY<COORD>                            axis_type;
  typedef typename axis_type::index               coordinate_index_type;
  typedef CoordinateSystem<COORD, NUMDIMS, ARRAY> this_type;

 protected:
  std::array<std::shared_ptr<axis_type>, NUMDIMS> axes;

 public:
#if SERIALIZATION_ENABLED

  template<class Archive>
  void serialize(Archive& ar, const unsigned int version)
  {
    ar& axes;
  }

#endif

  // make sure compiler generates all of the special member functions we need.
  CoordinateSystem()                        = default;
  CoordinateSystem(const CoordinateSystem&) = default;
  CoordinateSystem(CoordinateSystem&&)      = default;
  ~CoordinateSystem() = default;
  CoordinateSystem& operator=(CoordinateSystem& rhs) = default;
  CoordinateSystem& operator=(CoordinateSystem&& rhs) = default;

  template<typename... Args>
  CoordinateSystem(Args... args)
  {
    init_imp<0>(args...);
  }

  template<typename I>
  CoordinateSystem(std::array<I, NUMDIMS> sizes)
  {
    for (size_t i = 0; i < NUMDIMS; i++)
      axes[i] = std::make_shared<axis_type>(boost::extents[sizes[i]]);
  }

  CoordinateSystem(const std::array<std::shared_ptr<axis_type>, NUMDIMS>& axes_)
  {
    // we need to deep copy the axes, not just the shared pointer to them.
    for (size_t i = 0; i < NUMDIMS; i++){
      axes[i] = std::make_shared<axis_type>(*axes_[i]);
    }
  }

  /** Returns size of the i'th axis. */
  auto size(int i) const
  {
    if (i < 0) {
      size_t n = 1;
      for (auto& a : axes) n *= a->size();

      return n;
    }

    // NUMDIMS is unsigned, so this will eval to true if i is negative
    if (i >= static_cast<int>(NUMDIMS)) return 0ul;

    return axes[i]->size();
  }

  /** Set coorinate values
   *
   * This function accepts a set of Range objects that are used to
   * set the coordinate values.
   * */
  template<typename... Args>
  auto set(Args... args)
  {
    set_imp<0>(args...);
  }

  /** Return i'th axis */
  const auto& operator[](size_t i) const { return *axes[i]; }
  auto&       operator[](size_t i) { return *axes[i]; }

  /** Return i'th axis */
  const auto& getAxis(size_t i) const { return *axes[i]; }
  auto&       getAxis(size_t i) { return *axes[i]; }

  /** Return pointer to i'th axis */
  const auto getAxisPtr(size_t i) const { return axes[i]; }
  auto       getAxisPtr(size_t i) { return axes[i]; }

  /** Return pointer to axes array */
  const auto getAxes() const { return axes; }
  auto       getAxes() { return axes; }

  /** Return coordinate specified by indecies given as arguments */
  template<typename... Args>
  auto operator()(Args... args) const
  {
    return this->getCoord(args...);
  }

  /** Return coordinate specified by indices given as arguments */
  template<typename... Args>
  auto getCoord(Args... args) const
  {
    std::array<COORD, NUMDIMS> c;
    getCoord_imp<0>(c, args...);
    return c;
  }

  /** Return coordinate specified by indices given in an index container */
  template<typename I,
           typename std::enable_if<IsIndexCont<I>::value, int>::type = 0>
  auto getCoord(I ind) const
  {
    std::array<COORD, NUMDIMS> c;
    for (size_t i = 0; i < NUMDIMS; ++i) c[i] = axes[i]->operator[](ind[i]);
    return c;
  }

  /** For a 1D coordinate system, return the coordinate directly, instead of an
   * array containing the coordinates. */
  template<typename I,
           typename std::enable_if<std::is_integral<I>::value, int>::type = 0>
  auto getCoord(I i) const
  {
    BOOST_STATIC_ASSERT_MSG(NUMDIMS == 1,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "getCoord called with wrong "
                            "number of arguments.");
    return axes[0]->operator[](i);
  }

  /** Return a sliced coordinate system view based on an index generator. */
  template<int NDims>
  const auto slice(
      const boost::detail::multi_array::index_gen<NUMDIMS, NDims>& ind) const
  {
    // Notes:
    // NUMDIMS is the number of dimensions of this coordinate system
    // NDims is the number of dimensions of the coordinate system we need to
    // create.
    boost::multi_array_types::index_gen indices;
    // create an array of axis views that will be used to construct the new
    // coordinate system.
    std::array<std::shared_ptr<view1D<COORD> >, NDims> new_axes;
    int                                                ii = 0;
    // loop through each axis
    for (size_t i = 0; i < NUMDIMS; i++) {
      // skip degenerate axes
      if (!ind.ranges_[i].degenerate_) {
        new_axes[ii] = std::shared_ptr<view1D<COORD>>(axes[i]->operator[](
            indices[IRange(ind.ranges_[i].start_, ind.ranges_[i].finish_,
                           ind.ranges_[i].stride_)]));
        ii++;
      }
    }
    return CoordinateSystem<COORD, NDims, view1D>(new_axes);
  }

  /** Return a sliced coordinate system view based on an index generator. */
  template<int NDims>
  auto slice(const boost::detail::multi_array::index_gen<NUMDIMS, NDims>& ind)
  {
    // Notes:
    // NUMDIMS is the number of dimensions of this coordinate system
    // NDims is the number of dimensions of the coordinate system we need to
    // create.
    boost::multi_array_types::index_gen indices;
    // create an array of axis views that will be used to construct the new
    // coordinate system.
    std::array<std::shared_ptr<view1D<COORD> >, NDims> new_axes;
    size_t                                             ii = 0;
    // loop through each axis
    for (size_t i = 0; i < NUMDIMS; i++) {
      // skip degenerate axes
      if (!ind.ranges_[i].degenerate_) {
        new_axes[ii] = std::make_shared<view1D<COORD>>(axes[i]->operator[](
            indices[IRange(ind.ranges_[i].start_, ind.ranges_[i].finish_,
                           ind.ranges_[i].stride_)]));
        ii++;
      }
    }
    return CoordinateSystem<COORD, NDims, view1D>(new_axes);
  }

  /** WARNING: this does not have the same meaning as std::lower_bound.*/
  template<typename... Args>
  auto lower_bound(Args... args) const
  {
    std::array<int, NUMDIMS> ind;
    lower_bound_imp<0>(ind, args...);
    return ind;
  }

  template<typename... Args>
  auto upper_bound(Args... args) const
  {
    std::array<int, NUMDIMS> ind;
    upper_bound_imp<0>(ind, args...);
    return ind;
  }

  template<typename... Args>
  auto nearest(Args... args) const
  {
    std::array<int, NUMDIMS> ind;
    nearest_imp<0>(ind, args...);
    return ind;
  }

  // helper functions/implementations
 protected:
  template<int II, typename N, typename... Args>
  typename std::enable_if<std::is_integral<N>::value, void>::type init_imp(
      N n, Args... args)
  {
    axes[II] = std::make_shared<axis_type>(boost::extents[n]);
    init_imp<II + 1>(args...);
  }

  template<int II, typename... Args>
  void init_imp(std::shared_ptr<axis_type>& axis, Args... args)
  {
    axes[II] = axis;
    init_imp<II + 1>(args...);
  }

  template<int II>
  void init_imp()
  {
    BOOST_STATIC_ASSERT_MSG(II == NUMDIMS,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "constructor called with wrong "
                            "number of arguments.");
  }

  template<int II, typename R, typename... Args>
  typename std::enable_if<!std::is_same<R, boost::none_t>::value, void>::type
  set_imp(R range, Args... args)
  {
    size_t N = axes[II]->size();
    for (size_t i = 0; i < N; ++i) axes[II]->operator[](i) = range(i, N);

    set_imp<II + 1>(args...);
  }

  template<int II, typename R, typename... Args>
  typename std::enable_if<std::is_same<R, boost::none_t>::value, void>::type
  set_imp(R, Args... args)
  {
    set_imp<II + 1>(args...);
  }

  template<int II>
  void set_imp()
  {
    BOOST_STATIC_ASSERT_MSG(II == NUMDIMS,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "set method called with wrong "
                            "number of arguments.");
  }

  template<int II, typename C, typename I, typename... Args>
  void getCoord_imp(C& c, I i, Args... args) const
  {
    c[II] = axes[II]->operator[](i);
    getCoord_imp<II + 1>(c, args...);
  }

  template<int II, typename C>
  void getCoord_imp(C& c) const
  {
    BOOST_STATIC_ASSERT_MSG(II == NUMDIMS,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "getCoord called with wrong "
                            "number of arguments.");
  }

  template<int II, typename IND, typename C, typename... Args>
  void lower_bound_imp(IND& ind, C c, Args... args) const
  {
    ind[II] = std::upper_bound(axes[II]->begin(), axes[II]->end(), c) -
              axes[II]->begin() - 1;
    lower_bound_imp<II + 1>(ind, args...);
  }

  template<int II, typename IND>
  void lower_bound_imp(IND& ind) const
  {
    BOOST_STATIC_ASSERT_MSG(II == NUMDIMS,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "lower_bound called with wrong "
                            "number of arguments.");
  }

  template<int II, typename IND, typename C, typename... Args>
  void upper_bound_imp(IND& ind, C c, Args... args) const
  {
    ind[II] = std::upper_bound(axes[II]->begin(), axes[II]->end(), c) -
              axes[II]->begin();
    upper_bound_imp<II + 1>(ind, args...);
  }

  template<int II, typename IND>
  void upper_bound_imp(IND& ind) const
  {
    BOOST_STATIC_ASSERT_MSG(II == NUMDIMS,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "upper_bound called with wrong "
                            "number of arguments.");
  }

  template<int II, typename IND, typename C, typename... Args>
  void nearest_imp(IND& ind, C c, Args... args) const
  {
    // special cases:
    // coordinate is less than smallest
    if (c < axes[II]->operator[](0)) {
      ind[II] = 0;
    }
    // coordinate is greater than largest
    else if (c > axes[II]->operator[](axes[II]->size() - 1)) {
      ind[II] = axes[II]->size() - 1;
    } else {
      // get index of element that is just below coordinate
      ind[II] = std::upper_bound(axes[II]->begin(), axes[II]->end(), c) -
                axes[II]->begin() - 1;
      // now determine if coordinate is closer to the upper bound or lower bound
      // coord - lower bound divided by upper bound minus lower bound gives the
      // dimensionless coordinate between 0 and 1. if this is less than 0.5, we
      // want to return the lower bound index. if itis greater than 0.5, we want
      // to return the upper bound index don't have to worry about special cases
      // above here.
      ind[II] +=
          2 * (c - axes[II]->operator[](ind[II])) /
          (axes[II]->operator[](ind[II] + 1) - axes[II]->operator[](ind[II]));
    }

    nearest_imp<II + 1>(ind, args...);
  }

  template<int II, typename IND>
  void nearest_imp(IND& ind) const
  {
    BOOST_STATIC_ASSERT_MSG(II == NUMDIMS,
                            "CoordinateSystem<COORD,NUMDIMS> "
                            "nearest called with wrong "
                            "number of arguments.");
  }
};

#endif  // include protector


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include "CameraMoveController.h"

const float expectedNormalizedAngle = 0.09999;
const float expectedAngle = 35.9964;
const float angleFactor = 360;

BOOST_AUTO_TEST_CASE( TestSubtractTwoFloats ) {
	float value = SubTwoFloats(0.1, 0.00001);
	BOOST_CHECK_EQUAL(value, expectedNormalizedAngle);
	BOOST_CHECK_EQUAL(value*angleFactor, expectedNormalizedAngle*angleFactor);
	BOOST_CHECK_EQUAL(value*angleFactor, 0.09999*angleFactor);
};

BOOST_AUTO_TEST_CASE( TestForStateMachine ) {
	CameraMoveController c;
	BOOST_CHECK_EQUAL(c.GetCurrentState(), EngineState::HomePosition);
	c.StopCamera();
	BOOST_CHECK_EQUAL(c.GetCurrentState(), EngineState::Stopped);
	c.GoToHome();
	BOOST_CHECK_EQUAL(c.GetCurrentState(), EngineState::HomePosition);
};

BOOST_AUTO_TEST_CASE( TestForStateMachineMoveTests ) {
	CameraMoveController c;
	c.MoveCamera();
	BOOST_CHECK_EQUAL(c.GetCurrentPos(), 0.5);
	c.MoveCamera();
	BOOST_CHECK_EQUAL(c.GetCurrentPos(), 0.2);
};