/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * 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.
 */
/*!
 * \file
 * \brief Test of mathemttical functions for variable class.
 */
#include "num_collect/auto_diff/backward/variable_math.h"

#include <Eigen/Core>
#include <catch2/catch_template_test_macros.hpp>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "num_collect/auto_diff/backward/differentiate.h"

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE("num_collect::auto_diff::backward::exp", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::differentiate;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("process an argument with node") {
        const auto var = variable_type(1.234, variable_tag());

        const variable_type res = exp(var);
        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(std::exp(var.value())));
        REQUIRE(res.node());

        const scalar_type coeff = differentiate(res, var);
        REQUIRE_THAT(coeff, Catch::Matchers::WithinRel(std::exp(var.value())));
    }

    SECTION("process an argument without node") {
        const auto var = variable_type(1.234, constant_tag());

        const variable_type res = exp(var);
        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(std::exp(var.value())));
        REQUIRE_FALSE(res.node());
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE("num_collect::auto_diff::backward::log", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::differentiate;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("process an argument with node") {
        const auto var = variable_type(1.234, variable_tag());

        const variable_type res = log(var);
        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(std::log(var.value())));
        REQUIRE(res.node());

        const scalar_type coeff = differentiate(res, var);
        REQUIRE_THAT(coeff, Catch::Matchers::WithinRel(1 / var.value()));
    }

    SECTION("process an argument without node") {
        const auto var = variable_type(1.234, constant_tag());

        const variable_type res = log(var);
        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(std::log(var.value())));
        REQUIRE_FALSE(res.node());
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::sqrt", "", float, double) {
    using scalar_type = TestType;
    using variable_type =
        num_collect::auto_diff::backward::variable<scalar_type>;
    using num_collect::auto_diff::backward::constant_tag;
    using num_collect::auto_diff::backward::differentiate;
    using num_collect::auto_diff::backward::variable_tag;

    SECTION("process an argument with node") {
        const auto var = variable_type(1.234, variable_tag());

        const variable_type res = sqrt(var);
        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(std::sqrt(var.value())));
        REQUIRE(res.node());

        const scalar_type coeff = differentiate(res, var);
        REQUIRE_THAT(coeff,
            Catch::Matchers::WithinRel(
                1 / static_cast<scalar_type>(2) / std::sqrt(var.value())));
    }

    SECTION("process an argument without node") {
        const auto var = variable_type(1.234, constant_tag());

        const variable_type res = log(var);
        REQUIRE_THAT(
            res.value(), Catch::Matchers::WithinRel(std::log(var.value())));
        REQUIRE_FALSE(res.node());
    }
}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix
  Eigen::Matrix<double, 4, 4> elem_mat;

  //====================
  // Your code goes here
  //====================
  Eigen::Matrix<double,4,4> laplace_elem_mat;
  Eigen::Matrix<double,4,4> mass_elem_mat;

  lf::uscalfe::LinearFELaplaceElementMatrix laplace_elmat_builder;
  laplace_elem_mat = laplace_elmat_builder.Eval(cell);

  switch (ref_el) {
      case (lf::base::RefEl::kTria()): {
          double area =
              0.5*
              ( (vertices(0,1)-vertices(0,0))*(vertices(1,2)-vertices(1,0))-
                (vertices(1,1)-vertices(1,0))*(vertices(0,2)-vertices(0,0)))
          mass_elem_mat <<
          2.0,1.0,1.0,0.0,
          1.0,2.0,1.0.0.0,
          1.0,1.0,2.0,0.0,
          0.0,0.0,0.0,0.0;

          mass_elem_mat *= area/12.0;
          break;
      }
      case (lf::base::RefEL::kQuad()): {
          double area =
              (vertices(0,1)-vertices(0,0))*(vertices(1,3)-vertices(1,0));
          mass_elem_mat <<
          4.0,2.0,1.0,2.0,
          2.0,4.0,2.0,1.0,
          1.0,2.0,4.0,2.0,
          2.0,1.0,2.0,4.0;

          mass_elem_mat *= area/36.0;
          break;
      }
  }
  elem_mat = laplace_elem_mat + mass_elem_mat;
  return elem_mat;
}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


#include <ros/ros.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>

#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include "kalman.hpp"

//#include <people_msgs/PositionMeasurement.h>
#include <geometry_msgs/PoseStamped.h>

#include <visualization_msgs/Marker.h>

template <typename T> std::string tostr(const T& t)
{
    std::ostringstream os; os<<t; return os.str();
}

//static std::string fixed_frame = "base_link";
static std::string fixed_frame = "map";

class KalmanTracker{
private:
    ros::Rate r;
    ros::Subscriber person_pos_sub;
    ros::Publisher markers_pub,text_pub,estimate_pos_pub,estimate_arrow_pub;
    ros::NodeHandle nh1;
    ros::Time start,now;
    Eigen::VectorXd x,y;
    Eigen::MatrixXd A; // System dynamics matrix
    Eigen::MatrixXd C; // Output matrix
    Eigen::MatrixXd Q; // Process noise covariance
    Eigen::MatrixXd R; // Measurement noise covariance
    Eigen::MatrixXd P; // Estimate error covariance
    KalmanFilter estimate_X,estimate_Y;
    int n; // Number of states
    int m; // Number of measurements
    int init_f,update_f;
    double dt; // Time step
    double t; // Time
    double sigma_obs;
public:
    KalmanTracker(ros::NodeHandle nh):
    nh1(nh),r(10),n(6),m(2),dt(1.0/10),sigma_obs(0.002),init_f(0)
    {
        x = Eigen::VectorXd(n);
        A = Eigen::MatrixXd(n,n);
        C = Eigen::MatrixXd(m,n);
        Q = Eigen::MatrixXd(n,n);
        R = Eigen::MatrixXd(m,m);
        P = Eigen::MatrixXd(n,n);
        // Discrete LTI projectile motion, measuring position only
        const double dt2 = 0.5 * dt * dt;
        A << 1, 0, dt,  0, dt2,   0,
                 0, 1,  0, dt,   0, dt2,
                 0, 0,  1,  0,  dt,   0,
                 0, 0,  0,  1,   0,  dt,
                 0, 0,  0,  0,   1,   0,
                 0, 0,  0,  0,   0,   1;
        C << 1, 0, 0, 0, 0, 0,
                  0, 1, 0, 0, 0, 0;
        // Reasonable covariance matrices
        Q = Eigen::MatrixXd::Identity(n, n) * 1.0e-6;;
        R = Eigen::MatrixXd::Identity(m, m) * pow(sigma_obs, 2);
        P = Eigen::MatrixXd::Identity(n, n) * 1.0e-6;
        estimate_X=KalmanFilter(dt,A, C, Q, R, P);
        estimate_X.init(0,x);
        //start = now = ros::Time::now();

        person_pos_sub = nh1.subscribe("filter_measurement",1,&KalmanTracker::msgCallback_PeopleTracker, this);

        markers_pub = nh1.advertise<visualization_msgs::Marker>("/visualization_estimate_marker", 20);//Output of KF
        estimate_pos_pub = nh1.advertise<geometry_msgs::PoseStamped>("estimate_pos", 1);
        estimate_arrow_pub = nh1.advertise<visualization_msgs::Marker>("/visualization_estimate_arrow", 1);
        //text_pub = nh1.advertise<visualization_msgs::Marker>("tracking_text", 20);
        //estimate_pos_pub = nh1.advertise<geometry_msgs::PoseStamped>("human_pose", 20);

        /*
        std::cout << "A: \n" << A << std::endl;
        std::cout << "C: \n" << C << std::endl;
        std::cout << "Q: \n" << Q << std::endl;
        std::cout << "R: \n" << R << std::endl;
        std::cout << "P: \n" << P << std::endl;
        */
    }

    void msgCallback_PeopleTracker(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {

        if(msg->pose.position.z == 1)
        {
        // Best guess of initial states
            //std::cout<<"---Initialization---"<<std::endl;
            //start = now = ros::Time::now();
            x <<msg->pose.position.x, msg->pose.position.y,0.0,0.0,0.0, 0.0;

            std::cout<<"init_measurement: "<<msg->pose.position.x<<", "<<msg->pose.position.y<<std::endl;
            estimate_X.init(t,x);
            std::cout<<"init_estimate        : "<<estimate_X.state()[0]<<", "<<estimate_X.state()[1]<<std::endl;

            //init_f=1;
        }
        else
        {
            //now = ros::Time::now();
            //std::cout<<"---Update---"<<std::endl;
            Eigen::VectorXd z(m);
            z << msg->pose.position.x,msg->pose.position.y;
            //std::cout<<"update_measurement: "<<msg->pose.position.x<<", "<<msg->pose.position.y<<std::endl;
            estimate_X.update(z);
            //std::cout<<"update_estimate        : "<<estimate_X.state()[0]<<", "<<estimate_X.state()[1]<<std::endl;
        }
        double estimate_x = estimate_X.state()[0];
        double estimate_y = estimate_X.state()[1];

        //std::cout <<"x_hat=" << estimate_X.state().transpose()<<std::endl;
        //std::cout <<"y_hat=" << estimate_Y.state().transpose()<<std::endl;

        //std::cout<<""<<std::endl;

        visualization_msgs::Marker m;
        m.header.stamp = ros::Time::now();
        m.header.frame_id = fixed_frame;
        m.scale.x = 0.2;
        m.scale.y = 0.2;
        m.scale.z = 0.05;
        m.color.a = 1.0f;
        m.color.r = 0.0f;
        m.color.g = 1.0f;
        m.color.b = 0.0f;

        m.type = m.SPHERE;
        m.lifetime = ros::Duration(0.5);
        m.pose.position.x = estimate_x;
        m.pose.position.y = estimate_y;

        geometry_msgs::PoseStamped estimate_pos;
        estimate_pos.pose.position.x = estimate_x;
        estimate_pos.pose.position.y = estimate_y;
        estimate_pos.pose.position.z = 0;

        visualization_msgs::Marker estimateposearrow;
        double yaw = atan2(estimate_y,estimate_x);
        //std::cout<<"yaw: "<<yaw<<std::endl;

        estimateposearrow.header.frame_id = fixed_frame;;
        estimateposearrow.header.stamp = ros::Time::now();
        estimateposearrow.ns = "basic_shapes";
        estimateposearrow.type = visualization_msgs::Marker::ARROW;
        estimateposearrow.action = visualization_msgs::Marker::ADD;
        estimateposearrow.pose.position.x = estimate_x;
        estimateposearrow.pose.position.y = estimate_y;
        estimateposearrow.pose.orientation=tf::createQuaternionMsgFromYaw(yaw);

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        estimateposearrow.scale.x = 0.3;
        estimateposearrow.scale.y = 0.1;
        estimateposearrow.scale.z = 0.1;
        // Set the color -- be sure to set alpha to something non-zero!
        estimateposearrow.color.r = 0.0f;
        estimateposearrow.color.g = 1.0f;
        estimateposearrow.color.b = 0.0f;
        estimateposearrow.color.a = 1.0f;

        estimateposearrow.lifetime = ros::Duration();

        estimate_arrow_pub.publish(estimateposearrow);
        markers_pub.publish(m);
        estimate_pos_pub.publish(estimate_pos);

        //estimate_pos.header.stamp = ros::Time::now();
        //estimate_pos.header.frame_id = fixed_frame;
        //estimate_pos.name = "tracking";

        //std::cout<<estimate_X.cov()<<std::endl;
        /*estimate_pos.covariance[0] = (estimate_X.cov()(0,0)+estimate_Y.cov()(0,0))/2;
        estimate_pos.covariance[1] = 0.0;
        estimate_pos.covariance[2] = 0.0;
        estimate_pos.covariance[3] = 0.0;
        estimate_pos.covariance[4] = (estimate_X.cov()(1,1)+estimate_Y.cov()(1,1))/2;
        estimate_pos.covariance[5] = 0.0;
        estimate_pos.covariance[6] = 0.0;
        estimate_pos.covariance[7] = 0.0;
        estimate_pos.covariance[8] = (estimate_X.cov()(2,2)+estimate_Y.cov()(2,2))/2;
        */
        /*
        if(init_f==1)
        {
            markers_pub.publish(m);
            estimate_pos_pub.publish(estimate_pos);
        }
        */

    //r.sleep();
    }
};

// 購読者ノードのメイン関数
int main(int argc, char **argv)
{
    // ノード名の初期化
    ros::init(argc, argv, "person_tracking_kalman");
    // ROSシステムとの通信のためのノードのハンドルを宣言
    ros::NodeHandle nh;
    KalmanTracker kt(nh);
    ros::spin();
    return 0;
}


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include parser_def.hpp directly!"
#endif

#pragma once

#include "ast.hpp"
#include "ast_adapted.hpp"
#include "../math.hpp"
#include "parser.hpp"

#include <boost/math/constants/constants.hpp>
#include <boost/spirit/home/x3.hpp>

#include <cmath>
#include <iostream>
#include <limits>
#include <string>

namespace matheval {

namespace x3 = boost::spirit::x3;

namespace parser {

// LOOKUP

struct constant_ : x3::symbols<double> {
    constant_() {
        // clang-format off
        add
            ("e"      , boost::math::constants::e<double>())
            ("epsilon", std::numeric_limits<double>::epsilon())
            ("phi"    , boost::math::constants::phi<double>())
            ("pi"     , boost::math::constants::pi<double>())
            ;
        // clang-format on
    }
} constant;

struct ufunc_ : x3::symbols<double (*)(double)> {
    ufunc_() {
        // clang-format off
        add
            ("abs"   , static_cast<double (*)(double)>(&std::abs))
            ("acos"  , static_cast<double (*)(double)>(&math::acos))
            ("acosh" , static_cast<double (*)(double)>(&math::acosh))
            ("asin"  , static_cast<double (*)(double)>(&math::asin))
            ("asinh" , static_cast<double (*)(double)>(&std::asinh))
            ("atan"  , static_cast<double (*)(double)>(&std::atan))
            ("atanh" , static_cast<double (*)(double)>(&math::atanh))
            ("cbrt"  , static_cast<double (*)(double)>(&std::cbrt))
            ("ceil"  , static_cast<double (*)(double)>(&std::ceil))
            ("cos"   , static_cast<double (*)(double)>(&math::cos))
            ("cosh"  , static_cast<double (*)(double)>(&std::cosh))
            ("deg"   , static_cast<double (*)(double)>(&math::deg))
            ("erf"   , static_cast<double (*)(double)>(&std::erf))
            ("erfc"  , static_cast<double (*)(double)>(&std::erfc))
            ("exp"   , static_cast<double (*)(double)>(&std::exp))
            ("exp2"  , static_cast<double (*)(double)>(&std::exp2))
            ("floor" , static_cast<double (*)(double)>(&std::floor))
            ("isinf" , static_cast<double (*)(double)>(&math::isinf))
            ("isnan" , static_cast<double (*)(double)>(&math::isnan))
            ("log"   , static_cast<double (*)(double)>(&math::log))
            ("log2"  , static_cast<double (*)(double)>(&math::log2))
            ("log10" , static_cast<double (*)(double)>(&math::log10))
            ("rad"   , static_cast<double (*)(double)>(&math::rad))
            ("round" , static_cast<double (*)(double)>(&std::round))
            ("sgn"   , static_cast<double (*)(double)>(&math::sgn))
            ("sin"   , static_cast<double (*)(double)>(&math::sin))
            ("sinh"  , static_cast<double (*)(double)>(&std::sinh))
            ("sqrt"  , static_cast<double (*)(double)>(&math::sqrt))
            ("tan"   , static_cast<double (*)(double)>(&math::tan))
            ("tanh"  , static_cast<double (*)(double)>(&std::tanh))
            ("tgamma", static_cast<double (*)(double)>(&math::tgamma))
            ;
        // clang-format on
    }
} ufunc;

struct tfunc_ : x3::symbols<double (*)(double, double, double)> {
    tfunc_() {
        // clang-format off
        add
	  ("ifelse", static_cast<double (*)(double, double, double)>(&math::ifelse))
            ;
        // clang-format on
    }
} tfunc;

struct bfunc_ : x3::symbols<double (*)(double, double)> {
    bfunc_() {
        // clang-format off
        add
            ("atan2", static_cast<double (*)(double, double)>(&std::atan2))
            ("max"  , static_cast<double (*)(double, double)>(&std::fmax))
            ("min"  , static_cast<double (*)(double, double)>(&std::fmin))
            ("pow"  , static_cast<double (*)(double, double)>(&math::pow))
            ;
        // clang-format on
    }
} bfunc;

struct unary_op_ : x3::symbols<double (*)(double)> {
    unary_op_() {
        // clang-format off
        add
            ("+", static_cast<double (*)(double)>(&math::plus))
            ("-", static_cast<double (*)(double)>(&math::minus))
            ("!", static_cast<double (*)(double)>(&math::unary_not))
            ;
        // clang-format on
    }
} unary_op;

struct additive_op_ : x3::symbols<double (*)(double, double)> {
    additive_op_() {
        // clang-format off
        add
            ("+", static_cast<double (*)(double, double)>(&math::plus))
            ("-", static_cast<double (*)(double, double)>(&math::minus))
            ;
        // clang-format on
    }
} additive_op;

struct multiplicative_op_ : x3::symbols<double (*)(double, double)> {
    multiplicative_op_() {
        // clang-format off
        add
            ("*", static_cast<double (*)(double, double)>(&math::multiplies))
            ("/", static_cast<double (*)(double, double)>(&math::divides))
            ("%", static_cast<double (*)(double, double)>(&math::fmod))
            ;
        // clang-format on
    }
} multiplicative_op;

struct logical_op_ : x3::symbols<double (*)(double, double)> {
    logical_op_() {
        // clang-format off
        add
            ("&&", static_cast<double (*)(double, double)>(&math::logical_and))
            ("||", static_cast<double (*)(double, double)>(&math::logical_or))
            ;
        // clang-format on
    }
} logical_op;

struct relational_op_ : x3::symbols<double (*)(double, double)> {
    relational_op_() {
        // clang-format off
        add
            ("<" , static_cast<double (*)(double, double)>(&math::less))
            ("<=", static_cast<double (*)(double, double)>(&math::less_equals))
            (">" , static_cast<double (*)(double, double)>(&math::greater))
            (">=", static_cast<double (*)(double, double)>(&math::greater_equals))
            ;
        // clang-format on
    }
} relational_op;

struct equality_op_ : x3::symbols<double (*)(double, double)> {
    equality_op_() {
        // clang-format off
        add
            ("==", static_cast<double (*)(double, double)>(&math::equals))
            ("!=", static_cast<double (*)(double, double)>(&math::not_equals))
            ;
        // clang-format on
    }
} equality_op;

struct power_ : x3::symbols<double (*)(double, double)> {
    power_() {
        // clang-format off
        add
            ("**", static_cast<double (*)(double, double)>(&math::pow))
            ;
        // clang-format on
    }
} power;

// ADL markers

struct expression_class;
struct logical_class;
struct equality_class;
struct relational_class;
struct additive_class;
struct multiplicative_class;
struct factor_class;
struct primary_class;
struct unary_class;
struct binary_class;
struct ternary_class;
struct variable_class;

// clang-format off

// Rule declarations

auto const expression     = x3::rule<expression_class    , ast::expression>{"expression"};
auto const logical        = x3::rule<logical_class       , ast::expression>{"logical"};
auto const equality       = x3::rule<equality_class      , ast::expression>{"equality"};
auto const relational     = x3::rule<relational_class    , ast::expression>{"relational"};
auto const additive       = x3::rule<additive_class      , ast::expression>{"additive"};
auto const multiplicative = x3::rule<multiplicative_class, ast::expression>{"multiplicative"};
auto const factor         = x3::rule<factor_class        , ast::expression>{"factor"};
auto const primary        = x3::rule<primary_class       , ast::operand   >{"primary"};
auto const unary          = x3::rule<unary_class         , ast::unary_op  >{"unary"};
auto const binary         = x3::rule<binary_class        , ast::binary_op >{"binary"};
auto const ternary        = x3::rule<ternary_class       , ast::ternary_op>{"ternary"};
auto const variable       = x3::rule<variable_class      , std::string    >{"variable"};

// Rule defintions

auto const expression_def =
    logical
    ;

auto const logical_def =
    equality >> *(logical_op > equality)
    ;

auto const equality_def =
    relational >> *(equality_op > relational)
    ;

auto const relational_def =
    additive >> *(relational_op > additive)
    ;

auto const additive_def =
    multiplicative >> *(additive_op > multiplicative)
    ;

auto const multiplicative_def =
    factor >> *(multiplicative_op > factor)
    ;

auto const factor_def =
    primary >> *( power > factor )
    ;

auto const unary_def =
    ufunc > '(' > expression > ')'
    ;

auto const binary_def =
    bfunc > '(' > expression > ',' > expression > ')'
    ;

auto const ternary_def =
    tfunc > '(' > expression > ',' > expression > ',' > expression > ')'
    ;

auto const variable_def =
    x3::raw[x3::lexeme[x3::alpha >> *(x3::alnum | '_')]]
    ;

auto const primary_def =
      x3::double_
    | ('(' > expression > ')')
    | (unary_op > primary)
    | ternary
    | binary
    | unary
    | constant
    | variable
    ;

BOOST_SPIRIT_DEFINE(
    expression,
    logical,
    equality,
    relational,
    additive,
    multiplicative,
    factor,
    primary,
    unary,
    binary,
    ternary,
    variable
)

// clang-format on

struct expression_class {
    template <typename Iterator, typename Exception, typename Context>
    x3::error_handler_result on_error(Iterator &, Iterator const &last,
                                      Exception const &x, Context const &) {
        std::cout << "Expected " << x.which() << " at \""
                  << std::string{x.where(), last} << "\"" << std::endl;
        return x3::error_handler_result::fail;
    }
};

} // namespace parser

parser::expression_type grammar() { return parser::expression; }

} // namespace matheval


#include <random>
#include <Eigen/Dense>
#include "HODLR_Tree.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <squaredeMat.hpp> //squared exponential kernel
#include <squaredeP1Mat.hpp>

using std::normal_distribution;
namespace py = pybind11;

// AZ: draw random samples from fstar
Eigen::MatrixXd predict(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::MatrixXd Xtest, Eigen::VectorXd sig_samps, Eigen::VectorXd rho_samps, Eigen::VectorXd tau_samps, double multiplier, int M, double tol, int nsamps) {
    /*
    Temporarily assumes regression = True
    */

  //////////// PASS THE GENERATOR //////////

    std::cout << "hullo there" << std::endl;

    // Create the standard normal generator
    normal_distribution<double> norm(0, 1);
    std::mt19937 rng;
    auto r_std_normal = bind(norm, rng);

    ////Number of observations in train and test
    int Ntest =  Xtest.rows();
    int N =  X.rows();
    int D =  X.cols();

    int n_levels = log(N / M) / log(2);
    bool is_sym = true;
    bool is_pd  = true;

    double tau;
    double rho;
    double sig;

    // Sigma square and SSR
    double sigsq; // = pow(sig, 2.0);
    double tmpSSR;

    // Allocate fstarsamp
    Eigen::MatrixXd fstarsamp(nsamps, Ntest);
    Eigen::VectorXd KobsNew(N);

    for (int s = 0; s < nsamps; s++) {

      sig = sig_samps(s);
      tau = tau_samps(s);
      rho = rho_samps(s);

      if (s % 100 == 0) {
        // printf("PREDICT: Sig, Tau, Rho: s = %d, %.2f \t%.2f \t%.2f\n", s, sig, tau, rho);
      }

      sigsq = pow(sig, 2.0);

      SQRExponentialP1_Kernel* L  = new SQRExponentialP1_Kernel(X, N, sig, rho, tau);
      HODLR_Tree* T = new HODLR_Tree(n_levels, tol, L); // With noise (i.e. Sigma + I/tau)
      T->assembleTree(is_sym, is_pd);
      T->factorize();

      // VO: Should be able to do this through matrix operations
      // AZ: cycle through the observations in the test set
      for (int i = 0; i < Ntest; i++) {
        ////// Sample a draw of the GP function f*|f,sig,rho,tau,x*.
        ////// Assume a squared exponential Gaussian Process with params sigf and rho,
        ////// based on observed function f at new test points x*.
        Eigen::RowVectorXd Xtest_i = Xtest.row(i);
        // Get covariance between X and Xtest
        for (int j = 0; j < N; j++) {
          Eigen::RowVectorXd tmp = X.row(j) - Xtest_i;
          tmpSSR = 0.0;
          for (int d = 0; d < D; d++) {
            tmpSSR = tmpSSR + pow(tmp(d), 2.0);
          }
          KobsNew(j) = sigsq * exp(- tmpSSR * rho);
        }
        // Get variance at Xtest
        double kNewNew = sigsq + 1e-8;
        // Get posterior mean and variance of f* at point xtest(i)
        double sdstar = pow(kNewNew - (multiplier * KobsNew.transpose() * T->solve(tau * KobsNew))(0,0), 0.5);
        double mustar = (multiplier * KobsNew.transpose() * T->solve(tau * Y))(0,0);

        auto normal_samp = r_std_normal();

        fstarsamp(s, i) =  sdstar * normal_samp + mustar;
        //std::normal_distribution<double> distribution(mustar, sdstar);
        //fstarsamp(i) = distribution(generator);
      }
    }
    return fstarsamp;
}

void predict_module(py::module &m) {
    m.def("predict_f", &predict, "predicted samples of f at new X");
}


#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/src/Core/ArithmeticSequence.h>

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

#include <string.h>
#include <iostream>

#include "Data.h"
#include "ConvLayer.h"
#include "FilterGenerator.h"
#include "Nonlinearity.h"
#include "MaxPool.h"
#include "FullyConnected.h"
#include "SoftMax.h"

void train(std::vector<Eigen::MatrixXd>& TRAINING_DATA, int learning_rate)
{
	// create filters for convolutional layer
	int num_filters = 2;
	int filter_dim_x = 10;
	int filter_dim_y = 10;

	FilterGenerator filters(num_filters,filter_dim_x,filter_dim_y);
	filters.create_filters();
	std::vector<Eigen::MatrixXd> conv_filters;
	conv_filters = filters.output_filters;

	// get input image dimensions
	int input_dim_x = TRAINING_DATA[0].cols();
	int input_dim_y = TRAINING_DATA[0].rows();
	int input_dim_c = 1;
	int stride_x = 1;
	int stride_y = 1;

	ConvLayer convlayer_1(input_dim_x, input_dim_y, input_dim_c, filter_dim_x, filter_dim_y, stride_x, stride_y, num_filters);

	// max pool dimensions setup
	int conv_output_dim_x = convlayer_1.GetOutputDimX();
	int conv_output_dim_y = convlayer_1.GetOutputDimY();
	int m_filter_dim_x = 2;
	int m_filter_dim_y = 2;
	int m_stride_x = 1;
	int m_stride_y = 1;
	MaxPool maxpool_1(conv_output_dim_x, conv_output_dim_y, input_dim_c, m_filter_dim_x, m_filter_dim_y, m_stride_x, m_stride_y);

	// nonlinearity
	Nonlinearity sigmoid_1;

	// fully connected layer
	FullyConnected fullyconnected_1;
	int num_weight_matrices = num_filters;
	int fc_filter_dim_x = 2;
	int fc_filter_dim_y = maxpool_1.GetOutputDimX() * maxpool_1.GetOutputDimY();

	FilterGenerator fc_weights_init(num_filters, fc_filter_dim_x, fc_filter_dim_y);
	fc_weights_init.create_filters();
	std::vector<Eigen::MatrixXd> fc_weights;
	fc_weights = fc_weights_init.output_filters;

	// soft max layer
	SoftMax softmax;

	std::vector<int> actual_class;
	for (int i = 0; i < 72; i++)
	{
		actual_class.push_back(1);
	}

	int epoch_total = 1;
	int SIZE_OF_DATASET = TRAINING_DATA.size();

	for (int epoch = 0; epoch < epoch_total; epoch++)
	{
		for (int i = 0; i < TRAINING_DATA.size(); i++)
		{
			printf("\nFOR IMAGE %i: \n", i + 1);

			// FORWARD PROP

			convlayer_1.Forward(TRAINING_DATA[i], conv_filters);
			maxpool_1.Forward(convlayer_1.output);
			sigmoid_1.sigmoid(maxpool_1.output);
			fullyconnected_1.Forward(sigmoid_1.sigmoid_output, fc_weights);
			softmax.SoftMaxLoss(fullyconnected_1.output, actual_class[i]);

			// BACKPROP
			softmax.Backprop(fullyconnected_1.output, actual_class[i]);
			fullyconnected_1.Backprop(softmax.delta_softmax, fc_weights, sigmoid_1.sigmoid_output);
			sigmoid_1.Backprop_sigmoid(fullyconnected_1.delta_FC, maxpool_1.output);
			maxpool_1.Backprop(sigmoid_1.delta_sigmas, convlayer_1.output);
			convlayer_1.Backprop(maxpool_1.delta_matrices, TRAINING_DATA[i]);

			for (int i = 0; i < num_filters; i++)
			{
				fc_weights[i] = fc_weights[i] - learning_rate * fullyconnected_1.delta_weights[i];
				conv_filters[i] = conv_filters[i] - learning_rate * convlayer_1.delta_filters[i];
			}

		}
		softmax.SoftMaxLossTotal();																	// just calculates the loss
		std::cout << "\nThe loss for the dataset is: " << softmax.loss_for_dataset << std::endl;

		/*
		if (epoch_total % epoch == 10)
		{
			printf("EPOCH:\t %i \tLOSS: %i", epoch, softmax.loss_for_dataset);
		}
		*/
		softmax.accumulated_loss_values.clear();										// clear values for next epoch
		softmax.SoftMaxLossTotal();
		softmax.accumulated_loss_values = { 1, 2, 3 };
		std::cout << "\nThe loss for the dataset is: " << softmax.loss_for_dataset << std::endl;

	}

}

int main(int argv, char** argc)
{
	// TEST MATRICES FOR FEEDFORWARD:

	Eigen::MatrixXd matrix_1;
	matrix_1.resize(6, 6);
	matrix_1 << 0.1, 0.2, 0.9, 0.4, 0.3, 0.4,
				0.8, 0.1, 0.4, 0.9, 0.6, 0.7,
				0.1, 0.4, 0.2, 0.6, 0.7, 0.8,
				0.3, 0.2, 0.2, 0.1, 0.3, 0.5,
				0.7, 0.9, 0.2, 0.4, 0.7, 0.1,
				0.8, 0.6, 0.5, 0.2, 0.3, 0.1;
	Eigen::MatrixXd matrix_2;
	matrix_2.resize(6, 6);
	matrix_2 << 0.1, 0.7, 0.9, 0.7, 0.3, 0.1,
				0.7, 0.9, 0.2, 0.4, 0.7, 0.1,
				0.1, 0.4, 0.2, 0.3, 0.7, 0.1,
				0.1, 0.1, 0.2, 0.2, 0.7, 0.1,
				0.8, 0.2, 0.4, 0.1, 0.1, 0.1,
				0.1, 0.8, 0.2, 0.1, 0.1, 0.1;
	Eigen::MatrixXd matrix_3;
	matrix_3.resize(6, 6);
	matrix_3 << 0.1, 0.2, 0.9, 0.4, 0.3, 0.1,
				0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
				0.1, 0.4, 0.3, 0.6, 0.7, 0.8,
				0.1, 0.3, 0.2, 0.6, 0.7, 0.3,
				0.8, 0.2, 0.4, 0.7, 0.3, 0.6,
				0.5, 0.2, 0.9, 0.4, 0.3, 0.4;

	std::vector<Eigen::MatrixXd> input_matrices;
	input_matrices.push_back(matrix_1);
	input_matrices.push_back(matrix_2);
	input_matrices.push_back(matrix_3);

	std::cout << "THE RESULTS OF TRAIN: " << std::endl;

	// CALLING DATA HANDLER:
	Data train_images_yes;

	train_images_yes.load_images("Cellphones/training/training/test/cracked/");
	train_images_yes.set_label();
	train_images_yes.convert_images();

	Data train_images_no;
	train_images_no.load_images("Cellphones/training/training/test/intact");
	train_images_no.set_label();

	train(train_images_yes.image_matrices, 0.5);

	return 0;
}


/*
Copyright 2014, 2015 Rogier van Dalen.

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.
*/

/** \file
Define a Cartesian product of magmas.
*/

#ifndef MATH_PRODUCT_HPP_INCLUDED
#define MATH_PRODUCT_HPP_INCLUDED

#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>

#include <boost/utility/enable_if.hpp>
#include <boost/functional/hash_fwd.hpp>

#include "meta/vector.hpp"
#include "meta/all_of_c.hpp"

#include "utility/returns.hpp"
#include "utility/type_sequence_traits.hpp"

#include "rime/core.hpp"

#include "range/core.hpp"
#include "range/tuple.hpp"
#include "range/call_unpack.hpp"
#include "range/equal.hpp"
#include "range/less_lexicographical.hpp"
#include "range/transform.hpp"
#include "range/any_of.hpp"
#include "range/all_of.hpp"
#include "range/hash_range.hpp"

#include "magma.hpp"
#include "detail/tuple_helper.hpp"

namespace math {

template <class Operation = void> struct with_inverse;

/**
Magma that is the Cartesian product of a number of magmas.

If \a Inverse is with_inverse with an operation, then the inverse of this
operation is implemented.
In that case, the inverse cannot be defined on that component, and therefore
the inverse cannot be defined for the whole product.
If any component of this product is an annihilator for that operation, the whole
product must therefore be an annihilator.
Therefore, with <c>with_inverse \<Operation></c>, is_annihilator returns \c true
if any component returns \c true for is_annihilator.
Since any annihilator should be equal, \c equal returns \c true for two products
that have any annihilator for that operation.

To produce (as opposed to detect) an annihilator, each of the components must
have an annihilator for that operation.

\c order is not defined for the product: even if an operation implements a
strict weak ordering on each component, that does not implement any ordering on
the product.

Operations on this product are associative, commutative, idempotent,
distributive, and the product is a semiring, if and only if each of the
components is.

Products supports Boost.Hash, if \c boost/functional/hash.hpp is included.
If the hash values of the components of two products are the same, then the hash
value of the two products will be the same.

\todo This is a heterogeneous tuple. Is that good enough? I think so.
OpenFst has Power<W,n> and SparsePower<W>, which hold n or a variable number
of components.
This may be useful for unknown file formats, but we'd need an any_magma, and
the ability to live with a performance hit.

\tparam Components
    The list of components, given as math::over.
\tparam Inverse
    (optional)
    An inverse operation that is allowed, given as math::with_inverse.
*/
template <class Components, class Inverse = with_inverse<>> class product;

template <class ComponentTags, class Inverse> struct product_tag;

template <class ... Components, class Inverses>
    struct decayed_magma_tag <product <over <Components ...>, Inverses>>
{
    typedef product_tag <
            over <typename decayed_magma_tag <Components>::type ...>, Inverses>
        type;
};

template <class ... Components, class Inverse>
    class product <over <Components ...>, Inverse>
{
public:
    typedef meta::vector <Components ...> component_types;
    typedef range::tuple <Components ...> components_type;
    typedef Inverse inverse_specification;

    static_assert (meta::all_of_c <is_magma <Components>::value ...>::value,
        "Not all components passed to math::product are magmas.");

private:
    components_type components_;

    // Private type so that it cannot be constructed exept inside this class.
    struct dummy_type {};

public:
    // All elements explicitly convertible: implicit constructor.
    template <class ... Arguments, class Enable = typename boost::enable_if <
        utility::are_constructible <
            meta::vector <Components ...>, meta::vector <Arguments ...>>>::type>
    explicit product (Arguments && ... arguments)
    : components_ (std::forward <Arguments> (arguments) ...) {}

    product (product const &) = default;
    product (product &&) = default;

    /**
    Construct from a product with different component types, all of which are
    implicitly convertible to the component types of this.
    This constructor is implicit.
    \param other The product to copy.
    */
    template <class ... OtherComponents, class Enable = typename
        boost::enable_if <utility::are_convertible <
            meta::vector <OtherComponents const & ...>,
            meta::vector <Components ...>>
        >::type>
    product (product <over <OtherComponents ...>, Inverse> const & other)
    : components_ (other.components()) {}

    /**
    Construct from a product with different component types, at least one of
    which is explicitly convertible and not implicitly convertible.
    This constructor is explicit.
    \param other The product to copy.
    \internal \param dummy
        A dummy argument to distinguish this constructor from the implicit one.
    */
    template <class ... OtherComponents>
    explicit product (
        product <over <OtherComponents ...>, Inverse> const & other, typename
        boost::enable_if <
            tuple_helper::components_constructible_only <
                meta::vector <Components ...>,
                meta::vector <OtherComponents const & ...>>,
            dummy_type>::type = dummy_type())
    : components_ (other.components()) {}

    product & operator = (product const &) = default;
    product & operator = (product &&) = default;

    components_type & components() { return components_; }
    components_type const & components() const { return components_; }
};

namespace callable {

    template <class Inverses> struct make_product {
        template <class ... Components>
            product <over <Components ...>, Inverses>
            operator() (Components const & ... components) const
        { return product <over <Components ...>, Inverses> (components ...); }
    };

    template <class Inverses> struct make_product_over {
        template <class Components>
            auto operator() (Components && components) const
        RETURNS (range::call_unpack (
            make_product <Inverses>(), std::forward <Components> (components)));
    };

} // namespace callable

template <class Inverses, class ... Components>
    inline auto make_product (Components const & ... components)
RETURNS (callable::make_product <Inverses>() (components ...));

template <class Inverses, class Components>
    inline auto make_product_over (Components && components)
RETURNS (callable::make_product_over <Inverses>() (
    std::forward <Components> (components)));

namespace product_detail {

    template <class Type> struct is_product_tag : std::false_type {};
    template <class ComponentTags, class Inverses>
        struct is_product_tag <product_tag <ComponentTags, Inverses>>
    : std::true_type {};

} // namespace product_detail

MATH_MAGMA_GENERATE_OPERATORS (product_detail::is_product_tag)

namespace operation {

    /* Queries. */

    template <class Tags, class Inverses>
        struct is_member <product_tag <Tags, Inverses>>
    {
        template <class Product> auto operator() (Product const & product) const
        RETURNS (range::all_of (
            range::transform (product.components(), math::is_member)));
    };

    // is_annihilator.
    /*
    If the operation has an inverse: any component being an annihilator makes
    the whole product an annihilator.
    If not, then the default implementation (compare component-per-component
    with the result of annihilator()) works.
    */
    template <class ComponentTags, class Operation>
        struct is_annihilator <
            product_tag <ComponentTags, with_inverse <Operation>>, Operation>
    {
        template <class Product> auto operator() (Product const & product) const
        RETURNS (range::any_of (range::transform (
            product.components(), callable::is_annihilator <Operation>())));
    };

    // equal.
    // With no inverse: just compare components.
    template <class ComponentTags>
        struct equal <product_tag <ComponentTags, with_inverse<>>>
    : tuple_helper::equal_components <math::callable::equal> {};

    // With inverse: compare annihilators equal, otherwise compare components.
    template <class ComponentTags, class Operation>
        struct equal <product_tag <ComponentTags, with_inverse <Operation>>>
    : tuple_helper::equal_if_annihilator <Operation,
        tuple_helper::equal_components <math::callable::equal>> {};

    // approximately_equal.
    template <class ComponentTags>
        struct approximately_equal <product_tag <ComponentTags, with_inverse<>>>
    : tuple_helper::equal_components <math::callable::approximately_equal> {};

    template <class ComponentTags, class Operation>
        struct approximately_equal <
            product_tag <ComponentTags, with_inverse <Operation>>>
    : tuple_helper::equal_if_annihilator <Operation,
        tuple_helper::equal_components <math::callable::approximately_equal>>
    {};

    // compare.
    template <class ... ComponentTags>
        struct compare <product_tag <over <ComponentTags ...>, with_inverse<>>,
            typename boost::enable_if <meta::all_of_c <
                is_implemented <compare <ComponentTags>>::value ...>>::type>
    : tuple_helper::compare_components <math::callable::compare> {};

    // With inverse: annihilators go at the end.
    template <class ... ComponentTags, class Operation>
        struct compare <product_tag <over <ComponentTags ...>,
            with_inverse <Operation>>,
            typename boost::enable_if <boost::mpl::and_ <
                meta::all_of_c <
                    is_implemented <compare <ComponentTags>>::value ...>,
                // Only instantiate this if the Operation is not void.
                // GCC 4.6 requires this, or it won't realise the the
                // specialisation above is better.
                boost::mpl::not_ <std::is_same <Operation, void>>
            >>::type>
    : tuple_helper::compare_if_annihilator <Operation,
        tuple_helper::compare_components <math::callable::compare>> {};

    namespace tuple_helper {

        template <class ComponentTags, class Inverses>
            struct get_components <product_tag <ComponentTags, Inverses>>
        {
            template <class Product> auto operator() (Product const & p) const
            RETURNS (p.components());
        };

    } // namespace tuple_helper

    /* Produce. */

    // non_member could be implemented if all components implement it.
    // Otherwise, what value to pick for the other components?

    template <class ... Tags, class Inverses, class Operation>
        struct identity <product_tag <over <Tags ...>, Inverses>, Operation>
    : tuple_helper::nullary_operation <callable::make_product <Inverses>,
        meta::vector <identity <Tags, Operation> ...>> {};

    // annihilator: implemented if all components have an annihilator...
    // (otherwise, what value to pick for the other components?)
    template <class ... Tags, class Inverses, class Operation>
        struct annihilator <product_tag <over <Tags ...>, Inverses>, Operation,
        typename boost::enable_if <meta::all_of_c <
            is_implemented <annihilator <Tags, Operation>>::value ...
        >>::type>
    : tuple_helper::nullary_operation <callable::make_product <Inverses>,
        meta::vector <annihilator <Tags, Operation> ...>> {};

    // ... and there is at least one component.
    template <class Inverses, class Operation>
        struct annihilator <product_tag <over<>, Inverses>, Operation>
    : unimplemented {};

    /* Binary operations. */

    template <class ... Tags, class Inverses>
        struct times <product_tag <over <Tags ...>, Inverses>>
    : tuple_helper::binary_operation <callable::make_product <Inverses>,
        meta::vector <times <Tags> ...>> {};

    template <class ... Tags, class Inverses>
        struct plus <product_tag <over <Tags ...>, Inverses>>
    : tuple_helper::binary_operation <callable::make_product <Inverses>,
        meta::vector <plus <Tags> ...>> {};

    // is_semiring iff all components are semirings...
    template <class ... Tags, class Inverses,
            class Direction, class Operation1, class Operation2>
        struct is_semiring <product_tag <over <Tags ...>, Inverses>, Direction,
            Operation1, Operation2>
    : meta::all_of_c <
        is_semiring <Tags, Direction, Operation1, Operation2>::value ...> {};

    // ... except when the product is empty.
    template <class Inverses, class Direction,
        class Operation1, class Operation2>
    struct is_semiring <product_tag <over<>, Inverses>, Direction,
        Operation1, Operation2>
    : rime::false_type {};

    template <class ... Tags, class Inverses,
            class Direction, class Operation1, class Operation2>
        struct is_distributive <product_tag <over <Tags ...>, Inverses>,
            Direction, Operation1, Operation2>
    : meta::all_of_c <
        is_distributive <Tags, Direction, Operation1, Operation2>::value ...>
    {};

    template <class ... Tags,
            class ... Components1, class ... Components2, class Inverses>
        struct unify_type <product_tag <over <Tags ...>, Inverses>,
            product <over <Components1 ...>, Inverses>,
            product <over <Components2 ...>, Inverses>>
    {
        // Unify both underlying types separately.
        typedef product <over <
            typename unify_type <Tags, Components1, Components2>::type ...>,
            Inverses> type;
    };

    // divide: only if the template parameter Inverses is with_inverse <times>.
    template <class ... Tags, class Direction>
        struct divide <product_tag <over <Tags ...>,
            with_inverse <callable::times>>, Direction>
    : tuple_helper::binary_operation <
        callable::make_product <with_inverse <callable::times>>,
        meta::vector <divide <Tags, Direction> ...>> {};

    // minus: only if the template parameter Inverses is with_inverse <plus>.
    template <class ... Tags, class Direction>
        struct minus <product_tag <over <Tags ...>,
            with_inverse <callable::plus>>, Direction>
    : tuple_helper::binary_operation <
        callable::make_product <with_inverse <callable::plus>>,
        meta::vector <minus <Tags, Direction> ...>> {};

    template <class ... Tags, class Direction, class Operation>
        struct invert <product_tag <over <Tags ...>, with_inverse <Operation>>,
            Direction, Operation>
    : tuple_helper::unary_operation <
        callable::make_product <with_inverse <Operation>>,
        meta::vector <invert <Tags, Direction, Operation> ...>> {};

    template <class ... Tags, class Inverse, class Operation>
        struct reverse <product_tag <over <Tags ...>, Inverse>, Operation>
    : tuple_helper::unary_operation <
        callable::make_product <Inverse>,
        meta::vector <reverse <Tags, Operation> ...>> {};

    template <class ... Tags, class Inverses>
        struct print <product_tag <over <Tags ...>, Inverses>>
    : tuple_helper::print_components <meta::vector <Tags ...>> {};

} // namespace operation

// Boost.Hash support.

namespace product_detail {

    // Products with inverses need to treat annihilators specially.
    static std::size_t constexpr annihilator_hash =
        std::size_t (0xcba51c150183b7f1 & std::size_t (-1));

} // namespace product_detail

// Without an inverse: just combine the hash values of the components.
template <class Components>
    inline std::size_t hash_value (
        product <Components, with_inverse<>> const & p)
{ return range::hash_range (p.components()); }

// With an inverse: if p is an annihilator, then return a special hash value.
template <class Components, class Operation>
    inline std::size_t hash_value (
        product <Components, with_inverse <Operation>> const & p)
{
    if (is_annihilator <Operation> (p))
        return product_detail::annihilator_hash;
    return range::hash_range (p.components());
}

} // namespace math

#endif // MATH_PRODUCT_HPP_INCLUDED


/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program 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 2 of the License, or
    (at your option) any later version.

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

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "graphics.hpp"
#include "rectangle_rotator.hpp"
#include <math.h>
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "unit_test.hpp"

#if defined(_MSC_VER)
#include <boost/math/special_functions/round.hpp>
#define bmround	boost::math::round
#else
#define bmround	std::round
#endif

void rotate_rect(GLshort center_x, GLshort center_y, float rotation, GLshort* rect_vertexes){

	point p;

	float rotate_radians = (rotation * M_PI)/180;

	//rect r(rect_vertexes[0],rect_vertexes[1],rect_vertexes[4]-rect_vertexes[0],rect_vertexes[5]-rect_vertexes[1]);

	p = rotate_point_around_origin_with_offset( rect_vertexes[0], rect_vertexes[1], rotate_radians, center_x, center_y );
	rect_vertexes[0] = p.x;
	rect_vertexes[1] = p.y;

	p = rotate_point_around_origin_with_offset( rect_vertexes[2], rect_vertexes[3], rotate_radians, center_x, center_y );
	rect_vertexes[2] = p.x;
	rect_vertexes[3] = p.y;

	p = rotate_point_around_origin_with_offset( rect_vertexes[4], rect_vertexes[5], rotate_radians, center_x, center_y );
	rect_vertexes[4] = p.x;
	rect_vertexes[5] = p.y;

	p = rotate_point_around_origin_with_offset( rect_vertexes[6], rect_vertexes[7], rotate_radians, center_x, center_y );
	rect_vertexes[6] = p.x;
	rect_vertexes[7] = p.y;

}

void rotate_rect(const rect& r, GLfloat angle, GLshort* output){

	point offset;
	offset.x = r.x() + r.w()/2;
	offset.y = r.y() + r.h()/2;

	point p;

	p = rotate_point_around_origin_with_offset( r.x(), r.y(), angle, offset.x, offset.y );
	output[0] = p.x;
	output[1] = p.y;

	p = rotate_point_around_origin_with_offset( r.x2(), r.y(), angle, offset.x, offset.y );
	output[2] = p.x;
	output[3] = p.y;

	p = rotate_point_around_origin_with_offset( r.x2(), r.y2(), angle, offset.x, offset.y );
	output[4] = p.x;
	output[5] = p.y;

	p = rotate_point_around_origin_with_offset( r.x(), r.y2(), angle, offset.x, offset.y );
	output[6] = p.x;
	output[7] = p.y;

}

point rotate_point_around_origin_with_offset(int x1, int y1, float alpha, int u1, int v1){

	point beta = rotate_point_around_origin(x1 - u1, y1 - v1, alpha);

	beta.x += u1;
	beta.y += v1;

	return beta;
}

point rotate_point_around_origin(int x1, int y1, float alpha){

	point beta;

	/*   //we actually don't need the initial theta and radius.  This is why:
	x2 = R * (cos(theta) * cos(alpha) + sin(theta) * sin(alpha))
	y2 = R * (sin(theta) * cos(alpha) + cos(theta) * sin(alpha));
	but
	R * (cos(theta)) = x1
	R * (sin(theta)) = x2
	this collapses the above to:  */

	beta.x = bmround(x1 * cos(alpha)) - bmround(y1 * sin(alpha));
	beta.y = bmround(y1 * cos(alpha)) + bmround(x1 * sin(alpha));

	return beta;
}

/*UNIT_TEST(rotate_test) {
	std::cerr << "rotating_a_point \n";
	std::cerr << rotate_point_around_origin( 1000, 1000, (M_PI/2)).to_string() << "\n";  //Should be -1000,1000
	std::cerr << rotate_point_around_origin_with_offset( 11000, 1000, (M_PI/2), 10000,0).to_string() << "\n"; //Should be 9000,1000

	GLshort myOutputData[8];
	rect r(10, 10, 20, 30);
	rotate_rect(r, (M_PI*2), myOutputData);

	std::cerr << "Outputting point list \n";
	for(int i=0;i<8;++i){
		std::cerr << myOutputData[i] << " ";
		if(i%2){ std::cerr << "\n";}
	}
}*/

BENCHMARK(rect_rotation) {
	rect r(10, 10, 20, 30);
	GLshort output[8];
	BENCHMARK_LOOP {
		rotate_rect(r, 75, output);
	}
}


#include <iostream>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

using namespace std;
using namespace Eigen;

#ifndef SCALAR
#define SCALAR float
#endif

#ifndef SIZE
#define SIZE 8
#endif

typedef SCALAR Scalar;
typedef NumTraits<Scalar>::Real RealScalar;
typedef Matrix<RealScalar,Dynamic,Dynamic> A;
typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
typedef Matrix<Scalar,Dynamic,Dynamic> C;
typedef Matrix<RealScalar,Dynamic,Dynamic> M;

template<typename Transformation, typename Data>
EIGEN_DONT_INLINE void transform(const Transformation& t, Data& data)
{
  EIGEN_ASM_COMMENT("begin");
  data = t * data;
  EIGEN_ASM_COMMENT("end");
}

template<typename Scalar, typename Data>
EIGEN_DONT_INLINE void transform(const Quaternion<Scalar>& t, Data& data)
{
  EIGEN_ASM_COMMENT("begin quat");
  for(int i=0;i<data.cols();++i)
    data.col(i) = t * data.col(i);
  EIGEN_ASM_COMMENT("end quat");
}

template<typename T> struct ToRotationMatrixWrapper
{
  enum {Dim = T::Dim};
  typedef typename T::Scalar Scalar;
  ToRotationMatrixWrapper(const T& o) : object(o) {}
  T object;
};

template<typename QType, typename Data>
EIGEN_DONT_INLINE void transform(const ToRotationMatrixWrapper<QType>& t, Data& data)
{
  EIGEN_ASM_COMMENT("begin quat via mat");
  data = t.object.toRotationMatrix() * data;
  EIGEN_ASM_COMMENT("end quat via mat");
}

template<typename Scalar, int Dim, typename Data>
EIGEN_DONT_INLINE void transform(const Transform<Scalar,Dim,Projective>& t, Data& data)
{
  data = (t * data.colwise().homogeneous()).template block<Dim,Data::ColsAtCompileTime>(0,0);
}

template<typename T> struct get_dim { enum { Dim = T::Dim }; };
template<typename S, int R, int C, int O, int MR, int MC>
struct get_dim<Matrix<S,R,C,O,MR,MC> > { enum { Dim = R }; };

template<typename Transformation, int N>
struct bench_impl
{
  static EIGEN_DONT_INLINE void run(const Transformation& t)
  {
    Matrix<typename Transformation::Scalar,get_dim<Transformation>::Dim,N> data;
    data.setRandom();
    bench_impl<Transformation,N-1>::run(t);
    BenchTimer timer;
    BENCH(timer,10,100000,transform(t,data));
    cout.width(9);
    cout << timer.best() << " ";
  }
};

template<typename Transformation>
struct bench_impl<Transformation,0>
{
  static EIGEN_DONT_INLINE void run(const Transformation&) {}
};

template<typename Transformation>
EIGEN_DONT_INLINE void bench(const std::string& msg, const Transformation& t)
{
  cout << msg << " ";
  bench_impl<Transformation,SIZE>::run(t);
  std::cout << "\n";
}

int main(int argc, char ** argv)
{
  Matrix<Scalar,3,4> mat34; mat34.setRandom();
  Transform<Scalar,3,Isometry> iso3(mat34);
  Transform<Scalar,3,Affine> aff3(mat34);
  Transform<Scalar,3,AffineCompact> caff3(mat34);
  Transform<Scalar,3,Projective> proj3(mat34);
  Quaternion<Scalar> quat;quat.setIdentity();
  ToRotationMatrixWrapper<Quaternion<Scalar> > quatmat(quat);
  Matrix<Scalar,3,3> mat33; mat33.setRandom();

  cout.precision(4);
  std::cout
     << "N          ";
  for(int i=0;i<SIZE;++i)
  {
    cout.width(9);
    cout << i+1 << " ";
  }
  cout << "\n";

  bench("matrix 3x3", mat33);
  bench("quaternion", quat);
  bench("quat-mat  ", quatmat);
  bench("isometry3 ", iso3);
  bench("affine3   ", aff3);
  bench("c affine3 ", caff3);
  bench("proj3     ", proj3);
}


#include "CurveFitting.hpp"
#include <boost/algorithm/string.hpp>
#include <fstream>
#include <iostream>
#include <string>

int main(int argc, char** argv)
{
    try{
        if(argc!=2){ throw std::string("too few args."); }
        std::string filename(argv[1]);
        std::ifstream ifs(filename);
        if(!ifs.is_open()){ throw std::string("cannot open file")+filename; }

        std::vector<double> x;
        std::vector<double> y;
        std::string line;

        for(int i=0; std::getline(ifs,line); i++){
            std::vector<std::string> elements;
            boost::split(elements, line, boost::is_any_of(","));
            if(elements.size()==1){
                x.push_back(i);
                y.push_back(std::stod(elements[0]));
            }else if(elements.size()==2){
                x.push_back(std::stod(elements[0]));
                y.push_back(std::stod(elements[1]));
            }else{
                throw std::string("bad file format.");
            }
        }

        {
            std::cout << "---Line Fitting---" << std::endl;
            LineFitting line_fit(x,y);

            Eigen::VectorXd x_val(line_fit.num_param());
            x_val.setOnes();
            line_fit.solve(x_val);
            std::cout << x_val << std::endl;

            std::vector<double> vec_x(x_val.data(),x_val.data()+line_fit.num_param());
            std::cout << "residual(vec_x)=" << line_fit.residual(vec_x) << std::endl;
        }

        {
            std::cout << "---Polynomial Fitting---" << std::endl;
            PolynomialFitting poly_fit(x,y,3);

            std::vector<double> x_val(poly_fit.num_param(),1.0);
            poly_fit.solve(x_val);
            std::cout << Eigen::Map<Eigen::VectorXd>(x_val.data(),x_val.size()) << std::endl;

            std::cout << "residual(x_val)=" << poly_fit.residual(x_val) << std::endl;
        }

        {
            std::cout << "---Catenary Fitting---" << std::endl;
            CatenaryFitting catenary_fit(x,y);

            std::vector<double> x_val{3.5, 9.5, -20.0};
            catenary_fit.solve(x_val);
            std::cout << Eigen::Map<Eigen::VectorXd>(x_val.data(),x_val.size()) << std::endl;

            std::cout << "residual(x_val)=" << catenary_fit.residual(x_val) << std::endl;
        }
        {
            std::cout << "---Exponential Fitting---" << std::endl;
            ExponentialFitting exponential_fit(x,y);

            Eigen::VectorXd x_val(exponential_fit.num_param());
            x_val << 10.0, 2.0, 0.0, 0.0;
            exponential_fit.solve(x_val);
            std::cout << x_val << std::endl;

            std::vector<double> vec_x(x_val.data(),x_val.data()+exponential_fit.num_param());
            std::cout << "residual(vec_x)=" << exponential_fit.residual(vec_x) << std::endl;
        }
        {
            std::cout << "---InvCycloid Fitting---" << std::endl;
            InvCycloidFitting invCycloid_fit(y,x); /// Inv -> x y exchanging!

            Eigen::VectorXd x_val(invCycloid_fit.num_param());
            x_val << 3.0, 0.0, 0.0;
            invCycloid_fit.solve(x_val);
            std::cout << x_val << std::endl;

            std::vector<double> vec_x(x_val.data(),x_val.data()+invCycloid_fit.num_param());
            std::cout << "residual(vec_x)=" << invCycloid_fit.residual(vec_x) << std::endl;
        }
    }catch(std::string message){
        std::cout << message << std::endl;
        std::cout << "Usage: CurveFitting data_file.csv" << std::endl;
    }

}


/**
 * Exercise 8 : Read in a gph-file, interpretes it as a Steiner-problem and solves it.
 *
 * @author FirstSanny
 */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <utility>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include "Steiner.h"
#include "GraphChecker.h"

// Constants
namespace {

	const char* FILEEND = ".gph";

}

// declaring print
using std::cout;
using std::endl;
using std::flush;
using std::cerr;
using std::string;

// declaring types
namespace po = boost::program_options;

/** Parsing the arguments given via command line */
po::variables_map parseCommandLine(po::options_description desc, int argn,
		char* argv[]) {
	desc.add_options()//
			("help,h", "produce help message")//
			("start_node,sn", po::value<std::vector<int >>(),"node, where to start")//
			("input-file", po::value<string>(), "input file");
	po::positional_options_description p;
	p.add("input-file", 1);
	p.add("start_node", -1);
	po::variables_map vm;
	po::store(
			po::command_line_parser(argn, argv).options(desc).positional(p).run(),
			vm);
	po::notify(vm);
	return vm;
}

/** Reading in a Graphfile, computes the Steiner */
int main(int argn, char *argv[]) {
	boost::timer::auto_cpu_timer t;
	if (argn <= 1) {
		cerr << "ERROR : There was no filename" << endl;
		return 1;
	}

	po::options_description desc("Allowed options");
	po::variables_map vm = parseCommandLine(desc, argn, argv);

	if (vm.count("help")) {
	    cout << desc << "\n";
	    return 1;
	}

	std::ifstream fileStream;
	if(vm.count("input-file") == 0){
		cerr << "No input-file was given!" << endl;
		return 1;
	}

	std::vector<int > startnodes;
	if(vm.count("start_node") == 0){
		cout << "using default startnode 2" << endl;
		startnodes = std::vector<int >();
		startnodes.push_back(2);
	} else {
		startnodes = vm["start_node"].as<std::vector<int >>();
	}

	string filename = vm["input-file"].as<string >();
	if(filename.find(FILEEND) == std::string::npos){
		filename += FILEEND;
	}
	cout << "Going to parse the file " << filename << endl;
	fileStream.open(filename.c_str(), std::ios::in);

	if ( (fileStream.rdstate()) != 0 ){
		std::perror("ERROR : Encoutered Problem opening file");
		return 1;
	}

	string line;

	unsigned int edgeCount;
	unsigned int vertexCount;

	if(std::getline(fileStream, line)){
		sscanf(line.c_str(), "%d %d", &vertexCount, &edgeCount);
		cout << "Vertexcount: " << vertexCount << endl;
		cout << "Edgecount: " << edgeCount << endl;
		line.clear();
		vertexCount++;
	} else {
		cerr << "ERROR : File was empty" << endl;
		return 1;
	}

	Edges* edges = new Edges(edgeCount);
	Weights* weights = new Weights(edgeCount);

	cout << "Reading edges..." << flush;
	int i = 0;
	while (getline(fileStream, line)) {
		int start;
		int end;
		double weight;
		int count = sscanf(line.c_str(), "%d %d %lf", &start, &end, &weight);
		if (count != 3) {
			line.clear();
			continue;
		}
		edges->at(i) = std::make_pair(start, end);
		weights->at(i) = weight;
		i++;
		line.clear();
	}
	cout << "done" << endl << endl;

	Steiner** steiners = new Steiner*[startnodes.size()];
	cout << "Solves Steiner problem for startnodes ";
	for(unsigned int i = 0; i < startnodes.size(); i++){
		cout << startnodes[i];
		if(i != startnodes.size() - 1){
			cout << ", ";
		} else {
			cout << endl;
		}
	}
	#pragma omp parallel for
	for(unsigned int i = 0; i < startnodes.size(); i++){
		steiners[i] = new Steiner();
		steiners[i]->steiner(vertexCount, edges, *weights, startnodes[i]);
		cout << "Objective value of Steiner-tree for startnode " << startnodes[i] << ": " << steiners[i]->getWeight() << endl;
	}

	Steiner* s = steiners[0];
	int node = startnodes[0];
	int weight = s->getWeight();
	cout << "Searching the one with least weight..." << flush;
	for(unsigned int i = 0; i < startnodes.size(); i++){
		if(weight > steiners[i]->getWeight()){
			s = steiners[i];
			node = startnodes[i];
			weight = steiners[i]->getWeight();
		}
	}
	cout << "done" << endl;
	cout << "It's the one with startnode " << node << endl << endl;

	cout << "Checking for cycle..." << flush;
	Edges steinerEdges = s->getEdges();
	GraphChecker* checker = new GraphChecker(steinerEdges, s->getNodes());
	if(checker->hasCycle()){
		cout << "failed" << endl;
		return 1;
	}
	cout << "passed" << endl;

	cout << "Checking if graph is connected..." << flush;
	if(!checker->isConnected()){
		cout << "failed" << endl;
		return 1;
	}
	cout << "passed" << endl << endl;

	cout << "Edges:" << endl;
	for(unsigned int i = 0; i < steinerEdges.size(); i++){
		Edge edge = steinerEdges[i];
		cout << edge.first << " " << edge.second << endl;
	}

	delete edges;
	delete weights;
	delete checker;
	for(unsigned int i = 0; i < startnodes.size(); i++){
		delete steiners[i];
	}
	delete [] steiners;

	return 0;
}


#include <catch/catch2.hpp>

#include <orient/from_quaternion.hpp>

#include <Eigen/Geometry>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h>

TEST_CASE("rotationMatrixFromQuaternion"){
  Eigen::Quaterniond eq;
  SECTION("zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{0.f, u};
  }
  SECTION("almost_zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{1e-10, u};
  }
  SECTION("random"){
    eq = Eigen::Quaterniond::UnitRandom();
  }
  Eigen::Vector4d q;
  q << eq.w(), eq.x(), eq.y(), eq.z();
  Eigen::Matrix3d expected = eq.toRotationMatrix();
  Eigen::Matrix3d actual = orient::rotationMatrixFromQuaternion(q);
  CHECK( actual.isApprox( expected ) );
}

TEST_CASE("rotationMatrixFromQuaternion_derivative")
{
  Eigen::Quaterniond eq;
  SECTION("zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{0.f, u};
  }
  SECTION("almost_zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{1e-10, u};
  }
  SECTION("random"){
    eq = Eigen::Quaterniond::UnitRandom();
  }

  Eigen::Vector4d q;
  q << eq.w(), eq.x(), eq.y(), eq.z();

  auto num = gtsam::numericalDerivative11(orient::rotationMatrixFromQuaternion<double>, q);
  const auto [v, J] = orient::rotationMatrixFromQuaternionWD(q);
  CHECK( v.isApprox(orient::rotationMatrixFromQuaternion(q)) );
  CHECK( J.isApprox( num, 1e-9) );
}

TEST_CASE("angleAxisFromQuaternion")
{
  Eigen::Quaterniond eq;
  SECTION("zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{0.f, u};
  }
  SECTION("almost_zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{1e-11, u};
  }
  SECTION("almost_zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{M_PI/4, u};
  }
  SECTION("random"){
    eq = Eigen::Quaterniond::UnitRandom();
  }
  Eigen::Vector4d q;
  q << eq.w(), eq.x(), eq.y(), eq.z();

  Eigen::AngleAxisd eaa{eq};
  Eigen::Vector3d expected = eaa.angle() * eaa.axis();
  Eigen::Vector3d actual = orient::angleAxisFromQuaternion(q);

  Eigen::Matrix3d eR = gtsam::Rot3::Rodrigues(expected).matrix();
  Eigen::Matrix3d aR = gtsam::Rot3::Rodrigues(actual).matrix();
  CHECK( aR.isApprox(eR, 1e-9) );
}

TEST_CASE("angleAxisFromQuaternion_derivative")
{
  Eigen::Quaterniond eq;
  SECTION("almost_zero_angle"){
    Eigen::Vector3d u = Eigen::Vector3d::Random();
    u *= u.squaredNorm();
    eq = Eigen::AngleAxisd{1e-10, u};
  }
  SECTION("random"){
    eq = Eigen::Quaterniond::UnitRandom();
  }
  Eigen::Vector4d q;
  q << eq.w(), eq.x(), eq.y(), eq.z();

  auto num = gtsam::numericalDerivative11(orient::angleAxisFromQuaternion<double>, q);
  const auto [v, J] = orient::angleAxisFromQuaternionWD(q);
  CHECK( v.isApprox( orient::angleAxisFromQuaternion(q)) );
  CHECK( J.isApprox( num, 1e-9) );
}


#include <mylib.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

namespace mylib
{
    float calculate()
    {
        using Eigen::Vector3f;
        return Vector3f{1,0,0}.cross(Vector3f{0,1,0}).dot(Vector3f{0,0,2});
    }
}


#ifndef GRAPHCLASS
#define GRAPHCLASS

#include "graph_node.hpp"

#include <Eigen/Dense>
#include <memory>
#include <stddef.h>
#include <vector>

class UndirectedGraph {
public:
  // constructors
  UndirectedGraph() = delete;
  UndirectedGraph(const size_t num_nodes, const double lower_weight,
                  const double upper_weight, const double vertex_prob,
                  const int max_trials); // random initialization
  UndirectedGraph(
      const size_t num_nodes, const double lower_weight,
      const double upper_weight, const double vertex_prob,
      const std::string path_to_weights_file); // vertex initialization based on
                                               // given weights file

  // destructors
  ~UndirectedGraph() = default;

  // methods
  double get_shortest_path_costs() const;
  double get_avg_vertex_costs() const;
  std::vector<size_t> get_shortest_path_idxs() const;
  void print_shortest_path_idxs() const;
  void print_weight_matrix() const;
  bool is_connected() const;
  void find_path(const int &start_idx, const int &finish_idx);

private:
  void create_random_graph();
  void create_graph_based_on_file();
  void calculate_avg_vertex_weights();
  bool valid_start_end_nodes(const int &start_node_idx,
                             const int &end_node_idx);

  double lower_vertex_weight_;
  double upper_vertex_weight_;
  double vertex_prob_;
  double avg_vertex_cost_;
  double shortest_path_cost_;
  size_t num_nodes_;
  size_t num_edges_;
  bool dijkstra_run_;
  Eigen::MatrixXd
      vertex_weights_; // every row and column describes a node, the values
                       // within the matrix are the vertex weights
  Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> connection_matrix_;
  std::vector<std::shared_ptr<GraphNode>> graph_nodes_;
  std::vector<std::shared_ptr<GraphNode>> shortest_path_nodes_;
  std::vector<int> shortest_path_idxs_;
};

#endif /* GRAPHCLASS */