/**
 * @file
 * This file is part of SeisSol.
 *
 * @author Sebastian Rettenberger (sebastian.rettenberger AT tum.de, http://www5.in.tum.de/wiki/index.php/Sebastian_Rettenberger)
 *
 * @section LICENSE
 * Copyright (c) 2015-2016, SeisSol Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. 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.
 *
 * @section DESCRIPTION
 **/

#include <cstring>
#include <limits>
#include <string>

#include <pthread.h>

#include <pstream.h>

#include <Eigen/Dense>

#include <netcdf.h>

#include <proj_api.h>

#include "utils/args.h"
#include "utils/logger.h"
#include "utils/stringutils.h"

const static float MIN_VALID = -10000;

typedef Eigen::Array<unsigned long, 3, 1> ularr3;
typedef Eigen::Array<unsigned long, 2, 1> ularr2;

struct inputData_t
{
        Eigen::Array3d minVert;
        ularr3 gridSize;
	Eigen::Array3d gridDist;

	const char* meshProj;
	bool meshIsGeo;
	const char* queryProj;
	bool queryIsGeo;

	redi::rpstream& query;
};

/**
 * Check netCDF return value for errors
 */
static void checkNcIError(int error)
{
	if (error != NC_NOERR)
		logError() << "Error while reading netCDF file:" << nc_strerror(error);
}

/**
 * Check netCDF return value for errors
 */
static void checkNcOError(int error)
{
	if (error != NC_NOERR)
		logError() << "Error while writing netCDF file:" << nc_strerror(error);
}

/**
 * The input thread for the query program
 */
static void* runInput(void* p)
{
	inputData_t* i = reinterpret_cast<inputData_t*>(p);

	// Project from mesh to sample code
	unsigned long totalArea = i->gridSize(0) * i->gridSize(1);
	double* projX = new double[totalArea];
	double* projY = new double[totalArea];

	const bool meshIsGeo = i->meshIsGeo;

	for (unsigned long y = 0; y < i->gridSize(1); y++) {
		for (unsigned long x = 0; x < i->gridSize(0); x++) {
			ularr2 pos(x, y);
                        Eigen::Array2d coord = i->minVert.block<2,1>(0,0) + pos.cast<double>() * i->gridDist.block<2,1>(0,0);
			if (meshIsGeo)
				coord *= Eigen::Array2d(DEG_TO_RAD, DEG_TO_RAD);

			projX[y*i->gridSize(0) + x] = coord(0);
			projY[y*i->gridSize(0) + x] = coord(1);
		}
	}

	// Project coordinates
	projPJ pjMesh = pj_init_plus(i->meshProj);
	projPJ pjQuery = pj_init_plus(i->queryProj);
	if (pj_transform(pjMesh, pjQuery, totalArea, 1, projX, projY, 0L) != 0)
		logError() << "Coordinate transformation failed";

	// Convert to degree
	if (i->queryIsGeo) {
		for (unsigned long x = 0; x < totalArea; x++) {
			projX[x] *= RAD_TO_DEG;
			projY[x] *= RAD_TO_DEG;
		}
	}

	// Send the input data
	i->query << (totalArea * i->gridSize(2)) << std::endl;
	for (unsigned long z = 0; z < i->gridSize(2); z++) {
		double depth = i->minVert(2) + z * i->gridDist(2);
		for (unsigned long x = 0; x < totalArea; x++)
			i->query << projX[x] << ' ' << projY[x] << ' ' << depth << std::endl;
	}

	// Send eof
	i->query << redi::peof;

	delete [] projX;
	delete [] projY;

	return 0L;
}

static float vs2mu(float vs, float rho)
{
	return vs*vs * rho;
}

static float vp2lambda(float vp, float rho, float mu)
{
	return vp*vp * rho - 2*mu;
}

int main(int argc, char* argv[])
{
	// Parse command line arguments
	utils::Args args;
	args.addOption("mesh", 'm', "The mesh for which the data should be collected");
	args.addOption("border", 'b', "Border around the mesh for which data should be included",
			utils::Args::Required, false);
	args.addOption("size", 's', "Size of the out grid (Format x:y:z)");
	args.addOption("query", 'q', "Path to the query program (Default: ./scripts/vx_lite_wrapper)",
			utils::Args::Required, false);
//	args.addOption("elevation", 'e', "Path to x_SRTM3, y_SRTM3, z_SRTM3 containing an elevation map\n"
//			"                              If empty, elevation instead of elevation offset is used for querying.",
//			utils::Args::Required, false);
	// See http://spatialreference.org/
	args.addOption("mesh-proj", 0, "Mesh coordinate projection (Default: UTM11)",
			utils::Args::Required, false);
	args.addOption("mesh-proj-geo", 0, "Mesh coordinates are geographic locations (Default: No)",
			utils::Args::Required, false);
	args.addOption("query-proj", 0, "Query coordinate projection (Default: WGS84)",
			utils::Args::Required, false);
	args.addOption("query-proj-geo", 0, "Query coordinates are geographic locations (Default: Yes)",
			utils::Args::Required, false);
	args.addOption("paraview", 'p', "Viewable by Paraview", utils::Args::No, false);
	args.addOption("chunk-size", 'c', "Chunk size for the netCDF file",
			utils::Args::Required, false);
	args.addAdditionalOption("output", "netCDF output file");

	// Parse/check command line arguments
	if (!args.parse(argc, argv) == utils::Args::Success)
		return 1;

	// Get the grid size
	std::vector<std::string> sizeVec = utils::StringUtils::split(args.getArgument<std::string>("size"), ':');
	if (sizeVec.size() != 3)
		logError() << "Size format must be \"x:y:z\"";
	ularr3 gridSize(utils::StringUtils::parse<unsigned long>(sizeVec[0]),
			utils::StringUtils::parse<unsigned long>(sizeVec[1]),
			utils::StringUtils::parse<unsigned long>(sizeVec[2]));

	logInfo() << "Grid size:" << utils::nospace
			<< "[" << gridSize(0) << ", " << gridSize(1) << ", " << gridSize(2) << "]";

	// Read the coordinates from the mesh
	const char* mesh = args.getArgument<const char*>("mesh");

	int ncFile;
	checkNcIError(nc_open(mesh, NC_NOWRITE, &ncFile));

	// Get the number of partitions
	int ncDimPart;
	checkNcIError(nc_inq_dimid(ncFile, "partitions", &ncDimPart));
	size_t partitions;
	checkNcIError(nc_inq_dimlen(ncFile, ncDimPart, &partitions));

	// Get max number of vertices
	int ncDimVertices;
	checkNcIError(nc_inq_dimid(ncFile, "vertices", &ncDimVertices));
	size_t maxVertices;
	checkNcIError(nc_inq_dimlen(ncFile, ncDimVertices, &maxVertices));

	// Get min/max vertices
	Eigen::Array3d minVert(
			std::numeric_limits<double>::infinity(),
			std::numeric_limits<double>::infinity(),
			std::numeric_limits<double>::infinity());
	Eigen::Array3d maxVert(
			-std::numeric_limits<double>::infinity(),
			-std::numeric_limits<double>::infinity(),
			-std::numeric_limits<double>::infinity());

	double* vertices = new double[maxVertices*3];

	int ncVarVrtxSize;
	checkNcIError(nc_inq_varid(ncFile, "vertex_size", &ncVarVrtxSize));
	int ncVarVrtxCoords;
	checkNcIError(nc_inq_varid(ncFile, "vertex_coordinates", &ncVarVrtxCoords));
	for (size_t i = 0; i < partitions; i++) {
		size_t start[3] = {i, 0, 0};
		int size;
		checkNcIError(nc_get_var1_int(ncFile, ncVarVrtxSize, start, &size));

		size_t count[3] = {1, static_cast<size_t>(size), 3};
		checkNcIError(nc_get_vara_double(ncFile, ncVarVrtxCoords, start, count, vertices));

		for (int j = 0; j < size; j++) {
			Eigen::Array3d v = Eigen::Map<Eigen::Array3d>(&vertices[j*3]);

			minVert = minVert.min(v);
			maxVert = maxVert.max(v);
		}
	}

	checkNcIError(nc_close(ncFile));

	delete [] vertices;

	// Add borders
	double border = args.getArgument("border", 0.);
	minVert -= Eigen::Array3d(border, border, border);
	maxVert += Eigen::Array3d(border, border, border);

	logInfo() << "Grid dimension (incl. border):" << utils::nospace
			<< "[" << minVert(0) << ", " << minVert(1) << ", " << minVert(2) << "] x ["
			<< maxVert(0) << ", " << maxVert(1) << ", " << maxVert(2) << "]";

	// Distance between grid points
	Eigen::Vector3d gridDist = (maxVert - minVert)
			* (gridSize - ularr3(1, 1, 1)).cast<double>().inverse();

	logInfo() << "Grid interval:" << utils::nospace
			<< "[" << gridDist(0) << ", " << gridDist(1) << ", " << gridDist(2) << "]";

	// Create query process
	redi::rpstream pstream(args.getArgument("query", "./scripts/vx_lite_wrapper"),
			redi::pstreambuf::pstdin | redi::pstreambuf::pstdout);

	// Start the input and error thread
	inputData_t inputData {
		minVert,
		gridSize,
		gridDist,
		args.getArgument("mesh-proj", "+proj=utm +zone=11 +ellps=WGS84 +datum=WGS84 +units=m +no_defs"),
		args.getArgument("mesh-proj-geo", false),
		args.getArgument("query-proj", "+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs"),
		args.getArgument("query-proj-geo", true),
		pstream
	};

	pthread_t inputThread;
	pthread_create(&inputThread, 0L, runInput, &inputData);

	// Viewable by Paraview?
	bool paraview = args.getArgument("paraview", false);

	// Open the netCDF file
	const char* outputName = args.getAdditionalArgument<const char*>("output");
	checkNcOError(nc_create(outputName, NC_NETCDF4, &ncFile));

	// Create dimensions
	int ncDims[3];
	checkNcOError(nc_def_dim(ncFile, "x", gridSize(0), &ncDims[2]));
	checkNcOError(nc_def_dim(ncFile, "y", gridSize(1), &ncDims[1]));
	checkNcOError(nc_def_dim(ncFile, "z", gridSize(2), &ncDims[0]));

	// Create dimension variables
	int ncX, ncY, ncZ;
	checkNcOError(nc_def_var(ncFile, "x", NC_FLOAT, 1, &ncDims[2], &ncX));
	checkNcOError(nc_def_var(ncFile, "y", NC_FLOAT, 1, &ncDims[1], &ncY));
	checkNcOError(nc_def_var(ncFile, "z", NC_FLOAT, 1, &ncDims[0], &ncZ));

	// Create variables
	int ncData, ncRho, ncMu, ncLambda;
	if (paraview) {
		checkNcOError(nc_def_var(ncFile, "rho", NC_FLOAT, 3, ncDims, &ncRho));
		checkNcOError(nc_def_var(ncFile, "mu", NC_FLOAT, 3, ncDims, &ncMu));
		checkNcOError(nc_def_var(ncFile, "lambda", NC_FLOAT, 3, ncDims, &ncLambda));
	} else {
		// Create compound type
		int ncType;
		checkNcOError(nc_def_compound(ncFile, 3*sizeof(float), "material", &ncType));
		checkNcOError(nc_insert_compound(ncFile, ncType, "rho", 0, NC_FLOAT));
		checkNcOError(nc_insert_compound(ncFile, ncType, "mu", sizeof(float), NC_FLOAT));
		checkNcOError(nc_insert_compound(ncFile, ncType, "lambda", 2*sizeof(float), NC_FLOAT));

		checkNcOError(nc_def_var(ncFile, "data", ncType, 3, ncDims, &ncData));

		if (args.isSet("chunk-size")) {
			unsigned int chunkSize = args.getArgument<unsigned int>("chunk-size");

			size_t chunks[3] = {chunkSize, chunkSize, chunkSize};
			checkNcOError(nc_def_var_chunking(ncFile, ncData, NC_CHUNKED, chunks));
		}
	}

	checkNcOError(nc_enddef(ncFile));

	// Fill dimension variables
	float* x = new float[gridSize(0)];
	for (unsigned int i = 0; i < gridSize(0); i++) {
		x[i] = minVert(0) + gridDist(0) * i;
	}
	checkNcOError(nc_put_var_float(ncFile, ncX, x));
	delete [] x;

	float* y = new float[gridSize(1)];
	for (unsigned int i = 0; i < gridSize(1); i++) {
		y[i] = minVert(1) + gridDist(1) * i;
	}
	checkNcOError(nc_put_var_float(ncFile, ncY, y));
	delete [] y;

	float* z = new float[gridSize(2)];
	for (unsigned int i = 0; i < gridSize(2); i++) {
		z[i] = minVert(2) + gridDist(2) * i;
	}
	checkNcOError(nc_put_var_float(ncFile, ncZ, z));
	delete [] z;

	// Read the data from the query tool
	unsigned long totalArea = gridSize(0) * gridSize(1);
	float* data = new float[totalArea*3];

	// Initialize data with invalid values
	for (unsigned long i = 0; i < totalArea*3; i++)
		data[i] = std::numeric_limits<float>::infinity();

	for (unsigned long z = 0; z < gridSize(2); z++) {
		for (unsigned long i = 0; i < totalArea; i++) {
			// Ignore the coordinates
			float x, y, z;
			pstream.out() >> x >> y >> z;

			float vp, vs, rho;
			pstream.out() >> vp >> vs >> rho;

			float mu, lambda;
			if (vp >= MIN_VALID && vs >= MIN_VALID && rho >= MIN_VALID) {
				mu = vs2mu(vs, rho);
				lambda = vp2lambda(vp, rho, mu);

				if (paraview) {
					data[i] = rho;
					data[i + totalArea] = mu;
					data[i + totalArea*2] = lambda;
				} else {
					data[i*3] = rho;
					data[i*3 + 1] = mu;
					data[i*3 + 2] = lambda;
				}
			} else {
				if (z == 0)
					logError() << "Invalid value in deepest level.";
			}
		}

		size_t start[3] = {z, 0, 0};
		size_t count[3] = {1, gridSize(1), gridSize(0)};

		if (paraview) {
			checkNcOError(nc_put_vara_float(ncFile, ncRho, start, count, data));
			checkNcOError(nc_put_vara_float(ncFile, ncMu, start, count, &data[totalArea]));
			checkNcOError(nc_put_vara_float(ncFile, ncLambda, start, count, &data[totalArea*2]));
		} else {
			checkNcOError(nc_put_vara(ncFile, ncData, start, count, data));
		}
	}

	pthread_join(inputThread, 0L);

	// Cleanup
	checkNcOError(nc_close(ncFile));

	delete [] data;

	logInfo() << "Finished";
}


#include "SPlisHSPlasH/Common.h"
#include "GL/glew.h"
#include "Visualization/MiniGL.h"
#include "GL/glut.h"
#include "SPlisHSPlasH/TimeManager.h"
#include <Eigen/Dense>
#include <iostream>
#include "SPlisHSPlasH/Utilities/Timing.h"
#include "Utilities/PartioReaderWriter.h"
#include "SPlisHSPlasH/StaticRigidBody.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/Utilities/PoissonDiskSampling.h"
#include "Demos/Common/DemoBase.h"
#include "Utilities/FileSystem.h"

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
#define new DEBUG_NEW
#endif
#endif

using namespace SPH;
using namespace Eigen;
using namespace std;

void timeStep();
void initBoundaryData();
void render();
void renderBoundary();
void reset();

DemoBase base;

// main
int main(int argc, char** argv)
{
    REPORT_MEMORY_LEAKS;

    base.init(argc, argv, "StaticBoundaryDemo");
    initBoundaryData();
    base.buildModel();

    ////////////////////////////////////////////////////////////////////////////////
    auto scene            = base.getScene();
    auto simulationMethod = base.getSimulationMethod();
    base.m_MeshWriter->reset_buffer();
    base.m_MeshWriter->getBuffer().push_back(static_cast<unsigned int>(scene.boundaryModels.size() - 1));

    static std::vector<Vector3r> vertices;
    static std::vector<Vector3r> normals;
    for(unsigned int i = 0; i < base.getScene().boundaryModels.size(); i++)
    {
        if(!scene.boundaryModels[i]->isWall)
        {
            FluidModel::RigidBodyParticleObject* rb  = simulationMethod.model.getRigidBodyParticleObject(i);
            RigidBodyObject*                     rbo = rb->m_rigidBody;
            StaticRigidBody*                     srb = dynamic_cast<StaticRigidBody*>(rbo);

            assert(srb != nullptr);
            TriangleMesh&      mesh   = srb->getGeometry();
            const unsigned int nFaces = mesh.numFaces();
            auto faces             = mesh.getFaces();
            auto faceVertices      = mesh.getVertices();
            auto faceVertexNormals = mesh.getVertexNormals();

            vertices.resize(0);
            vertices.reserve(nFaces * 3);
            normals.resize(0);
            normals.reserve(nFaces * 3);

            for(unsigned int i = 0; i < nFaces; ++i)
            {
                for(unsigned int j = 0; j < 3; ++j)
                {
                    unsigned int v_index = faces[i * 3 + j];
                    vertices.push_back(faceVertices[v_index]);
                    normals.push_back(faceVertexNormals[v_index]);
                }
            }

            base.m_MeshWriter->getBuffer().push_back(static_cast<unsigned int>(vertices.size()));
            base.m_MeshWriter->getBuffer().push_back_to_float_array(vertices, false);
            base.m_MeshWriter->getBuffer().push_back_to_float_array(normals, false);
        }
    }
    base.m_MeshWriter->flush_buffer_async(1);

    ////////////////////////////////////////////////////////////////////////////////
    MiniGL::setClientIdleFunc(50, timeStep);
    MiniGL::setKeyFunc(0, 'r', reset);
    MiniGL::setClientSceneFunc(render);

    glutMainLoop();

    base.cleanup();

    Timing::printAverageTimes();
    Timing::printTimeSums();

    return 0;
}

void reset()
{
    Timing::printAverageTimes();
    Timing::reset();

    base.getSimulationMethod().simulation->reset();
    TimeManager::getCurrent()->setTime(0.0);
}

void timeStep()
{
    if((base.getPauseAt() > 0.0) && (base.getPauseAt() < TimeManager::getCurrent()->getTime()))
        base.setPause(true);

    if(base.getPause())
        return;

    // Simulation code
    for(unsigned int i = 0; i < base.getNumberOfStepsPerRenderUpdate(); i++)
    {
        START_TIMING("SimStep");
        base.getSimulationMethod().simulation->step();
        STOP_TIMING_AVG;

        base.getSimulationMethod().model.writeFrameFluidData(TimeManager::getCurrent()->getTime());
    }
}

void render()
{
    MiniGL::coordinateSystem();

    base.renderFluid();
    renderBoundary();
}

void renderBoundary()
{
    DemoBase::SimulationMethod& simulationMethod      = base.getSimulationMethod();
    Shader&                     shader                = base.getShader();
    Shader&                     meshShader            = base.getMeshShader();
    SceneLoader::Scene&         scene                 = base.getScene();
    const int                   renderWalls           = base.getRenderWalls();
    GLint                       context_major_version = base.getContextMajorVersion();

    float                       wallColor[4] = { 0.1f, 0.6f, 0.6f, 1.0f };
    if((renderWalls == 1) || (renderWalls == 2))
    {
        if(context_major_version > 3)
        {
            shader.begin();
            glUniform3fv(shader.getUniform("color"), 1, &wallColor[0]);
            glEnableVertexAttribArray(0);
            for(int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
            {
                if((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
                {
                    FluidModel::RigidBodyParticleObject* rb = simulationMethod.model.getRigidBodyParticleObject(body);
                    glVertexAttribPointer(0, 3, GL_DOUBLE, GL_FALSE, 0, &simulationMethod.model.getPosition(body + 1, 0));
                    glDrawArrays(GL_POINTS, 0, rb->numberOfParticles());
                }
            }
            glDisableVertexAttribArray(0);
            shader.end();
        }
        else
        {
            glDisable(GL_LIGHTING);
            glPointSize(4.0f);

            glBegin(GL_POINTS);
            for(int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
            {
                if((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
                {
                    FluidModel::RigidBodyParticleObject* rb = simulationMethod.model.getRigidBodyParticleObject(body);
                    for(unsigned int i = 0; i < rb->numberOfParticles(); i++)
                    {
                        glColor3fv(wallColor);
                        glVertex3v(&simulationMethod.model.getPosition(body + 1, i)[0]);
                    }
                }
            }
            glEnd();
            glEnable(GL_LIGHTING);
        }
    }
    else if((renderWalls == 3) || (renderWalls == 4))
    {
        for(int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
        {
            if((renderWalls == 3) || (!scene.boundaryModels[body]->isWall))
            {
                meshShader.begin();
                glUniform1f(meshShader.getUniform("shininess"), 5.0f);
                glUniform1f(meshShader.getUniform("specular_factor"), 0.2f);

                GLfloat matrix[16];
                glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
                glUniformMatrix4fv(meshShader.getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
                GLfloat pmatrix[16];
                glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
                glUniformMatrix4fv(meshShader.getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);

                glUniform3fv(meshShader.getUniform("surface_color"), 1, wallColor);

                FluidModel::RigidBodyParticleObject* rb = simulationMethod.model.getRigidBodyParticleObject(body);
                MiniGL::drawMesh(((StaticRigidBody*)rb->m_rigidBody)->getGeometry(), wallColor);

                meshShader.end();
            }
        }
    }
}

void initBoundaryData()
{
    std::string         base_path = FileSystem::getFilePath(base.getSceneFile());
    SceneLoader::Scene& scene     = base.getScene();
    const bool          useCache  = base.getUseParticleCaching();

    for(unsigned int i = 0; i < scene.boundaryModels.size(); i++)
    {
        string                meshFileName = FileSystem::normalizePath(base_path + "/" + scene.boundaryModels[i]->meshFile);

        std::vector<Vector3r> boundaryParticles;
        if(scene.boundaryModels[i]->samplesFile != "")
        {
            string particleFileName = base_path + "/" + scene.boundaryModels[i]->samplesFile;
            PartioReaderWriter::readParticles(particleFileName, scene.boundaryModels[i]->translation, scene.boundaryModels[i]->rotation, scene.boundaryModels[i]->scale[0], boundaryParticles);
        }

        // Cache sampling
        std::string mesh_base_path   = FileSystem::getFilePath(scene.boundaryModels[i]->meshFile);
        std::string mesh_file_name   = FileSystem::getFileName(scene.boundaryModels[i]->meshFile);
        std::string scene_path       = FileSystem::getFilePath(base.getSceneFile());
        std::string scene_file_name  = FileSystem::getFileName(base.getSceneFile());
        string      cachePath        = scene_path + "/" + mesh_base_path + "/Cache";
        string      particleFileName = FileSystem::normalizePath(cachePath + "/" + scene_file_name + "_" + mesh_file_name + "_" + std::to_string(i) + ".bgeo");

        StaticRigidBody* rb  = new StaticRigidBody();
        TriangleMesh&    geo = rb->getGeometry();
        OBJLoader::loadObj(meshFileName, geo, scene.boundaryModels[i]->scale);
        for(unsigned int j = 0; j < geo.numVertices(); j++)
            geo.getVertices()[j] = scene.boundaryModels[i]->rotation * geo.getVertices()[j] + scene.boundaryModels[i]->translation;

        geo.updateNormals();
        geo.updateVertexNormals();

        if(scene.boundaryModels[i]->samplesFile == "")
        {
            bool foundCacheFile = false;
            if(useCache)
            {
                foundCacheFile = PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), 1.0, boundaryParticles);
                if(foundCacheFile)
                    std::cout << "Loaded cached boundary sampling: " << particleFileName << "\n";
            }

            if(!useCache || !foundCacheFile)
            {
                std::cout << "Surface sampling of " << meshFileName << "\n";
                START_TIMING("Poisson disk sampling");
                PoissonDiskSampling sampling;
                sampling.sampleMesh(geo.numVertices(), geo.getVertices().data(), geo.numFaces(), geo.getFaces().data(), scene.particleRadius, 10, 1, boundaryParticles);
                STOP_TIMING_AVG;

                // Cache sampling
                if(useCache && (FileSystem::makeDir(cachePath) == 0))
                {
                    std::cout << "Save particle sampling: " << particleFileName << "\n";
                    PartioReaderWriter::writeParticles(particleFileName, (unsigned int)boundaryParticles.size(), boundaryParticles.data(), NULL, scene.particleRadius);
                }
            }
        }

        base.getSimulationMethod().model.addRigidBodyObject(rb, static_cast<unsigned int>(boundaryParticles.size()), &boundaryParticles[0]);
    }
}


#ifndef __MECHANISM_ROTATIONAL_KALMAN__
#define __MECHANISM_ROTATIONAL_KALMAN__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <limits>
#include <stdexcept>

namespace manipulation_algorithms{
  /**
    Class that estimates a rotational DOF given measurements of the angular velocity along it.
  **/
  class RotationalEstimator : public AlgorithmBase
  {
  public:
    RotationalEstimator();
    ~RotationalEstimator();

    /**
      Estimate the rotational dof from measurements of relative angular velocity,
      in the same frame..

      @param w_r The measured rotational velocity.
      @param dt The elapsed time between estimate steps.
      @output The rotational dof estimate.
    **/
    Eigen::Vector3d estimate(const Eigen::Vector3d &w_r, double dt);

    /**
      Initialize the estimator

      @param k The initial dof estimate.
    **/
    void initialize(const Eigen::Vector3d &k);
    virtual bool getParams(const ros::NodeHandle &n);

  private:
    Eigen::Vector3d k_;
    Eigen::Matrix3d P_, R_, Q_;
  };
}
#endif


//Copyright (c) 2015 Zachary Kann
//
//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.

// ---
// Author: Zachary Kann

#include "boost/multi_array.hpp"
#include <armadillo>
#include "z_constants.hpp"

typedef boost::multi_array<double, 4> hypercube;

#ifndef _Z_VEC_HPP_
#define _Z_VEC_HPP_

extern void SetupDx(arma::cube& xoo, arma::cube& xio, arma::cube& xii,
                    arma::mat& roo2, arma::mat& rio2, arma::mat& rii2,
                    const arma::mat& x, const arma::mat& xion,
                    arma::icube& shiftoo, arma::icube& shiftio,
                    arma::icube& shiftii, const int numMols,
                    const int numIons, const arma::rowvec& box);

extern void SetupDxIO(arma::cube& xio, const arma::mat& x,
                       const arma::mat& xion, arma::icube& shiftio,
                       const int numMols, const int  numIons,
                       const arma::rowvec& box);

extern void SetupShiftOO(const arma::mat& x, arma::icube& shiftoo,
                          const int numMols, const arma::rowvec& box);

extern void FindDxNoShift(arma::rowvec& result, const arma::rowvec& vec1,
                           const arma::rowvec& vec2, const arma::rowvec& box);

extern void FindDx(arma::rowvec& result, const arma::rowvec& vec1,
                   const arma::rowvec& vec2, const arma::rowvec& box,
                   arma::irowvec& shift);

template <typename type1>
arma::rowvec VeclikeToRow(const type1& vec1, const int  length)
{
    arma::rowvec result(length);
    for (int i=0; i<length; i++)
    {
        result[i] = vec1[i];
    }
    return result;
}

inline arma::rowvec UseDx(const arma::rowvec& vec1, const arma::rowvec& vec2,
                          const arma::rowvec& box, const arma::irowvec& shift) {
  return vec1 - vec2 + shift%box; }

#endif


//
// Created by dch on 18/02/16.
//

#include <boost/program_options.hpp>
#include <cuNDArray.h>
#include <cuNDArray_fileio.h>
#include "CT_acquisition.h"
#include "CTProjectionOperator.h"
#include <boost/filesystem.hpp>
#include <boost/regex.hpp>
#include <algorithm>
#include <cuCgSolver.h>
#include <cuGpBbSolver.h>
#include <cuNlcgSolver.h>

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;
namespace fs = boost::filesystem;

std::vector<string> get_dcm_files(std::string dir){

    fs::directory_iterator end_itr;
    const boost::regex filter(".*\.dcm");

    std::vector<std::string> files;
    for (fs::directory_iterator i (dir); i!= end_itr; i++){
        if (!fs::is_regular(i->status())) continue;
        boost::smatch what;

        if (!boost::regex_match(i->path().filename().string(),what,filter)) continue;

        //std::cout  << i->path().filename() << std::endl;
        files.push_back(i->path().string());
    }
    sort(files.begin(),files.end());

    return files;
}

int main(int argc, char** argv){

    floatd3 imsize_in_mm;
    float offset;
    uintd3 imageSize;
    int device;
    po::options_description desc("Allowed options");
    desc.add_options()
            ("help", "produce help message")
            ("dir", po::value<string>()->multitoken())

            ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
            ("dimensions,d",po::value<floatd3>(&imsize_in_mm)->default_value(floatd3(400,400,500)),"Image dimensions in mm. Overwrites voxelSize.")
            ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
            ("offset",po::value<float>(&offset)->default_value(0),"Offset of image");
	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

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

    cudaSetDevice(device);
    cudaDeviceReset();

    auto files = read_dicom_projections(get_dcm_files(vm["dir"].as<string>()));
    std::cout << " Penguins ";
    std::cout << files->geometry.detectorFocalCenterAxialPosition.size() << std::endl;
    std::vector<float>& axials =  files->geometry.detectorFocalCenterAxialPosition;
    std::cout << "Axials size " << axials.size() << std::endl;
    auto mean_offset = std::accumulate(axials.begin(),axials.end(),0.0f)/axials.size();
    std::cout << "Mean offset " << mean_offset << std::endl;
    for ( auto & z : axials)
        z -= mean_offset+offset;

    //for (auto x  : axials)
    //    std::cout << x << std::endl;
    std::cout << "Axials start " << axials[0] << " Axials end " << axials.back() << std::endl;

    std::cout << "Minimum angle " << *std::min_element(files->geometry.detectorFocalCenterAngularPosition.begin(),files->geometry.detectorFocalCenterAngularPosition.end())  << " max " << *std::max_element(files->geometry.detectorFocalCenterAngularPosition.begin(),files->geometry.detectorFocalCenterAngularPosition.end()) << std::endl;

    auto E = boost::make_shared<CTProjectionOperator<cuNDArray>>();
    std::vector<size_t> imdims {imageSize[0],imageSize[1],imageSize[2]};
    cuNDArray<float> image(imdims);

    cuNDArray<float> projections(files->projections );

    std::cout << "Projection memory size " << projections.get_number_of_bytes()/(1024*1024) << "MB" << std::endl;
    E->set_domain_dimensions(image.get_dimensions().get());
    E->set_codomain_dimensions(projections.get_dimensions().get());
    std::cout << "Starting setup" << std::endl;
    E->setup(files,imsize_in_mm);
    std::cout << "Setup done" << std::endl;

    std::cout << "Projections size: " << projections.get_size(0) << " " << projections.get_size(1) << " " << projections.get_size(2) << std::endl;
    //E->mult_MH(&projections,&image,false);
    write_nd_array(&projections,"projections.real");
    //cuGpBbSolver<float> solver;
    //cuNlcgSolver<float> solver;
    cuCgSolver<float> solver;
    //solver.set_non_negativity_constraint(true);
    solver.set_max_iterations(50);
    solver.set_tc_tolerance(1e-8);

    solver.set_encoding_operator(E);
    solver.set_output_mode(cuCgSolver<float>::OUTPUT_VERBOSE);
    auto result = solver.solve(&projections);

    write_nd_array(result.get(),"test.real");

    //fill(result.get(),1.0f);
    auto proj2 = projections;
    E->mult_M(result.get(),&proj2,false);
    write_nd_array(&proj2,"projections2.real");
    float scaling = dot(&proj2,&projections)/dot(&projections,&projections);
    std::cout << "Scaling " << scaling << std::endl;
    proj2 /= scaling;
    proj2 -= projections;
    write_nd_array(&proj2,"projections3.real");

}


#ifndef CORE_HPP
#define CORE_HPP

#include<stdio.h>
#include<iostream>
#include<math.h>
#include <unistd.h>
#include <armadillo>
#include <fstream>
#include<map>
#include<string>
#include<iomanip>
#include<limits>
#include<string>
#include<regex>
#include<sstream>
#include<tuple>

#define ARMA_DONT_PRINT_ERRORS
using namespace arma;

typedef std::map<char, float> char_float_map;
typedef std::map<char, double> char_double_map;
typedef std::vector<std::string> vec_str;
typedef std::vector<double> vec_doub;
typedef std::vector<long double> vec_ldoub;
typedef std::tuple <double, double> xy_tuple;

struct TempqDers
{
	double dq1[4];
	double dq2[4];

	TempqDers()
    {

    }
    void setTempdq()
	{
		for(int i=0; i<4; i++)
		{
			dq1[i] = 0.0;
			dq2[i] = 0.0;
		}
	}
};

struct Point
{
	int localID;
	double x, y;
	int left, right;
	int flag_1, flag_2; // Int8 in the Julia code
	double short_distance;
	int nbhs;
	// int conn[20];
	double nx, ny;
	// Size 4 (Pressure, vx, vy, density) x numberpts
	double prim[4];
	// double flux_res[4];
	// double q[4];
	// Size 2(x,y) 4(Pressure, vx, vy, density) numberpts
	// double dq1[4];
	// double dq2[4];
	double entropy;
	int xpos_nbhs, xneg_nbhs, ypos_nbhs, yneg_nbhs;
	// int xpos_conn[20];
	// int xneg_conn[20];
	// int ypos_conn[20];
	// int yneg_conn[20];
	double delta;
	// double max_q[4];
	// double min_q[4];
	// double prim_old[4];

	//Point Constructor

	Point() {}

};

struct Config
{
	struct Core
	{
		int points;
		double cfl;
		int max_iters;
		double mach;
		double aoa;
		double power;
		int limiter_flag;
		double vl_const;
		int initial_conditions_flag;
		int interior_points_normal_flag;
		double shapes;
		double rho_inf;
		double pr_inf;
		int threadsperblock;
		double gamma;
		int clcd_flag;
		char* tscheme;
		int euler;
		int rks;
		int restart;

		Core() {}

	};

	struct PointConfig
	{
		int wall, interior, outer;

		PointConfig() {}
	};

	struct Format
	{
		char* type;

		Format() {}
	};

	Core core = Core();
	PointConfig point_config = PointConfig();
	Format format = Format();

	Config() {}

	void setConfig()
	{
		core.points = 48738;
		core.cfl = 0.2;
		core.max_iters = 3;
		core.mach = 0.63;
		core.aoa = 2;
		core.power = 0.0;
		core.limiter_flag = 1;
		core.vl_const = 50;
		core.initial_conditions_flag = 0;
		core.interior_points_normal_flag = 0;
		core.shapes = 1.0;
		core.rho_inf = 1.0;
		core.pr_inf = 0.7142857142857142;
		core.threadsperblock = 32;
		core.gamma = 1.4;
		core.clcd_flag = 0;
		core.tscheme = "ssprk43";
		core.restart = 0;

		point_config.wall = 0;
		point_config.interior = 1;
		point_config.outer = 2;

		format.type = "quadtree";

		if(std::strcmp(core.tscheme, "first") == 0)
		{
			core.rks = 1;
			core.euler = 2;
		}

		else if(std::strcmp(core.tscheme, "ssprk43") == 0)
		{
			core.rks = 4;
			core.euler = 1;
		}

	}

	void printConfig()
	{
		cout<<"Core points: "<<core.points<<endl;
		cout<<"Core cfl: "<<core.cfl<<endl;
		cout<<"Core max_iters: "<<core.max_iters<<endl;
		cout<<"Core mach: "<<core.mach<<endl;
		cout<<"Core aoa: "<<core.aoa<<endl;
		cout<<"Core power: "<<core.power<<endl;
		cout<<"Core limiter_flag: "<<core.limiter_flag<<endl;
		cout<<"Core vl_const: "<<core.vl_const<<endl;
		cout<<"Core initial_conditions_flag: "<<core.initial_conditions_flag<<endl;
		cout<<"Core interior_points_normal_flag: "<<core.interior_points_normal_flag<<endl;
		cout<<"Core shapes: "<<core.shapes<<endl;
		cout<<"Core rho_inf: "<<core.rho_inf<<endl;
		cout<<"Core pr_inf: "<<core.pr_inf<<endl;
		cout<<"Core threadsperblock: "<<core.threadsperblock<<endl;
		cout<<"Core gamma: "<<core.gamma<<endl;
		cout<<"Core clcd_flag: "<<core.clcd_flag<<endl;

		cout<<"Config wall: "<<point_config.wall<<endl;
		cout<<"Config interior: "<<point_config.interior<<endl;
		cout<<"Config outer: "<<point_config.outer<<endl;

		cout<<"Format type: "<<format.type<<endl;

	}
};

double calculateTheta(Config configData);

void getInitialPrimitive(Config configData, double primal[4]);

void placeNormals(Point* globaldata, int idx, Config configData, long long interior, long long wall, long long outer);

xy_tuple calculateNormals(xy_tuple left, xy_tuple right, double mx, double my);

void calculateConnectivity(Point* globaldata, int idx, int* xpos_conn, int* xneg_conn, int* ypos_conn, int* yneg_conn, int* connec);

void fpi_solver(int iter, Point* globaldata, Config configData, double res_old[1], int numPoints, TempqDers* tempdq, int* xpos_conn, int* xneg_conn, int* ypos_conn, int* yneg_conn, \
	int* connec, double* prim, double* flux_res, double* q, double* dq1, double* dq2, double* max_q, double* min_q, double* prim_old);

void q_variables(Point* globaldata, int numPoints, double* prim, double* q);

void q_var_derivatives(Point* globaldata, int numPoints, double power, double* q, double* dq1, double* dq2, int* connec, double* max_q, double* min_q);

void q_var_derivatives_innerloop(Point* globaldata, int numPoints, double power, TempqDers* tempdq, int* connec, double* q, double* dq1, double* dq2);

template <class Type>
bool isNan(Type var);

#endif


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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.
 */

#ifndef DART_DYNAMICS_BALLJOINT_HPP_
#define DART_DYNAMICS_BALLJOINT_HPP_

#include <Eigen/Dense>

#include "dart/dynamics/GenericJoint.hpp"

namespace dart {
namespace dynamics {

/// class BallJoint
class BallJoint : public GenericJoint<math::SO3Space>
{
public:

  friend class Skeleton;

  using Base = GenericJoint<math::SO3Space>;

  struct Properties : Base::Properties
  {
    DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(Properties)

    Properties(const Base::Properties& properties = Base::Properties());

    virtual ~Properties() = default;
  };

  /// Destructor
  virtual ~BallJoint();

  // Documentation inherited
  const std::string& getType() const override;

  /// Get joint type for this class
  static const std::string& getStaticType();

  // Documentation inherited
  bool isCyclic(std::size_t _index) const override;

  /// Get the Properties of this BallJoint
  Properties getBallJointProperties() const;

  /// Convert a rotation into a 3D vector that can be used to set the positions
  /// of a BallJoint. The positions returned by this function will result in a
  /// relative transform of
  /// getTransformFromParentBodyNode() * _rotation * getTransformFromChildBodyNode().inverse()
  /// between the parent BodyNode and the child BodyNode frames when applied to
  /// a BallJoint.
  template <typename RotationType>
  static Eigen::Vector3s convertToPositions(const RotationType& _rotation)
  {
    return math::logMap(_rotation);
  }

  /// Convert a BallJoint-style position vector into a transform
  static Eigen::Isometry3s convertToTransform(const Eigen::Vector3s& _positions);

  /// Convert a BallJoint-style position vector into a rotation matrix
  static Eigen::Matrix3s convertToRotation(const Eigen::Vector3s& _positions);

  // Documentation inherited
  Eigen::Matrix<s_t, 6, 3> getRelativeJacobianStatic(
      const Eigen::Vector3s& _positions) const override;

  math::Jacobian getRelativeJacobianDeriv(std::size_t index) const override;
  math::Jacobian finiteDifferenceRelativeJacobianDeriv(std::size_t index) const;

  math::Jacobian getRelativeJacobianTimeDerivDeriv(std::size_t index) const override;
  math::Jacobian finiteDifferenceRelativeJacobianTimeDerivDeriv(std::size_t index) const;

  math::Jacobian getRelativeJacobianTimeDerivDeriv2(std::size_t index) const override;
  math::Jacobian finiteDifferenceRelativeJacobianTimeDerivDeriv2(std::size_t index) const;

  // Documentation inherited
  Eigen::Matrix<s_t, 6, 3> getRelativeJacobianInPositionSpaceStatic(
      const Eigen::Vector3s& _positions) const override;

  // Documentation inherited
  Eigen::Vector3s getPositionDifferencesStatic(
      const Eigen::Vector3s& _q2, const Eigen::Vector3s& _q1) const override;

  /*
  // This gets the world axis screw at the current position, without moving the joint.
  Eigen::Vector6s getWorldAxisScrewForPosition(int dof) const override;
  */

  // This computes the world axis screw at a given position, without moving the joint.
  Eigen::Vector6s getWorldAxisScrewAt(Eigen::Vector3s pos, int dof) const;

  // This estimates the new world screw axis at `axisDof` when we perturbe `rotateDof` by `eps`
  Eigen::Vector6s estimatePerturbedScrewAxisForPosition(
    int axisDof,
    int rotateDof,
    s_t eps);

  // This estimates the new world screw axis at `axisDof` when we perturbe `rotateDof` by `eps`
  Eigen::Vector6s estimatePerturbedScrewAxisForForce(
    int axisDof,
    int rotateDof,
    s_t eps);

  // Returns the gradient of the screw axis with respect to the rotate dof
  Eigen::Vector6s getScrewAxisGradientForPosition(
    int axisDof,
    int rotateDof);

  // Returns the gradient of the screw axis with respect to the rotate dof
  Eigen::Vector6s getScrewAxisGradientForForce(
    int axisDof,
    int rotateDof);

protected:

  /// Constructor called by Skeleton class
  BallJoint(const Properties& properties);

  // Documentation inherited
  Joint* clone() const override;

  using Base::getRelativeJacobianStatic;

  // Documentation inherited
  void integratePositions(s_t _dt) override;

#ifndef DART_USE_IDENTITY_JACOBIAN
  // Documentation inherited
  void integrateVelocities(s_t dt) override;
#endif

  // Documentation inherited
  Eigen::VectorXs integratePositionsExplicit(const Eigen::VectorXs& pos, const Eigen::VectorXs& vel, s_t dt) override;

  /// Returns d/dpos of integratePositionsExplicit()
  Eigen::MatrixXs getPosPosJacobian(const Eigen::VectorXs& pos, const Eigen::VectorXs& vel, s_t _dt) override;

  /// Returns d/dvel of integratePositionsExplicit()
  Eigen::MatrixXs getVelPosJacobian(const Eigen::VectorXs& pos, const Eigen::VectorXs& vel, s_t _dt) override;

  /// Returns d/dpos of integratePositionsExplicit() by finite differencing
  Eigen::MatrixXs finiteDifferencePosPosJacobian(const Eigen::VectorXs& pos, const Eigen::VectorXs& vel, s_t _dt);

  /// Returns d/dvel of integratePositionsExplicit() by finite differencing
  Eigen::MatrixXs finiteDifferenceVelPosJacobian(const Eigen::VectorXs& pos, const Eigen::VectorXs& vel, s_t _dt);

  // Documentation inherited
  void updateDegreeOfFreedomNames() override;

  // Documentation inherited
  void updateRelativeTransform() const override;

  // Documentation inherited
  void updateRelativeJacobian(bool _mandatory = true) const override;

  // Documentation inherited
  void updateRelativeJacobianTimeDeriv() const override;

protected:
#ifdef DART_USE_IDENTITY_JACOBIAN
  /// Access mR, which is an auto-updating variable
  const Eigen::Isometry3s& getR() const;

  /// Rotation matrix dependent on the generalized coordinates
  ///
  /// Do not use directly! Use getR() to access this
  mutable Eigen::Isometry3s mR;
#endif
public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

}  // namespace dynamics
}  // namespace dart

#endif  // DART_DYNAMICS_BALLJOINT_HPP_


// (C) 2014 Arek Olek

#include <algorithm>
#include <functional>
#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>

#include <omp.h>

#include "bfs.hpp"
#include "dfs.hpp"
#include "fifodfs.hpp"
#include "fivethree.hpp"
#include "lost.hpp"
#include "rdfs.hpp"
#include "random.hpp"
#include "ilst.hpp"
#include "greedy.hpp"

#include "test_suite.hpp"

#include "debug.hpp"
#include "graph.hpp"
#include "options.hpp"
#include "range.hpp"
#include "timing.hpp"

boost::adjacency_matrix<boost::undirectedS> typedef amatrix;
boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS> typedef ahash;
boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS> typedef alist;
boost::adjacency_list<boost::slistS, boost::vecS, boost::undirectedS> typedef aslist;
boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS> typedef aset;
boost::adjacency_list<boost::multisetS, boost::vecS, boost::undirectedS> typedef amultiset;
boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> typedef avec;

template<class Graph, class Tree>
std::function<Tree(Graph&)> make_construction(std::string name,
                                              unsigned index,
                                              unsigned size,
                                              unsigned seed,
                                              std::string sseed) {
  if (name == "bfs")
    return bfs_tree<Graph, Tree> ;
  if (name == "dfs")
    return dfs_tree<Graph, Tree> ;
  if (name == "rdfs")
    return rdfs_tree<Graph, Tree> ;
  if (name == "rdfs50")
    return rdfs_best_tree<Graph, Tree> ;
  if (name == "fifo")
    return fifo_dfs_tree<Graph, Tree> ;
  if (name == "random")
    return std::bind(random_tree<Graph, Tree>, std::placeholders::_1, seed);
  if (name == "wilson")
    return std::bind(wilson_tree<Graph, Tree>, std::placeholders::_1, seed);
  if (name == "greedy")
    return std::bind(greedy_tree<Graph, Tree>, std::placeholders::_1, seed);
  if (name == "ilst")
    return ilst<Graph, Tree> ;
  if (name == "5/3") {
    return [=](const Graph& G) {
      std::default_random_engine generator(seed);
      Tree g(num_vertices(G));
      copy_edges_shuffled(five_three_tree<Graph, Tree>(G), g, generator);
      return g;
    };
  }
  if (found(".xml", name)) {
    return [=](const Graph& G) {
      real_suite<Tree> suite(name, size, sseed);
      return std::get<0>(suite.get(index));
    };
  }
  throw std::invalid_argument("Unknown construction method: " + name);
}

template<class Graph>
std::function<unsigned(Graph&)> make_upper(std::string name) {
  if (name == "53") {
    return [](const Graph& G) {
      return 5*(num_vertices(G)-3)/6;
    };
  }
  return upper_bound<Graph> ;
}

std::string type(amatrix const & G) { return "matrix"; }
std::string type(ahash const & G) { return "unordered_set"; }
std::string type(aset const & G) { return "set"; }
std::string type(amultiset const & G) { return "multiset"; }
std::string type(alist const & G) { return "list"; }
std::string type(aslist const & G) { return "slist"; }
std::string type(avec const & G) { return "vector"; }

template <class Graph, class Tree, class Suite, class Strings>
void run(Suite& suite,
         Strings const & constructions,
         Strings const & improvements,
         bool scratch,
         std::string seed) {
#ifndef NPROGRESS
  timing total(CLOCK_REALTIME);
  total.start();
#endif
  auto graph_type = type(Graph(0));
  auto tree_type = type(Tree(0));
  #pragma omp parallel
  {
    auto id = omp_get_thread_num();
    double elapsed_c, elapsed_i;
    std::stringstream buffer;
    timing timer;
    #pragma omp for schedule(dynamic) nowait
    for(uint i = 0; i < suite.size(); ++i) {
      auto trial = suite.get(i);
      auto G = std::get<0>(trial);

      for (auto cname : constructions) {
        auto construct = make_construction<Graph, Tree>(cname, i, suite.size(), suite.get_seed(i), seed);
        auto upper = make_upper<Graph>(suite.type());
        timer.start();
        auto T = construct(G);
        elapsed_c = timer.stop();

        assert(num_edges(T) == num_vertices(T) - 1);
        assert(is_connected(T));
        assert(is_subgraph(T, G));

        const auto tree(T);

        for (auto iname : improvements) {
          if (scratch) T = tree;
          auto num_internal_before = num_internal(T);
          auto improve = make_improvement<Graph, Tree>(iname);

          timer.start();
          auto rules = improve(G, T);
          elapsed_i = timer.stop();

          buffer
#ifdef GRAPH_REPR
            << graph_type << '\t'
#endif
#ifdef TREE_REPR
            << tree_type << '\t'
#endif
            << std::get<1>(trial) << '\t'
            << id << '\t'
            << suite.type() << '\t'
            << std::get<2>(trial) << '\t'
            << std::get<3>(trial) << '\t'
            << num_vertices(G) << '\t'
            << num_edges(G) << '\t'
            << upper(G) << '\t'
            << cname << '\t'
            << iname << '\t'
            << num_internal_before << '\t'
            << num_internal(T) << '\t'
            << elapsed_c << '\t'
            << elapsed_i << '\t'
            << sum(rules) << '\t'
            << join(rules, "-")
            << std::endl;
            ;
#ifndef NDEBUG
          if (suite.size() < 10)
            show("graph" + std::to_string(i) + graph_type + ".dot", G, T);
          //if(rules[rules.size()-2] > 10) show("graph" + std::to_string(i) + ".dot", G, T);
#endif
          if (!scratch) elapsed_c += elapsed_i;
        }
      }

#ifndef NPROGRESS
      if (id == 0 || i + 1 == suite.size()) {
        std::cerr << '\t' << 100 * (i + 1) / suite.size() << "% of "
        << suite.size();
        std::cerr << '\t' << readable(total.stop()) << " elapsed    \r";
      }
#endif
    }
    #pragma omp critical
    std::cout << buffer.str() << std::flush;
  }
#ifndef NPROGRESS
  std::cerr << std::endl;
#endif
}

template <class Graph, class Tree, class Sizes, class Params, class Strings>
void run(std::string t,
         unsigned z,
         Sizes sizes,
         Params params,
         Strings const & constructions,
         Strings const & improvements,
         bool scratch,
         std::string seed) {
  if (found(".xml", t)) {
    real_suite<Graph> suite(t, z, seed);
    run<Graph, Tree>(suite, constructions, improvements, scratch, seed);
  } else if (found(".", t)) {
    file_suite<Graph> suite(t, z, seed);
    run<Graph, Tree>(suite, constructions, improvements, scratch, seed);
  } else {
    test_suite<Graph> suite(t, z, sizes, params, seed);
    run<Graph, Tree>(suite, constructions, improvements, scratch, seed);
  }
}

template <class Graph, class Sizes, class Degrees, class Strings>
void run(std::string tree,
         std::string model,
         unsigned sample,
         Sizes sizes,
         Degrees degrees,
         Strings const & constructions,
         Strings const & improvements,
         bool scratch,
         std::string seed) {
#ifdef TREE_REPR
  if (tree == "matrix")
    run<Graph, amatrix>  (model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (tree == "unordered_set")
    run<Graph, ahash>    (model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (tree == "multiset")
    run<Graph, amultiset>(model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (tree == "list")
    run<Graph, alist>    (model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (tree == "set")
    run<Graph, aset>     (model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (tree == "vector")
    run<Graph, avec>     (model, sample, sizes, degrees, constructions, improvements, scratch, seed);
#endif
  if (tree == "slist")
    run<Graph, aslist>   (model, sample, sizes, degrees, constructions, improvements, scratch, seed);
}

template <class Sizes, class Degrees, class Strings>
void run(std::string graph,
         std::string tree,
         std::string model,
         unsigned sample,
         Sizes sizes,
         Degrees degrees,
         Strings const & constructions,
         Strings const & improvements,
         bool scratch,
         std::string seed) {
#ifdef GRAPH_REPR
  if (graph == "matrix")
    run<amatrix>  (tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (graph == "unordered_set")
    run<ahash>    (tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (graph == "multiset")
    run<amultiset>(tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (graph == "slist")
    run<aslist>   (tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (graph == "vector")
    run<avec>     (tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
  if (graph == "list")
    run<alist>    (tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
#endif
  if (graph == "set")
    run<aset>     (tree, model, sample, sizes, degrees, constructions, improvements, scratch, seed);
}

std::string time_seed() {
  return std::to_string(std::chrono::system_clock::now().time_since_epoch().count());
}

int main(int argc, char** argv){
  using std::string;
  using std::vector;

  std::ios_base::sync_with_stdio(0);

  options opt(argc, argv);
  auto z = opt.get<int>("-z", 100);
  auto graphs = opt.getList<string>("-g", {"set"});
  auto trees = opt.getList<string>("-t", {"slist"});
  auto sizes = opt.getList<unsigned>("-n", {1000});
  auto tests = opt.getList<string>("-m", {"gnp+mst"});
  auto degrees = opt.getList<double>("-d", { 3. });
  auto constructions = opt.getList<string>("-c", {"wilson"});
  auto improvements = opt.getList<string>("-i", {"none"});
  auto scratch = opt.has("--scratch");
  auto seed = opt.get<string>("-s", time_seed());

  std::cout << "# Seed: " << seed << std::endl;

  std::cout
#ifdef GRAPH_REPR
    << "graph\t"
#endif
#ifdef TREE_REPR
    << "tree\t"
#endif
    << "run\t"
    << "thread\t"
    << "model\t"
    << "degree\t"
    << "param\t"
    << "vertices\t"
    << "edges\t"
    << "upper\t"
    << "construction\t"
    << "improvement\t"
    << "before\t"
    << "internal\t"
    << "ctime\t"
    << "itime\t"
    << "steps\t"
    << "rules"
    << std::endl;

  for(auto t : tests)
    for(auto graph : graphs)
      for(auto tree : trees)
        run(graph, tree, t, z, sizes, degrees, constructions, improvements,
            scratch, seed);

  return 0;
}


#include <boost/numeric/odeint/stepper/rosenbrock4.hpp>


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE PatternsTest

#include <memory>
#include <vector>

#include <boost/test/unit_test.hpp>

#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/nll.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

BOOST_AUTO_TEST_CASE(Atan2GradOps) {
  // () -> [Atan2GradArg0] -> ()
  // () -> [Atan2GradArg1] -> ()
  // should become
  //
  // Combined by transform
  // () -> [Square] -> [Add] -> (*)
  // () -> [Square] -^
  //
  // For arg 0 (*) -> [Div] -> [Reduce] -> ()
  // For arg 1 (*) -> [Div] -> [Neg] -> [Reduce] -> ()

  // Build an onnx model
  auto builder     = Builder::create();
  auto aiOnnx      = builder->aiOnnxOpset9();
  auto aiGraphcore = builder->aiGraphcoreOpset1();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto atan2Out = aiGraphcore.atan2({input1, input2});
  auto l1       = builder->aiGraphcoreOpset1().l1loss({atan2Out}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art       = AnchorReturnType("All");
  auto dataFlow  = DataFlow(1,
                           {{atan2Out, art},
                            {reservedGradientPrefix() + input1, art},
                            {reservedGradientPrefix() + input2, art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              {},
              Patterns::create({"Atan2Arg0GradOp", "Atan2Arg1GradOp"})
                  .enableRuntimeAsserts(false)});

  // Check the ir
  // Sum of op coun as above
  BOOST_CHECK(ir.opsOfType(Onnx::CustomOperators::Square).size() == 2);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Add).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Div).size() == 2);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Neg).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::ReduceSum).size() == 2);
}

BOOST_AUTO_TEST_CASE(PostNRepl_IdentityOp) {
  // clang-format off
  //
  // (*) -> [Identity] -> () -> [Identity] -> (*)
  //     -> [Identity] -> () -> [Identity] ->  ()
  //     -> [Identity] -> () -> [Identity] ->  () -> [Identity] -> (*)
  //
  // where (*) are Anchors should become
  //
  // (*) -> [Identity] -> (*) -> [Identity] -> (*)
  //
  // clang-format on

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();
  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto i1 = builder->addInputTensor(shape);
  std::vector<TensorId> tensorIds{i1};
  // Create a chain of identity ops
  for (int i = 0; i < 6; i++) {
    auto x = aiOnnx.identity({tensorIds[tensorIds.size() - 1]});
    tensorIds.push_back(x);
  }
  auto l1 = builder->aiGraphcoreOpset1().l1loss({tensorIds.back()}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art       = AnchorReturnType("All");
  auto dataFlow  = DataFlow(1, {{tensorIds.back(), art}, {tensorIds[2], art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              {},
              Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false)});

  // Check the ir
  // All but one of the identityOps should have been removed from the ir
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Identity).size() == 2);

  // All but the 1st, 3rd and last tensors should have been removed
  for (int i = 0; i < tensorIds.size(); i++) {
    bool tensorExists = ir.getMainGraphTensors().contains(tensorIds[i]);
    bool shouldExist  = i == 0 | i == 2 | i == 6;
    BOOST_CHECK(tensorExists == shouldExist);
  }
}

BOOST_AUTO_TEST_CASE(PreUniRepl) {
  // {(i1), (i2)} -> [Add] -> () -> [Pad] -> () -> [Identity] -> (identOut)
  //
  // should become
  //
  // {(i1), (i2)} -> [Add] -> () -> [Identity] -> (identOut)

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto padIn    = aiOnnx.add({input1, input2});
  auto padOut   = aiOnnx.pad({padIn}, {0, 0}, "constant", 0.0);
  auto identOut = aiOnnx.identity({padOut});

  auto l1 = builder->aiGraphcoreOpset1().l1loss({identOut}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto dataFlow  = DataFlow(1, {{identOut, AnchorReturnType("All")}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              {},
              Patterns::create({"PreUniRepl"}).enableRuntimeAsserts(false)});

  // Check the ir
  // the PadOp should have been removed
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Pad).size() == 0);
  // padIn should have been removed
  BOOST_CHECK(ir.getMainGraphTensors().contains(padIn) == false);
}

BOOST_AUTO_TEST_CASE(OpToIdentity) {
  // {(i1), (i2)} -> [Add] -> () -> [Pad] -> () -> [Identity] -> (identOut)
  //
  // should become
  //
  // {(i1), (i2)} -> [Add] -> () -> [Identity] () -> [Identity] -> (identOut)

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto padIn    = aiOnnx.add({input1, input2});
  auto padOut   = aiOnnx.pad({padIn}, {0, 0}, "constant", 0.0);
  auto identOut = aiOnnx.identity({padOut});
  auto l1       = builder->aiGraphcoreOpset1().l1loss({identOut}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto dataFlow  = DataFlow(1, {{identOut, AnchorReturnType("All")}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              {},
              Patterns::create({"OpToIdentity"}).enableRuntimeAsserts(false)});

  // Check the ir
  // the PadOp should have been replaced with an IdentityOp
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Pad).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Identity).size() == 2);
}

BOOST_AUTO_TEST_CASE(GatherToIdentity) {
  // {(i1), (i2)} -> [Gather] -> ()
  //
  // should become
  //
  // (i1) -> [Identity] -> ()

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape1{"FLOAT", std::vector<int64_t>{2, 1, 2}};
  TensorInfo shape2{"INT32", std::vector<int64_t>{1}};

  auto input1 = builder->addInputTensor(shape1);
  auto input2 = builder->addInputTensor(shape2);

  auto out = aiOnnx.gather({input1, input2}, 1);
  auto l1  = builder->aiGraphcoreOpset1().l1loss({out}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  auto dataFlow  = DataFlow(1, {{out, AnchorReturnType("All")}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              {},
              Patterns::create({"OpToIdentity"}).enableRuntimeAsserts(false)});

  // Check the ir
  // the GatherOp should have been replaced with an IdentityOp
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Gather).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Identity).size() == 1);
}

BOOST_AUTO_TEST_CASE(ScaleByOne) {
  // () -> [Scale(by 1.0)] -> ()
  //
  // should become
  //
  // () -> [Identity] -> ()
  // Build an onnx model
  auto builder     = Builder::create();
  auto aiOnnx      = builder->aiOnnxOpset9();
  auto aiGraphcore = builder->aiGraphcoreOpset1();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto inp = builder->addInputTensor(shape);

  auto scale = aiGraphcore.scale({inp}, 1.0f);

  builder->addOutputTensor(scale);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{scale, art}});
  auto device   = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              {},
              nullptr,
              *device,
              {},
              Patterns::create({"OpToIdentity"}).enableRuntimeAsserts(false)});

  // Check the ir
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Identity).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiGraphcore::OpSet1::Scale).size() == 0);
}

BOOST_AUTO_TEST_CASE(ScaleByNegativeOne) {
  // () -> [Scale(by -1.0)] -> ()
  //
  // should become
  //
  // () -> [Negate] -> ()
  // Build an onnx model
  auto builder     = Builder::create();
  auto aiOnnx      = builder->aiOnnxOpset9();
  auto aiGraphcore = builder->aiGraphcoreOpset1();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto inp = builder->addInputTensor(shape);

  auto scale = aiGraphcore.scale({inp}, -1.0f);

  builder->addOutputTensor(scale);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{scale, art}});
  auto device   = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare(
      {modelProto,
       InputShapeInfo(),
       dataFlow,
       {},
       nullptr,
       *device,
       {},
       Patterns::create({"NegativeOneScale"}).enableRuntimeAsserts(false)});

  // Check the ir
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Neg).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiGraphcore::OpSet1::Scale).size() == 0);
}

BOOST_AUTO_TEST_CASE(SubtractArg1GradOp) {
  // () -> [SubtractGradArg1Op] -> ()
  //
  // should become
  //
  // () -> [Negate] -> () -> [ReduceSum] -> ()
  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto subtractOut = aiOnnx.sub({input1, input2});
  auto identOut    = aiOnnx.identity({subtractOut});
  auto l1          = builder->aiGraphcoreOpset1().l1loss({identOut}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art       = AnchorReturnType("All");
  auto dataFlow  = DataFlow(1,
                           {{identOut, art},
                            {reservedGradientPrefix() + input1, art},
                            {reservedGradientPrefix() + input2, art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare(
      {modelProto,
       InputShapeInfo(),
       dataFlow,
       l1,
       &optimizer,
       *device,
       {},
       Patterns::create({"SubtractArg1GradOp"}).enableRuntimeAsserts(false)});

  // Check the ir
  // SubtractArg1Grad should have been replaced with Negate and ReduceSum
  BOOST_CHECK(ir.opsOfType(Onnx::GradOperators::SubArg1Grad).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Neg).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::ReduceSum).size() == 1);
}

BOOST_AUTO_TEST_CASE(ReciprocalGradOp) {
  // () -> [ReciprocalGrad] -> ()
  //
  // should become
  //
  // () -> [Square] -> () -> [Reciprocal] -> () -> [Negate] -> ()

  // Build an onnnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto input = builder->addInputTensor(shape);

  auto output = aiOnnx.reciprocal({input});

  auto l1 = builder->aiGraphcoreOpset1().l1loss({output}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art      = AnchorReturnType("ALL");
  auto dataFlow = DataFlow(
      1, {{output, art}, {reservedGradientPrefix() + input, art}, {l1, art}});
  auto optimizer = ConstSGD(0.01);

  auto opts   = SessionOptions();
  auto device = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare(
      {modelProto,
       InputShapeInfo(),
       dataFlow,
       l1,
       &optimizer,
       *device,
       opts,
       Patterns::create({"ReciprocalGradOp"}).enableRuntimeAsserts(false)});

  // Check the ir
  // ReciprocalGradOp should have been replace with SquareOp, ReciprocalOp,
  // NegateOp, and a MulOp
  BOOST_CHECK(ir.opsOfType(Onnx::GradOperators::ReciprocalGrad).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::CustomOperators::Square).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Reciprocal).size() == 2);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Neg).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Mul).size() == 1);
}

// When a pattern replaces an op with another op
// it should copy any debug context to it
BOOST_AUTO_TEST_CASE(Attribute_Inheritance) {
  // {(i1), (i2)} -> [Add] -> () -> [Pad] -> () -> [Identity] -> (identOut)
  //
  // should become
  //
  // {(i1), (i2)} -> [Add] -> () -> [Identity] -> () -> [Identity] -> (identOut)

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  int64_t vgNumber   = 20;
  std::string opName = "MyPadOp";

  auto recompute = RecomputeType::Recompute;

  auto padIn = aiOnnx.add({input1, input2});
  builder->virtualGraph(padIn, vgNumber);

  // Op to be replaced by pattern
  auto padOut = aiOnnx.pad({padIn}, {0, 0}, "constant", 0.0, opName);
  builder->virtualGraph(padOut, vgNumber);
  builder->recomputeOutputInBackwardPass(padOut, recompute);

  auto identOut = aiOnnx.identity({padOut});
  builder->recomputeOutputInBackwardPass(identOut, recompute);
  builder->virtualGraph(identOut, vgNumber);

  auto l1 = builder->aiGraphcoreOpset1().l1loss({identOut}, 0.1);
  builder->virtualGraph(l1, vgNumber);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto dataFlow  = DataFlow(1, {{identOut, AnchorReturnType("All")}});
  auto optimizer = ConstSGD(0.01);

  auto device = createTestDevice(TEST_TARGET);

  SessionOptions opts;
  opts.virtualGraphMode = VirtualGraphMode::Manual;

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              opts,
              Patterns::create({"OpToIdentity"}).enableRuntimeAsserts(false)});

  // Check the PadOp has been removed
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Pad).size() == 0);

  // Check attributes of the replaced op:
  auto tensor = ir.getMainGraphTensors().get(padOut);
  auto op     = tensor->getProducer();

  // name
  BOOST_CHECK(op->name().find(opName) == 0);

  // virtual graph id
  BOOST_CHECK(op->getVirtualGraphId() == vgNumber);

  // recomputation
  if (recompute == RecomputeType::Recompute) {
    BOOST_CHECK(op->settings.recomputeType == RecomputeType::Recompute);
  } else {
    BOOST_CHECK(op->settings.recomputeType == RecomputeType::Checkpoint ||
                op->settings.recomputeType == RecomputeType::Undefined);
  }
}

BOOST_AUTO_TEST_CASE(PadSumPatternTest) {
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2, 2, 2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto padded1 =
      aiOnnx.pad({input1}, {0, 0, 0, 0, 2, 0}, "constant", 0, "pad1");
  auto padded2 =
      aiOnnx.pad({input2}, {0, 2, 0, 0, 0, 0}, "constant", 0, "pad2");

  auto out = aiOnnx.sum({padded1, padded2}, "sum");

  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  auto dataFlow = DataFlow(1, {{out, AnchorReturnType("All")}});
  auto device   = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              {},
              nullptr,
              *device,
              {},
              Patterns::create({"PadSum"}).enableRuntimeAsserts(false)});

  // Check the ir
  // Sum op should have been replaced with a concat
  BOOST_CHECK(ir.opsOfType(popart::Onnx::AiOnnx::OpSet9::Sum).size() == 0);
  BOOST_CHECK(ir.opsOfType(popart::Onnx::AiOnnx::OpSet9::Concat).size() == 1);
}

// Checking PreUniRepl doesn't throw an exception
// when PadOp connects to graph input
BOOST_AUTO_TEST_CASE(PreUniRepl_0) {
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2, 2, 2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto padded1 =
      aiOnnx.pad({input1}, {0, 0, 0, 0, 0, 0}, "constant", 0, "pad1");
  auto padded2 =
      aiOnnx.pad({input2}, {0, 0, 0, 0, 0, 0}, "constant", 0, "pad2");

  auto out = aiOnnx.concat({padded1, padded2}, 1, "concat");

  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  auto dataFlow = DataFlow(1, {{out, AnchorReturnType("All")}});
  auto device   = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              {},
              nullptr,
              *device,
              {},
              Patterns::create({"PreUniRepl"}).enableRuntimeAsserts(false)});
}

BOOST_AUTO_TEST_CASE(SumToAddTest) {
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2, 2}};

  auto input1 = builder->addInputTensor(shape);
  auto input2 = builder->addInputTensor(shape);

  auto out = aiOnnx.sum({input1, input2});

  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  auto dataFlow = DataFlow(1, {{out, AnchorReturnType("All")}});

  SessionOptions userOptions;

  std::map<std::string, std::string> deviceOpts{{"numIPUs", "2"}};
  auto device = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              {},
              nullptr,
              *device,
              userOptions,
              Patterns::create({"SumToAdd"}).enableRuntimeAsserts(false)});

  BOOST_CHECK(ir.opsOfType(Onnx::Operators::Sum_8).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::Operators::Add_7).size() == 1);
}

BOOST_AUTO_TEST_CASE(Expm1GradOpTest) {
  // Build an onnnx model
  auto builder = Builder::create();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};
  auto input = builder->addInputTensor(shape);

  auto output = builder->aiGraphcoreOpset1().expm1({input});
  auto l1     = builder->aiGraphcoreOpset1().l1loss({output}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art      = AnchorReturnType("ALL");
  auto dataFlow = DataFlow(
      1, {{output, art}, {reservedGradientPrefix() + input, art}, {l1, art}});
  auto optimizer = ConstSGD(0.01);

  auto opts   = SessionOptions();
  auto device = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              opts,
              Patterns::create({"Expm1GradOp"}).enableRuntimeAsserts(false)});

  // Check the ir
  // Expm1Grad should have been replace with Mul and Add.
  BOOST_CHECK(ir.opsOfType(Onnx::GradOperators::Expm1Grad).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Mul).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::Operators::Add_7).size() == 1);
}

BOOST_AUTO_TEST_CASE(Log1pGradOpTest) {
  // Build an onnnx model
  auto builder = Builder::create();

  TensorInfo shape{"FLOAT", std::vector<int64_t>{2}};
  auto input = builder->addInputTensor(shape);

  auto output = builder->aiGraphcoreOpset1().log1p({input});
  auto l1     = builder->aiGraphcoreOpset1().l1loss({output}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  // Add the last tensor, and the 3rd tensor as anchors
  auto art      = AnchorReturnType("ALL");
  auto dataFlow = DataFlow(
      1, {{output, art}, {reservedGradientPrefix() + input, art}, {l1, art}});
  auto optimizer = ConstSGD(0.01);

  auto opts   = SessionOptions();
  auto device = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              l1,
              &optimizer,
              *device,
              opts,
              Patterns::create({"Log1pGradOp"}).enableRuntimeAsserts(false)});

  // Check the ir
  // Log1pGrad should have been replace with Div and Add.
  BOOST_CHECK(ir.opsOfType(Onnx::GradOperators::Log1pGrad).size() == 0);
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Div).size() == 1);
  BOOST_CHECK(ir.opsOfType(Onnx::Operators::Add_7).size() == 1);
}


#ifndef STAN_MATH_TORSTEN_DEF_HPP
#define STAN_MATH_TORSTEN_DEF_HPP

#include <Eigen/Dense>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/is_var.hpp>
#include <stan/math/rev/scal/meta/is_var.hpp>
#include <stan/math/torsten/is_detected.hpp>

namespace refactor {
    template<typename T>
    using PKRec = Eigen::Matrix<T, 1, Eigen::Dynamic>;

    template<typename T>
    using PMXLin = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
}

namespace torsten {

  template <class T>
  using rate_t = typename std::decay<decltype(std::declval<T>().rate())>::type::value_type; // NOLINT

  template <class T>
  using init_t = typename std::decay<decltype(std::declval<T>().y0())>::type::value_type; // NOLINT

  template <class T>
  using par_t = typename std::decay<decltype(std::declval<T>().par())>::type::value_type; // NOLINT

  template <class T>
  using detime_t = typename std::decay<decltype(std::declval<T>().t0())>::type; // NOLINT

  template <class T>
  using scalar_t = typename stan::return_type<detime_t<T>, rate_t<T>, par_t<T>, init_t<T>>::type; // NOLINT

  template <class T>
  using aug_par_t = typename stan::return_type<rate_t<T>, par_t<T>, init_t<T>>::type; // NOLINT

  template <class T>
  using f_t = typename std::decay<decltype(std::declval<T>().f())>::type; // NOLINT

  template <class T>
  using has_var_rate = stan::is_var<rate_t<T> >;

  template <class T>
  using has_var_init = stan::is_var<init_t<T> >;

  template <class T>
  using has_var_par = stan::is_var<par_t<T> >;

}

#endif


//          Copyright Gavin Band 2008 - 2012.
// 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)

#include <memory>
#include <vector>
#include <set>
#include <boost/ptr_container/ptr_vector.hpp>
#include "genfile/VariantEntry.hpp"
#include "genfile/SampleFilter.hpp"
#include "genfile/impl/cast_types_for_comparison.hpp"
#include "genfile/VariableInRangeSampleFilter.hpp"

namespace genfile {
	VariableInRangeSampleFilter::VariableInRangeSampleFilter( std::string const& variable, genfile::VariantEntry lower_bound, genfile::VariantEntry upper_bound ):
		m_variable( variable ),
		m_lower_bound( lower_bound ),
		m_upper_bound( upper_bound )
	{
		// Always compare numbers as doubles.
		if( m_lower_bound.is_int() ) {
			m_lower_bound = double( m_lower_bound.as< int >() ) ;
		}
		if( m_upper_bound.is_int() ) {
			m_upper_bound = double( m_upper_bound.as< int >() ) ;
		}

		if(
			( m_lower_bound.is_string() && m_upper_bound.is_string() )
			|| ( m_lower_bound.is_double() && m_upper_bound.is_double() )
		) {
			// ok
		} else {
			throw genfile::BadArgumentError(
				"VariableInRangeSampleFilter::VariableInRangeSampleFilter()",
				"upper_bound=\"" + genfile::string_utils::to_string( upper_bound ) + "\"",
				"Lower and upper bound must both be strings or numbers."
			) ;
		}
	}

	bool VariableInRangeSampleFilter::test( genfile::CohortIndividualSource const& source, std::size_t i, DetailBlock* detail ) const {
		genfile::VariantEntry value = source.get_entry( i, variable() ) ;
		bool in_range = false ;
		if( !value.is_missing() ) {
			in_range = compare_impl( value, m_lower_bound, m_upper_bound ) ;
		}
		if( detail ) {
			(*detail)( 0, 0 ) = in_range ;
		}
		return in_range ;
	}

	VariableInInclusiveRangeSampleFilter::VariableInInclusiveRangeSampleFilter( std::string const& variable, genfile::VariantEntry lower_bound, genfile::VariantEntry upper_bound ):
		VariableInRangeSampleFilter( variable, lower_bound, upper_bound )
	{}

	void VariableInInclusiveRangeSampleFilter::summarise( std::ostream& oStream ) const {
		oStream << "VariableInInclusiveRangeSampleFilter( " << lower_bound() << ", " << upper_bound() << " )" ;
	}

	bool VariableInInclusiveRangeSampleFilter::compare_impl( genfile::VariantEntry const& value, genfile::VariantEntry const& lower, genfile::VariantEntry const& upper ) const {
		bool result = true ;
		genfile::VariantEntry v = value, a = lower, b = upper ;
		impl::cast_types_for_comparison( v, a ) ;
		result &= ( v >= a ) ;
		v = value ;
		impl::cast_types_for_comparison( v, b ) ;
		result &= ( v <= b ) ;
		return result ;
	}

	VariableInExclusiveRangeSampleFilter::VariableInExclusiveRangeSampleFilter( std::string const& variable, genfile::VariantEntry lower_bound, genfile::VariantEntry upper_bound ):
		VariableInRangeSampleFilter( variable, lower_bound, upper_bound )
	{}

	void VariableInExclusiveRangeSampleFilter::summarise( std::ostream& oStream ) const {
		oStream << "VariableInExclusiveRangeSampleFilter( " << lower_bound() << ", " << upper_bound() << " )" ;
	}

	bool VariableInExclusiveRangeSampleFilter::compare_impl( genfile::VariantEntry const& value, genfile::VariantEntry const& lower, genfile::VariantEntry const& upper ) const {
		bool result = true ;
		genfile::VariantEntry v = value, a = lower, b = upper ;
		impl::cast_types_for_comparison( v, a ) ;
		result &= ( v > a ) ;
		v = value ;
		impl::cast_types_for_comparison( v, b ) ;
		result &= ( v < b ) ;
		return result ;
	}
}


#ifndef INITIAL_NAV_STATE_HPP
#define INITIAL_NAV_STATE_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <okvis/ImuMeasurements.hpp>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/Time.hpp>

namespace swift_vio {
struct InitialNavState {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  bool initializeToCustomPose;
  bool startInMotion;
  okvis::Time stateTime;  // epoch for the initialization values
  Eigen::Vector3d p_WS;
  Eigen::Quaterniond q_WS;
  Eigen::Vector3d v_WS;
  Eigen::Vector3d std_p_WS;
  Eigen::Vector3d
      std_q_WS;  // std of $\delta \theta$ which is expressed in the world frame
  Eigen::Vector3d std_v_WS;

  InitialNavState();

  InitialNavState(const InitialNavState& rhs);

  void updatePose(const okvis::kinematics::Transformation& T_WS,
                  const okvis::Time state_time);

  void toInformation(Eigen::Matrix<double, 6, 6>* information) const;

  void toCovariance(Eigen::Matrix<double, 6, 6>* covariance) const;

  InitialNavState& operator=(const InitialNavState& other);

  std::string toString() const;
};

/**
 * @brief alignZ
 * @param a_S
 * @param[out] q_WS q_WS * a_S = [0, 0, s]
 */
void alignZ(const Eigen::Vector3d &a_S, Eigen::Quaterniond *q_WS);

/**
 * @brief Initialise pose from IMU measurements. For convenience as static.
 * @param[in]  imuMeasurements The IMU measurements to be used for this.
 * @param[out] T_WS initialised pose.
 * @return True if successful.
 */
bool initPoseFromImu(const okvis::ImuMeasurementDeque &imuMeasurements,
                     okvis::kinematics::Transformation &T_WS);

void initBiasesFromStaticImu(const okvis::ImuMeasurementDeque &imuMeasurements,
                             const Eigen::Vector3d &gravityB,
                             okvis::ImuMeasurement *biases);
}  // namespace swift_vio
#endif  // INITIAL_NAV_STATE_HPP