#include <dice.hpp>

#include <boost/program_options.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>

#include <iostream>
#include <string>
#include <regex>
#include <vector>
#include <thread>
#include <chrono>

namespace po = boost::program_options;

const std::string BAD_ROLL_MSG = "Please specify number and type of dice. Format: #d#";

bool dieSort(diceutil::Die d1, diceutil::Die d2) { return d1.getVal() < d2.getVal(); }

void parseRoll(std::string rollStr, unsigned int keepHighest, unsigned int keepLowest) {
    std::regex dieRollRegex("[0-9]+d[0-9]+");
    if (std::regex_match(rollStr, dieRollRegex)) {
        // Parse the roll string
        auto dLoc = rollStr.find('d');
        uint nDice = std::stoi( rollStr.substr(0, dLoc) );
        uint nSides = std::stoi( rollStr.substr(dLoc + 1) );
        printf("Rolling %d Dice w/ %d Sides Each...\n\n", nDice, nSides);
        std::this_thread::sleep_for(std::chrono::seconds(1));

        // Vector to hold dice
        std::vector<diceutil::Die> dice;

        for (int i = 0; i < nDice; i++) {
            diceutil::Die d{nSides};
            d.roll();
            dice.push_back(d);
        }

        std::sort(dice.begin(), dice.end(), dieSort);
        int total = 0;
        int numElements = dice.size();

        if (keepHighest) {
            // only print highest n
            std::cout << "KEPT: ";
            for (int i = numElements - 1; i > numElements - 1 - keepHighest; i--) {
                std::cout << dice[i].getVal() << "  ";
                total += dice[i].getVal();
            }
            std::cout << std::endl;

            std::cout << "DISCARDED: ";
            for (int j = numElements - 1 - keepHighest; j >= 0; j--) {
                std::cout << dice[j].getVal() << "  ";
            }
        } else if (keepLowest) {
            // only print lowest n
            std::cout << "KEPT: ";
            for (int i = 0; i < keepLowest; i++) {
                std::cout << dice[i].getVal() << "  ";
                total += dice[i].getVal();
            }
            std::cout << std::endl;

            std::cout << "DISCARDED: ";
            for (int j = keepLowest; j < numElements; j++) {
                std::cout << dice[j].getVal() << "  ";
            }
        } else {
            // print 'em all
            for (diceutil::Die d: dice) {
                std::cout << d.getVal() << "  ";
                total += d.getVal();
            }
        }
        std::cout << std::endl << std::endl << "TOTAL: " << total << std::endl;
        std::cout << std::endl << std::endl;

    } else {
        std::cout << BAD_ROLL_MSG << std::endl;
        exit(1);
    }
}

int main(int argc, char* argv[]) {
    // Set up the command line arguments
    po::options_description desc("Available Options");
    desc.add_options()
        ("help", "Help Message")
        ("highest,h", po::value<unsigned int>(), "only take highest n rolls")
        ("lowest,l", po::value<unsigned int>(), "only take lowest n rolls")
        ("dice", po::value<std::string>(), "dice to roll")
    ;
    po::positional_options_description p;
    p.add("dice", 1);

    po::variables_map vars;
    po::store(po::command_line_parser(argc, argv).
          options(desc).positional(p).run(), vars);
    po::notify(vars);
    ////////////////////////////////////////

    // Grab the values for the high / low args
    const unsigned int keepHighest = (vars.count("highest") ? vars["highest"].as<unsigned int>() : 0);
    const unsigned int keepLowest = (vars.count("lowest") ? vars["lowest"].as<unsigned int>() : 0);

    // If both provided, notify and exit
    if (keepLowest && keepHighest) {
        std::cout << "Please only provide either -h [ --highest ] OR -l [ --lowest ], not both" << std::endl;
        return 1;
    }

    if (vars.count("dice")) {
        // Make sure the dice arg is actually supplied
        const std::string diceRollStr(vars["dice"].as<std::string>());

        parseRoll(diceRollStr, keepHighest, keepLowest);

        return 0;
    } else {
        std::cout << BAD_ROLL_MSG << std::endl;
        std::cout << desc << std::endl;
        return 1;
    }
}


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#ifdef STAN_OPENCL
#include <stan/math/opencl/opencl_context.hpp>
#include <stan/math/opencl/multiply.hpp>
#include <stan/math/opencl/tri_inverse.hpp>
#include <stan/math/opencl/transpose.hpp>
#include <stan/math/opencl/copy.hpp>
#endif
namespace stan {
namespace math {

/**
 * Returns the solution of the system Ax=b when A is triangular.
 * @tparam TriView Specifies whether A is upper (Eigen::Upper)
 * or lower triangular (Eigen::Lower).
 * @tparam T1 type of elements in A
 * @tparam T2 type of elements in b
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @tparam R2 number of rows in b
 * @tparam C2 number of columns in b
 * @param A Triangular matrix.
 * @param b Right hand side matrix or vector.
 * @return x = A^-1 b, solution of the linear system.
 * @throws std::domain_error if A is not square or the rows of b don't
 * match the size of A.
 */
template <int TriView, typename T1, typename T2, int R1, int C1, int R2, int C2>
inline Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                     R1, C2>
mdivide_left_tri(const Eigen::Matrix<T1, R1, C1> &A,
                 const Eigen::Matrix<T2, R2, C2> &b) {
  check_square("mdivide_left_tri", "A", A);
  check_multiplicable("mdivide_left_tri", "A", A, "b", b);
  return promote_common<Eigen::Matrix<T1, R1, C1>, Eigen::Matrix<T2, R1, C1> >(
             A)
      .template triangularView<TriView>()
      .solve(
          promote_common<Eigen::Matrix<T1, R2, C2>, Eigen::Matrix<T2, R2, C2> >(
              b));
}

/**
 * Returns the solution of the system Ax=b when A is triangular and b=I.
 * @tparam T type of elements in A
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @param A Triangular matrix.
 * @return x = A^-1 .
 * @throws std::domain_error if A is not square
 */
template <int TriView, typename T, int R1, int C1>
inline Eigen::Matrix<T, R1, C1> mdivide_left_tri(
    const Eigen::Matrix<T, R1, C1> &A) {
  check_square("mdivide_left_tri", "A", A);
  int n = A.rows();
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
  b.setIdentity(n, n);
  A.template triangularView<TriView>().solveInPlace(b);
  return b;
}

/**
 * Returns the solution of the system Ax=b when A is triangular
 * and A and b are matrices of doubles.
 * @tparam TriView Specifies whether A is upper (Eigen::Upper)
 * or lower triangular (Eigen::Lower).
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @tparam R2 number of rows in b
 * @tparam C2 number of columns in b
 * @param A Triangular matrix.
 * @param b Right hand side matrix or vector.
 * @return x = A^-1 b, solution of the linear system.
 * @throws std::domain_error if A is not square or the rows of b don't
 * match the size of A.
 */
template <int TriView, int R1, int C1, int R2, int C2>
inline Eigen::Matrix<double, R1, C2> mdivide_left_tri(
    const Eigen::Matrix<double, R1, C1> &A,
    const Eigen::Matrix<double, R2, C2> &b) {
  check_square("mdivide_left_tri", "A", A);
  check_multiplicable("mdivide_left_tri", "A", A, "b", b);
#ifdef STAN_OPENCL
  if (A.rows()
      >= opencl_context.tuning_opts().tri_inverse_size_worth_transfer) {
    matrix_cl A_cl(A);
    matrix_cl b_cl(b);
    matrix_cl A_inv_cl(A.rows(), A.cols());
    if (TriView == Eigen::Lower) {
      A_inv_cl = tri_inverse<TriangularViewCL::Lower>(A_cl);
    } else {
      A_inv_cl = tri_inverse<TriangularViewCL::Upper>(A_cl);
    }
    matrix_cl C_cl = A_inv_cl * b_cl;
    return from_matrix_cl(C_cl);
  } else {
#endif
    return A.template triangularView<TriView>().solve(b);
#ifdef STAN_OPENCL
  }
#endif
}

/**
 * Returns the solution of the system Ax=b when A is triangular, b=I and
 * both are matrices of doubles.
 * @tparam TriView Specifies whether A is upper (Eigen::Upper)
 * or lower triangular (Eigen::Lower).
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @param A Triangular matrix.
 * @return x = A^-1 .
 * @throws std::domain_error if A is not square
 */
template <int TriView, int R1, int C1>
inline Eigen::Matrix<double, R1, C1> mdivide_left_tri(
    const Eigen::Matrix<double, R1, C1> &A) {
  check_square("mdivide_left_tri", "A", A);
  const int n = A.rows();
#ifdef STAN_OPENCL
  if (A.rows()
      >= opencl_context.tuning_opts().tri_inverse_size_worth_transfer) {
    matrix_cl A_cl(A);
    if (TriView == Eigen::Lower) {
      A_cl = tri_inverse<TriangularViewCL::Lower>(A_cl);
    } else {
      A_cl = tri_inverse<TriangularViewCL::Upper>(A_cl);
    }
    return from_matrix_cl(A_cl);
  } else {
#endif
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> b;
    b.setIdentity(n, n);
    A.template triangularView<TriView>().solveInPlace(b);
    return b;
#ifdef STAN_OPENCL
  }
#endif
}

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


/*
 * Copyright (c) 2019, The Robot Studio
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @file arm_transform.cpp
 * @author Cyril Jourdan
 * @date Sep 26, 2017
 * @version 0.1.0
 * @brief Implementation file for the arm transform
 *
 * Contact: contact@therobotstudio.com
 * Created on : Jul 17, 2012
 */

#include <ros/ros.h>
#include <string>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
//#include <boost/lexical_cast.hpp>
#include "std_msgs/Int16.h"
#include <std_msgs/Float32.h>

#include <math.h>

using namespace std;

double calculateAlpha(tf::StampedTransform tf_hand, tf::StampedTransform tf_elbow, tf::StampedTransform tf_shoulder)
{
	double rightHand[3] = {tf_hand.getOrigin().x(), tf_hand.getOrigin().y(), tf_hand.getOrigin().z()};
	double rightElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double rightShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};

	//Al-Kashi triangle formula
	double num =  pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)
				+ pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2)
				- pow(rightShoulder[0]-rightHand[0], 2) - pow(rightShoulder[1]-rightHand[1], 2) - pow(rightShoulder[2]-rightHand[2], 2);

	double den = 2 * sqrt(pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)) * sqrt(pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2));

	return acos(num/den);
}

double calculateBeta(tf::StampedTransform tf_elbow, tf::StampedTransform tf_neck, tf::StampedTransform tf_shoulder)
{
	double rightElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double neck[3] = {tf_neck.getOrigin().x(), tf_neck.getOrigin().y(), tf_neck.getOrigin().z()};
	double rightShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};

	//Al-Kashi triangle formula
	double num =  pow(rightElbow[0]-rightShoulder[0], 2) + pow(rightElbow[1]-rightShoulder[1], 2) + pow(rightElbow[2]-rightShoulder[2], 2)
						+ pow(rightShoulder[0]-neck[0], 2) + pow(rightShoulder[1]-neck[1], 2) + pow(rightShoulder[2]-neck[2], 2)
						- pow(neck[0]-rightElbow[0], 2) - pow(neck[1]-rightElbow[1], 2) - pow(neck[2]-rightElbow[2], 2);

	double den = 2 * sqrt(pow(rightElbow[0]-rightShoulder[0], 2) + pow(rightElbow[1]-rightShoulder[1], 2) + pow(rightElbow[2]-rightShoulder[2], 2)) * sqrt(pow(rightShoulder[0]-neck[0], 2) + pow(rightShoulder[1]-neck[1], 2) + pow(rightShoulder[2]-neck[2], 2));

	return acos(num/den);
}

double calculateTheta(tf::StampedTransform tf_elbow, tf::StampedTransform tf_neck, tf::StampedTransform tf_shoulder,  tf::StampedTransform tf_torso)
{
	double rightElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double neck[3] = {tf_neck.getOrigin().x(), tf_neck.getOrigin().y(), tf_neck.getOrigin().z()};
	double rightShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};
	double torso[3] = {tf_torso.getOrigin().x(), tf_torso.getOrigin().y(), tf_torso.getOrigin().z()};

	////Theta is the angle between two planes : TNE and SNE
	double a1 = (rightShoulder[1]-torso[1])*(neck[2]-torso[2]) - (rightShoulder[2]-torso[2])*(neck[1]-torso[1]);
	double b1 = (rightShoulder[2]-torso[2])*(neck[0]-torso[0]) - (rightShoulder[0]-torso[0])*(neck[2]-torso[2]);
	double c1 = (rightShoulder[0]-torso[0])*(neck[1]-torso[1]) - (rightShoulder[1]-torso[1])*(neck[0]-torso[0]);

	double a2 = (rightShoulder[1]-rightElbow[1])*(neck[2]-rightElbow[2]) - (rightShoulder[2]-rightElbow[2])*(neck[1]-rightElbow[1]);
	double b2 = (rightShoulder[2]-rightElbow[2])*(neck[0]-rightElbow[0]) - (rightShoulder[0]-rightElbow[0])*(neck[2]-rightElbow[2]);
	double c2 = (rightShoulder[0]-rightElbow[0])*(neck[1]-rightElbow[1]) - (rightShoulder[1]-rightElbow[1])*(neck[0]-rightElbow[0]);

	double num = a1*a2 + b1*b2 + c1*c2;

	double den = sqrt((a1*a1+b1*b1+c1*c1)*(a2*a2+b2*b2+c2*c2));

	return acos(num/den);
}

double calculatePhi(tf::StampedTransform tf_hand, tf::StampedTransform tf_neck, tf::StampedTransform tf_shoulder,  tf::StampedTransform tf_elbow)
{
	double H[3] = {tf_hand.getOrigin().x(), tf_hand.getOrigin().y(), tf_hand.getOrigin().z()};
	double N[3] = {tf_neck.getOrigin().x(), tf_neck.getOrigin().y(), tf_neck.getOrigin().z()};
	double S[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};
	double E[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};

	//Phi is the angle between two planes : SNE and SHE
	float n1[3] = {(N[1]-S[1])*(E[2]-S[2])-(N[2]-S[2])*(E[1]-S[1]),
					(N[2]-S[2])*(E[0]-S[0])-(N[0]-S[0])*(E[2]-S[2]),
					(N[0]-S[0])*(E[1]-S[1])-(N[1]-S[1])*(E[0]-S[0])}; //normal plane vector : SNE

	float n2[3] = {(H[1]-S[1])*(E[2]-S[2])-(H[2]-S[2])*(E[1]-S[1]),
						(H[2]-S[2])*(E[0]-S[0])-(H[0]-S[0])*(E[2]-S[2]),
						(H[0]-S[0])*(E[1]-S[1])-(H[1]-S[1])*(E[0]-S[0])}; //normal plane vector : SHE

	float a1,b1,c1;
	a1 = n1[0];
	b1 = n1[1];
	c1 = n1[2];

	float a2,b2,c2;
	a2 = n2[0];
	b2 = n2[1];
	c2 = n2[2];

	double angle_phi = acos(fabs((a1*a2 + b1*b2 + c1*c2)/(sqrt((a1*a1+b1*b1+c1*c1)*(a2*a2+b2*b2+c2*c2)))));

	//std::cerr << "angle_phi" << angle_phi << std::endl;

	return angle_phi;

	//right
	/*
	double a1 = (rightShoulder[1]-rightHand[1])*(rightElbow[2]-rightHand[2]) - (rightShoulder[2]-rightHand[2])*(rightElbow[1]-rightHand[1]);
	double b1 = (rightShoulder[2]-rightHand[2])*(rightElbow[0]-rightHand[0]) - (rightShoulder[0]-rightHand[0])*(rightElbow[2]-rightHand[2]);
	double c1 = (rightShoulder[0]-rightHand[0])*(rightElbow[1]-rightHand[1]) - (rightShoulder[1]-rightHand[1])*(rightElbow[0]-rightHand[0]);

	double a2 = (rightShoulder[1]-rightHand[1])*(neck[2]-rightHand[2]) - (rightShoulder[2]-rightHand[2])*(neck[1]-rightHand[1]);
	double b2 = (rightShoulder[2]-rightHand[2])*(neck[0]-rightHand[0]) - (rightShoulder[0]-rightHand[0])*(neck[2]-rightHand[2]);
	double c2 = (rightShoulder[0]-rightHand[0])*(neck[1]-rightHand[1]) - (rightShoulder[1]-rightHand[1])*(neck[0]-rightHand[0]);

	double num = a1*a2 + b1*b2 + c1*c2;

	double den = sqrt((a1*a1+b1*b1+c1*c1)*(a2*a2+b2*b2+c2*c2));

	return acos(num/den);
	*/
}
/*
double calculateLambda(tf::StampedTransform tf_hand, tf::StampedTransform tf_elbow, tf::StampedTransform tf_shoulder)
{
	double leftHand[3] = {tf_hand.getOrigin().x(), tf_hand.getOrigin().y(), tf_hand.getOrigin().z()};
	double leftElbow[3] = {tf_elbow.getOrigin().x(), tf_elbow.getOrigin().y(), tf_elbow.getOrigin().z()};
	double leftShoulder[3] = {tf_shoulder.getOrigin().x(), tf_shoulder.getOrigin().y(), tf_shoulder.getOrigin().z()};

	//Al-Kashi triangle formula
	double num =  pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)
				+ pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2)
				- pow(rightShoulder[0]-rightHand[0], 2) - pow(rightShoulder[1]-rightHand[1], 2) - pow(rightShoulder[2]-rightHand[2], 2);

	double den = 2 * sqrt(pow(rightHand[0]-rightElbow[0], 2) + pow(rightHand[1]-rightElbow[1], 2) + pow(rightHand[2]-rightElbow[2], 2)) * sqrt(pow(rightShoulder[0]-rightElbow[0], 2) + pow(rightShoulder[1]-rightElbow[1], 2) + pow(rightShoulder[2]-rightElbow[2], 2));

	return acos(num/den);
}
*/
//sensor_msgs::JointState prev_joint_state;
/*
string playerNb_str = "1";
string frame_str = "/openni_depth_frame";
*/
string playerNb_str = "0";
string frame_str = "/camera_depth_optical_frame";
/*
void playerNumber_cb(const std_msgs::Int16ConstPtr& number)
{
	playerNb_str = boost::lexical_cast<string>(number->data);

	//ROS_INFO("Player Number is %s", playerNb_str);
	//cout << "number = " << playerNb_str << endl;
	ROS_INFO("number = %d", number->data);

	if(number->data == 0)
	{
		frame_str = "/camera_depth_optical_frame";
	}
	else
	{
		frame_str = "/openni_depth_frame";
	}
}
*/
int main(int argc, char** argv)
{
	ros::init(argc, argv, "arm_transform");
	ros::NodeHandle n;
	ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states_kinect", 100);
	ros::Publisher test_pub = n.advertise<std_msgs::Float32>("/test", 1);

	ros::Rate loop_rate(50);
	//ros::Rate loop_rate(10);

	tf::TransformListener listener_neck, listener_torso, listener_shoulder, listener_elbow, listener_hand;
	tf::StampedTransform transform_neck, transform_torso, transform_shoulder, transform_elbow, transform_hand;

	//left part
	tf::TransformListener listener_leftShoulder, listener_leftElbow, listener_leftHand;
	tf::StampedTransform transform_leftShoulder, transform_leftElbow, transform_leftHand;

	//read the player number : 0 used for the posture generated by IR markers, other numbers used by openni_tracker
	//ros::Subscriber sub_playerNumber = n.subscribe("/playerNumber", 1, &playerNumber_cb);

	bool error = false;

	//tf::TransformBroadcaster transform;
	//transform.sendTransform(tf::StampedTransform(null, ros::Time::now(), ));

	// message declarations
	sensor_msgs::JointState joint_state;
/*
	//init joint state
	//update joint_state
	joint_state.header.stamp = ros::Time::now();
	joint_state.name.resize(6);
	joint_state.position.resize(6);
	joint_state.name[0] ="elbow_alpha_joint";
	joint_state.position[0] = M_PI/2;
	joint_state.name[1] ="shoulder_beta_joint";
	joint_state.position[1] = M_PI/2;
	joint_state.name[2] ="shoulder_theta_joint";
	joint_state.position[2] = M_PI/2;
	joint_state.name[3] ="shoulder_phi_joint";
	joint_state.position[3] = 3*M_PI/4;
	joint_state.name[4] ="elbow_twist_joint";
	joint_state.position[4] = M_PI/2;
	joint_state.name[5] ="wrist_joint";
	joint_state.position[5] = M_PI/2;

	prev_joint_state = joint_state;
*/
	//uint32_t begin, end, loopTime;

	while (ros::ok())
	{/*
		begin = ros::WallTime::now().nsec;
		error = false;
*/

		//grab tf skeleton joints
		try
		{
			listener_neck.lookupTransform(frame_str, "/neck_" + playerNb_str, ros::Time(0), transform_neck);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_torso.lookupTransform(frame_str, "/torso_" + playerNb_str, ros::Time(0), transform_torso);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_shoulder.lookupTransform(frame_str, "/left_shoulder_" + playerNb_str, ros::Time(0), transform_shoulder);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_elbow.lookupTransform(frame_str, "/left_elbow_" + playerNb_str, ros::Time(0), transform_elbow);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_hand.lookupTransform(frame_str, "/left_hand_" + playerNb_str, ros::Time(0), transform_hand);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}
/*
		//get the left arm transform
		try
		{
			listener_leftShoulder.lookupTransform(frame_str, "/right_shoulder_" + playerNb_str, ros::Time(0), transform_leftShoulder);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_leftElbow.lookupTransform(frame_str, "/right_elbow_" + playerNb_str, ros::Time(0), transform_leftElbow);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}

		try
		{
			listener_leftHand.lookupTransform(frame_str, "/right_hand_" + playerNb_str, ros::Time(0), transform_leftHand);
		}
		catch (tf::TransformException ex)
		{
			//ROS_ERROR("%s",ex.what());
			error = true;
		}
*/
		//ROS_INFO("transform_han %f", transform_hand.getOrigin().x());

		//update joint_state
		joint_state.header.stamp = ros::Time::now();
		joint_state.name.resize(4);//6
		joint_state.position.resize(4);//6
		joint_state.name[0] ="elbow_alpha_joint";
		joint_state.position[0] = M_PI - calculateAlpha(transform_hand, transform_elbow, transform_shoulder);
		joint_state.name[1] ="shoulder_beta_joint";
		joint_state.position[1] = M_PI - calculateBeta(transform_elbow, transform_neck, transform_shoulder);
		joint_state.name[2] ="shoulder_theta_joint";
		joint_state.position[2] = calculateTheta(transform_elbow, transform_neck, transform_shoulder, transform_torso) - M_PI/2;
		joint_state.name[3] ="shoulder_phi_joint";
		joint_state.position[3] = calculatePhi(transform_hand, transform_neck, transform_shoulder, transform_elbow);

		/*
		joint_state.name[4] ="elbow_twist_joint";
		joint_state.position[4] = 2*calculatePhi(transform_leftHand, transform_neck, transform_leftShoulder, transform_leftElbow) - 3*M_PI/4;
		joint_state.name[5] ="wrist_joint";
		joint_state.position[5] = M_PI - calculateAlpha(transform_leftHand, transform_leftElbow, transform_leftShoulder);
*/
		//test
		std_msgs::Float32 test;
		test.data = M_PI;//(float)transform_hand.getOrigin().x();

		test_pub.publish(test);

		//send the joint state and transform
		joint_pub.publish(joint_state);
/*
			prev_joint_state = joint_state;
		}
		else
		{
			//std::cerr << "Error" << std::endl;
			joint_pub.publish(prev_joint_state);
		}
*/

		ros::spinOnce(); //reasign a new player if need

		// This will adjust as needed per iteration
		loop_rate.sleep();

		//if(error) ROS_INFO("Error !!!");
/*
		end = ros::WallTime::now().nsec;
		loopTime = end - begin;
		ROS_INFO("loopTime =  %d", loopTime);
	*/
	}

	return 0;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LPMF_HPP

#include <stan/math/prim/mat/err/check_simplex.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/sum.hpp>
#include <stan/math/prim/mat/meta/index_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>
#include <vector>

namespace stan {
  namespace math {

    // Categorical(n|theta)  [0 < n <= N;   0 <= theta[n] <= 1;  SUM theta = 1]
    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(int n,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      static const char* function("categorical_lpmf");

      using boost::math::tools::promote_args;
      using std::log;

      int lb = 1;

      check_bounded(function, "Number of categories", n, lb, theta.size());
      check_simplex(function, "Probabilities parameter", theta);

      if (include_summand<propto, T_prob>::value)
        return log(theta(n - 1));
      return 0.0;
    }

    template <typename T_prob>
    inline
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(const typename
                    math::index_type<Eigen::Matrix<T_prob,
                    Eigen::Dynamic, 1> >::type n,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      return categorical_lpmf<false>(n, theta);
    }

    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(const std::vector<int>& ns,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      static const char* function("categorical_lpmf");

      using boost::math::tools::promote_args;
      using std::log;

      int lb = 1;

      for (size_t i = 0; i < ns.size(); ++i)
        check_bounded(function, "element of outcome array", ns[i],
                      lb, theta.size());

      check_simplex(function, "Probabilities parameter", theta);

      if (!include_summand<propto, T_prob>::value)
        return 0.0;

      if (ns.size() == 0)
        return 0.0;

      Eigen::Matrix<T_prob, Eigen::Dynamic, 1> log_theta(theta.size());
      for (int i = 0; i < theta.size(); ++i)
        log_theta(i) = log(theta(i));

      Eigen::Matrix<typename boost::math::tools::promote_args<T_prob>::type,
                    Eigen::Dynamic, 1> log_theta_ns(ns.size());
      for (size_t i = 0; i < ns.size(); ++i)
        log_theta_ns(i) = log_theta(ns[i] - 1);

      return sum(log_theta_ns);
    }

    template <typename T_prob>
    inline
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_lpmf(const std::vector<int>& ns,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      return categorical_lpmf<false>(ns, theta);
    }

  }
}
#endif


/** nonfinite_num_facet.cpp
 *
 * Copyright (c) 2011 Francois Mauger
 * Copyright (c) 2011 Paul A. Bristow
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt
 * or copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 * This simple program illustrates how to use the
 * `boost/math/nonfinite_num_facets.hpp' material from the original
 * Floating Point  Utilities contribution by Johan Rade.
 * Floating Point Utility library has been accepted into Boost,
 * but the utilities have been/will be incorporated into Boost.Math library.
 *
\file

\brief A fairly simple example of using non_finite_num facet for
C99 standard output of infinity and NaN.

\detail  This program illustrates how to use the
 `boost/math/nonfinite_num_facets.hpp' material from the original
  Floating Point  Utilities contribution by Johan Rade.
  Floating Point Utility library has been accepted into Boost,
  but the utilities have been/will be incorporated into Boost.Math library.

  Based on an example from Francois Mauger.

  Double and float variables are assigned ordinary finite values (pi),
  and nonfinite like infinity and NaN.

  These values are then output and read back in, and then redisplayed.

*/

#ifdef _MSC_VER
#   pragma warning(disable : 4127) // conditional expression is constant.
#endif

#include <iostream>
#include <iomanip>
using std::cout;
using std::endl;

#include <limits> // numeric_limits
using std::numeric_limits;

#include <boost/cstdint.hpp>

#include <boost/math/special_functions/nonfinite_num_facets.hpp>

static const char sep = ','; // Separator of bracketed float and double values.

// Use max_digits10 (or equivalent) to obtain
// all potentially significant decimal digits for the floating-point types.

#ifdef BOOST_NO_CXX11_NUMERIC_LIMITS
  std::streamsize  max_digits10_float = 2 + std::numeric_limits<float>::digits * 30103UL / 100000UL;
  std::streamsize  max_digits10_double = 2 + std::numeric_limits<double>::digits * 30103UL / 100000UL;
#else
  // Can use new C++0X max_digits10 (the maximum potentially significant digits).
  std::streamsize  max_digits10_float = std::numeric_limits<float>::max_digits10;
  std::streamsize  max_digits10_double = std::numeric_limits<double>::max_digits10;
#endif

/* A class with a float and a double */
struct foo
{
  foo () : fvalue (3.1415927F), dvalue (3.1415926535897931)
  {
  }
  // Set both the values to -infinity :
  void minus_infinity ()
  {
    fvalue = -std::numeric_limits<float>::infinity ();
    dvalue = -std::numeric_limits<double>::infinity ();
    return;
  }
  // Set the values to +infinity :
  void plus_infinity ()
  {
    fvalue = +std::numeric_limits<float>::infinity ();
    dvalue = +std::numeric_limits<double>::infinity ();
    return;
  }
  // Set the values to NaN :
  void nan ()
  {
    fvalue = +std::numeric_limits<float>::quiet_NaN ();
    dvalue = +std::numeric_limits<double>::quiet_NaN ();
    return;
  }
  // Print a foo:
  void print (std::ostream & a_out, const std::string & a_title)
  {
    if (a_title.empty ()) a_out << "foo";
    else a_out << a_title;
    a_out << " : " << std::endl;
    a_out << "|-- " << "fvalue = ";

    a_out.precision (max_digits10_float);
    a_out << fvalue << std::endl;
    a_out << "`-- " << "dvalue = ";
    a_out.precision (max_digits10_double);
    a_out << dvalue << std::endl;
    return;
  }

  // I/O operators for a foo structure of a float and a double :
  friend std::ostream & operator<< (std::ostream & a_out, const foo & a_foo);
  friend std::istream & operator>> (std::istream & a_in, foo & a_foo);

  // Attributes :
  float  fvalue; // Single precision floating number.
  double dvalue; // Double precision floating number.
};

std::ostream & operator<< (std::ostream & a_out, const foo & a_foo)
{ // Output bracketed FPs, for example "(3.1415927,3.1415926535897931)"
  a_out.precision (max_digits10_float);
  a_out << "(" << a_foo.fvalue << sep ;
  a_out.precision (max_digits10_double);
  a_out << a_foo.dvalue << ")";
  return a_out;
}

std::istream & operator>> (std::istream & a_in, foo & a_foo)
{ // Input bracketed floating-point values into a foo structure,
  // for example from "(3.1415927,3.1415926535897931)"
  char c = 0;
  a_in.get (c);
  if (c != '(')
  {
    std::cerr << "ERROR: operator>> No ( " << std::endl;
    a_in.setstate(std::ios::failbit);
    return a_in;
  }
  float f;
  a_in >> std::ws >> f;
  if (! a_in)
  {
    return a_in;
  }
  a_in >> std::ws;
  a_in.get (c);
  if (c != sep)
  {
    std::cerr << "ERROR: operator>> c='" << c << "'" << std::endl;
    std::cerr << "ERROR: operator>> No '" << sep << "'" << std::endl;
    a_in.setstate(std::ios::failbit);
    return a_in;
  }
  double d;
  a_in >> std::ws >> d;
  if (! a_in)
  {
    return a_in;
  }
  a_in >> std::ws;
  a_in.get (c);
  if (c != ')')
  {
    std::cerr << "ERROR: operator>> No ) " << std::endl;
    a_in.setstate(std::ios::failbit);
    return a_in;
  }
  a_foo.fvalue = f;
  a_foo.dvalue = d;
  return a_in;
} // std::istream & operator>> (std::istream & a_in, foo & a_foo)

int main ()
{
  std::cout << "nonfinite_num_facet simple example." << std::endl;

   if((std::numeric_limits<double>::has_infinity == false) || (std::numeric_limits<double>::infinity() == 0))
  {
    std::cout << "Infinity not supported on this platform." << std::endl;
    return 0;
  }

  if((std::numeric_limits<double>::has_quiet_NaN == false) || (std::numeric_limits<double>::quiet_NaN() == 0))
  {
    std::cout << "NaN not supported on this platform." << std::endl;
    return 0;
  }

#ifdef BOOST_NO_CXX11_NUMERIC_LIMITS
  cout << "BOOST_NO_CXX11_NUMERIC_LIMITS is defined, so no max_digits10 available either:"
     "\n we'll have to calculate our own version." << endl;
#endif
  std::cout << "std::numeric_limits<float>::max_digits10 is " << max_digits10_float << endl;
  std::cout << "std::numeric_limits<double>::max_digits10 is " << max_digits10_double << endl;

   std::locale the_default_locale (std::locale::classic ());

  {
    std::cout << "Write to a string buffer (using default locale) :" << std::endl;
    foo f0; // pi
    foo f1; f1.minus_infinity ();
    foo f2; f2.plus_infinity ();
    foo f3; f3.nan ();

    f0.print (std::cout, "f0"); // pi
    f1.print (std::cout, "f1"); // +inf
    f2.print (std::cout, "f2"); // -inf
    f3.print (std::cout, "f3"); // NaN

    std::ostringstream oss;
    std::locale C99_out_locale (the_default_locale, new boost::math::nonfinite_num_put<char>);
    oss.imbue (C99_out_locale);
    oss.precision (15);
    oss << f0 << f1 << f2 << f3;
    std::cout << "Output in C99 format is: \"" << oss.str () << "\"" << std::endl;
    std::cout << "Output done." << std::endl;
  }

  {
    std::string the_string = "(3.1415927,3.1415926535897931)(-inf,-inf)(inf,inf)(nan,nan)"; // C99 format
    // Must have correct separator!
    std::cout << "Read C99 format from a string buffer containing \"" << the_string << "\""<< std::endl;

    std::locale C99_in_locale (the_default_locale, new boost::math::nonfinite_num_get<char>);
    std::istringstream iss (the_string);
    iss.imbue (C99_in_locale);

    foo f0, f1, f2, f3;
    iss >> f0 >> f1 >> f2 >> f3;
    if (! iss)
    {
       std::cerr << "Input Format error !" << std::endl;
    }
    else
    {
      std::cerr << "Input OK." << std::endl;
      cout << "Display in default locale format " << endl;
      f0.print (std::cout, "f0");
      f1.print (std::cout, "f1");
      f2.print (std::cout, "f2");
      f3.print (std::cout, "f3");
    }
    std::cout << "Input done." << std::endl;
  }

  std::cout << "End nonfinite_num_facet.cpp" << std::endl;
  return 0;
} // int main()

 // end of test_nonfinite_num_facets.cpp

/*

Output:

nonfinite_num_facet simple example.
  std::numeric_limits<float>::max_digits10 is 8
  std::numeric_limits<double>::max_digits10 is 17
  Write to a string buffer (using default locale) :
  f0 :
  |-- fvalue = 3.1415927
  `-- dvalue = 3.1415926535897931
  f1 :
  |-- fvalue = -1.#INF
  `-- dvalue = -1.#INF
  f2 :
  |-- fvalue = 1.#INF
  `-- dvalue = 1.#INF
  f3 :
  |-- fvalue = 1.#QNAN
  `-- dvalue = 1.#QNAN
  Output in C99 format is: "(3.1415927,3.1415926535897931)(-inf,-inf)(inf,inf)(nan,nan)"
  Output done.
  Read C99 format from a string buffer containing "(3.1415927,3.1415926535897931)(-inf,-inf)(inf,inf)(nan,nan)"
  Display in default locale format
  f0 :
  |-- fvalue = 3.1415927
  `-- dvalue = 3.1415926535897931
  f1 :
  |-- fvalue = -1.#INF
  `-- dvalue = -1.#INF
  f2 :
  |-- fvalue = 1.#INF
  `-- dvalue = 1.#INF
  f3 :
  |-- fvalue = 1.#QNAN
  `-- dvalue = 1.#QNAN
  Input done.
  End nonfinite_num_facet.cpp

*/


#define BOOST_TEST_MODULE pcraster mathx dir_conv
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "mathx.h"

BOOST_AUTO_TEST_CASE(scale_deg)
{
  BOOST_CHECK(ScaleDeg(45) == 45);
  BOOST_CHECK(ScaleDeg(-300) == 60);

  BOOST_CHECK(ScaleDeg(0) == 0);
  BOOST_CHECK(ScaleDeg(-360) == 0);
  BOOST_CHECK(ScaleDeg(360) == 0);

  // sin bug in pcrcalc Mon Aug 14 10:14:28 CEST 2000:
  BOOST_CHECK(ScaleDeg(-80640) == 0);
}


// MIT License
//
// Copyright (c) 2020 Lennart Braun
//
// 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 "tensor_op.h"

#include <boost/functional/hash.hpp>
#include <cassert>

namespace MOTION::tensor {

bool Conv2DOp::verify() const noexcept {
  bool result = true;
  result = result && (output_shape_ == compute_output_shape());
  result = result && strides_[0] > 0 && strides_[1] > 0;
  // maybe add more checks here
  return result;
}

std::array<std::size_t, 3> Conv2DOp::compute_output_shape() const noexcept {
  const auto compute_output_dimension = [](auto input_size, auto kernel_size, auto padding_begin,
                                           auto padding_end, auto stride) {
    assert(stride != 0);
    return (input_size - kernel_size + padding_begin + padding_end + stride) / stride;
  };

  std::array<std::size_t, 3> output_shape;
  output_shape[0] = kernel_shape_[0];
  output_shape[1] =
      compute_output_dimension(input_shape_[1], kernel_shape_[2], pads_[0], pads_[2], strides_[0]);
  output_shape[2] =
      compute_output_dimension(input_shape_[2], kernel_shape_[3], pads_[1], pads_[3], strides_[1]);
  return output_shape;
}

std::size_t Conv2DOp::compute_output_size() const noexcept {
  assert(verify());
  auto output_shape = compute_output_shape();
  return output_shape[0] * output_shape[1] * output_shape[2];
}

std::size_t Conv2DOp::compute_input_size() const noexcept {
  assert(verify());
  return input_shape_[0] * input_shape_[1] * input_shape_[2];
}

std::size_t Conv2DOp::compute_kernel_size() const noexcept {
  assert(verify());
  return kernel_shape_[0] * kernel_shape_[1] * kernel_shape_[2] * kernel_shape_[3];
}

std::size_t Conv2DOp::compute_bias_size() const noexcept {
  assert(verify());
  return kernel_shape_[0];
}

std::pair<std::size_t, std::size_t> Conv2DOp::compute_input_matrix_shape() const noexcept {
  assert(verify());
  std::size_t num_rows = kernel_shape_[1] * kernel_shape_[2] * kernel_shape_[3];
  std::size_t num_columns = output_shape_[1] * output_shape_[2];
  return {num_rows, num_columns};
}

std::pair<std::size_t, std::size_t> Conv2DOp::compute_kernel_matrix_shape() const noexcept {
  assert(verify());
  std::size_t num_rows = kernel_shape_[0];
  std::size_t num_columns = kernel_shape_[1] * kernel_shape_[2] * kernel_shape_[3];
  return {num_rows, num_columns};
}

std::pair<std::size_t, std::size_t> Conv2DOp::compute_output_matrix_shape() const noexcept {
  assert(verify());
  std::size_t num_rows = kernel_shape_[0];
  std::size_t num_columns = output_shape_[1] * output_shape_[2];
  return {num_rows, num_columns};
}

TensorDimensions Conv2DOp::get_input_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = input_shape_[0],
          .height_ = input_shape_[1],
          .width_ = input_shape_[2]};
}

TensorDimensions Conv2DOp::get_kernel_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = kernel_shape_[0],
          .num_channels_ = kernel_shape_[1],
          .height_ = kernel_shape_[2],
          .width_ = kernel_shape_[3]};
}

TensorDimensions Conv2DOp::get_output_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = output_shape_[0],
          .height_ = output_shape_[1],
          .width_ = output_shape_[2]};
}

bool Conv2DOp::operator==(const Conv2DOp& other) const noexcept {
  assert(verify());
  assert(other.verify());
  bool result = true;
  result = result && kernel_shape_ == other.kernel_shape_;
  result = result && input_shape_ == other.input_shape_;
  result = result && output_shape_ == other.output_shape_;
  result = result && dilations_ == other.dilations_;
  result = result && pads_ == other.pads_;
  result = result && strides_ == other.strides_;
  return result;
}

bool GemmOp::verify() const noexcept {
  bool result = true;
  std::size_t m = input_A_shape_[0];
  std::size_t k = input_A_shape_[1];
  if (transA_) {
    std::swap(m, k);
  }
  std::size_t n = input_B_shape_[transB_ ? 0 : 1];
  result = result && (k == input_B_shape_[transB_ ? 1 : 0]);
  result = result && (m == output_shape_[0]);
  result = result && (n == output_shape_[1]);
  // maybe add more checks here
  return result;
}

std::array<std::size_t, 2> GemmOp::compute_output_shape() const noexcept {
  return {input_A_shape_[transA_ ? 1 : 0], input_B_shape_[transB_ ? 0 : 1]};
}

std::size_t GemmOp::compute_output_size() const noexcept {
  assert(verify());
  auto output_shape = compute_output_shape();
  return output_shape[0] * output_shape[1];
}

std::size_t GemmOp::compute_input_A_size() const noexcept {
  assert(verify());
  return input_A_shape_[0] * input_A_shape_[1];
}

std::size_t GemmOp::compute_input_B_size() const noexcept {
  assert(verify());
  return input_B_shape_[0] * input_B_shape_[1];
}

TensorDimensions GemmOp::get_input_A_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = 1,
          .height_ = input_A_shape_[0],
          .width_ = input_A_shape_[1]};
}

TensorDimensions GemmOp::get_input_B_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = 1,
          .height_ = input_B_shape_[0],
          .width_ = input_B_shape_[1]};
}

TensorDimensions GemmOp::get_output_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = 1,
          .height_ = output_shape_[0],
          .width_ = output_shape_[1]};
}

bool GemmOp::operator==(const GemmOp& other) const noexcept {
  assert(verify());
  assert(other.verify());
  bool result = true;
  result = result && input_A_shape_ == other.input_A_shape_;
  result = result && input_B_shape_ == other.input_B_shape_;
  result = result && output_shape_ == other.output_shape_;
  return result;
}

TensorDimensions flatten(const TensorDimensions& dims, std::size_t axis) {
  std::size_t height = 1;
  std::size_t width = 1;
  switch (axis) {
    case 0:
      width *= dims.batch_size_;
      [[fallthrough]];
    case 1:
      width *= dims.num_channels_;
      [[fallthrough]];
    case 2:
      width *= dims.height_;
      [[fallthrough]];
    case 3:
      width *= dims.width_;
  }
  switch (axis) {
    case 4:
      height *= dims.width_;
      [[fallthrough]];
    case 3:
      height *= dims.height_;
      [[fallthrough]];
    case 2:
      height *= dims.num_channels_;
      [[fallthrough]];
    case 1:
      height *= dims.batch_size_;
  }
  return {.batch_size_ = 1, .num_channels_ = 1, .height_ = height, .width_ = width};
}

bool MaxPoolOp::verify() const noexcept {
  bool result = true;
  result = result && (output_shape_ == compute_output_shape());
  result = result && strides_[0] > 0 && strides_[1] > 0;
  result = kernel_shape_[0] <= input_shape_[1] && kernel_shape_[1] <= input_shape_[2];
  // maybe add more checks here
  return result;
}

std::array<std::size_t, 3> MaxPoolOp::compute_output_shape() const noexcept {
  const auto compute_output_dimension = [](auto input_size, auto kernel_size, auto stride) {
    assert(stride != 0);
    return (input_size - kernel_size + stride) / stride;
  };

  std::array<std::size_t, 3> output_shape;
  output_shape[0] = input_shape_[0];
  output_shape[1] =
      compute_output_dimension(input_shape_[1], kernel_shape_[2], strides_[0]);
  output_shape[2] =
      compute_output_dimension(input_shape_[2], kernel_shape_[3], strides_[1]);
  return output_shape;
}

std::size_t MaxPoolOp::compute_kernel_size() const noexcept {
  assert(verify());
  return kernel_shape_[0] * kernel_shape_[1];
}

std::size_t MaxPoolOp::compute_input_size() const noexcept {
  assert(verify());
  return input_shape_[0] * input_shape_[1] * input_shape_[2];
}

std::size_t MaxPoolOp::compute_output_size() const noexcept {
  assert(verify());
  return output_shape_[0] * output_shape_[1] * output_shape_[2];
}

TensorDimensions MaxPoolOp::get_input_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = input_shape_[0],
          .height_ = input_shape_[1],
          .width_ = input_shape_[2]};
}

TensorDimensions MaxPoolOp::get_output_tensor_dims() const noexcept {
  assert(verify());
  return {.batch_size_ = 1,
          .num_channels_ = output_shape_[0],
          .height_ = output_shape_[1],
          .width_ = output_shape_[2]};
}

}  // namespace MOTION::tensor

namespace std {

std::size_t std::hash<MOTION::tensor::Conv2DOp>::operator()(
    const MOTION::tensor::Conv2DOp& op) const noexcept {
  std::size_t seed = 0;
  boost::hash_combine(seed,
                      boost::hash_range(std::begin(op.kernel_shape_), std::end(op.kernel_shape_)));
  boost::hash_combine(seed,
                      boost::hash_range(std::begin(op.input_shape_), std::end(op.input_shape_)));
  boost::hash_combine(seed, boost::hash_range(std::begin(op.dilations_), std::end(op.dilations_)));
  boost::hash_combine(seed, boost::hash_range(std::begin(op.pads_), std::end(op.pads_)));
  boost::hash_combine(seed, boost::hash_range(std::begin(op.strides_), std::end(op.strides_)));
  return seed;
}

std::size_t std::hash<MOTION::tensor::GemmOp>::operator()(
    const MOTION::tensor::GemmOp& op) const noexcept {
  std::size_t seed = 0;
  boost::hash_combine(
      seed, boost::hash_range(std::begin(op.input_A_shape_), std::end(op.input_A_shape_)));
  boost::hash_combine(
      seed, boost::hash_range(std::begin(op.input_B_shape_), std::end(op.input_B_shape_)));
  return seed;
}

}  // namespace std


/*
 * ex5.cpp
 *
 * 	\brief     5. und 7. Aufgabe
 *  \details   Liesst eine .gph Datei ein und berechnet den laengsten kuerzesten pfad von jeder Ecke zum Root des Graphen.
 *  \author    Christopher Wyczisk
 *  \date      20.07.2017
 */

#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include "dijkstra.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/timer/timer.hpp>

using namespace std;
using namespace boost::timer;

// helper
vector<pair<int, int>> kanten;
vector<int> gewichte;
unsigned int anzahlEckenGlobal;

/**
 *\typedef Adjancen des Graphen
 */
typedef boost::adjacency_list<boost::listS, boost::vecS,
	boost::undirectedS, boost::no_property,
	boost::property<boost::edge_weight_t, int>> graph;

/**
 * \typedef Die Informationen ueber die Ecken des Graphen
 */
typedef boost::graph_traits <graph>::vertex_descriptor eckenDto;

/**
 * Einlesen der Graph Datei und Fehlerbehandlung.
 *
 * \param argc
 * \param argv
 * \param infile input Graph-File
 */
bool graphEinlesen(int argc, char** argv, ifstream& file) {
	if(argc <= 1) {
		cout << "Sie haben keinen Pfad zu einer .gph Datei angegeben." << endl;
		return false;
	} else {
		file.open(argv[1], ios::in);
	}

	if (!file) {
		cout << "Die von Ihnen angegebene Datei konnte nicht geoeffnet werden." << endl;
		return false;
	}

	return true;
}

/**
 * Konsolen-Ausgabe der Anzahl an Ecken und des Gewichts.
 *
 * \param anzahlEcken
 * \param laegsterKuerzesterPfadGewicht
 */
void ergebnisAusgabe(int anzahlEcken, int laegsterKuerzesterPfadGewicht) {
	cout << "RESULT VERTEX " << anzahlEcken << endl;
	cout << "RESULT DIST " << laegsterKuerzesterPfadGewicht << endl;
}

/**
 * Erstellt den Graphen.
 *
 * \param file
 */
bool graphErstellen(ifstream& file) {
	stringstream str;
	string zeile;
	int anzahlKanten;
	int start;
	int end;
	int gewicht;

	if (getline(file, zeile)) {
		str.str(zeile);
		str >> anzahlEckenGlobal >> anzahlKanten;
		anzahlEckenGlobal++;
	} else {
		cerr << "Das .gph File ist leer." << endl;
		return false;
	}

	while (getline(file, zeile)) {
		str.clear();
		str.str(zeile);
		str >> start >> end >> gewicht;
		kanten.push_back(make_pair(start, end));
		gewichte.push_back(gewicht);
	}

	return true;
}

/**
 * Berechnet das Gewicht des longest-shortest Pfad.
 *
 * \param graph der graph
 * \param anzahlEcken
 * \param laengsterKuerzesterPfadGewicht
 */
void berechneLongesteShortesWeigth(graph graph, int& anzahlEcken, int& laegsterKuerzesterPfadGewicht) {
	vector<eckenDto> pfadRichtungen(anzahlEckenGlobal);
	vector<int> gewichteList(anzahlEckenGlobal);

	boost::dijkstra_shortest_paths(
		graph, 1,
		boost::predecessor_map(	boost::make_iterator_property_map(pfadRichtungen.begin(), get(boost::vertex_index, graph)))
			.distance_map(
					boost::make_iterator_property_map(gewichteList.begin(), get(boost::vertex_index, graph
					))));

	// berechnet das Gewicht
	for(int i = 1; i < anzahlEckenGlobal; i++) {
		if (gewichteList[i + 1] > laegsterKuerzesterPfadGewicht) {
			laegsterKuerzesterPfadGewicht = gewichteList[i + 1];
			anzahlEcken = i + 1;
		}
	}
}

string konsolenabfrageWelcheMethode() {
	string eingabe = "";
	while(eingabe.compare("a") && eingabe.compare("b")) {
		cout << "Geben Sie ein, welcher Algorithmus verwendet werden soll (a=Boots, b=eigene Impelementierung):" << endl;
		cin >> eingabe;
	}
	return eingabe;
}

/**
 * Main.
 */
int main(int argc, char* argv[]) {
	ifstream file;
	bool fileEingelesen = graphEinlesen(argc, argv, file);

	string eingabe = konsolenabfrageWelcheMethode();

	boost::timer::cpu_timer t;
	if(fileEingelesen) {
		//erstelle den Graph
		bool erstellt = graphErstellen(file);
		if(erstellt && !eingabe.compare("a")) {
			graph graph{kanten.begin(), kanten.end(), gewichte.begin(), anzahlEckenGlobal};

			// berechnen des gewichts des longesten shortesten pfad
			int laegsterKuerzesterPfadGewicht = -1;
			int anzahlEcken = -1;
			berechneLongesteShortesWeigth(graph, anzahlEcken, laegsterKuerzesterPfadGewicht);

			ergebnisAusgabe(anzahlEcken, laegsterKuerzesterPfadGewicht);
		}
		else if(erstellt) {
			vector<int> mapGewichte(anzahlEckenGlobal);
	    	dijkstra dijkstra(gewichte, kanten, anzahlEckenGlobal);
			mapGewichte = dijkstra.berechneKuerzestenPfad(1);

			// berechnen des gewichts des longesten shortesten pfad
			int laegsterKuerzesterPfadGewicht = -1;
			int anzahlEcken = -1;
			int absGewichte;
			for(unsigned int i = 2; i < anzahlEckenGlobal; i++) {
				absGewichte = mapGewichte[i];
				if (absGewichte > laegsterKuerzesterPfadGewicht) {
					laegsterKuerzesterPfadGewicht = absGewichte;
					anzahlEcken = i;
				}
			}

			ergebnisAusgabe(anzahlEcken, laegsterKuerzesterPfadGewicht);
		}
	}

	boost::timer::cpu_times zeit = t.elapsed();
	std::cout << "WALL-CLOCK " << zeit.wall / 1e9 << "s" << std::endl;
	std::cout << "USER TIME " << zeit.user / 1e9 << "s" << std::endl;
	return 0;
}


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../jngen.h"

BOOST_AUTO_TEST_SUITE(geometry)

BOOST_AUTO_TEST_CASE(comparators) {
    using namespace jngen;

    BOOST_CHECK(lt(0.0, 1e-5));
    BOOST_CHECK(lt((long double)0.0, 1e-5));
    BOOST_CHECK(lt(0, 1e-5));
    BOOST_CHECK(!eq(0, 1e-5));
    BOOST_CHECK(eq(1 + 1e-10, 1 - 1e-10));
}

BOOST_AUTO_TEST_CASE(points_generation) {
    auto a = rnda.randomfAll([]() { return rndg.point(3); });
    BOOST_TEST(a.size() == 16);

    auto b = rnda.randomfAll([]() { return rndg.point(2, 3); });
    BOOST_TEST(b.size() == 4);

    auto c = rnda.randomf(100, []() { return rndg.pointf(1); });
    BOOST_TEST(c.size() == 100);

    auto d = rnda.randomfAll([]() { return rndg.point(-2, 2); });
    BOOST_TEST(d.size() == 25);
}

BOOST_AUTO_TEST_CASE(polygon) {
    rnd.seed(123);
    auto p = rndg.convexPolygon(10, 100);
    // TODO: check that it is convex
    BOOST_TEST(p.size() == 10);
}

BOOST_AUTO_TEST_CASE(points_in_general_position) {
    rnd.seed(123);

    int n = 5;
    for (int test = 0; test < 10; ++test) {
        n += 5;
        auto pts = rndg.pointsInGeneralPosition(n, n);
        for (int i = 0; i < n; ++i) {
            for (int j = 0; j < i; ++j) {
                for (int k = 0; k < j; ++k) {
                    BOOST_CHECK((pts[i] - pts[j]) % (pts[i] - pts[k]) != 0);
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <boost/random/discrete_distribution.hpp>

namespace calotypes
{

/*! \brief Weighted sampling with replacement. */
template <class Engine>
class NaiveWeightedSampling
{
public:

	NaiveWeightedSampling() {}

	static void Sample( const std::vector<double>& weights, unsigned int numDraws,
						std::vector<unsigned int>& inds, Engine& engine )
	{
		inds.resize( numDraws );
		boost::random::discrete_distribution<> dist( weights );
		for( unsigned int i = 0; i < numDraws; i++ )
		{
			inds[i] = dist( engine );
		}
	}
};

template <class Engine>
class LowVarianceWeightedSampling
{
public:

	LowVarianceWeightedSampling() {}

	static void Sample( const std::vector<double>& weights, unsigned int numDraws,
						std::vector<unsigned int>& inds, Engine& engine )
	{
		inds.resize( numDraws );
		inds.clear();

		double totalWeight = 0;
		for( unsigned int i = 0; i < weights.size(); i++ ) { totalWeight += weights[i]; }

		double stride = totalWeight/( numDraws + 1 );

		boost::random::uniform_01<> dist;
		double currentPoint = stride*dist( engine );

		double accumulatedWeights = weights[0];

		unsigned int ind = 0;
		while( inds.size() < numDraws )
		{
			if( accumulatedWeights >= currentPoint )
			{
				inds.push_back( ind );
				currentPoint += stride;
			}
			else
			{
				// Sometimes numerical precision errors cause the last element to not get added
				if( ind == weights.size()-1 ) {
					inds.push_back( ind );
					break;
				}
				ind++;
				accumulatedWeights += weights[ind];
			}
		}
	}
};

} // end namespace calotypes


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_IEEE_HPP_INCLUDED
#define BOOST_SIMD_IEEE_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-ieee Ieee functions

      Those functions provides scalar and SIMD algorithms for inspecting, generating or
      decomposing IEEE 754 floating point numbers. Operations like exponent and mantissa
      extraction, floating point modulo, IEEE bit patterns manipulation and magnitude comparison
      are provided.

  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-ieee Ieee Callable Objects
    Callable objects version of @ref group-ieee

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-ieee section.
  **/
} }

#include <boost/simd/function/bitfloating.hpp>
#include <boost/simd/function/bitinteger.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/eps.hpp>
#include <boost/simd/function/exponentbits.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/fmax.hpp>
#include <boost/simd/function/fmin.hpp>
#include <boost/simd/function/fpclassify.hpp>
#include <boost/simd/function/frac.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/ilogb.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/mantissa.hpp>
#include <boost/simd/function/maxmag.hpp>
#include <boost/simd/function/maxnum.hpp>
#include <boost/simd/function/maxnummag.hpp>
#include <boost/simd/function/minmag.hpp>
#include <boost/simd/function/minnum.hpp>
#include <boost/simd/function/minnummag.hpp>
#include <boost/simd/function/modf.hpp>
#include <boost/simd/function/negate.hpp>
#include <boost/simd/function/negatenz.hpp>
#include <boost/simd/function/nextafter.hpp>
#include <boost/simd/function/next.hpp>
#include <boost/simd/function/nextpow2.hpp>
#include <boost/simd/function/predecessor.hpp>
#include <boost/simd/function/prev.hpp>
#include <boost/simd/function/safe_max.hpp>
#include <boost/simd/function/safe_min.hpp>
#include <boost/simd/function/saturate.hpp>
#include <boost/simd/function/sbits.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/successor.hpp>
#include <boost/simd/function/ulpdist.hpp>
#include <boost/simd/function/ulp.hpp>

#endif


#include <Eigen/Dense>
#include "DepthCamera.h"
#include "PointCloud.h"
#include "PassThroughFilter.h"
#include "RadiusOutlierFilter.h"
#include "PointCloudRecorder.h"
#include "PointCloudInterface.h"
#include "Transform.h"

using namespace std;

int main()
{

	//! Init Recorder
	PointCloudRecorder pclRecorder;
	//! Set the fileName
	pclRecorder.setFileName("cloud.txt");

	//! Init camera
	DepthCamera camera1, camera2;
	//! Set fileNames
	camera1.setfileName("camera1.txt");
	camera2.setfileName("camera2.txt");

	//! Init RadiusFilter
	RadiusOutlierFilter rof;
	//! Set the radius
	rof.setRadius(25.0);

	//! Init ConstraitFilter
	PassThroughFilter filter1, filter2;
	//! Set upper lower boundaries for Camera1
	filter1.setlowerLimitX(0.0);
	filter1.setupperLimitX(400.0);
	filter1.setlowerLimitY(0.0);
	filter1.setupperLimitY(400.0);
	filter1.setlowerLimitZ(-45.0);
	filter1.setupperLimitZ(45.0);

	//! Set upper lower boundaries for Camera2
	filter2.setlowerLimitX(0.0);
	filter2.setupperLimitX(500.0);
	filter2.setlowerLimitY(0.0);
	filter2.setupperLimitY(500.0);
	filter2.setlowerLimitZ(-45.0);
	filter2.setupperLimitZ(45.0);

	//! Init FilterPipe
	FilterPipe fp1, fp2;
	//! Add filters
	fp1.addFilter(&rof);
	fp1.addFilter(&filter1);

	//! Add filters
	fp2.addFilter(&rof);
	fp2.addFilter(&filter2);

	//! Assign filters to DepthCameras
	camera1.setFilterPipe(&fp1);
	camera2.setFilterPipe(&fp2);

	//! Init Transform
	Transform tr1, tr2;

	//! Declare the angles and coordinates for translation
	Eigen::Vector3d angles1(0, 0, -90);
	Eigen::Vector3d angles2(0, 0, 90);
	Eigen::Vector3d trans1(100, 500, 50);
	Eigen::Vector3d trans2(550, 150, 50);

	//! Set data for Camera1
	tr1.setRotation(angles1);
	tr1.setTranslation(trans1);
	//! Initialize the transform matrix
	tr1.initialize();

	//! Set data for Camera2
	tr2.setRotation(angles2);
	tr2.setTranslation(trans2);
	//! Initialize the transform matrix
	tr2.initialize();

	//! Assign transform object to DepthCameras
	camera1.setTransform(&tr1);
	camera2.setTransform(&tr2);

	//! Init PointCloudInterfase
	PointCloudInterface pci1;
	//! Add generators
	pci1.addGenerator(&camera1);
	pci1.addGenerator(&camera2);
	//! Set recorder
	pci1.setRecorder(&pclRecorder);

	//! Generate result
	pci1.generate();

	//! Record result using given recorder
	pci1.record();

	//! EXIT
	return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/**
 * @file
 * @ingroup tests
 * @brief  Test for the PrecondType::HB preconditioner with simple heat transfer equation.
 *
 * Testprogram for Kaskade: tests wether the error between calculated and exact solution behaves as expected.
 * This test was built by changing the stationary heat transfer example.
 *
 * Variational functional, boundary conditions and f are adapted in hbPrec.hh.
 * Only uses ansatz functions of order 1.
 */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"
#include "fem/norms.hh"
#include "linalg/direct.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "utilities/enums.hh"
#include "utilities/kaskopt.hh"
#include "mg/hb.hh"

#include "utilities/kaskopt.hh"

using namespace Kaskade;
#include "hbPrec.hh"

/**
 * @brief  This represents the known exact solution.
 * It is \f$ u = \cos (2 \pi x) + \exp (4(y-0.5)^2) \f$.
 */
struct HBPrecSolution
{
   using Scalar = double;
   static int const components = 1;
   using ValueType = Dune::FieldVector<Scalar,components>;

   template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }

   template <class Cell>
   ValueType value(Cell const& cell,Dune::FieldVector<typename Cell::Geometry::ctype,Cell::dimension> const& localCoordinate) const
   {
     Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);
     return std::cos(2*PI*x[0]) + std::exp(4*(x[1]-0.5)*(x[1]-0.5));
   }
};

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start heat transfer test program" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 0;
  bool dump = false;
  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  //using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = HBPrecFunctional<double,VariableSet>;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  //ansatz orders to be tested
  int order = 1;
  constexpr int maxRefSteps = 9;
  //error values for corresponding refinement
  double errors[maxRefSteps] = {0.65,0.13,0.034,0.0086,0.0021,5.4e-4,1.35e-4,3.4e-5,8.4e-6};
  bool valid = true;
  std::stringstream message("Test succeeded", std::stringstream::out);

  int verbosity   = getParameter(pt, "verbosity", 1);
  // if true, then the test result will be written in a file
  bool result = getParameter(pt, "result",0);
  int onlyLowerTriangle = false;

  MatrixProperties property = MatrixProperties::SYMMETRIC;

  int blocks = getParameter(pt,"blocks",40);
  int nthreads = getParameter(pt,"threads",4);
  double rowBlockFactor = getParameter(pt,"rowBlockFactor",2.0);

  property = MatrixProperties::SYMMETRIC;

  if(verbosity > 0) {
    std::cout << "original mesh shall be refined : " << maxRefSteps << " times" << std::endl;
    std::cout << "discretization order           : " << order << std::endl;
    std::cout << "output level (verbosity)       : " << verbosity << std::endl;
  }

  Dune::GridFactory<Grid> factory;

  // vertex coordinates v[0], v[1]
  Dune::FieldVector<double,dim> v;
  v[0]=0; v[1]=0; factory.insertVertex(v);
  v[0]=1; v[1]=0; factory.insertVertex(v);
  v[0]=1; v[1]=1; factory.insertVertex(v);
  v[0]=0; v[1]=1; factory.insertVertex(v);
  // triangle defined by 3 vertex indices
  std::vector<unsigned int> vid(3);
  Dune::GeometryType gt(Dune::GeometryType::simplex,2);
  vid[0]=0; vid[1]=1; vid[2]=2; factory.insertElement(gt,vid);
  vid[0]=0; vid[1]=2; vid[2]=3; factory.insertElement(gt,vid);
  std::unique_ptr<Grid> grid( factory.createGrid() ) ;
  // the coarse grid will be refined three times
  grid->globalRefine(0);
  // some information on the refined mesh
  std::cout << std::endl << "Grid: " << grid->size(0) << " triangles, " << std::endl;
  std::cout << "      " << grid->size(1) << " edges, " << std::endl;
  std::cout << "      " << grid->size(2) << " points" << std::endl;

  // a gridmanager is constructed
  // as connector between geometric and algebraic information
  GridManager<Grid> gridManager(std::move(grid));

  // construction of finite element space for the scalar solution T.
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&temperatureSpace);

  // construct variable list.
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable

  std::string varNames[1] = { "u" };

  VariableSet variableSet(spaces,varNames);

  // construct variational functional
  Functional F;
  if(verbosity > 0) {
    std::cout << std::endl << "no of variables = " << nvars << std::endl;
    std::cout << "no of equations = " << neq   << std::endl;
    size_t dofs = variableSet.degreesOfFreedom(0,nvars);
    std::cout << "number of degrees of freedom = " << dofs   << std::endl;
  }

  //construct Galerkin representation
  Assembler assembler(gridManager,spaces);

  gridManager.enforceConcurrentReads(true);
  assembler.setNSimultaneousBlocks(blocks);
  assembler.setRowBlockFactor(rowBlockFactor);

  for(int refSteps = 0;refSteps<maxRefSteps;refSteps++) {
    gridManager.globalRefine(1);

    boost::timer::cpu_timer assembTimer;
    VariableSet::VariableSet u(variableSet);

    size_t nnz = assembler.nnz(0,1,0,1,onlyLowerTriangle);
    if(verbosity > 0) {
      std::cout << "number of nonzero elements in the stiffness matrix: " << nnz << std::endl << std::endl;
    }

    CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,1>::init(spaces));
    solution = 0;

    assembler.assemble(linearization(F,u),assembler.MATRIX|assembler.RHS|assembler.VALUE,nthreads,verbosity);
    if(verbosity > 0) {
      std::cout << "computing time for assemble: " << boost::timer::format(assembTimer.elapsed()) << "\n";
    }

    CoefficientVectors rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);

    boost::timer::cpu_timer iteTimer;
    int iteSteps = getParameter(pt, "solver.iteMax", 1000);
    double iteEps = getParameter(pt, "solver.iteEps", 1.0e-10);
    Dune::InverseOperatorResult res;
    const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
    StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
    int lookAhead = getParameter(pt, "solver.lookAhead", 50);
    termination.setLookAhead(lookAhead);

    HierarchicalBasisPreconditioner<Grid,AssembledGalerkinOperator<Assembler,0,1,0,1>::range_type, AssembledGalerkinOperator<Assembler,0,1,0,1>::range_type > hb(gridManager.grid());
    CG<LinearSpace,LinearSpace> cg(A,hb,defaultScalarProduct,termination,verbosity);
    cg.apply(solution,rhs,res);

    solution *= -1.0;
    u.data = solution.data;

    if(verbosity > 0) {
      std::cout << "iterative solve eps= " << iteEps << ": "
    << (res.converged?"converged":"failed") << " after "
    << res.iterations << " steps, rate="
    << res.conv_rate << ", computing time=" << (double)(iteTimer.elapsed().user)/1e9 << "s\n";
    }

    //calculate error in l2norm
    VariableSet::VariableSet func( variableSet ) ;
    interpolateGloballyWeak<PlainAverage>(boost::fusion::at_c<0>(func.data),HBPrecSolution());
    u -= func;
    L2Norm l2 ;
    double nrm2 = l2( boost::fusion::at_c<0>(u.data) ) ;
    if(verbosity > 0) {
      std::cout << "error in l2norm: " << nrm2 << std::endl;
    }
    // query whether error is low enough
    if(!(nrm2<=errors[refSteps])) {
      valid = false;
      message << "Test failed: The error after " << refSteps << " refinements was too high at the test with ansatz functions of order 1.";
    }
  }

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << message.str() << std::endl;
  if(result) {
    std::string description = "Test with example stationary heat transfer, 2D. Used iterate solver IterateType::CG, PrecondType::HB preconditioner:";
    std::ofstream outfile("../testResult.txt", std::ofstream::out | std::ofstream::app);
    outfile << description << std::endl << message.str() << std::endl << std::endl;
    outfile.close();
  }
}


/**
 * @file dcgan_test.cpp
 * @author Shikhar Jaiswal
 *
 * Tests the DCGAN network.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>
#include <mlpack/methods/ann/loss_functions/sigmoid_cross_entropy_error.hpp>
#include <mlpack/methods/ann/gan/gan.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::math;
using namespace mlpack::regression;
using namespace std::placeholders;

BOOST_AUTO_TEST_SUITE(DCGANNetworkTest);

/*
 * Tests the DCGAN implementation on the MNIST dataset.
 * It's not viable to train on bigger parameters due to time constraints.
 * Please refer mlpack/models repository for the tutorial.
 */
BOOST_AUTO_TEST_CASE(DCGANMNISTTest)
{
  size_t dNumKernels = 32;
  size_t discriminatorPreTrain = 5;
  size_t batchSize = 5;
  size_t noiseDim = 100;
  size_t generatorUpdateStep = 1;
  size_t numSamples = 10;
  double stepSize = 0.0003;
  double eps = 1e-8;
  size_t numEpoches = 1;
  double tolerance = 1e-5;
  int datasetMaxCols = 10;
  bool shuffle = true;
  double multiplier = 10;

  Log::Info << std::boolalpha
      << " batchSize = " << batchSize << std::endl
      << " generatorUpdateStep = " << generatorUpdateStep << std::endl
      << " noiseDim = " << noiseDim << std::endl
      << " numSamples = " << numSamples << std::endl
      << " stepSize = " << stepSize << std::endl
      << " numEpoches = " << numEpoches << std::endl
      << " tolerance = " << tolerance << std::endl
      << " shuffle = " << shuffle << std::endl;

  arma::mat trainData;
  trainData.load("mnist_first250_training_4s_and_9s.arm");
  Log::Info << arma::size(trainData) << std::endl;

  trainData = trainData.cols(0, datasetMaxCols - 1);

  size_t numIterations = trainData.n_cols * numEpoches;
  numIterations /= batchSize;

  Log::Info << "Dataset loaded (" << trainData.n_rows << ", "
            << trainData.n_cols << ")" << std::endl;
  Log::Info << trainData.n_rows << "--------" << trainData.n_cols << std::endl;

  // Create the Discriminator network.
  FFN<SigmoidCrossEntropyError<> > discriminator;
  discriminator.Add<Convolution<> >(1, dNumKernels, 4, 4, 2, 2, 1, 1, 28, 28);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(dNumKernels, 2 * dNumKernels, 4, 4, 2, 2,
      1, 1, 14, 14);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(2 * dNumKernels, 4 * dNumKernels, 4, 4,
      2, 2, 1, 1, 7, 7);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(4 * dNumKernels, 8 * dNumKernels, 4, 4,
      2, 2, 2, 2, 3, 3);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(8 * dNumKernels, 1, 4, 4, 1, 1,
      1, 1, 2, 2);

  // Create the Generator network.
  FFN<SigmoidCrossEntropyError<> > generator;
  generator.Add<TransposedConvolution<> >(noiseDim, 8 * dNumKernels, 2, 2,
      1, 1, 0, 0, 1, 1, 2, 2);
  generator.Add<BatchNorm<> >(1024);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(8 * dNumKernels, 4 * dNumKernels,
      2, 2, 1, 1, 0, 0, 2, 2, 3, 3);
  generator.Add<BatchNorm<> >(1152);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(4 * dNumKernels, 2 * dNumKernels,
      5, 5, 2, 2, 1, 1, 3, 3, 7, 7);
  generator.Add<BatchNorm<> >(3136);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(2 * dNumKernels, dNumKernels, 4, 4,
      2, 2, 1, 1, 7, 7, 14, 14);
  generator.Add<BatchNorm<> >(6272);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(dNumKernels, 1, 4, 4, 2, 2, 1, 1,
      14, 14, 28, 28);
  generator.Add<TanHLayer<> >();

  // Create DCGAN.
  GaussianInitialization gaussian(0, 1);
  ens::Adam optimizer(stepSize, batchSize, 0.9, 0.999, eps, numIterations,
      tolerance, shuffle);
  std::function<double()> noiseFunction = [] () {
      return math::RandNormal(0, 1);};
  GAN<FFN<SigmoidCrossEntropyError<> >, GaussianInitialization,
      std::function<double()>, DCGAN> dcgan(generator, discriminator, gaussian,
      noiseFunction, noiseDim, batchSize, generatorUpdateStep,
      discriminatorPreTrain, multiplier);

  Log::Info << "Training..." << std::endl;
  double objVal = dcgan.Train(trainData, optimizer);

  // Test that objective value returned by GAN::Train() is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);

  // Generate samples.
  Log::Info << "Sampling..." << std::endl;
  arma::mat noise(noiseDim, batchSize);
  size_t dim = std::sqrt(trainData.n_rows);
  arma::mat generatedData(2 * dim, dim * numSamples);

  for (size_t i = 0; i < numSamples; i++)
  {
    arma::mat samples;
    noise.imbue( [&]() { return noiseFunction(); } );

    dcgan.Generator().Forward(noise, samples);
    samples.reshape(dim, dim);
    samples = samples.t();

    generatedData.submat(0, i * dim, dim - 1, i * dim + dim - 1) = samples;

    samples = trainData.col(math::RandInt(0, trainData.n_cols));
    samples.reshape(dim, dim);
    samples = samples.t();

    generatedData.submat(dim,
        i * dim, 2 * dim - 1, i * dim + dim - 1) = samples;
  }

  Log::Info << "Output generated!" << std::endl;

  // Check that Serialization is working correctly.
  arma::mat orgPredictions;
  dcgan.Predict(noise, orgPredictions);

  GAN<FFN<SigmoidCrossEntropyError<> >, GaussianInitialization,
      std::function<double()>, DCGAN> dcganText(generator, discriminator,
      gaussian, noiseFunction, noiseDim, batchSize, generatorUpdateStep,
      discriminatorPreTrain, multiplier);

  GAN<FFN<SigmoidCrossEntropyError<> >, GaussianInitialization,
      std::function<double()>, DCGAN> dcganXml(generator, discriminator,
      gaussian, noiseFunction, noiseDim, batchSize, generatorUpdateStep,
      discriminatorPreTrain, multiplier);

  GAN<FFN<SigmoidCrossEntropyError<> >, GaussianInitialization,
      std::function<double()>, DCGAN> dcganBinary(generator, discriminator,
      gaussian, noiseFunction, noiseDim, batchSize, generatorUpdateStep,
      discriminatorPreTrain, multiplier);

  SerializeObjectAll(dcgan, dcganXml, dcganText, dcganBinary);

  arma::mat predictions, xmlPredictions, textPredictions, binaryPredictions;
  dcgan.Predict(noise, predictions);
  dcganXml.Predict(noise, xmlPredictions);
  dcganText.Predict(noise, textPredictions);
  dcganBinary.Predict(noise, binaryPredictions);

  CheckMatrices(orgPredictions, predictions);
  CheckMatrices(orgPredictions, xmlPredictions);
  CheckMatrices(orgPredictions, textPredictions);
  CheckMatrices(orgPredictions, binaryPredictions);
}

/*
 * Tests the DCGAN implementation with minibatch layer on the MNIST dataset.
 * It's not viable to train on bigger parameters due to time constraints.

BOOST_AUTO_TEST_CASE(DCGANMNISTTest)
{
  size_t dNumKernels = 32;
  size_t discriminatorPreTrain = 5;
  size_t batchSize = 5;
  size_t noiseDim = 100;
  size_t generatorUpdateStep = 1;
  size_t numSamples = 1000;
  double stepSize = 0.0003;
  double eps = 1e-8;
  size_t numEpoches = 5;
  double tolerance = 1e-5;
  int datasetMaxCols = 10;
  bool shuffle = true;
  double multiplier = 10;

  Log::Info << std::boolalpha
      << " batchSize = " << batchSize << std::endl
      << " generatorUpdateStep = " << generatorUpdateStep << std::endl
      << " noiseDim = " << noiseDim << std::endl
      << " numSamples = " << numSamples << std::endl
      << " stepSize = " << stepSize << std::endl
      << " numEpoches = " << numEpoches << std::endl
      << " tolerance = " << tolerance << std::endl
      << " shuffle = " << shuffle << std::endl;

  arma::mat trainData;
  trainData.load("mnist_first250_training_4s_and_9s.arm");
  Log::Info << arma::size(trainData) << std::endl;

  // trainData = trainData.cols(0, datasetMaxCols - 1);

  size_t numIterations = trainData.n_cols * numEpoches;
  // numIterations /= batchSize;

  Log::Info << "Dataset loaded (" << trainData.n_rows << ", "
            << trainData.n_cols << ")" << std::endl;
  Log::Info << trainData.n_rows << "--------" << trainData.n_cols << std::endl;

  // Create the Discriminator network
  FFN<SigmoidCrossEntropyError<> > discriminator;
  discriminator.Add<Convolution<> >(1, dNumKernels, 4, 4, 2, 2, 1, 1, 28, 28);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(dNumKernels, 2 * dNumKernels, 4, 4, 2, 2,
      1, 1, 14, 14);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(2 * dNumKernels, 4 * dNumKernels, 4, 4,
      2, 2, 1, 1, 7, 7);
  discriminator.Add<MiniBatchDiscrimination<> >(4 * dNumKernels, 4 * dNumKernels + 10, 100);
  discriminator.Add<Linear<> >(4 * dNumKernels + 10, 1);

  // Create the Generator network
  FFN<SigmoidCrossEntropyError<> > generator;
  generator.Add<TransposedConvolution<> >(noiseDim, 8 * dNumKernels, 2, 2,
      1, 1, 1, 1, 1, 1);
  generator.Add<BatchNorm<> >(1024);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(8 * dNumKernels, 4 * dNumKernels,
      2, 2, 1, 1, 0, 0, 2, 2);
  generator.Add<BatchNorm<> >(1152);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(4 * dNumKernels, 2 * dNumKernels,
      5, 5, 2, 2, 1, 1, 3, 3);
  generator.Add<BatchNorm<> >(3136);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(2 * dNumKernels, dNumKernels, 8, 8,
      1, 1, 1, 1, 7, 7);
  generator.Add<BatchNorm<> >(6272);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(dNumKernels, 1, 15, 15, 1, 1, 1, 1,
      14, 14);
  generator.Add<TanHLayer<> >();

  // Create DCGAN
  GaussianInitialization gaussian(0, 1);
  ens::Adam optimizer(stepSize, batchSize, 0.9, 0.999, eps, numIterations,
      tolerance, shuffle);
  std::function<double()> noiseFunction = [] () {
      return math::RandNormal(0, 1);};
  GAN<FFN<SigmoidCrossEntropyError<> >, GaussianInitialization,
      std::function<double()>, DCGAN> dcgan(trainData, generator, discriminator,
      gaussian, noiseFunction, noiseDim, batchSize, generatorUpdateStep,
      discriminatorPreTrain, multiplier);

  Log::Info << "Training..." << std::endl;
  double objVal = dcgan.Train(optimizer);

  // Test that objective value returned by GAN::Train() is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);

  // Generate samples
  Log::Info << "Sampling..." << std::endl;
  arma::mat noise(noiseDim, 1);
  size_t dim = std::sqrt(trainData.n_rows);
  arma::mat generatedData(2 * dim, dim * numSamples);

  for (size_t i = 0; i < numSamples; i++)
  {
    arma::mat samples;
    noise.imbue( [&]() { return noiseFunction(); } );

    dcgan.Generator().Forward(noise, samples);
    samples.reshape(dim, dim);
    samples = samples.t();

    generatedData.submat(0, i * dim, dim - 1, i * dim + dim - 1) = samples;

    samples = trainData.col(math::RandInt(0, trainData.n_cols));
    samples.reshape(dim, dim);
    samples = samples.t();

    generatedData.submat(dim,
        i * dim, 2 * dim - 1, i * dim + dim - 1) = samples;
  }

  // CNN digit recogniser model from mlpack/model respository.
  model.Add<Convolution<> >(1,6,5,5,1,1,0,0,28,28);

  // Add first ReLU.
  model.Add<LeakyReLU<> >();

  // Add first pooling layer. Pools over 2x2 fields in the input.
  model.Add<MaxPooling<> >(2,2,2,2,true);

  // Add the second convolution layer.
  model.Add<Convolution<> >(6,16,5,5,1,1,0,0,12,12);
  // Add the second ReLU.
  model.Add<LeakyReLU<> >();

  // Add the second pooling layer.
  model.Add<MaxPooling<> >(2, 2, 2, 2, true);

  // Add the final dense layer.
  model.Add<Linear<> >(16*4*4, 10);
  model.Add<LogSoftMax<> >();

  arma::mat labels = arma::zeros(1, trainData.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);
  labels += 1;

  ens::SGD<AdamUpdate> optimizer2(1.2e-3, 50, 40 * 10000, 1e-8, true,
        ens::AdamUpdate(1e-8, 0.9, 0.999));
  model.Train(trainData, labels, optimizer2);

  Log::Info << InceptionScore(model, generatedData, 50) << std::endl;
  Log::Info << "Output generated!" << std::endl;
}
*/

/*
 * Tests the DCGAN implementation on the CelebA dataset.
 * It's currently not possible to run this every time due to time constraints.
 * Please refer mlpack/models repository for the tutorial.

BOOST_AUTO_TEST_CASE(DCGANCelebATest)
{
  size_t dNumKernels = 64;
  size_t discriminatorPreTrain = 300;
  size_t batchSize = 1;
  size_t noiseDim = 100;
  size_t generatorUpdateStep = 1;
  size_t numSamples = 10;
  double stepSize = 0.0003;
  double eps = 1e-8;
  size_t numEpoches = 20;
  double tolerance = 1e-5;
  int datasetMaxCols = -1;
  bool shuffle = true;
  double multiplier = 10;

  Log::Info << std::boolalpha
      << " batchSize = " << batchSize << std::endl
      << " generatorUpdateStep = " << generatorUpdateStep << std::endl
      << " noiseDim = " << noiseDim << std::endl
      << " numSamples = " << numSamples << std::endl
      << " stepSize = " << stepSize << std::endl
      << " numEpoches = " << numEpoches << std::endl
      << " tolerance = " << tolerance << std::endl
      << " shuffle = " << shuffle << std::endl;

  arma::mat trainData;
  trainData.load("celeba.csv");
  Log::Info << arma::size(trainData) << std::endl;

  if (datasetMaxCols > 0)
    trainData = trainData.cols(0, datasetMaxCols - 1);

  size_t numIterations = trainData.n_cols * numEpoches;
  numIterations /= batchSize;

  Log::Info << "Dataset loaded (" << trainData.n_rows << ", "
            << trainData.n_cols << ")" << std::endl;
  Log::Info << trainData.n_rows << "--------" << trainData.n_cols << std::endl;

  // Create the Discriminator network.
  FFN<SigmoidCrossEntropyError<> > discriminator;
  discriminator.Add<Convolution<> >(3, dNumKernels, 4, 4, 2, 2, 1, 1, 64, 64);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(dNumKernels, 2 * dNumKernels, 4, 4, 2, 2,
      1, 1, 32, 32);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(2 * dNumKernels, 4 * dNumKernels, 4, 4,
      2, 2, 1, 1, 16, 16);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(4 * dNumKernels, 8 * dNumKernels, 4, 4,
      2, 2, 1, 1, 8, 8);
  discriminator.Add<LeakyReLU<> >(0.2);
  discriminator.Add<Convolution<> >(8 * dNumKernels, 1, 4, 4, 1, 1,
      0, 0, 4, 4);

  // Create the Generator network.
  FFN<SigmoidCrossEntropyError<> > generator;
  generator.Add<TransposedConvolution<> >(noiseDim, 8 * dNumKernels, 4, 4,
      1, 1, 2, 2, 1, 1);
  generator.Add<BatchNorm<> >(4096);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(8 * dNumKernels, 4 * dNumKernels,
      5, 5, 1, 1, 1, 1, 4, 4);
  generator.Add<BatchNorm<> >(8192);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(4 * dNumKernels, 2 * dNumKernels,
      9, 9, 1, 1, 1, 1, 8, 8);
  generator.Add<BatchNorm<> >(16384);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(2 * dNumKernels, dNumKernels, 17, 17,
      1, 1, 1, 1, 16, 16);
  generator.Add<BatchNorm<> >(32768);
  generator.Add<ReLULayer<> >();
  generator.Add<TransposedConvolution<> >(dNumKernels, 3, 33, 33, 1, 1, 1, 1,
      32, 32);
  generator.Add<TanHLayer<> >();

  // Create DCGAN.
  GaussianInitialization gaussian(0, 1);
  ens::Adam optimizer(stepSize, batchSize, 0.9, 0.999, eps, numIterations,
      tolerance, shuffle);
  std::function<double()> noiseFunction = [] () {
      return math::RandNormal(0, 1);};
  GAN<FFN<SigmoidCrossEntropyError<> >, GaussianInitialization,
      std::function<double()>, DCGAN> dcgan(trainData, generator, discriminator,
      gaussian, noiseFunction, noiseDim, batchSize, generatorUpdateStep,
      discriminatorPreTrain, multiplier);

  Log::Info << "Training..." << std::endl;
  dcgan.Train(optimizer);

  // Generate samples.
  Log::Info << "Sampling..." << std::endl;
  arma::mat noise(noiseDim, 1);
  size_t dim = std::sqrt(trainData.n_rows);
  arma::mat generatedData(2 * dim, dim * numSamples);

  for (size_t i = 0; i < numSamples; i++)
  {
    arma::mat samples;
    noise.imbue( [&]() { return noiseFunction(); } );

    dcgan.Generator().Forward(noise, samples);
    samples.reshape(dim, dim);
    samples = samples.t();

    generatedData.submat(0, i * dim, dim - 1, i * dim + dim - 1) = samples;

    samples = trainData.col(math::RandInt(0, trainData.n_cols));
    samples.reshape(dim, dim);
    samples = samples.t();

    generatedData.submat(dim,
        i * dim, 2 * dim - 1, i * dim + dim - 1) = samples;
  }

  Log::Info << "Output generated!" << std::endl;
}
*/

BOOST_AUTO_TEST_SUITE_END();


#ifndef TYPES_HPP
#define TYPES_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace eigenml {

    // general type of matrices
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::VectorXd Vector;

    // integer type of matrices
    typedef Eigen::MatrixXi MatrixI;
    typedef Eigen::VectorXi VectorI;

    // typedef for histogram types
    typedef std::map<double, double> Histogram;

    // value that the split criterion should return
    typedef std::pair<double, double> ValueAndWeight;

    // sorted X columns by values
    typedef std::vector<size_t> IdxVector;

    // types of model
    // allows to specialize types of variables using traits
    enum ModelType {
        kSupervisedClassifier,
        kSupervisedRegressor
    };
}

#endif // TYPES_HPP