#include <QApplication>
#include <QFileInfo>
#include <QDir>
#include "GLPointCloudViewer.h"
#include "QImageWidget.h"
#include "QKinectGrabber.h"
#include "QKinectIO.h"
#include <iostream>
#include <iterator>
#include <array>
#include <memory>
#include <time.h>

#include "Volumetric_helper.h"
#include "Timer.h"
#include <Eigen/Dense>
#include <cuda_runtime.h>
#include <vector_types.h>
#include "cuda_kernels/cuda_kernels.h"

#include "GLPointCloud.h"

#include "helper_cuda.h"
#include "helper_image.h"

static QVector3D colors[7] = {
	QVector3D(1, 0, 0), QVector3D(0, 1, 0), QVector3D(0, 0, 1),
	QVector3D(1, 1, 0), QVector3D(1, 0, 1), QVector3D(0, 1, 1),
	QVector3D(1, 1, 1) };

static void export_obj_float4(const std::string& filename, const std::vector<float4>& vertices, const std::vector<float4>& normals)
{
	std::ofstream file;
	file.open(filename);
	//for (const float4 v : vertices)
	//	file << std::fixed << "v " << v.x << ' ' << v.y << ' ' << v.z << std::endl;
	//for (const float4 n : normals)
	//	file << std::fixed << "vn " << n.x << ' ' << n.y << ' ' << n.z << std::endl;

	for (int i = 0; i < vertices.size(); ++i)
	{
		const float4& v = vertices[i];
		const float4& n = normals[i];
		file << std::fixed << "v "
			<< v.x << ' ' << v.y << ' ' << v.z << ' '
			<< ((n.x * 0.5) + 0.5) * 255 << ' ' << ((n.y * 0.5) + 0.5) * 255 << ' ' << ((n.z * 0.5) + 0.5) * 255
			<< std::endl;
	}

	file.close();
}

void convert_normal_to_rgba(std::vector<float4>& normals)
{
	for (float4& n : normals)
	{
		n.x = (n.x * 0.5) + 0.5;
		n.y = (n.y * 0.5) + 0.5;
		n.z = (n.z * 0.5) + 0.5;
		n.w = (n.w * 0.5) + 0.5;
	}
}

void run_back_projection_with_normal_estimate(
	std::vector<float4>& vertices,
	std::vector<float4>& normals,
	const std::vector<ushort>& depth_buffer,
	uint width,
	uint height,
	ushort max_depth)
{
	StopWatchInterface *kernel_timer = nullptr;

	ushort* h_depth_buffer = (ushort*)depth_buffer.data();

	size_t in_pitch, out_pitch;

	ushort* d_depth_buffer = nullptr;
	// copy image data to array
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer,
		in_pitch,
		h_depth_buffer,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	float4* d_vertex_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	float4* d_normal_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_normal_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	sdkCreateTimer(&kernel_timer);
	sdkStartTimer(&kernel_timer);

	Eigen::Matrix4f h_inverse_projection = perspective_matrix_inverse<float>(fov_y, aspect_ratio, near_plane, far_plane);
	//bilateralFilter_normal_estimate_float4((OutputPixelType*)dOutputImage, (InputPixelType*)dInputImage, width, height, in_pitch, out_pitch, max_depth, euclidean_delta, filter_radius, iterations, kernel_timer);
	back_projection_with_normal_estimation(d_vertex_buffer, d_normal_buffer, d_depth_buffer, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());

	checkCudaErrors(cudaDeviceSynchronize());
	sdkStopTimer(&kernel_timer);
	std::cout << "Kernel Timer                              : " << kernel_timer->getTime() << " msec" << std::endl;
	sdkDeleteTimer(&kernel_timer);

	vertices.resize(depth_buffer.size());
	normals.resize(depth_buffer.size());

	cudaMemcpy2D(
		vertices.data(),
		sizeof(float4) * width,
		d_vertex_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		normals.data(),
		sizeof(float4) * width,
		d_normal_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	checkCudaErrors(cudaFree(d_depth_buffer));
	checkCudaErrors(cudaFree(d_vertex_buffer));
	checkCudaErrors(cudaFree(d_normal_buffer));
}

void run_icp_with_normal_estimate(
	std::vector<float4>& vertices_0,
	std::vector<float4>& vertices_1,
	std::vector<float4>& normals,
	std::vector<ushort2>& indices,
	std::vector<float>& distances,
	const std::vector<ushort>& depth_buffer_0,
	const std::vector<ushort>& depth_buffer_1,
	uint width,
	uint height,
	ushort max_depth,
	const ushort half_window_search_size)
{
	StopWatchInterface *kernel_timer = nullptr;

	ushort* h_depth_buffer_0 = (ushort*)depth_buffer_0.data();
	ushort* h_depth_buffer_1 = (ushort*)depth_buffer_1.data();

	size_t in_pitch, out_pitch, index_pitch, distance_pitch;

	//
	// copy depth buffer data to array
	//
	ushort* d_depth_buffer_0 = nullptr;
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer_0, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer_0,
		in_pitch,
		h_depth_buffer_0,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	ushort* d_depth_buffer_1 = nullptr;
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer_1, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer_1,
		in_pitch,
		h_depth_buffer_1,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	//
	// allocate vertex buffer in gpu
	//
	float4* d_vertex_buffer_0;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer_0,
		&out_pitch,
		width * sizeof(float4),
		height));

	float4* d_vertex_buffer_1;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer_1,
		&out_pitch,
		width * sizeof(float4),
		height));

	//
	//  allocate normal buffer in gpu
	//
	float4* d_normal_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_normal_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	sdkCreateTimer(&kernel_timer);
	sdkStartTimer(&kernel_timer);

	Eigen::Matrix4f h_inverse_projection = perspective_matrix_inverse<float>(fov_y, aspect_ratio, near_plane, far_plane);
	back_projection_with_normal_estimation(d_vertex_buffer_0, d_normal_buffer, d_depth_buffer_0, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());
	back_projection_with_normal_estimation(d_vertex_buffer_1, d_normal_buffer, d_depth_buffer_1, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());

	//
	// allocate index buffer in gpu
	//
	ushort2* d_index_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_index_buffer,
		&index_pitch,
		width * sizeof(ushort2),
		height));

	//
	// allocate index buffer in gpu
	//
	float* d_distances_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_distances_buffer,
		&distance_pitch,
		width * sizeof(float),
		height));

	icp_matching_vertices(d_index_buffer, d_distances_buffer, d_vertex_buffer_0, d_vertex_buffer_1, width, height, out_pitch, index_pitch, half_window_search_size);

	checkCudaErrors(cudaDeviceSynchronize());
	sdkStopTimer(&kernel_timer);
	std::cout << "Kernel Timer                              : " << kernel_timer->getTime() << " msec" << std::endl;
	sdkDeleteTimer(&kernel_timer);

	vertices_0.resize(depth_buffer_0.size());
	vertices_1.resize(depth_buffer_1.size());
	normals.resize(depth_buffer_0.size());
	indices.resize(depth_buffer_0.size());
	distances.resize(depth_buffer_0.size());

	cudaMemcpy2D(
		vertices_0.data(),
		sizeof(float4) * width,
		d_vertex_buffer_0,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		vertices_1.data(),
		sizeof(float4) * width,
		d_vertex_buffer_1,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		normals.data(),
		sizeof(float4) * width,
		d_normal_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		indices.data(),
		sizeof(ushort2) * width,
		d_index_buffer,
		index_pitch,
		sizeof(ushort2) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		distances.data(),
		sizeof(float) * width,
		d_distances_buffer,
		distance_pitch,
		sizeof(float) * width,
		height,
		cudaMemcpyDeviceToHost);

	for (int i = 0; i < width * height; ++i)
	{
		ushort2 index = indices[i];
		int ii = index.y * width + index.x;
		//std::cout << index.x << ", " << index.y << std::endl;

		if (ii > width * height)
		{
			std::cout << ii << " ---> " << width * height << std::endl;
		}
	}

	checkCudaErrors(cudaFree(d_depth_buffer_0));
	checkCudaErrors(cudaFree(d_depth_buffer_1));
	checkCudaErrors(cudaFree(d_vertex_buffer_0));
	checkCudaErrors(cudaFree(d_vertex_buffer_1));
	checkCudaErrors(cudaFree(d_normal_buffer));
	checkCudaErrors(cudaFree(d_index_buffer));
	checkCudaErrors(cudaFree(d_distances_buffer));
}

void convertKinectFrame2QImage(const KinectFrame& frame, QImage& depthImage)
{
	QVector<QRgb> colorTable;
	for (int i = 0; i < 256; ++i)
		colorTable.push_back(qRgb(i, i, i));
	depthImage = QImage(frame.depth_width(), frame.depth_height(), QImage::Format::Format_Indexed8);
	depthImage.setColorTable(colorTable);

	// set pixels to depth image
	for (int y = 0; y < depthImage.height(); y++)
	{
		for (int x = 0; x < depthImage.width(); x++)
		{
			const unsigned short depth = frame.depth[y * frame.depth_width() + x];
			depthImage.scanLine(y)[x] = static_cast<uchar>((float)depth / (float)frame.depth_max_distance() * 255.f);;
		}
	}
}

template<typename Type>
void cpu_test(const KinectFrame frame_0, const KinectFrame& frame_1)//, std::vector<Eigen::Vector3f>& vertices_0, std::vector<Eigen::Vector3f>& vertices_1)
{
	Type window_width = 640.0f;
	Type window_height = 480.0f;
	Type near_plane = 0.1f;
	Type far_plane = 10240.0f;
	Type fovy = 60.0f;
	Type aspect_ratio = window_width / window_height;

	Eigen::Matrix<Type, 4, 4> proj_inv = perspective_matrix_inverse(fovy, aspect_ratio, near_plane, far_plane);

	std::vector<Eigen::Matrix<Type, 3, 1>> vertices_0;
	std::vector<Eigen::Matrix<Type, 3, 1>> vertices_1;
	vertices_0.clear();
	vertices_1.clear();

	int d = 0;
	for (int y = 0; y < frame_0.depth_height(); ++y)
	{
		for (int x = 0; x < frame_0.depth_width(); ++x)
		{
			Eigen::Matrix<Type, 3, 1> v = window_coord_to_3d(Eigen::Matrix<Type, 2, 1>(x, y), (Type)frame_0.depth.at(d), proj_inv, (int)window_width, (int)window_height);
			++d;

			vertices_0.push_back(v);
		}
	}

	d = 0;
	for (int y = 0; y < frame_1.depth_height(); ++y)
	{
		for (int x = 0; x < frame_1.depth_width(); ++x)
		{
			Eigen::Matrix<Type, 3, 1> v = window_coord_to_3d(Eigen::Matrix<Type, 2, 1>(x, y), (Type)frame_1.depth.at(d), proj_inv, (int)window_width, (int)window_height);
			++d;

			vertices_1.push_back(v);
		}
	}

	Eigen::Matrix<Type, 3, 3> R = Eigen::Matrix<Type, 3, 3>::Zero();
	Eigen::Matrix<Type, 3, 1> t = Eigen::Matrix<Type, 3, 1>::Zero();

	ComputeRigidTransform(vertices_0, vertices_1, R, t);

	std::cout << std::fixed
		<< "ComputeRigidTransform: " << std::endl
		<< "Rotate" << std::endl << R << std::endl
		<< "Translate" << std::endl << t.transpose() << std::endl
		<< std::endl;
}

int main(int argc, char **argv)
{
	srand(time(NULL));

	std::string filename_0 = "../../data/knt_frames/frame_27.knt";
	std::string filename_1 = "../../data/knt_frames/frame_27.knt";
	ushort half_window_search_size = 3;
	ushort max_distance = 10;

	if (argc < 3)
	{
		std::cerr << "Usage: ICP_gpu.exe ../../data/knt_frames/frame_27.knt ../../data/knt_frames/frame_27.knt 3 10"
		<< std::endl
		<< "The app will continue with default parameters."
		<< std::endl;
	}
	else
	{
		filename_0 = argv[1];
		filename_1 = argv[2];
	}

	if (argc > 3)
		half_window_search_size = atoi(argv[3]);

	if (argc > 4)
		max_distance = atoi(argv[4]);

	Timer timer;

	timer.start();
	KinectFrame frame_0, frame_1;
	QKinectIO::loadFrame(QString::fromStdString(filename_0), frame_0);
	QKinectIO::loadFrame(QString::fromStdString(filename_1), frame_1);
	timer.print_interval("Importing kinect frames (.knt)            : ");

	//std::vector<Eigen::Vector3f> verts_0, verts_1;
	//cpu_test<float>(frame_0, frame_1);
	//return 0;

	std::vector<float4> vertices_0, vertices_1, normals;
	std::vector<ushort2> indices;
	std::vector<float> distances;
	std::vector<Eigen::Vector3f> vertices_00, vertices_11;

	timer.start();
	run_icp_with_normal_estimate(
		vertices_0,
		vertices_1,
		normals,
		indices,
		distances,
		frame_0.depth,
		frame_1.depth,
		frame_0.depth_width(),
		frame_0.depth_height(),
		frame_0.depth_max_distance(),
		half_window_search_size);
	timer.print_interval("Running normal estimate in GPU            : ");

	vertices_1.clear();
	for (const ushort2 index : indices)
	{
		float4 v = vertices_0.at(index.y * frame_0.depth_width() + index.x);
		vertices_1.push_back(v);
	}

	//for (int i = 0; i < vertices_0.size(); ++i)
	//{
	//	const float4& v0 = vertices_0.at(i);
	//	const float4& v1 = vertices_1.at(i);

	//	if (v0.z > 0.1 && v1.z > 0.1)
	//	{
	//		vertices_00.push_back(Eigen::Vector3f((Eigen::Vector4f(v0.x, v0.y, v0.z, v0.w) / v0.w).head<3>()));
	//		vertices_11.push_back(Eigen::Vector3f((Eigen::Vector4f(v1.x, v1.y, v1.z, v1.w) / v1.w).head<3>()));
	//	}
	//}

	for (int y = 0; y < frame_0.depth_height(); ++y)
	{
		for (int x = 0; x < frame_0.depth_width(); ++x)
		{
			int i = y * frame_0.depth_width() + x;

			float distance = distances[i];

			if (distance > max_distance)
				continue;

			ushort2 index = indices[i];
			int ii = index.y * frame_0.depth_width() + index.x;

			const float4& v0 = vertices_0.at(ii);
			const float4& v1 = vertices_1.at(ii);

			vertices_00.push_back(Eigen::Vector3f((Eigen::Vector4f(v0.x, v0.y, v0.z, v0.w) / v0.w).head<3>()));
			vertices_11.push_back(Eigen::Vector3f((Eigen::Vector4f(v1.x, v1.y, v1.z, v1.w) / v1.w).head<3>()));

		}

	}

	std::cout << "Vertices size " << vertices_0.size() << ", " << vertices_1.size() << std::endl;
	std::cout << "Vertices size " << vertices_00.size() << ", " << vertices_11.size() << std::endl;

	Eigen::Matrix<float, 3, 3> R = Eigen::Matrix<float, 3, 3>::Zero();
	Eigen::Matrix<float, 3, 1> t = Eigen::Matrix<float, 3, 1>::Zero();

	ComputeRigidTransform(vertices_00, vertices_11, R, t);

	std::cout << std::fixed
		<< "ComputeRigidTransform: " << std::endl
		<< "Rotate" << std::endl << R << std::endl
		<< "Translate" << std::endl << t.transpose() << std::endl
		<< std::endl;

	return 0;

	//
	// Viewer
	//
	QApplication app(argc, argv);

	QSurfaceFormat format;
	format.setDepthBufferSize(24);
	QSurfaceFormat::setDefaultFormat(format);

	GLPointCloudViewer glwidget;
	glwidget.resize(1024, 848);
	glwidget.move(0, 0);
	glwidget.setWindowTitle("Point Cloud");
	glwidget.show();

	std::shared_ptr<GLPointCloud> cloud_0(new GLPointCloud);
	cloud_0->initGL();
	//cloud_0->setVertices((float*)&vertices_0.data()[0], (uint)vertices_0.size(), 4);
	cloud_0->setVertices((float*)&vertices_00.data()[0], (uint)vertices_00.size(), 4);
	cloud_0->setColor(QVector3D(1, 0, 0));

	std::shared_ptr<GLPointCloud> cloud_1(new GLPointCloud);
	cloud_1->initGL();
	//cloud_1->setVertices((float*)&vertices_1.data()[0], (uint)vertices_1.size(), 4);
	cloud_1->setVertices((float*)&vertices_11.data()[0], (uint)vertices_11.size(), 4);
	cloud_1->setColor(QVector3D(0, 0, 1));

	glwidget.addPointCloud(cloud_0);
	glwidget.addPointCloud(cloud_1);

	glwidget.setWeelSpeed(0.1f);
	glwidget.setPosition(0, 0, -0.5f);

	return app.exec();
}


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_TRI_LOW_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_TRI_LOW_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mdivide_right_tri.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>

namespace stan {
  namespace math {

    /**
     * Returns the solution of the system tri(A)x=b when tri(A) is a
     * lower triangular view of the matrix A.
     * @param A Matrix.
     * @param b Right hand side matrix or vector.
     * @return x = b * tri(A)^-1, 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 <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_right_tri_low(const Eigen::Matrix<T1, R1, C1> &b,
                          const Eigen::Matrix<T2, R2, C2> &A) {
      return mdivide_right_tri<Eigen::Lower>
        (promote_common<Eigen::Matrix<T1, R1, C1>,
         Eigen::Matrix<T2, R1, C1> >(b),
         promote_common<Eigen::Matrix<T1, R2, C2>,
         Eigen::Matrix<T2, R2, C2> >(A));
    }

  }
}
#endif


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

  @copyright 2016 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_ARCH_COMMON_SIMD_FUNCTION_ROUND_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ROUND_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/none.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(round_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return a0;
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(round_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        const A0 v = bs::abs(a0);
        const A0 c = bs::ceil(v);
        return if_else(v > Maxflint<A0>(), a0, copysign(if_dec(c-Half<A0>() > v, c), a0));
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(round_
                             , (typename A0, typename A1, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_<bd::single_<A0>, X>
                             , bs::pack_<bd::integer_<A1>, X>
                             )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
        A0 fac = tenpower(a1);
        A0 tmp1 = round(a0*fac)/fac;
        return if_else(is_ltz(a1), round(tmp1), tmp1);
      }
   };

   BOOST_DISPATCH_OVERLOAD(round_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bd::scalar_<bd::unsigned_<A1>>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const & a0,  A1 const & a1) const
      {
        using itype_t = bd::as_integer_t<A0, unsigned>;
        return round(a0, splat<itype_t>(a1));
      }
   };

   BOOST_DISPATCH_OVERLOAD(round_
                          , (typename A0, typename X, typename A1)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bd::scalar_<bd::integer_<A1>>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const & a0,  A1 const & a1) const
      {
        using itype_t = bd::as_integer_t<A0>;
        return round(a0, splat<itype_t>(a1));
      }
   };
} } }

#endif


//
// Copyright (C) 2016 LAAS-CNRS
//
// Author: Rohan Budhiraja
//

#ifndef SOT_TOOLS_KINEMATIC_PLANNER_HH
#define SOT_TOOLS_KINEMATIC_PLANNER_HH

/* STD */
#include <string>
#include <sstream>
#include <list>
#include <complex>

/* dynamic-graph */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>

#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>

/*Eigen*/
#include <Eigen/StdVector>
#include <unsupported/Eigen/FFT>
#include <unsupported/Eigen/Splines>
#include <unsupported/Eigen/MatrixFunctions>

/* BOOST */
//#include <boost/filesystem.hpp>

namespace dynamicgraph {
namespace sot {
namespace tools {

using dynamicgraph::Entity;

class KinematicPlanner : public Entity {
 public:
  DYNAMIC_GRAPH_ENTITY_DECL();
  typedef std::vector<Eigen::ArrayXd, Eigen::aligned_allocator<Eigen::ArrayXd> > stdVectorofArrayXd;

  typedef std::vector<Eigen::ArrayXXd, Eigen::aligned_allocator<Eigen::ArrayXXd> > stdVectorofArrayXXd;

  /*-----SIGNALS--------*/
  typedef int Dummy;
  /*
    dynamicgraph::SignalPtr<double,int> distToDrawerSIN;
    dynamicgraph::SignalPtr<double,int> objectPositionInDrawerSIN;

    dynamicgraph::SignalTimeDependent<Dummy,int> trajectoryReadySINTERN;

    dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> upperBodyJointPositionSOUT;
    dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> upperBodyJointVelocitySOUT;
    dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> freeFlyerVelocitySOUT;
  */
  /* --- CONSTRUCTOR --- */
  KinematicPlanner(const std::string& name);
  virtual ~KinematicPlanner(void);
  // Sources
  Eigen::ArrayXd npSource;
  Eigen::ArrayXXd pSource1;
  Eigen::ArrayXXd pSource2;

  stdVectorofArrayXXd pSourceDelayed1;
  stdVectorofArrayXXd pSourceDelayed2;
  // Delays
  Eigen::ArrayXXd pDelay1;
  Eigen::ArrayXXd pDelay2;

  // Non Periodic Weights
  Eigen::ArrayXXd wNonPeriodic;  // Eigen::Array<double, 480,4>

  // Periodic Weights
  stdVectorofArrayXXd wPeriodic1;
  stdVectorofArrayXXd wPeriodic2;

  // Mean joint angles
  Eigen::ArrayXXd mJointAngle;

  // Number of Trajectories Created
  //	int nTrajectories; //30
  int nJoints;      // 16
  int nGaitCycles;  // 4
  int nTimeSteps;   // 160
  int nSources1;    // 5
  int nSources2;    // 4
  /*! @} */
  std::list<dynamicgraph::SignalBase<int>*> genericSignalRefs;

  // Load Motion Capture outputs
  template <typename Derived>
  void read2DArray(std::string& fileName, Eigen::DenseBase<Derived>& outArr);

  void setParams(const double& _distanceToDrawer, const double& _objectPositionInDrawer, const std::string& dir);
  void loadSourceDelays(const std::string& dir);
  void loadTrainingParams(const std::string& dir, dynamicgraph::Matrix& q, dynamicgraph::Matrix& beta3,
                          Eigen::ArrayXd& mwwn, double& sigma2, int& N, int& K);
  dynamicgraph::Vector createSubGoals(double D, double P);
  void delaySources();
  void blending();
  void smoothEnds(Eigen::Ref<Eigen::ArrayXd> tr);
  void bSplineInterpolate(Eigen::ArrayXXd& tr, int factor);
  int& runKinematicPlanner(int& dummy, int time);
  void goalAdaption(dynamicgraph::Vector& goals, const std::string&);
  void savitzkyGolayFilter(Eigen::Ref<Eigen::ArrayXXd> allJointTraj, int polyOrder, int frameSize);

  bool parametersSet;
};  // class KinematicPlanner
}  // namespace tools
}  // namespace sot
}  // namespace dynamicgraph

#endif  // SOT_TOOLS_KINEMATIC_PLANNER_HH


#include "kmers/dirichlet-sampler.hpp"
#include "kmers/fasta-parser.hpp"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <locale>
#include <stdexcept>
#include <vector>

int64_t num_kmers(int64_t K) {
  int64_t y = 1;
  for (int k = 0; k < K; ++k) {
    y *= 4;
  }
  return y;
}

template <typename T>
void write_val(std::ostream& out, const T& x) {
  out.write((char *)(&x), sizeof(T));
}

template <typename T>
void write_array(std::ostream& out, const T* x, int32_t N) {
  for (int32_t n = 0; n < N; ++n)
    write_val<T>(out, x[n]);
}

struct number_format : public std::numpunct<char> {
  char do_thousands_sep() const { return ','; }
  std::string do_grouping() const { return "\03"; }
};

bool valid_base(char c) {
    switch (c) {
    case 'A':
    case 'C':
    case 'G':
    case 'T': return true;
    default: return false;
    }
}

bool valid_bases(const std::string& seq) {
  return std::all_of(seq.begin(), seq.end(),
                     [](char x) { return valid_base(x); });
}

template <typename F1, typename F2>
struct coupler {
  F1& f1_;
  F2& f2_;
  coupler(F1& f1, F2& f2) : f1_(f1), f2_(f2) { }
  void operator()(const std::string& id, const std::string& seq) {
    f1_(id, seq);
    f2_(id, seq);
  }
  void report() {
    f1_.report();
    f2_.report();
  }
};
template <typename F>
F couple(F& f) {
  return f;
}

template <typename F1, typename F2>
coupler<F1, F2> couple(F1& f1, F2& f2) {
  return coupler<F1, F2>(f1, f2);
}

struct shredder {
  const int64_t K_;
  std::vector<int32_t> count_;
  shredder(int64_t K) : K_(K), count_(fasta::kmers(K), 0) { }
  void operator()(const std::string& id, const std::string& seq) {
    for (int32_t start = 0; start < (seq.size() - K_ + 1); ++start) {
      std::string kmer = seq.substr(start, K_);
      try {
	int32_t id = fasta::kmer_id(kmer);
	++count_[id];
      } catch (...) {
	std::cout << "     # illegal kmer = |" << kmer << "|"
		  << "   in ref id = " << id.substr(0, std::min<size_t>(15U, id.size()))
		  << std::endl;
      }
    }
  }
  void report() {
    std::cout << "Writing histogram to histo.csv" << std::endl;
    std::ofstream f("histo.csv");
    f << "id,count" << std::endl;
    for (size_t i = 0; i < count_.size(); ++i)
      f << i << "," << count_[i] << std::endl;
    f.close();
  }
};

struct validator {
  size_t invalid_count_;
  validator() : invalid_count_(0) { }
  void operator()(const std::string& id, const std::string& seq) {
    for (char c : seq) {
      if (!valid_base(c)) {
	++invalid_count_;
	std::cout << "expecting one of {A,C,G,T}, found base = " << c
		  << ";  seq id = " << id
		  << std::endl;
      }
    }
  }
  void report() const {
    std::cout << invalid_count_ << " invalid sequences"
	      << std::endl;
  }
};

struct counter {
  size_t num_targets_;
  size_t num_bases_;
  counter() : num_targets_(0), num_bases_(0) { }
  void operator()(const std::string id, const std::string& seq) {
    ++num_targets_;
    num_bases_ += seq.size();
    if ((num_targets_ % 10000) == 0)
      std::cout << "     # targets = " << num_targets_ << std::endl;
  }
  void report() const {
    number_format nf;
    std::cout.imbue({std::locale(), &nf});
    std::cout << "counter.report():  " << num_targets_ << " targets"
	      << std::endl;
    std::cout << "counter.report():  " << num_bases_ << " bases"
	      << std::endl;
  }
};

struct triplet_counter {
  int32_t K_;
  int32_t ref_id_;
  std::vector<Eigen::Triplet<float, int32_t>> kmer_count_;
  triplet_counter(int32_t K) : K_(K), ref_id_(0), kmer_count_() { }
  void operator()(const std::string& id, const std::string& seq) {
    int32_t num_kmers = seq.size() - K_ + 1;
    float prob_kmer = 1.0f / num_kmers;
    for (int32_t start = 0; start < num_kmers; ++start) {
      std::string kmer = seq.substr(start, K_);
      try {
	int32_t kmer_id = fasta::kmer_id(kmer);
        kmer_count_.emplace_back(kmer_id, ref_id_, prob_kmer);
      } catch (...) {
	std::cout << "     # illegal kmer = |" << kmer << "|"
		  << "   in ref id = " << id.substr(0, std::min<size_t>(15U, id.size()))
		  << std::endl;
      }
    }
    ++ref_id_;
  }
  void write_matrix(const std::string& filename) const {
    int32_t M = num_kmers(K_);
    int32_t N = ref_id_;
    Eigen::SparseMatrix<float, Eigen::RowMajor> x(M, N);
    x.setFromTriplets(kmer_count_.begin(), kmer_count_.end());
    x.makeCompressed();

    std::fstream out(filename, std::ios::binary | std::ios::out);
    if (!out) {
      throw std::runtime_error("cannot open file = " + filename
                               + " for writing");
    }
    write_val<int>(out, x.rows());
    write_val<int>(out, x.cols());
    write_val<int>(out, x.nonZeros());
    int* outerIndices = x.outerIndexPtr();
    write_array<int>(out, outerIndices, x.rows() + 1);
    int* innerIndices = x.innerIndexPtr();
    write_array<int>(out, innerIndices, x.nonZeros());
    float* values = x.valuePtr();
    write_array<float>(out, values, x.nonZeros());
    out.close();
  }
  void report() const {
    // writing kmer_count_.size(), K_, or ref_id_ causes abnormal termination
    std::cout << "triplet_counter.report():  nothing to report"
              << std::endl;
  }
};

int main(int argc, char* argv[]) {
  std::string fastafile = argv[1];
  std::cout << "main:  fasta file = " << fastafile
            << std::endl;

  std::string binoutfile = argv[2];
  std::cout << "main:  binary output file = " << binoutfile
            << std::endl;

  std::size_t K = 10;
  std::cout << "main:  K = " << K
	    << std::endl;

  int32_t M = num_kmers(K);
  std::cout << "main:  num kmers = " << M
	    << std::endl;

  // shredder shred_handler = shredder(K);
  // validator validate_handler = validator();
  counter count_handler = counter();
  triplet_counter triplet_handler = triplet_counter(K);

  coupler<counter, triplet_counter> handler = couple(count_handler, triplet_handler);
  fasta::parse_file(fastafile, handler);
  handler.report();
  triplet_handler.write_matrix(binoutfile);

  std::cout << "main:  FINI." << std::endl;
  return 0;
}


/// \file

#include <iostream>
#include <stdio.h>
#include <fstream>
#include <Eigen/Dense>
#include "Perceptron.hpp"
#include <cstdlib>
#include <tbb/tbb.h>
#include <tbb/task_scheduler_init.h>

#define BLACK '#'
#define WHITE '_'

/// \brief Permet de savoir si dans quelle mode lire les metadonnees d'un fichier IDX (low-endien ou big-endian)
/// \param f Pointeur sur la chaine de caractere contenant le nom du fichier IDX
/// \return le mode selon lequel lire le fichier IDX (0 ou 1)
/// \warning teste uniquement sur des processeurs Intel!!!
int idxEndianness(char* f)
{
  int n = 0;
  char* r = nullptr;
  r = (char*)&n;
  std::ifstream in(f, std::ifstream::in);
  //On lit les 4 premiers octets
  in.read(r, 4);
  if(n != 2049 && n!= 2051)
  {
      return 0;
  }
  else
  {
      return 1;
  }
}

/// \brief Lire les metadonnees d'un fichier IDX (fonction recursive)
/// \param in Flux du fichier IDX a lire
/// \param mode Mode Mode pour lire le fichier selon l'endianness
/// \param n Reference vers la variable dans laquelle mettre la prochaine metadonnee a lire
/// \param args References vers les prochaines variable dans laquelle mettre les metadonnees suivantes a lire
/// \warning teste uniquement sur des processeurs Intel!!!
template<typename... Args>
void idxMeta(std::ifstream& in, int mode, std::size_t& n, Args& ...args)
{
  char* r = nullptr;
  r = (char*)&n;
  //On lit les 4 premiers octets
  in.read(r, 4);
  //Sur Intel, on les inverses
  if(mode==0)
  {
    char tmp;
    tmp = r[0];
    r[0] = r[3];
    r[3] = tmp;
    tmp = r[1];
    r[1] = r[2];
    r[2] = tmp;
  }
  idxMeta(in, mode, args...);
}

/// \brief Dernier appel recursif de idxMeta
/// \warning teste uniquement sur des processeurs Intel!!!
template<>
void idxMeta(std::ifstream& in, int mode, std::size_t& n)
{
  char* r = nullptr;
  r = (char*)&n;
  //On lit les 4 premiers octets
  in.read(r, 4);
  //Sur Intel, on les inverses
  if(mode == 0)
  {
    char tmp;
    tmp = r[0];
    r[0] = r[3];
    r[3] = tmp;
    tmp = r[1];
    r[1] = r[2];
    r[2] = tmp;
  }
}

/// \brief On charge dans une entree-sortie de reseau le prochain element qui se trouve dans un fichier IDX
/// \param fin Flux du fichier IDX a lire
/// \param vout Reference vers l'entree-sortie de reseau dans laquelle on charge le prochain element du fichier
/// \param size Taille du prochain element a lire
/// \return Rien
/// \tparam t Type de l'entree-sortie de reseau dans laquelle on charge le prochain element du fichier IDX
/// \tparam SIZE Taille de l'entree-sortie de reseau dans laquelle on charge le prochain element du fichier IDX
/// \warning teste uniquement sur des processeurs Intel!!!
template<typename t, int SIZE>
void nextIdx(std::ifstream& fin, neuralnetwork::InOut<t, SIZE>& vout, std::size_t size)
{
  for (int i = 0; i < size; ++i)
  {
    unsigned char r;
    fin.read((char*)&r, 1);
    vout(i) = r;
  }
}

/// \brief Conversion d'un label entier en label vecteur pour la classification de caractere numerique
/// \param in Label entier
/// \param out Label vecteur en sortie
/// \return Rien
/// \tparam t Type d'entree du reseau (a priori un double)
/// \tparam INPUT_SIZE Taille d'entree du reseau
template<typename t, int INPUT_SIZE>
void labelToVector(int in, neuralnetwork::InOut<t, INPUT_SIZE>& out)
{
    out << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
    out(in) = 1;
}

/// \brief Affiche une entree-sortie de reseau contenant une images de MNIST (principalement utile au debugage)
/// \param rows Nombre de lignes de l'image (a priori 28)
/// \param cols Nombre de colonnes de l'image (a priori 28)
/// \param v Entree-sortie de reseau a afficher
/// \return Rien
/// \tparam t Type de l'entree-sortie de reseau a afficher
/// \tparam SIZE Taille de l'entree-sortie a afficher
template<typename t, int SIZE>
void printInOut(std::size_t rows, std::size_t cols, neuralnetwork::InOut<t, SIZE> v)
{
  for(int i = 0; i < rows; ++i)
  {
    for(int j = 0; j < cols; ++j)
    {
      if (v(i*rows + j) != 0)
      {
        std::cout << BLACK;
      }
      else
      {
        std::cout << WHITE;
      }
    }
    std::cout << std::endl;
  }
}

/// \brief Entrainement du reseau pour la classification de caracteres
/// \param pathImages Pointeur vers la chaine de caractere contenant le nom de fichier des images d'entrainement
/// \param pathLabels Pointeur vers la chaine de caractere contenant le nom de fichier des labels d'entrainement
/// \param net Reseau a entrainer
/// \param step Pas d'apprentissage
/// \param nbIter Nombre d'iterations a effectuer sur l'ensemble des images d'entrainement
/// \param nbImages Nombre d'images a charger en memoire simultanement
/// \return -1 en cas de probleme d'allocation memoires (trop d'images a charger simultanement)
/// \tparam DEPTH Profondeur du reseau de neurones
template<std::size_t DEPTH>
int train(char* pathImages, char* pathLabels, neuralnetwork::Perceptron<DEPTH>& net, double step, std::size_t nbIter, std::size_t nbImages)
{

  std::size_t magicImTrain = 0, magicLabTrain = 0;
  std::size_t nbImagesTrain = 0;
  std::size_t rows = 0;
  std::size_t cols = 0;
  neuralnetwork::InOut<double, 784>* inIm = NULL;
  neuralnetwork::InOut<double, 1>* inLab = NULL;
  neuralnetwork::InOut<double, 10>* inLabVect = NULL;
  int mode = idxEndianness(pathLabels);
  //Si on a 0 en nombre d'exemples a charger en memoire on chargera tous les exemples de la base de donnees
  if(nbImages == 0)
  {
    std::ifstream flabels(pathLabels, std::ifstream::in);
    idxMeta(flabels, mode, magicLabTrain, nbImagesTrain);
    nbImages = nbImagesTrain;
    flabels.close();
  }

  inIm = new neuralnetwork::InOut<double, 784> [nbImages];
  inLab = new neuralnetwork::InOut<double, 1> [nbImages];
  inLabVect = new neuralnetwork::InOut<double, 10> [nbImages];
  if(inIm == NULL || inLab == NULL || inLabVect == NULL)
  {
    return -1;
  }

  //On entraine autant de fois que voulu
  for(int iter = 0; iter < nbIter; ++iter)
  {
    std::cout << "Iteration [" << iter << "/" << nbIter << "]" << std::endl;
    std::ifstream fimages(pathImages, std::ifstream::in);
    std::ifstream flabels(pathLabels, std::ifstream::in);
    idxMeta(flabels, mode, magicLabTrain, nbImagesTrain);
    idxMeta(fimages, mode, magicImTrain, nbImagesTrain, rows, cols);
    //Tant que l'on a pas fait tous les exemples
    for(int n = 0; n < nbImagesTrain; n+=nbImages)
    {
      std::cout << "Iteration [" << n << "/" << nbImagesTrain << "]" << std::endl;
      //On charge les exemples et les labels
      for(int i = 0; i < nbImages; ++i)
      {
        //On charge l'exemple et le label
        nextIdx(fimages, inIm[i], 784);
        nextIdx(flabels, inLab[i], 1);
        int valLab = inLab[i](0);
        //Conversion du label en vecteur
        labelToVector(valLab, inLabVect[i]);
      }
    //std::cout << "Iteration " << iter + 1 << ", entrainement avec les image de " << n << " a " << n+nbImages << "..." << std::endl;
    //On entraine sur le lot charge
    net.parallel_backpropagation(inIm, inLabVect, nbImages, step);
    //net.backpropagation(inIm, inLabVect, nbImages, step);
    }

    fimages.close();
    flabels.close();
  }

  delete [] inIm;
  delete [] inLab;
  delete [] inLabVect;

}

/// \brief Test du reseau pour la classification de caracteres
/// \param pathImages Pointeur vers la chaine de caractere contenant le nom de fichier des images de test
/// \param pathLabels Pointeur vers la chaine de caractere contenant le nom de fichier des labels de test
/// \param net Reseau a tester
/// \return Pourcentage de reussite
/// \tparam DEPTH Profondeur du reseau de neurones
template<std::size_t DEPTH>
double test(char* pathImages, char* pathLabels, neuralnetwork::Perceptron<DEPTH>& net)
{
  std::size_t magicImTrain = 0, magicLabTrain = 0, magicImTest = 0, magicLabTest = 0;
  std::size_t nbImagesTrain = 0, nbImagesTest = 0;
  std::size_t rows = 0;
  std::size_t cols = 0;
  neuralnetwork::InOut<double, 784> inImTest;
  neuralnetwork::InOut<double, 1> inLabTest;
  int mode = idxEndianness(pathLabels);
  std::ifstream fimagesTest(pathImages, std::ifstream::in);
  std::ifstream flabelsTest(pathLabels, std::ifstream::in);

  idxMeta(flabelsTest, mode, magicLabTest, nbImagesTest);
  idxMeta(fimagesTest, mode, magicImTest, nbImagesTest, rows, cols);

  int count = 0;
  //Pour tous les exemples de tests
  for(int i = 0; i < 10000; ++i)
  {
    neuralnetwork::InOut<double, 10> out;
    neuralnetwork::InOut<double, 10> realOut;

    //On charge les exemples avec leurs labels
    nextIdx(fimagesTest, inImTest, 784);
    nextIdx(flabelsTest, inLabTest, 1);

    //On convertit le label en vecteur
    labelToVector(inLabTest(0), realOut);

    //On fait evaluer l'exemple par le reseau de neurones
    //net.feedForward(inImTest, out, false, false);
    net.parallel_feedForward(inImTest, out);

    //Pour chaque element i du vecteur de sortie, s'il est superieur a 0.5, on estime que le reseau a trouve que l'exemple est i - 1
    //Par exemple si le reseau renvoi [0.12, 0.23, 0.87, 0.34, 0.45, 0.47, 0.37, 0.28, 0.19 , 0.09] alors on interprete que le solution trouvee est [0, 0, 1, 0, 0, 0, 0, 0, 0, 0] et donc que le nombre trouve est 2
    for(int j = 0; j < 10; ++j)
    {
      if(out(j) > 0.5)
      {
        out(j) = 1.0;
      }
      else
      {
        out(j) = 0.0;
      }
    }
    //Si la bonne solution est trouvee, alors on incremente le coompteur
    if(out == realOut)
    {
      ++count;
    }
  }

  //On retourne le pourcentage de reussite
  return count*100.0/10000.0;

}

/// \brief Itere un certain nombre de fois sur la base d'entrainement et apres chaque iteration test le reseau sur la base de test, le taux de reussite de chaque test est stocke dans un fichier
/// \param pathImTrain Pointeur vers la chaine de caractere contenant le nom de fichier des images d'entrainement
/// \param pathLabTrain Pointeur vers la chaine de caractere contenant le nom de fichier des labels d'entrainement
/// \param pathImTest Pointeur vers la chaine de caractere contenant le nom de fichier des images de test
/// \param pathLabTest Pointeur vers la chaine de caractere contenant le nom de fichier des labels de test
/// \param plot Pointeur vers la chaine de caractere contenant le nom de fichier dans lequel stocker les resultats
/// \param net Reseau a entrainer
/// \param step Pas d'apprentissage
/// \param nbIter Nombre d'iterations a effectuer sur l'ensemble des images d'entrainement
/// \param nbImages Nombre d'images a charger en memoire simultanement
/// \return Rien
/// \tparam DEPTH Profondeur du reseau de neurones
template<std::size_t DEPTH>
void bench(char* pathImTrain, char* pathLabTrain, char* pathImTest, char* pathLabTest, char* plot, neuralnetwork::Perceptron<DEPTH>& net, double step, std::size_t nbIter, std::size_t nbImages)
{
  std::ofstream fplot(plot, std::ofstream::out);
  for(int i = 0; i < nbIter; ++i)
  {
    train(pathImTrain, pathLabTrain, net, step, 1, nbImages);
    double resTest = test(pathImTest, pathLabTest, net);
    fplot << resTest << std::endl;
  }
}

int main()
{
  char pathImTrain [] = "../datas/imTrain";
  char pathLabTrain [] = "../datas/labTrain";
  char pathImTest [] = "../datas/imTest";
  char pathLabTest [] = "../datas/labTest";
  char pathPlot [] = "../benchmarks/plot.txt";

  //tbb::task_scheduler_init init(4);

  neuralnetwork::Perceptron<4>  net(
    -0.0001,
    0.0001,
    784,
    200, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID,
    200, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID,
    200, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID,
    10, neuralnetwork::activation::SIGMOID, neuralnetwork::activation::D_SIGMOID
  );
    net.initGraph(0.1);
    train(pathImTrain, pathLabTrain,  net, 0.1, 50, 1000);
    double res = test(pathImTest, pathLabTest, net);
    std::cout << "reussite de " << res << " pourcents apres entrainement" << std::endl;

  return 0;
}


#include <boost/test/unit_test.hpp>
#include <climits>
#include <iostream>

#define private public
#include "QuantizedLogProb.hh"
#undef private

using namespace std;

BOOST_AUTO_TEST_CASE(QuantizedLogProbInit)
{
    double minLogProb = -15.0;
    QuantizedLogProb qlp(minLogProb);
    BOOST_CHECK_EQUAL( qlp.m_minLogProb, minLogProb);
    BOOST_CHECK_EQUAL( qlp.m_quantizedLogProbs.size(), USHRT_MAX+1 );
    for (int i=1; i<(int)qlp.m_quantizedLogProbs.size(); i++)
        BOOST_CHECK( qlp.m_quantizedLogProbs[i] < qlp.m_quantizedLogProbs[i-1] );
    BOOST_CHECK_EQUAL( qlp.m_quantizedLogProbs[0], 0.0);
    BOOST_CHECK_EQUAL( qlp.m_quantizedLogProbs[USHRT_MAX], minLogProb);
}

BOOST_AUTO_TEST_CASE(QuantizedLogProbIndex)
{
    double minLogProb = -15.0;
    QuantizedLogProb qlp(minLogProb);
    double origLogProb = -8.0;
    unsigned short int qIdx = qlp.getQuantIndex(origLogProb);
    double qLogProb = qlp.getQuantizedLogProb(qIdx);
    BOOST_CHECK_CLOSE( origLogProb, qLogProb, 0.001);
    BOOST_CHECK_EQUAL( 34952, qIdx );
    BOOST_CHECK_EQUAL( qlp.getQuantIndex(0.0), 0 );
    BOOST_CHECK_EQUAL( qlp.getQuantIndex(minLogProb), USHRT_MAX );
}