/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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: 							Jaume Coll-Font
	 Last Modification:		September 6 2017
*/

#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>

// Tikhonov specific headers
#include <Core/Algorithms/Legacy/Inverse/TikhonovAlgoAbstractBase.h>
#include <Core/Algorithms/Legacy/Inverse/TikhonovImpl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithStandardTikhonovImpl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTikhonovSVD_impl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTSVD_impl.h>

// Datatypes
#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Math/MiscMath.h>
#include <unsupported/Eigen/Splines>

// SCIRun structural
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Base/AlgorithmVariableNames.h>
#include <Core/Logging/LoggerInterface.h>
#include <Core/Logging/Log.h>
#include <Core/Utils/Exception.h>

using namespace SCIRun;
using namespace SCIRun::Core;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Logging;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::Inverse;

// shared inputs
const AlgorithmInputName TikhonovAlgoAbstractBase::ForwardMatrix("ForwardMatrix");
const AlgorithmInputName TikhonovAlgoAbstractBase::MeasuredPotentials("MeasuredPotentials");
const AlgorithmInputName TikhonovAlgoAbstractBase::WeightingInSourceSpace("WeightingInSourceSpace");
const AlgorithmInputName TikhonovAlgoAbstractBase::WeightingInSensorSpace("WeightingInSensorSpace");

// Inputs specific from the Tikhonov SVD module
const AlgorithmInputName TikhonovAlgoAbstractBase::matrixU("matrixU");
const AlgorithmInputName TikhonovAlgoAbstractBase::singularValues("singularValues");
const AlgorithmInputName TikhonovAlgoAbstractBase::matrixV("matrixV");

// outputs
const AlgorithmOutputName TikhonovAlgoAbstractBase::InverseSolution("InverseSolution");
const AlgorithmOutputName TikhonovAlgoAbstractBase::RegularizationParameter("RegularizationParameter");
const AlgorithmOutputName TikhonovAlgoAbstractBase::LambdaArray("LambdaArray");
const AlgorithmOutputName TikhonovAlgoAbstractBase::Lambda_Index("Lambda_Index");
const AlgorithmOutputName TikhonovAlgoAbstractBase::RegInverse("RegInverse");

ALGORITHM_PARAMETER_DEF( Inverse, TikhonovImplementation);
ALGORITHM_PARAMETER_DEF( Inverse, RegularizationMethod);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationChoice);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaFromDirectEntry);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaMin);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaMax);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaNum);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaResolution);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaSliderValue);
//ALGORITHM_PARAMETER_DEF( Inverse, LambdaCorner);
//ALGORITHM_PARAMETER_DEF( Inverse, LCurveText);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationSolutionSubcase);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationResidualSubcase);

TikhonovAlgoAbstractBase::TikhonovAlgoAbstractBase()
{
	addParameter(Parameters::TikhonovImplementation, std::string("NoMethodSelected") );
	addOption(Parameters::RegularizationMethod, "lcurve", "single|slider|lcurve");
	addParameter(Parameters::regularizationChoice, 0);
	addParameter(Parameters::LambdaFromDirectEntry,1e-6);
	addParameter(Parameters::LambdaMin,1e-6);
	addParameter(Parameters::LambdaMax,1);
	addParameter(Parameters::LambdaNum,200);
	addParameter(Parameters::LambdaResolution,1e-6);
	addParameter(Parameters::LambdaSliderValue,0);
	addParameter(Parameters::regularizationSolutionSubcase,solution_constrained);
	addParameter(Parameters::regularizationResidualSubcase,residual_constrained);
}

////// CHECK IF INPUT MATRICES HAVE THE CORRECT SIZE
bool TikhonovAlgoAbstractBase::checkInputMatrixSizes( const AlgorithmInput & input) const
{
	auto forwardMatrix_ = input.get<Matrix>(ForwardMatrix);
	auto measuredData_ = input.get<Matrix>(MeasuredPotentials);
	auto sourceWeighting_ = input.get<Matrix>(WeightingInSourceSpace);
	auto sensorWeighting_ = input.get<Matrix>(WeightingInSensorSpace);

  const int M = forwardMatrix_->nrows();
  const int N = forwardMatrix_->ncols();

  // check that rows of fwd matrix equal number of measurements
  if ( M != measuredData_->nrows() )
  {
  	THROW_ALGORITHM_INPUT_ERROR("Input matrix dimensions must agree.");
  	return false;
  }

  // check source regularization matrix sizes
  if (sourceWeighting_)
  {
    if( get(Parameters::regularizationSolutionSubcase).toInt()==solution_constrained )
    {
      // check that the matrix is of appropriate size (equal number of rows as columns in fwd matrix)
      if ( N != sourceWeighting_->ncols() )
      {
    		THROW_ALGORITHM_INPUT_ERROR("Solution Regularization Matrix must have the same number of rows as columns in the Forward Matrix !");
    		return false;
      }
    }
    // otherwise, if the source regularization is provided as the squared version (RR^T)
    else if ( get(Parameters::regularizationSolutionSubcase).toInt()==solution_constrained_squared )
    {
      // check that the matrix is of appropriate size and squared (equal number of rows as columns in fwd matrix)
      if ( ( N != sourceWeighting_->nrows() ) || ( N != sourceWeighting_->ncols() ) )
      {
        THROW_ALGORITHM_INPUT_ERROR("The squared solution Regularization Matrix must have the same number of rows and columns and must be equal to the number of columns in the Forward Matrix !");
	      return false;
      }
    }
  }

  // check measurement regularization matrix sizes
  if (sensorWeighting_)
  {
    if (get(Parameters::regularizationResidualSubcase).toInt() == residual_constrained)
    {
      // check that the matrix is of appropriate size (equal number of rows as rows in fwd matrix)
      if(M != sensorWeighting_->ncols())
      {
        THROW_ALGORITHM_INPUT_ERROR("Data Residual Weighting Matrix must have the same number of rows as the Forward Matrix !");
        return false;
      }
    }
    // otherwise if the source covariance matrix is provided in squared form
    else if  ( get(Parameters::regularizationResidualSubcase).toInt() == residual_constrained_squared )
    {
      // check that the matrix is of appropriate size and squared (equal number of rows as rows in fwd matrix)
      if( (M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()) )
      {
        THROW_ALGORITHM_INPUT_ERROR("Squared data Residual Weighting Matrix must have the same number of rows and columns as number of rows in the Forward Matrix !");
        return false;
      }
    }
  }
  return true;
}

AlgorithmOutput TikhonovAlgoAbstractBase::run(const AlgorithmInput & input) const
{
	auto forwardMatrix = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::ForwardMatrix));
	auto measuredData = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::MeasuredPotentials));
	auto sourceWeighting = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSourceSpace));
	auto sensorWeighting = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSensorSpace));

	auto RegularizationMethod_gotten = getOption(Parameters::RegularizationMethod);
	auto implOption = get(Parameters::TikhonovImplementation).toString();

	// check input MATRICES
	checkInputMatrixSizes( input );

	// Determine specific Tikhonov Implementation
	std::shared_ptr<TikhonovImpl> algoImpl;
	if (implOption == "standardTikhonov")
  {
		int regularizationChoice = get(Parameters::regularizationChoice).toInt();
		int regularizationSolutionSubcase = get(Parameters::regularizationSolutionSubcase).toInt();
		int regularizationResidualSubcase = get(Parameters::regularizationResidualSubcase).toInt();

		algoImpl = std::make_shared<SolveInverseProblemWithStandardTikhonovImpl>( *forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting,
      regularizationChoice, regularizationSolutionSubcase, regularizationResidualSubcase);
	}
	else if (implOption == "TikhonovSVD")
  {
		// get TikhonovSVD special inputs
		auto matrixU = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixU));
		auto singularValues = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::singularValues));
		auto matrixV = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixV));

		// If there is a missing matrix from the precomputed SVD input
		if (!matrixU || !singularValues || !matrixV)
			algoImpl = std::make_shared<SolveInverseProblemWithTikhonovSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting);
		else
			algoImpl = std::make_shared<SolveInverseProblemWithTikhonovSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting, *matrixU, *singularValues, *matrixV);
	}
	else if (implOption == "TSVD")
  {
		// get TikhonovSVD special inputs
		auto matrixU = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixU));
		auto singularValues = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::singularValues));
		auto matrixV = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixV));

		// If there is a missing matrix from the precomputed SVD input
		if (!matrixU || !singularValues || !matrixV)
			algoImpl = std::make_shared<SolveInverseProblemWithTSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting);
		else
			algoImpl = std::make_shared<SolveInverseProblemWithTSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting, *matrixU, *singularValues, *matrixV);
	}
	else
  {
		THROW_ALGORITHM_PROCESSING_ERROR("Not a valid Tikhonov Implementation selection");
	}

  double lambda = 0;
  int lambda_index = 0;
  AlgorithmOutput output;
  DenseMatrixHandle lambdamatrix;
  //Get Regularization parameter(s) : Lambda
  if ((RegularizationMethod_gotten == "single") || (RegularizationMethod_gotten == "slider"))
  {
    if (RegularizationMethod_gotten == "single")
    {
      // Use single fixed lambda value, entered in UI
      lambda = get(Parameters::LambdaFromDirectEntry).toDouble();
    }
    else if (RegularizationMethod_gotten == "slider")
    {
      // Use single fixed lambda value, select via slider
      lambda = get(Parameters::LambdaSliderValue).toDouble();
    }
    lambdamatrix.reset(new DenseMatrix(1,1,lambda));
  }
  else if (RegularizationMethod_gotten == "lcurve")
  {
    lambda = computeLcurve( *algoImpl, input,  lambdamatrix, lambda_index);
  }
	else
	{
		THROW_ALGORITHM_PROCESSING_ERROR("Lambda selection was never set");
	}

  // compute final inverse solution
	auto solution = algoImpl->computeInverseSolution(lambda, true);

	// Set outputs

	output[InverseSolution] = boost::make_shared<DenseMatrix>(solution);
	output[RegularizationParameter] = boost::make_shared<DenseMatrix>(1, 1, lambda);
  output[LambdaArray] = lambdamatrix;
  output[Lambda_Index]= boost::make_shared<DenseMatrix>(1, 1, lambda_index);

	return output;
}

double TikhonovAlgoAbstractBase::computeLcurve( const SCIRun::Core::Algorithms::Inverse::TikhonovImpl& algoImpl, const AlgorithmInput & input , DenseMatrixHandle& lambdamatrix, int& lambda_index) const
{
	// get inputs
	auto forwardMatrix = input.get<Matrix>(TikhonovAlgoAbstractBase::ForwardMatrix);
	auto measuredData = input.get<Matrix>(TikhonovAlgoAbstractBase::MeasuredPotentials);
	auto sourceWeighting = input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSourceSpace);
	auto sensorWeighting = input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSensorSpace);
  // define the step size of the lambda vector to be computed  (distance between min and max divided by number of desired lambdas in log scale)
  const int nLambda = get(Parameters::LambdaNum).toInt();
	const double lambdaMin = get(Parameters::LambdaMin).toDouble();
	const double lambdaMax = get(Parameters::LambdaMax).toDouble();
	double lambda = 0;

	// prealocate vector of lambdas and eta and rho
  std::vector<double> rho(nLambda, 0.0);
  std::vector<double> eta(nLambda, 0.0);

  lambdamatrix.reset(new DenseMatrix(nLambda,3,0.0));

  auto lambdaArray = algoImpl.computeLambdaArray( lambdaMin, lambdaMax, nLambda );

  DenseMatrix CAx, Rx;
  DenseMatrix solution;

  lambdaArray[0] = lambdaMin;

  // for all lambdas
  for (int j = 0; j < nLambda; j++)
  {
    solution = algoImpl.computeInverseSolution( lambdaArray[j], false);
    lambdamatrix->put(j,0,lambdaArray[j]);

    // if using source regularization matrix, apply it to compute Rx (for the eta computations)
    if (sourceWeighting)
    {
      if (solution.nrows() == sourceWeighting->ncols()) // check that regularization matrix and solution match sizes
      {
        auto sw = castMatrix::toDense(sourceWeighting);
        Rx = (*sw) * solution;
      }
      else
      {
        BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage(" Solution weighting matrix unexpectedly does not fit to compute the weighted solution norm. "));
      }
    }
    else
      Rx = solution;

    auto forward = castMatrix::toDense(forwardMatrix);
    auto Ax = (*forward) * solution;
    auto measured = castMatrix::toDense(measuredData);
    auto residualSolution = Ax - (*measured);

    // if using source regularization matrix, apply it to compute Rx (for the eta computations)
    if (sensorWeighting)
    {
      auto sw = castMatrix::toDense(sensorWeighting);
      CAx = (*sw) * residualSolution;
    }
    else
      CAx = residualSolution;

    // compute rho and eta. Using Frobenious norm when using matrices
    rho[j] = CAx.norm();
    eta[j] = Rx.norm();
    lambdamatrix->put(j,1,rho[j]);
    lambdamatrix->put(j,2,eta[j]);
  }

  // Find corner in L-curve
  lambda = FindCorner( rho, eta, lambdaArray, nLambda,lambda_index);

	LOG_DEBUG("Lambda: {}", lambda);
  // TODO: update GUI

  return lambda;
}

///// Find Corner, find the maximal curvature which corresponds to the L-curve corner
double TikhonovAlgoAbstractBase::FindCorner( const std::vector<double>& rho, const std::vector<double>& eta, const std::vector<double>& lambdaArray, const int nLambda, int& lambda_index )
{

	DenseColumnMatrix lrho(nLambda);
	DenseColumnMatrix leta(nLambda);

	for (int i = 0; i < nLambda; i++)
	{
		lrho[i] = std::log10(rho[i]);
		leta[i] = std::log10(eta[i]);
	}

	// create L-curve
	DenseMatrix Gamma( 2, lrho.nrows());
	Gamma.row(0) = lrho;
	Gamma.row(1) = leta;//DenseColumnMatrix::LinSpaced(lrho.nrows(),0,1);

	// fit spline and compute curvature
	DenseColumnMatrix kappa = TikhonovAlgoAbstractBase::InterpolateCurvatureWithSplines( Gamma );

	// select maximum curvature
	kappa.maxCoeff(&lambda_index);

  	return lambdaArray[lambda_index];
}

SCIRun::Core::Datatypes::DenseColumnMatrix TikhonovAlgoAbstractBase::InterpolateCurvatureWithSplines( SCIRun::Core::Datatypes::DenseMatrix& samplePoints)
{

	// prealoate
	int numSamples = samplePoints.ncols();
	DenseColumnMatrix kappa = DenseMatrix::Zero( numSamples, 1 );

	// typedefs needed for the spline
	typedef Eigen::Spline<double,2> Spline2d;
	typedef Spline2d::KnotVectorType KnotVectorType;
	typedef Spline2d::ControlPointVectorType ControlPointVectorType;

	// fit cubic spline to data points
	ControlPointVectorType points = samplePoints;
	const Spline2d spline = Eigen::SplineFitting<Spline2d>::Interpolate(points,3);

	// determine position of samples along the spline curve
	KnotVectorType chord_lengths; // knot parameters
	Eigen::ChordLengths(points, chord_lengths);

	for (int i=0; i < numSamples; i++)
	{
		// compute derivatives up to 2nd order
		auto dpt = spline.derivatives( chord_lengths(i), 2);

		// compute curvature as:
		//			abs( ( ddrho*ddeta - ddrho*deta ) /  sqrt(  ( deta^2 + drho^2  )^2)  )
		kappa[i] = std::abs( (dpt(0,2) * dpt(1,2) - dpt(0,2) * dpt(1,1)) /  //compute curvature
	                       std::sqrt( std::pow(dpt(1,1)*dpt(1,1)+dpt(0,1)*dpt(0,1) , 3.0)) );

	}

	// return curvature for all points on the Lcurve
	return kappa;
}


/*
 * The MIT License
 *
 * Copyright 2018 Vindula Jayawardana and Matthew Zalesak.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "network.hpp"
#include "settings.hpp"

#include <iostream>
#include <fstream>
#include <map>
#include <math.h>
#include <queue>
#include <sstream>
#include <stdexcept>
#include <string>
#include <boost/algorithm/string.hpp>

using namespace std;
using namespace boost;

Network::Network()
{
    string line;
    ifstream timefile(DATAROOT + "/map/" + TIMEFILE);
    if (!timefile.is_open())
        throw runtime_error("Unable to open matrix file");

    int rowsize = 0;
    while (getline(timefile, line))
    {
        vector<int> row;
        row.reserve(rowsize);

        char * cstr = new char [line.length() + 1];
        std::strcpy(cstr, line.c_str());

        char* token;
        token = strtok(cstr, ",");
        while (token != NULL)
        {
            int entry_value = stoi(token);
            row.push_back(entry_value);
            token = strtok(NULL, ",");
        }
        delete[] cstr;
        time_matrix.push_back(row);
    }

    // WE ARE NOT USING DISTANCE HERE YET, SO JUST DUPLICATING TIME.
    //ifstream distancefile(DATAROOT + "/map/times.csv"); // full_distances.csv");
    ifstream distancefile(DATAROOT + "/map/" + TIMEFILE);
    if (!distancefile.is_open())
        throw runtime_error("Unable to open distances matrix file.");

    while (getline(distancefile, line))
    {
        vector<int> row;
        row.reserve(rowsize);

        char * cstr = new char [line.length() + 1];
        std::strcpy(cstr, line.c_str());

        char* token;
        token = strtok(cstr, ",");
        while (token != NULL)
        {
            int entry_value = stoi(token);
            row.push_back(entry_value);
            token = strtok(NULL, ",");
        }
        delete[] cstr;
        distance_matrix.push_back(row);
    }

    ifstream edgefile(DATAROOT + "/map/" + EDGECOST_FILE);
    if (edgefile.is_open())
    {
        while (getline(edgefile, line))
        {
            vector<string> fields;
            split(fields, line, is_any_of(","));
            int origin = stoi(fields[0]) - 1;
            int dest = stoi(fields[1]) - 1;
            int length = stoi(fields[2]);

            if (adjacency_list.size() < origin + 1)
                adjacency_list.resize(origin + 1);

            adjacency_list[origin].push_back(neighbor(dest, length));
        }
    }
    else
        throw runtime_error("Unable to open file for dijkstra shortest path calculation.");
}

/* Travel time between nodes. */
int Network::get_time(int node_one, int node_two) const
{
    if (node_one == -10)
        return DWELL_PICKUP;
    else if (node_one == -20)
        return DWELL_ALIGHT;
    else if (node_one < 0 || node_two < 0)
    {
        cout << "Network Error: Line " << __LINE__ << endl;
        getchar();
    }
    return time_matrix[node_one][node_two];
}

int Network::get_distance(int node_one, int node_two) const
{
    return get_time(node_one, node_two);
    if (node_one == -10 || node_one == -20)
        return 0;
    if (node_one < 0 || node_two < 0)
    {
        cout << "Negative index given to get_distance!  Network Line " << __LINE__ << endl;
        getchar();
    }
    return distance_matrix[node_one][node_two];
}

/* Specifically, this gets the distance offset. */
int Network::get_vehicle_offset(Vehicle const & v) const
{
    int origin = v.prev_node, destination = v.node;
    if (origin < 0 || destination < 0) // TODO Just error catcher, should remove.
        return 0;

    int time = get_time(origin, destination);
    int distance = get_distance(origin, destination);
    int time_offset = time - v.offset;

    if (time == 0)
        return 0;
    else
    {
        double percent = time_offset / time;
        double distance_offset_true = distance * percent;
        int distance_offset = int(floor(distance_offset_true));
        return distance_offset;
    }
}

int Network::get_vehicle_distance(Vehicle const & v, int node) const
{
    int origin = v.prev_node, destination = v.node;
    int current_leg = get_distance(origin, destination) - get_vehicle_offset(v);
    int final_leg = get_distance(destination, node);
    return current_leg + final_leg;
}

int Network::get_vehicle_time(Vehicle const & v, int node) const
{
    return v.offset + get_time(v.node, node);
}

vector<int> Network::dijkstra(int origin, int destination) const
{
    vector<int> path {origin};
    int here = origin;
    int count = 0;
    while (here != destination && here != -1 && count < 200)  // For each step of the way...
    {
        int best = get_time(here, destination) + 1;
        int node = -1;
        for (auto &n : adjacency_list[here])    // Try to take a strict step.
        {
            int n_node = n.target;
            int time = n.weight;

            if (n_node == destination)
            {
                node = n_node;
                break;
            }

            int follow_up = get_time(n_node, destination);
            if (time > 0 && time + follow_up < best)
            {
                best = time + follow_up;
                node = n_node;
            }
        }

        if (node == -1)                         // Logic if only good choices are zeros.
        {
            queue<neighbor> zeros;
            map<int,vector<int>> heritage;
            int comparison = get_time(here, destination);
            for (auto &n : adjacency_list[here])
                if (n.weight + get_time(n.target, destination) <= comparison)
                {
                    zeros.push(n);
                    heritage.emplace(n.target, initializer_list<int>{n.target});
                }

            while (zeros.size() && node == -1)
            {
                neighbor n = zeros.front();
                zeros.pop();

                for (auto &child : adjacency_list[n.target])
                {
                    if (child.weight + get_time(child.target, destination) <= comparison)
                    {
                        if (child.weight > 0 || child.target == destination)  // Golden case!  We found it!
                        {
                            path.insert(path.end(), heritage[n.target].begin(), heritage[n.target].end());
                            node = child.target;
                            break;
                        }
                        if (!heritage.count(child.target))  // Another zero we can consider!
                        {
                            zeros.push(child);
                            vector<int> new_heritage = heritage[n.target];
                            new_heritage.push_back(child.target);
                            heritage[child.target] = new_heritage;
                        }
                    }
                }
            }
        }

        path.push_back(node);
        here = node;
        count ++;
    }

    if (count > 200 || here == -1)
    {
        cout << "Oops!  Network dijkstra messed up somehow!" << endl;
        cout << "Query from " << origin << " to " << destination << endl;
        for (auto i : path)
            cout << i << "\t" << get_time(i, destination) << endl;
        getchar();
    }

    return path;
}


#pragma once

// deal.II includes -------------------------------------------------------
#include <deal.II/base/tensor.h>
// system includes --------------------------------------------------------
#include <boost/multi_array.hpp>
#include <iostream>
// own includes -----------------------------------------------------------
#include "var_form.hpp"

namespace boltzmann {

template <int DIMX>
class PolarXVarForm : public VarForm<DIMX, dealii::QGauss>
{
 private:
  typedef VarForm<DIMX, dealii::QGauss> base_class;

 public:
  static const int fe_dim;
  static const int n_quad_points;

  //@{
  /* Storage typedefs **/
  typedef dealii::Tensor<2, DIMX> T2_t;
  typedef dealii::Tensor<1, DIMX> T1_t;
  typedef boost::multi_array<T2_t, 2> T2_mat_t;
  typedef boost::multi_array<T1_t, 2> T1_mat_t;
  typedef boost::multi_array<double, 2> D_mat_t;
  //@}

 public:
  template <typename FE>
  PolarXVarForm(const FE& fe);
  void clear_storage();
  const T1_mat_t& T1() const { return T1_mat; }
  const T2_mat_t& T2() const { return T2_mat; }
  const D_mat_t& S0() const { return S0_mat; }
  const T1_mat_t& S1() const { return S1_mat; }

 protected:
  //@{
  /** local matrix storage */
  T2_mat_t T2_mat;
  T1_mat_t T1_mat;
  T1_mat_t S1_mat;
  D_mat_t S0_mat;
  //@}
};

template <int DIMX>
const int PolarXVarForm<DIMX>::fe_dim = 1;
template <int DIMX>
const int PolarXVarForm<DIMX>::n_quad_points = 2;

template <int DIMX>
template <typename FE>
PolarXVarForm<DIMX>::PolarXVarForm(const FE& fe)
    : base_class(fe, n_quad_points)
{
  int n_dofs_per_cell = fe.dofs_per_cell;
  T2_mat.resize(boost::extents[n_dofs_per_cell][n_dofs_per_cell]);
  T1_mat.resize(boost::extents[n_dofs_per_cell][n_dofs_per_cell]);
  S1_mat.resize(boost::extents[n_dofs_per_cell][n_dofs_per_cell]);
  S0_mat.resize(boost::extents[n_dofs_per_cell][n_dofs_per_cell]);
}

template <int DIMX>
void
PolarXVarForm<DIMX>::clear_storage()
{
  // const int n_dofs_per_cell = this->fe_values.dofs_per_cell;
  assert(this->fe_values.dofs_per_cell == 4);
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      T2_mat[i][j] = 0;
      T1_mat[i][j] = 0;
      S1_mat[i][j] = 0;
      S0_mat[i][j] = 0;
    }
  }
}

}  // end namespace boltzmann


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

// 素因数分解
std::unordered_map<long long, int> factor(long long n) {
	std::unordered_map<long long, int> factors;
	{
		int i = 0;
		while(n % 2 == 0) {
			n /= 2;
			i++;
		}
		if(i != 0) {
			factors[2] = static_cast<long long>(i);
		}
	}
	for(long long i = 3; i * i <= n; i += 2) {
		int j = 0;
		while(n % i == 0) {
			n /= i;
			j++;
		}
		if(j != 0) {
			factors[i] = j;
		}
	}
	if(n != 1) {
		factors[n] = 1;
	}
	return factors;
}

int main() {
	ll n;
	cin >> n;

	unordered_map<ll, int> factors;
	RANGE(i, 1, n + 1) {
		auto f = factor(i);
		EACH(e, f) {
			if(factors[e.first] != 0) {
				chmax(factors[e.first], e.second);
			} else {
				factors[e.first] = e.second;
			}
		}
	}

	ll product = 1;
	EACH(e, factors) {
		REP(i, e.second) { product *= e.first; }
	}
	product += 1;

	cout << product << endl;
	return 0;
}


#include <catch.hpp>

#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/si.hpp>

#include <UnitConvert.hpp>

TEST_CASE("BoostUnitRegisty")
{
  UnitConvert::BoostUnitRegistry<boost::units::si::system> ureg;

  SECTION("Conversions")
  {

  UnitConvert::Quantity<double> q;

  // length
  q = ureg.makeQuantity<double>(24, "cm");
  CHECK(q.to("m").value() == Approx(0.24));
  CHECK(q.to("cm").value() == Approx(24));
  CHECK(q.to("mm").value() == Approx(240));
  CHECK(q.to_base_units().value() == Approx(0.24));

  // mass
  q = ureg.makeQuantity<double>(24, "kg");
  CHECK(q.to("kg").value() == Approx(24));
  // CHECK( q.to("g").value() == Approx(24000) ); // 'g' does not exist yet
  CHECK(q.to("mkg").value() == Approx(24000));
  CHECK(q.to_base_units().value() == Approx(24));

  // time
  q = ureg.makeQuantity<double>(24, "ms");
  CHECK(q.to("ms").value() == Approx(24));
  CHECK(q.to("cs").value() == Approx(2.4));
  CHECK(q.to("s").value() == Approx(0.024));
  CHECK(q.to("ks").value() == Approx(0.000024));
  CHECK(q.to_base_units().value() == Approx(0.024));

  // electrical current
  q = ureg.makeQuantity<double>(24, "A");
  CHECK(q.to("mA").value() == Approx(24000));
  CHECK(q.to("cA").value() == Approx(2400));
  CHECK(q.to("A").value() == Approx(24));
  CHECK(q.to("kA").value() == Approx(0.024));
  CHECK(q.to_base_units().value() == Approx(24));

  // temperature
  q = ureg.makeQuantity<double>(24, "K");
  CHECK(q.to("mK").value() == Approx(24000));
  CHECK(q.to("cK").value() == Approx(2400));
  CHECK(q.to("K").value() == Approx(24));
  CHECK(q.to("kK").value() == Approx(0.024));
  CHECK(q.to_base_units().value() == Approx(24));

  // amount
  q = ureg.makeQuantity<double>(24, "mol");
  CHECK(q.to("mmol").value() == Approx(24000));
  CHECK(q.to("cmol").value() == Approx(2400));
  CHECK(q.to("mol").value() == Approx(24));
  CHECK(q.to("kmol").value() == Approx(0.024));
  CHECK(q.to_base_units().value() == Approx(24));

  // luminous intensity
  q = ureg.makeQuantity<double>(24, "cd");
  CHECK(q.to("mcd").value() == Approx(24000));
  CHECK(q.to("ccd").value() == Approx(2400));
  CHECK(q.to("cd").value() == Approx(24));
  CHECK(q.to("kcd").value() == Approx(0.024));
  CHECK(q.to_base_units().value() == Approx(24));

  // now add some non-base units
  ureg.addUnit(boost::units::cgs::mass());
  ureg.addUnit("N = kg m / s^2");
  ureg.addUnit("J = N m");
  ureg.addUnit("W = J s");

  q = ureg.makeQuantity<double>(24, "kg");
  CHECK(q.to("kg").value() == Approx(24));
  CHECK(q.to("g").value() == Approx(24000));  // 'g' exists now

  q = ureg.makeQuantity<double>(24, "kJ");
  CHECK(q.to("kJ").value() == Approx(24));
  CHECK(q.to("J").value() == Approx(24000));
  CHECK(q.to("mJ").value() == Approx(24000000));
  CHECK(q.to("g m^2 / s^2").value() == Approx(24000000));
  CHECK(q.to("g m^2 / ms^2").value() == Approx(24));
  CHECK(q.to_base_units().value() == Approx(24000));

  q = ureg.makeQuantity<double>(24, "mW");
  CHECK(q.to("kW").value() == Approx(0.000024));
  CHECK(q.to("W").value() == Approx(0.024));
  CHECK(q.to("mW").value() == Approx(24));
  CHECK(q.to_base_units().value() == Approx(0.024));
  }

  SECTION("Adding Boost.Units unit")
  {
    CHECK_THROWS( ureg.makeQuantity<double>("1 g") );
    ureg.addUnit(boost::units::cgs::mass());
    ureg.makeQuantity<double>("1 g");
  }

  SECTION("Converting to Boost.Units quantity")
  {
    auto L = ureg.makeQuantity<double>("100 cm").to<boost::units::si::length>();
    auto T = ureg.makeQuantity<double>("2 ms").to<boost::units::si::time>();

    auto V = L/T;

    CHECK( V.value() == Approx( (100./2)*(1./100)*(1000./1)   ) );
  }

}


// Copyright 2016-2018 Doug Moen
// Licensed under the Apache License, version 2.0
// See accompanying file LICENSE or https://www.apache.org/licenses/LICENSE-2.0

#include <cassert>
#include <cmath>
#include <cstdlib>
#include <string>

#include <boost/math/constants/constants.hpp>
#include <boost/filesystem.hpp>

#include <curv/arg.h>
#include <curv/builtin.h>
#include <curv/program.h>
#include <curv/exception.h>
#include <curv/file.h>
#include <curv/function.h>
#include <curv/shape.h>
#include <curv/system.h>
#include <curv/gl_context.h>
#include <curv/array_op.h>
#include <curv/analyser.h>
#include <curv/math.h>

using namespace std;
using namespace boost::math::double_constants;

namespace curv {

Shared<Meaning>
Builtin_Value::to_meaning(const Identifier& id) const
{
    return make<Constant>(share(id), value_);
}

struct Is_Null_Function : public Polyadic_Function
{
    Is_Null_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].is_null()};
    }
};
struct Is_Bool_Function : public Polyadic_Function
{
    Is_Bool_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].is_bool()};
    }
};
struct Is_Num_Function : public Polyadic_Function
{
    Is_Num_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].is_num()};
    }
};
struct Is_String_Function : public Polyadic_Function
{
    Is_String_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<String>() != nullptr};
    }
};
struct Is_List_Function : public Polyadic_Function
{
    Is_List_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<List>() != nullptr};
    }
};
struct Is_Record_Function : public Polyadic_Function
{
    Is_Record_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Structure>() != nullptr};
    }
};
struct Is_Fun_Function : public Polyadic_Function
{
    Is_Fun_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Function>() != nullptr};
    }
};

struct Bit_Function : public Polyadic_Function
{
    Bit_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        return {double(args[0].to_bool(At_Arg(args)))};
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto arg = f[0];
        if (arg.type != GL_Type::Bool)
            throw Exception(At_GL_Arg(0, f),
                "bit: argument is not a bool");
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = float("<<arg<<");\n";
        return result;
    }
};

struct Sqrt_Function : public Polyadic_Function
{
    Sqrt_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return sqrt(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("sqrt(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "sqrt");
    }
};
// log(x) is the natural logarithm of x
struct Log_Function : public Polyadic_Function
{
    Log_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return log(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("log(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "log");
    }
};
struct Abs_Function : public Polyadic_Function
{
    Abs_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return abs(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("abs(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "abs");
    }
};
struct Floor_Function : public Polyadic_Function
{
    Floor_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return floor(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("floor(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "floor");
    }
};
// round(x) -- round x to nearest integer.
// CPU: in case of tie, round to even.
// GPU: in case of tie, it's GPU/driver dependent.
struct Round_Function : public Polyadic_Function
{
    Round_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return rint(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("round(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "round");
    }
};
struct Sin_Function : public Polyadic_Function
{
    Sin_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return sin(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("sin(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "sin");
    }
};
struct Asin_Function : public Polyadic_Function
{
    Asin_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return asin(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("asin(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "asin");
    }
};
struct Cos_Function : public Polyadic_Function
{
    Cos_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return cos(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("cos(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "cos");
    }
};
struct Acos_Function : public Polyadic_Function
{
    Acos_Function() : Polyadic_Function(1) {}
    struct Scalar_Op {
        static double f(double x) { return acos(x); }
        static Shared<const String> callstr(Value x) {
            return stringify("acos(",x,")");
        }
    };
    static Unary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        return gl_call_unary_numeric(f, "acos");
    }
};
struct Atan2_Function : public Polyadic_Function
{
    Atan2_Function() : Polyadic_Function(2) {}

    struct Scalar_Op {
        static double f(double x, double y) { return atan2(x, y); }
        static const char* name() { return "atan2"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("atan2(",x,",",y,")");
        }
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(args[0], args[1], At_Arg(args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto x = f[0];
        auto y = f[1];

        GL_Type rtype = GL_Type::Bool;
        if (x.type == y.type)
            rtype = x.type;
        else if (x.type == GL_Type::Num)
            rtype = y.type;
        else if (y.type == GL_Type::Num)
            rtype = x.type;
        if (rtype == GL_Type::Bool)
            throw Exception(At_GL_Phrase(f.call_phrase_, &f),
                "GL domain error");

        GL_Value result = f.gl.newvalue(rtype);
        f.gl.out <<"  "<<rtype<<" "<<result<<" = atan(";
        gl_put_as(f, x, At_GL_Arg(0, f), rtype);
        f.gl.out << ",";
        gl_put_as(f, y, At_GL_Arg(1, f), rtype);
        f.gl.out << ");\n";
        return result;
    }
};

GL_Value gl_minmax(const char* name, Operation& argx, GL_Frame& f)
{
    auto list = dynamic_cast<List_Expr*>(&argx);
    if (list) {
        std::list<GL_Value> args;
        GL_Type type = GL_Type::Num;
        for (auto op : *list) {
            auto val = op->gl_eval(f);
            args.push_back(val);
            if (val.type == GL_Type::Num)
                ;
            else if (gl_type_count(val.type) >= 2) {
                if (type == GL_Type::Num)
                    type = val.type;
                else if (type != val.type)
                    throw Exception(At_GL_Phrase(op->source_, &f), stringify(
                        "GL: ",name,
                        ": vector arguments of different lengths"));
            } else {
                throw Exception(At_GL_Phrase(op->source_, &f), stringify(
                    "GL: ",name,": argument has bad type"));
            }
        }
        auto result = f.gl.newvalue(type);
        if (args.size() == 0)
            f.gl.out << "  " << type << " " << result << " = -0.0/0.0;\n";
        else if (args.size() == 1)
            return args.front();
        else {
            f.gl.out << "  " << type << " " << result << " = ";
            int rparens = 0;
            while (args.size() > 2) {
                f.gl.out << name << "(" << args.front() << ",";
                args.pop_front();
                ++rparens;
            }
            f.gl.out << name << "(" << args.front() << "," << args.back() << ")";
            while (rparens > 0) {
                f.gl.out << ")";
                --rparens;
            }
            f.gl.out << ";\n";
        }
        return result;
    } else {
        auto arg = argx.gl_eval(f);
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = ";
        if (arg.type == GL_Type::Vec2)
            f.gl.out << name <<"("<<arg<<".x,"<<arg<<".y);\n";
        else if (arg.type == GL_Type::Vec3)
            f.gl.out << name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z);\n";
        else if (arg.type == GL_Type::Vec4)
            f.gl.out << name<<"("<<name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z),"<<arg<<".w);\n";
        else
            throw Exception(At_GL_Phrase(argx.source_, &f), stringify(
                name,": argument is not a vector"));
        return result;
    }
}

struct Max_Function : public Polyadic_Function
{
    Max_Function() : Polyadic_Function(1) {}

    struct Scalar_Op {
        static double f(double x, double y) {
            // return NaN if either argument is NaN.
            if (x >= y) return x;
            if (x < y) return y;
            return 0.0/0.0;
        }
        static const char* name() { return "max"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("max(",x,",",y,")");
        }
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(-INFINITY, args[0], At_Arg(args));
    }
    GL_Value gl_call_expr(Operation& argx, const Call_Phrase*, GL_Frame& f)
    const override
    {
        return gl_minmax("max",argx,f);
    }
};

struct Min_Function : public Polyadic_Function
{
    Min_Function() : Polyadic_Function(1) {}

    struct Scalar_Op {
        static double f(double x, double y) {
            // return NaN if either argument is NaN
            if (x <= y) return x;
            if (x > y) return y;
            return 0.0/0.0;
        }
        static const char* name() { return "min"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("min(",x,",",y,")");
        }
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(INFINITY, args[0], At_Arg(args));
    }
    GL_Value gl_call_expr(Operation& argx, const Call_Phrase*, GL_Frame& f)
    const override
    {
        return gl_minmax("min",argx,f);
    }
};

// Generalized dot product that includes vector dot product and matrix product.
// Same as Mathematica Dot[A,B]. Like APL A+.×B, Python numpy.dot(A,B)
struct Dot_Function : public Polyadic_Function
{
    Dot_Function() : Polyadic_Function(2) {}
    Value call(Frame& args) override
    {
        return dot(args[0], args[1], At_Frame(&args));
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto a = f[0];
        auto b = f[1];
        if (gl_type_count(a.type) < 2)
            throw Exception(At_GL_Arg(0, f), "dot: argument is not a vector");
        if (a.type != b.type)
            throw Exception(At_GL_Arg(1, f), "dot: arguments have different types");
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = dot("<<a<<","<<b<<");\n";
        return result;
    }
};

struct Mag_Function : public Polyadic_Function
{
    Mag_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        // TODO: use hypot() or BLAS DNRM2 or Eigen stableNorm/blueNorm?
        // Avoids overflow/underflow due to squaring of large/small values.
        // Slower.  https://forum.kde.org/viewtopic.php?f=74&t=62402
        auto& list = arg_to_list(args[0], At_Arg(args));
        double sum = 0.0;
        for (auto val : list) {
            double x = val.get_num_or_nan();
            sum += x * x;
        }
        if (sum == sum)
            return {sqrt(sum)};
        throw Exception(At_Arg(args), "mag: domain error");
    }
    GL_Value gl_call(GL_Frame& f) const override
    {
        auto arg = f[0];
        if (gl_type_count(arg.type) < 2)
            throw Exception(At_GL_Arg(0, f), "mag: argument is not a vector");
        auto result = f.gl.newvalue(GL_Type::Num);
        f.gl.out << "  float "<<result<<" = length("<<arg<<");\n";
        return result;
    }
};

struct Count_Function : public Polyadic_Function
{
    Count_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>())
            return {double(list->size())};
        if (auto string = args[0].dycast<const String>())
            return {double(string->size())};
        throw Exception(At_Arg(args), "not a list or string");
    }
};
struct Fields_Function : public Polyadic_Function
{
    Fields_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        if (auto structure = args[0].dycast<const Structure>())
            return {structure->fields()};
        throw Exception(At_Arg(args), "not a record");
    }
};

struct Strcat_Function : public Polyadic_Function
{
    Strcat_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>()) {
            String_Builder sb;
            for (auto val : *list) {
                if (auto str = val.dycast<const String>())
                    sb << str;
                else
                    sb << val;
            }
            return {sb.get_string()};
        }
        throw Exception(At_Arg(args), "not a list");
    }
};
struct Repr_Function : public Polyadic_Function
{
    Repr_Function() : Polyadic_Function(1) {}
    Value call(Frame& args) override
    {
        String_Builder sb;
        sb << args[0];
        return {sb.get_string()};
    }
};
struct Decode_Function : public Polyadic_Function
{
    Decode_Function() : Polyadic_Function(1) {}
    Value call(Frame& f) override
    {
        String_Builder sb;
        At_Arg cx(f);
        auto list = f[0].to<List>(cx);
        for (size_t i = 0; i < list->size(); ++i)
            sb << (char)arg_to_int((*list)[i], 1, 127, At_Index(i,cx));
        return {sb.get_string()};
    }
};
struct Encode_Function : public Polyadic_Function
{
    Encode_Function() : Polyadic_Function(1) {}
    Value call(Frame& f) override
    {
        List_Builder lb;
        At_Arg cx(f);
        auto str = f[0].to<String>(cx);
        for (size_t i = 0; i < str->size(); ++i)
            lb.push_back({(double)(int)str->at(i)});
        return {lb.get_list()};
    }
};

struct Match_Function : public Polyadic_Function
{
    Match_Function() : Polyadic_Function(1) {}
    Value call(Frame& f) override
    {
        At_Arg ctx0(f);
        auto list = f[0].to<List>(ctx0);
        std::vector<Shared<Function>> cases;
        for (size_t i = 0; i < list->size(); ++i)
            cases.push_back(list->at(i).to<Function>(At_Index(i,ctx0)));
        return {make<Piecewise_Function>(cases)};
    }
};

// The filename argument to "file", if it is a relative filename,
// is interpreted relative to the parent directory of the script file from
// which "file" is called.
//
// Because "file" has this hidden parameter (the name of the script file from
// which it is called), it is not a pure function. For this reason, it isn't
// a function value at all, it's a metafunction.
struct File_Expr : public Just_Expression
{
    Shared<Operation> arg_;
    File_Expr(Shared<const Call_Phrase> src, Shared<Operation> arg)
    :
        Just_Expression(std::move(src)),
        arg_(std::move(arg))
    {}
    virtual Value eval(Frame& f) const override
    {
        auto& callphrase = dynamic_cast<const Call_Phrase&>(*source_);
        At_Phrase cx(*callphrase.arg_, &f);
        Value arg = arg_->eval(f);
        auto argstr = arg.to<String>(cx);
        namespace fs = boost::filesystem;
        fs::path filepath;
        auto caller_script_name = source_->location().script().name_;
        if (caller_script_name->empty()) {
            filepath = fs::path(argstr->c_str());
        } else {
            filepath = fs::path(caller_script_name->c_str()).parent_path()
                / fs::path(argstr->c_str());
        }
        auto file = make<File_Script>(make_string(filepath.c_str()), cx);
        Program prog{*file, f.system_};
        std::unique_ptr<Frame> f2 =
            Frame::make(0, f.system_, &f, &callphrase, nullptr);
        prog.compile(nullptr, &*f2);
        return prog.eval();
    }
};
struct File_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<File_Expr>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `print`, such as `print "foo"`.
struct Print_Action : public Just_Action
{
    Shared<Operation> arg_;
    Print_Action(
        Shared<const Phrase> source,
        Shared<Operation> arg)
    :
        Just_Action(std::move(source)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        Value arg = arg_->eval(f);
        if (auto str = arg.dycast<String>())
            f.system_.console() << *str;
        else
            f.system_.console() << arg;
        f.system_.console() << std::endl;
    }
};
/// The meaning of the phrase `print` in isolation.
struct Print_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Print_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Warning_Action : public Just_Action
{
    Shared<Operation> arg_;
    Warning_Action(
        Shared<const Phrase> source,
        Shared<Operation> arg)
    :
        Just_Action(std::move(source)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        Value arg = arg_->eval(f);
        Shared<String> msg;
        if (auto str = arg.dycast<String>())
            msg = str;
        else
            msg = stringify(arg);
        Exception exc{At_Phrase(*source_, &f), msg};
        f.system_.console() << "WARNING: " << exc << std::endl;
    }
};
/// The meaning of the phrase `warning` in isolation.
struct Warning_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Warning_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `error`, such as `error("foo")`.
struct Error_Operation : public Operation
{
    Shared<Operation> arg_;
    Error_Operation(
        Shared<const Phrase> source,
        Shared<Operation> arg)
    :
        Operation(std::move(source)),
        arg_(std::move(arg))
    {}
    [[noreturn]] void run(Frame& f) const
    {
        Value val = arg_->eval(f);
        Shared<const String> msg;
        if (auto s = val.dycast<String>())
            msg = s;
        else
            msg = stringify(val);
        throw Exception(At_Frame(&f), msg);
    }
    virtual void exec(Frame& f) const override
    {
        run(f);
    }
    virtual Value eval(Frame& f) const override
    {
        run(f);
    }
    virtual void generate(Frame& f, List_Builder&) const override
    {
        run(f);
    }
    virtual void bind(Frame& f, Record&) const override
    {
        run(f);
    }
};
/// The meaning of the phrase `error` in isolation.
struct Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Error_Operation>(share(ph), analyse_op(*ph.arg_, env));
    }
};

// exec(expr) -- a debug action that evaluates expr, then discards the result.
// It is used to call functions or scripts for their side effects.
struct Exec_Action : public Just_Action
{
    Shared<Operation> arg_;
    Exec_Action(
        Shared<const Phrase> source,
        Shared<Operation> arg)
    :
        Just_Action(std::move(source)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        arg_->eval(f);
    }
};
struct Exec_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Exec_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Assert_Action : public Just_Action
{
    Shared<Operation> arg_;
    Assert_Action(
        Shared<const Phrase> source,
        Shared<Operation> arg)
    :
        Just_Action(std::move(source)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f) const override
    {
        Value a = arg_->eval(f);
        if (!a.is_bool())
            throw Exception(At_Phrase(*source_, &f), "domain error");
        bool b = a.get_bool_unsafe();
        if (!b)
            throw Exception(At_Phrase(*source_, &f), "assertion failed");
    }
};
struct Assert_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto args = ph.analyse_args(env);
        if (args.size() != 1)
            throw Exception(At_Phrase(ph, env), "assert: expecting 1 argument");
        return make<Assert_Action>(share(ph), args.front());
    }
};

struct Assert_Error_Action : public Just_Action
{
    Shared<Operation> expected_message_;
    Shared<const String> actual_message_;
    Shared<Operation> expr_;

    Assert_Error_Action(
        Shared<const Phrase> source,
        Shared<Operation> expected_message,
        Shared<const String> actual_message,
        Shared<Operation> expr)
    :
        Just_Action(std::move(source)),
        expected_message_(std::move(expected_message)),
        actual_message_(std::move(actual_message)),
        expr_(std::move(expr))
    {}

    virtual void exec(Frame& f) const override
    {
        Value expected_msg_val = expected_message_->eval(f);
        auto expected_msg_str = expected_msg_val.to<const String>(
            At_Phrase(*expected_message_->source_, &f));

        if (actual_message_ != nullptr) {
            if (*actual_message_ != *expected_msg_str)
                throw Exception(At_Phrase(*source_, &f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        actual_message_,
                        "\""));
            return;
        }

        Value result;
        try {
            result = expr_->eval(f);
        } catch (Exception& e) {
            if (*e.shared_what() != *expected_msg_str) {
                throw Exception(At_Phrase(*source_, &f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        e.shared_what(),
                        "\""));
            }
            return;
        }
        throw Exception(At_Phrase(*source_, &f),
            stringify("assertion failed: expected error \"",
                expected_msg_str,
                "\", got value ", result));
    }
};
struct Assert_Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto parens = cast<Paren_Phrase>(ph.arg_);
        Shared<Comma_Phrase> commas = nullptr;
        if (parens) commas = cast<Comma_Phrase>(parens->body_);
        if (parens && commas && commas->args_.size() == 2) {
            auto msg = analyse_op(*commas->args_[0].expr_, env);
            Shared<Operation> expr = nullptr;
            Shared<const String> actual_msg = nullptr;
            try {
                expr = analyse_op(*commas->args_[1].expr_, env);
            } catch (Exception& e) {
                actual_msg = e.shared_what();
            }
            return make<Assert_Error_Action>(share(ph), msg, actual_msg, expr);
        } else {
            throw Exception(At_Phrase(ph, env),
                "assert_error: expecting 2 arguments");
        }
    }
};

struct Defined_Expression : public Just_Expression
{
    Shared<const Operation> expr_;
    Atom_Expr selector_;

    Defined_Expression(
        Shared<const Phrase> source,
        Shared<const Operation> expr,
        Atom_Expr selector)
    :
        Just_Expression(std::move(source)),
        expr_(std::move(expr)),
        selector_(std::move(selector))
    {
    }

    virtual Value eval(Frame& f) const override
    {
        auto val = expr_->eval(f);
        auto s = val.dycast<Structure>();
        if (s) {
            auto id = selector_.eval(f);
            return {s->hasfield(id)};
        } else {
            return {false};
        }
    }
};
struct Defined_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        auto dot = cast<Dot_Expr>(arg);
        if (dot != nullptr)
            return make<Defined_Expression>(
                share(ph), dot->base_, dot->selector_);
        throw Exception(At_Phrase(*ph.arg_, env),
            "defined: argument must be `expression.identifier`");
    }
};

const Namespace&
builtin_namespace()
{
    static const Namespace names = {
    {"pi", make<Builtin_Value>(pi)},
    {"tau", make<Builtin_Value>(two_pi)},
    {"inf", make<Builtin_Value>(INFINITY)},
    {"null", make<Builtin_Value>(Value())},
    {"false", make<Builtin_Value>(Value(false))},
    {"true", make<Builtin_Value>(Value(true))},
    {"is_null", make<Builtin_Value>(Value{make<Is_Null_Function>()})},
    {"is_bool", make<Builtin_Value>(Value{make<Is_Bool_Function>()})},
    {"is_num", make<Builtin_Value>(Value{make<Is_Num_Function>()})},
    {"is_string", make<Builtin_Value>(Value{make<Is_String_Function>()})},
    {"is_list", make<Builtin_Value>(Value{make<Is_List_Function>()})},
    {"is_record", make<Builtin_Value>(Value{make<Is_Record_Function>()})},
    {"is_fun", make<Builtin_Value>(Value{make<Is_Fun_Function>()})},
    {"bit", make<Builtin_Value>(Value{make<Bit_Function>()})},
    {"sqrt", make<Builtin_Value>(Value{make<Sqrt_Function>()})},
    {"log", make<Builtin_Value>(Value{make<Log_Function>()})},
    {"abs", make<Builtin_Value>(Value{make<Abs_Function>()})},
    {"floor", make<Builtin_Value>(Value{make<Floor_Function>()})},
    {"round", make<Builtin_Value>(Value{make<Round_Function>()})},
    {"sin", make<Builtin_Value>(Value{make<Sin_Function>()})},
    {"asin", make<Builtin_Value>(Value{make<Asin_Function>()})},
    {"cos", make<Builtin_Value>(Value{make<Cos_Function>()})},
    {"acos", make<Builtin_Value>(Value{make<Acos_Function>()})},
    {"atan2", make<Builtin_Value>(Value{make<Atan2_Function>()})},
    {"max", make<Builtin_Value>(Value{make<Max_Function>()})},
    {"min", make<Builtin_Value>(Value{make<Min_Function>()})},
    {"dot", make<Builtin_Value>(Value{make<Dot_Function>()})},
    {"mag", make<Builtin_Value>(Value{make<Mag_Function>()})},
    {"count", make<Builtin_Value>(Value{make<Count_Function>()})},
    {"fields", make<Builtin_Value>(Value{make<Fields_Function>()})},
    {"strcat", make<Builtin_Value>(Value{make<Strcat_Function>()})},
    {"repr", make<Builtin_Value>(Value{make<Repr_Function>()})},
    {"decode", make<Builtin_Value>(Value{make<Decode_Function>()})},
    {"encode", make<Builtin_Value>(Value{make<Encode_Function>()})},
    {"match", make<Builtin_Value>(Value{make<Match_Function>()})},
    {"file", make<Builtin_Meaning<File_Metafunction>>()},
    {"print", make<Builtin_Meaning<Print_Metafunction>>()},
    {"warning", make<Builtin_Meaning<Warning_Metafunction>>()},
    {"error", make<Builtin_Meaning<Error_Metafunction>>()},
    {"assert", make<Builtin_Meaning<Assert_Metafunction>>()},
    {"assert_error", make<Builtin_Meaning<Assert_Error_Metafunction>>()},
    {"exec", make<Builtin_Meaning<Exec_Metafunction>>()},
    {"defined", make<Builtin_Meaning<Defined_Metafunction>>()},
    };
    return names;
}

} // namespace curv


//==================================================================================================
/*!
  @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_SCALAR_FUNCTION_EXPM1_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_EXPM1_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/expm1_kernel.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/logeps.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( expm1_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      using sA0 = bd::scalar_of_t<A0>;
    #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0)) return Nan<A0>();
    #endif
      if((a0 < Logeps<A0>())) return Mone<A0>();
      if((a0 > Maxlog<A0>())) return  Inf<A0>();
      return detail::expm1_kernel<A0, sA0>::expm1(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( expm1_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::expm1(a0);
    }
  };
} } }

#endif


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include "Constraint.hpp"
#include "Scheduler.hpp"

#include <popsolver/Model.hpp>
#define BOOST_TEST_MODULE GenericAssignment
#include <boost/test/unit_test.hpp>

using namespace popsolver;

const Variable a(0);
const Variable b(1);

BOOST_AUTO_TEST_CASE(GenericAssignmentSimple) {
  GenericAssignment<DataType> assign(
      a, {},
      [](const std::vector<DataType> &values) -> boost::optional<DataType> {
        return DataType{1};
      });

  Domains domains;
  domains.emplace_back(DataType{0}, DataType{10}); // a

  Scheduler scheduler(domains, {&assign});

  BOOST_CHECK(assign.propagate(scheduler));

  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{1});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{1});
}

BOOST_AUTO_TEST_CASE(GenericAssignmentTakingValues) {
  GenericAssignment<DataType> assign(
      a, {b}, [](const std::vector<DataType> &values) -> DataType {
        return values[0] * DataType{2};
      });

  Domains domains;
  domains.emplace_back(DataType{0}, DataType{10}); // a
  domains.emplace_back(DataType{2}, DataType{2});  // b

  Scheduler scheduler(domains, {&assign});

  BOOST_CHECK(assign.propagate(scheduler));

  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{4});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{4});
}

BOOST_AUTO_TEST_CASE(GenericAssignmentOutOfRange) {
  GenericAssignment<DataType> assign(
      a, {}, [](const std::vector<DataType> &values) -> DataType {
        return DataType{1};
      });

  Domains domains;
  domains.emplace_back(DataType{5}, DataType{5});

  Scheduler scheduler(domains, {&assign});

  BOOST_CHECK(!assign.propagate(scheduler));
}

BOOST_AUTO_TEST_CASE(GenericAssignmentReturnsInvalid) {
  GenericAssignment<DataType> assign(
      a, {},
      [](const std::vector<DataType> &values) -> boost::optional<DataType> {
        return boost::none;
      });

  Domains domains;
  domains.emplace_back(DataType{0}, DataType{10});

  Scheduler scheduler(domains, {&assign});

  BOOST_CHECK(!assign.propagate(scheduler));
}

BOOST_AUTO_TEST_CASE(GenericAssignmentConstrainsDomainToRange) {
  // Given
  const auto min = DataType{std::numeric_limits<unsigned>::max()};
  const auto max = DataType{std::numeric_limits<unsigned>::max()} + DataType{5};

  unsigned callCount = 0;
  GenericAssignment<unsigned> assign(
      a, {b}, [&](const auto &values) -> boost::optional<DataType> {
        // Then expect the bounds constrained
        BOOST_CHECK_EQUAL(values[0], std::numeric_limits<unsigned>::max());
        callCount++;
        return DataType{values[0]};
      });
  Domains domains;
  domains.emplace_back(min, max); // a
  domains.emplace_back(min, max); // b
  Scheduler scheduler(domains, {&assign});

  // When
  BOOST_CHECK(assign.propagate(scheduler));
  // Then only called once with only value in range unsigned
  BOOST_CHECK_EQUAL(callCount, 1);
}


#include <Eigen/Dense>
#include <iostream>

using namespace std;

int main(int argc, char** argv) {
  Eigen::Vector2f v{1, 2};
  cout << "Point: {" << v << "}" << endl;
  return 0;
}


//---------------------------------//
//  This file is part of MuJoCo    //
//  Written by Emo Todorov         //
//  Copyright (C) 2017 Roboti LLC  //
//---------------------------------//

#include "mujoco.h"
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <chrono>
#include <math.h>
#include <time.h>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;
//-------------------------------- macro variables --------------------------------------

// model selection: PENDULUM CARTPOLE CART2POLE CART3POLE SWIMMER3 SWIMMER6 FISH
#define FISH
#define CTRL_LIMITTED false
#define STATE_LINEAR false
#define LOCAL true
#define PI 3.141592653

//-------------------------------- global variables -------------------------------------

// user customized parameters
#if defined(SWIMMER6)
const mjtNum t_step = 0.006;
const mjtNum cal_step = 0.006;
const mjtNum ptb_coef = 0.005;
const int step_max = 1500;
const int32_t roll_max = 1400;
const char* mname = "swimmer6.xml";
const int ctrl_num = 5;
const int NS = 16;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(SWIMMER3)
const mjtNum t_step = 0.005;//0.01
const mjtNum cal_step = 0.005;
const mjtNum ptb_coef = 0.008;
const int step_max = 1600;//800
const int32_t roll_max = 1200;
const char* mname = "swimmer3.xml";
const int ctrl_num = 2;
const int NS = 10;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(FISH)
const mjtNum t_step = 0.001;
const mjtNum cal_step = 0.001;
const mjtNum ptb_coef = 0.005;
const int step_max = 7000;//16000
const int roll_max = 1600;
const char* mname = "fish.xml";
const int ctrl_num = 5;
const int NS = 26;
mjtNum state_nominal[step_max + 1][NS] = { 0.0, 0.0, 0, 0, 0, 0, 1, 0, 0 };
#elif defined(CART2POLE)
const mjtNum t_step = 0.05;
const int32_t roll_max = 20000; //2w
const char* mname = "cart2pole.xml";
const int step_max = 120;
const int ctrl_num = 1;
const int NS = 6;
mjtNum ptb_coef = 0.04;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(CART7POLE)
const mjtNum t_step = 0.1;
const mjtNum ptb_coef = 1;
const int step_max = 100;
const int32_t roll_max = 20000;
const char* mname = "cart7pole.xml";
const int ctrl_num = 1;
const int NS = 16;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(CART3POLE)
const mjtNum t_step = 0.05;
const mjtNum ptb_coef = 0;
const int step_max = 140;
const int32_t roll_max = 20000;
const char* mname = "cart3pole.xml";
const int ctrl_num = 1;
const int NS = 8;
mjtNum state_nominal[step_max + 1][NS] = { 0 };
#elif defined(ACROBOT)
const mjtNum t_step = 0.01;
const mjtNum ptb_coef = 0.02;
const int step_max = 700;
const int32_t roll_max = 20000;
const char* mname = "acrobot.xml";
const int ctrl_num = 1;
const int NS = 4;
mjtNum state_nominal[step_max + 1][NS] = { -PI, 0, 0, 0 };
#elif defined(PENDULUM)
const mjtNum t_step = 0.1;
const mjtNum cal_step = 0.1;
mjtNum ptb_coef = 0.005;
const int step_max = 30;
const int32_t roll_max = 500;
const char* mname = "pendulum.xml";
const int ctrl_num = 1;
const int NS = 2;
mjtNum state_nominal[step_max + 1][NS] = { PI, 0.0 };
#elif defined(CARTPOLE)
const mjtNum t_step = 0.1;
const mjtNum cal_step = 0.1;
mjtNum ptb_coef = 0.003;
const int step_max = 30;
const int32_t roll_max = 600;
const char* mname = "cartpole.xml";
const int ctrl_num = 1;
const int NS = 4;
mjtNum state_nominal[step_max + 1][NS] = { 0, 0, 0, 0.0 };
#endif

// model
mjModel* m = 0;
mjData* d = 0;
char lastfile[1000] = "";
char error[1000];
static int tri_num = 0;
static int32_t trial = 0;
static int index = 0;
static bool cal_flag = true;
const long N = NS + ctrl_num;
const int n_check = 1;
mjtNum sysiderr = 0;
mjtNum delta_x1[12 + ctrl_num] = { 0 };
mjtNum delta_x2[12] = { 0 };
mjtNum mat[step_max][12][12 + ctrl_num] = { 0 };
//mjtNum sum[step_max][NS][NS + ctrl_num] = { 0 };
//mjtNum dx[step_max + 1][NS];
//mjtNum delta_xc[step_max + 1][NS + ctrl_num] = { 0 };
//mjtNum delta_xcs[step_max][NS] = { 0 };
mjtNum res[NS + ctrl_num][NS + ctrl_num], res6[NS + ctrl_num][NS + ctrl_num], res7[NS][NS + ctrl_num];
mjtNum ctrl_nominal[step_max * ctrl_num] = { 0 };
mjtNum ulim = 0;
int stepite = (int)(t_step / cal_step);
char ctrl_buff[20];
char para_buff[20];
char glstr[10];
char *str3, *str;
FILE *fp, *fp1, *fop, *fop1, *fop2;
char mfilename[30];
char kfilename[30];
char dfilename[30];
#if LOCAL == false
char mfilepre[17] = "../../../model/";
char kfilepre[17] = "../../../doc/";
char dfilepre[17] = "../../../data/";
#else
char mfilepre[15] = "";
char kfilepre[15] = "";
char dfilepre[15] = "";
#endif

// timer
double gettm(void)
{
    static chrono::system_clock::time_point _start = chrono::system_clock::now();
    chrono::duration<double> elapsed = chrono::system_clock::now() - _start;
    return elapsed.count();
}

// deallocate and print message
int finish(const char* msg = 0, mjModel* m = 0, mjData* d = 0)
{
    // deallocated everything
    if( d )
        mj_deleteData(d);
    if( m )
        mj_deleteModel(m);
    mj_deactivate();

    // print message
    if( msg )
        printf("%s\n", msg);

    return 0;
}

/* Calculate Determinant of the Matrix */
mjtNum determinant(mjtNum fh[N][N], mjtNum r)
{
	int z, j, i;
	mjtNum m = 1, k, det = 1.0;
	int rr = (int)r;
	mjtNum a[N][N];

	for (i = 0; i < rr; i++)
	{
		for (j = 0; j < r; j++)
		{
			a[i][j] = fh[i][j];
		}
	}

	for (z = 0; z<rr - 1; z++)
		for (i = z; i<rr - 1; i++)
		{
			if (a[z][z] == 0)
			{

				for (j = 0; j<rr; j++)
				{
					a[z][j] = a[z][j] + a[i + 1][j];
				}
			}
			if (a[z][z] != 0) {
				k = -a[i + 1][z] / a[z][z];
				for (j = z; j<rr; j++)a[i + 1][j] = k * a[z][j] + a[i + 1][j];
			}
		}

	for (z = 0; z<rr; z++)
	{
		det = det * (a[z][z]);
	}
	return (det);
}

/* Find transpose of matrix */
void transpose(mjtNum num[N][N], mjtNum fac[N][N], mjtNum r)
{
	int i, j, rr = (int)r;
	mjtNum b[N][N], d;

	for (i = 0; i < rr; i++)
	{
		for (j = 0; j < r; j++)
		{
			b[i][j] = fac[j][i];
		}
	}
	d = determinant(num, r);
	for (i = 0; i < rr; i++)
	{
		for (j = 0; j < r; j++)
		{
			res[i][j] = b[i][j] / d;
		}
	}
}

/* Inverse */
void cofactor(mjtNum num[N][N], mjtNum f)
{
	mjtNum b[N][N] = { 0 }, fac[N][N];
	int p, q, m, n, i, j;
	int ff = (int)f;

	for (q = 0; q < ff; q++)
	{
		for (p = 0; p < ff; p++)
		{
			m = 0;
			n = 0;
			for (i = 0; i < ff; i++)
			{
				for (j = 0; j < ff; j++)
				{
					if (i != q && j != p)
					{
						b[m][n] = num[i][j];
						if (n < (f - 2)) n++;
						else {
							n = 0;
							m++;
						}
					}
				}
			}
			fac[q][p] = pow(mjtNum(-1), q + p) * determinant(b, ff - 1);
		}
	}
	transpose(num, fac, ff);
}

// w x y z to roll pitch yaw
void quat2smpl(double *res, double *quat)
{
	double b0, b1, b2, b3;

	b0 = *quat;
	b1 = *(quat + 1);
	b2 = *(quat + 2);
	b3 = *(quat + 3);

	Quaterniond q = Eigen::Quaterniond(b0, b1, b2, b3); // w x y z
	auto r = q.toRotationMatrix().eulerAngles(0, 1, 2);

	*res = r[0];
	*(res + 1) = r[1];
	*(res + 2) = r[2];
}

// roll pitch yaw to w x y z
void smpl2quat(double *res, double *smpl)
{
	double rx, ry, rz; // roll pitch yaw

	rx = *smpl;
	ry = *(smpl + 1);
	rz = *(smpl + 2);

	if (rx > PI) rx = PI - rx;

	Quaterniond q = AngleAxisd(rx, Vector3d::UnitX())
		* AngleAxisd(ry, Vector3d::UnitY())
		* AngleAxisd(rz, Vector3d::UnitZ());

	Eigen::Vector4d b = q.coeffs(); // x y z w

	*res = b[3];
	*(res + 1) = b[0];
	*(res + 2) = b[1];
	*(res + 3) = b[2];
}

double gaussrand()
{
	static double U, V;
	static int phase = 0;
	double Z;

	if (phase == 0)
	{
		U = rand() / (RAND_MAX + 1.0);
		V = rand() / (RAND_MAX + 1.0);
		if (U == 0) U = 0.00001;
		Z = sqrt(-2.0 * log(U)) * sin(2.0 * PI * V);
	}
	else
	{
		Z = sqrt(-2.0 * log(U)) * cos(2.0 * PI * V);
	}
	phase = 1 - phase;
	return Z;
}

void get_nominal(mjModel* m, mjData* d)
{
	static int index = 0, i, y;
	mjtNum temp[4];
#if STATE_LINEAR == true
	static int step_max = 1;
#endif

	mju_zero(d->qpos, m->nq);
	mju_zero(d->qvel, m->nv);
	for (y = 0; y < NS/2; y++)
	{
		d->qpos[y] = state_nominal[index][2 * y];
	}
	for (y = 0; y < m->nv; y++)
	{
		d->qvel[y] = state_nominal[index][2 * y + 1];
	}
	for (y = 0; y < ctrl_num; y++)
	{
		d->ctrl[y] = ctrl_nominal[index * ctrl_num + y];
	}

	quat2smpl(temp, &(d->qpos[3]));
	for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = temp[k];

	//for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = d->qpos[k + 4];

	while (index < step_max)
	{
		for (i = 0; i < stepite; i++) mj_step(m, d);

		index++;
		for (y = 0; y < 3; y++)
		{
			state_nominal[index][2 * y] = d->qpos[y];
		}

		quat2smpl(temp, &(d->qpos[3]));
		for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = temp[k];

		//for (int k = 0; k < 3; k++)state_nominal[index][2 * k + 6] = d->qpos[k+4];

		for (y = 7; y < m->nq; y++)
		{
			state_nominal[index][2 * (y-1)] = d->qpos[y];
		}
		for (y = 0; y < m->nv; y++)
		{
			state_nominal[index][2 * y + 1] = d->qvel[y];
		}
		for (y = 0; y < ctrl_num; y++)
		{
			d->ctrl[y] = ctrl_nominal[index * ctrl_num + y];
		}
	}
}

void sysidcheck(mjModel* m, mjData* d)
{
	static int index = 0;
	static mjtNum t_init;
	const int step_max = 1500;
	int i, j, k;
	mjtNum dx[n_check][step_max][12];
	mjtNum delta_xc[n_check][step_max][12 + ctrl_num] = { 0 };
	mjtNum delta_xcs[n_check][step_max][12] = { 0 };
	mjtNum temp[4];

	for (i = 0; i < 17; i++)
	{
		for (j = 0; j < n_check; j++)
		{
			for (k = 0; k < step_max; k++) delta_xc[j][k][i] = 0.005 * ulim * gaussrand();
		}
	}
	for (i = 0; i < n_check; i++)
	{
		for (j = 0; j < step_max; j++) mju_mulMatVec(dx[i][j], *mat[j], delta_xc[i][j], 12, 12 + ctrl_num);
	}

	for (i = 0; i < n_check; i++)
	{
		index = 0;
		t_init = d->time;
		for (j = 0; j < 3; j++)
		{
			d->qpos[j] = delta_xc[i][index][2 * j] + state_nominal[index][2 * j];
		}

		//for (k = 0; k < 3; k++) d->qpos[k+4] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
		//d->qpos[3] = sqrt(1 - d->qpos[4] * d->qpos[4] - d->qpos[5] * d->qpos[5] - d->qpos[6] * d->qpos[6]);

		for (k = 0; k < 3; k++) temp[k] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
		smpl2quat(&(d->qpos[3]), temp);

		for (j = 7; j < m->nq; j++)
		{
			d->qpos[j] = state_nominal[index][2 * (j-1)];
		}
		//d->qpos[9] = delta_xc[i][index][12] + state_nominal[index][16];
		for (j = 0; j < 6; j++)
		{
			d->qvel[j] = delta_xc[i][index][2 * j + 1] + state_nominal[index][2 * j + 1];
		}
		for (j = 6; j < m->nv; j++)
		{
			d->qvel[j] = state_nominal[index][2 * j + 1];
		}
		//d->qvel[8] = delta_xc[i][index][13] + state_nominal[index][17];
		for (k = 0; k < ctrl_num; k++)
		{
			d->ctrl[k] = delta_xc[i][index][k + 12] + ctrl_nominal[index * ctrl_num + k];
		}
		while (index < step_max - 1)
		{
			if (d->time - t_init < t_step - 0.00001)
			{
				mj_step(m, d);
			}
			else {
				for (int y = 0; y < 3; y++)
				{
					delta_xcs[i][index][2 * y] = d->qpos[y] - state_nominal[index + 1][2 * y];
				}

				quat2smpl(temp, &(d->qpos[3]));
				for (int k = 0; k < 3; k++) delta_xcs[i][index][2 * k + 6] = temp[k] - state_nominal[index][2 * k + 6];

				//for (int k = 0; k < 3; k++) delta_xcs[i][index][2 * k + 6] = d->qpos[k+4] - state_nominal[index][2 * k + 6];

				//delta_xcs[i][index][12] = d->qpos[9] - state_nominal[index + 1][16];
				for (int y = 0; y < 6; y++)
				{
					delta_xcs[i][index][2 * y + 1] = d->qvel[y] - state_nominal[index + 1][2 * y + 1];
				}
				//delta_xcs[i][index][13] = d->qvel[8] - state_nominal[index + 1][17];

				index++;
				t_init = d->time;

				for (int y = 0; y < 3; y++)
				{
					d->qpos[y] = delta_xc[i][index][2 * y] + state_nominal[index][2 * y];
				}

				//for (int k = 0; k < 3; k++) d->qpos[k+4] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
				//d->qpos[3] = sqrt(1 - d->qpos[4] * d->qpos[4] - d->qpos[5] * d->qpos[5] - d->qpos[6] * d->qpos[6]);

				for (int k = 0; k < 3; k++) temp[k] = delta_xc[i][index][2 * k + 6] + state_nominal[index][2 * k + 6];
				smpl2quat(&(d->qpos[3]), temp);

				for (j = 7; j < m->nq; j++)
				{
					d->qpos[j] = state_nominal[index][2 * (j - 1)];
				}
				//d->qpos[9] = delta_xc[i][index][12] + state_nominal[index][16];
				for (int y = 0; y < 6; y++)
				{
					d->qvel[y] = delta_xc[i][index][2 * y + 1] + state_nominal[index][2 * y + 1];
				}
				for (int y = 6; y < m->nv; y++)
				{
					d->qvel[y] = state_nominal[index][2 * y + 1];
				}
				//d->qvel[8] = delta_xc[i][index][13] + state_nominal[index][17];
				for (int ci = 0; ci < ctrl_num; ci++)
				{
					d->ctrl[ci] = delta_xc[i][index][ci + 12] + ctrl_nominal[index * ctrl_num + ci];
				}
			}
		}
		for (int t = 0; t < 12; t++)
		{
			for (int h = 0; h < step_max; h++)
			{
				if (delta_xcs[i][h][t] != 0) sysiderr += fabs((dx[i][h][t] - delta_xcs[i][h][t]) / delta_xcs[i][h][t]);
			}
		}
	}
	sysiderr = sysiderr / (1.0*n_check*step_max*12);
}

void check(mjModel* m, mjData* d)
{
//	static int index = 0, h, u, y;
//#if STATE_LINEAR == true
//	static int step_max = 1;
//#endif
//
//	for (u = 0; u < NS + ctrl_num; u++)
//	{
//		for (h = 0; h < step_max; h++) delta_xc[h][u] = ptb_coef * ulim * gaussrand();
//	}
//	for (h = 0; h < step_max; h++)
//	{
//		mju_mulMatVec(dx[h + 1], *mat[h], delta_xc[h], NS, NS + ctrl_num);
//	}
//	while (index < step_max)
//	{
//		for (y = 0; y < m->nq; y++)
//		{
//			d->qpos[y] = delta_xc[index][2 * y] + state_nominal[index][2 * y];
//		}
//		for (y = 0; y < m->nv; y++)
//		{
//			d->qvel[y] = delta_xc[index][2 * y + 1] + state_nominal[index][2 * y + 1];
//		}
//		for (h = 0; h < ctrl_num; h++)
//		{
//			d->ctrl[h] = ctrl_nominal[index * ctrl_num + h] + delta_xc[index][h + NS];
//		}
//
//		for (h = 0; h < stepite; h++) mj_step(m, d);
//
//		for (y = 0; y < m->nq; y++)
//		{
//			delta_xcs[index][2 * y] = d->qpos[y] - state_nominal[index + 1][2 * y];
//		}
//		for (y = 0; y < m->nv; y++)
//		{
//			delta_xcs[index][2 * y + 1] = d->qvel[y] - state_nominal[index + 1][2 * y + 1];
//		}
//		index++;
//	}
}

void setcontrol(mjModel* m, mjData* d)
{
	mjtNum temp[4];
	static int index = 0, i, y;
#if STATE_LINEAR == true
	static int step_max = 1;
#endif

	while (index < step_max)
	{
		for (y = 0; y < 3; y++)
		{
			delta_x1[2 * y] = ptb_coef * ulim * gaussrand();
			d->qpos[y] = delta_x1[2 * y] + state_nominal[index][2 * y];
		}
		for (int k = 0; k < 3; k++) {
			delta_x1[2 * k + 6] = ptb_coef * ulim * gaussrand();

			//d->qpos[k+4] = delta_x1[2 * k + 6] + state_nominal[index][2 * k + 6];

			temp[k] = delta_x1[2 * k + 6] + state_nominal[index][2 * k + 6];
		}
		//d->qpos[3] = sqrt(1 - d->qpos[4] * d->qpos[4] - d->qpos[5] * d->qpos[5] - d->qpos[6] * d->qpos[6]);

		smpl2quat(&(d->qpos[3]), temp);
		for (int j = 7; j < m->nq; j++)
		{
			d->qpos[j] = state_nominal[index][2 * (j - 1)];
		}
		//delta_x1[12] = ptb_coef * ulim * gaussrand();
		//d->qpos[9] = delta_x1[12] + state_nominal[index][16];
		for (y = 0; y < 6; y++)
		{
			delta_x1[2 * y + 1] = ptb_coef * ulim * gaussrand();
			d->qvel[y] = delta_x1[2 * y + 1]+state_nominal[index][2 * y + 1];
		}
		for (y = 6; y < m->nv; y++)
		{
			d->qvel[y] = state_nominal[index][2 * y + 1];
		}
		//delta_x1[13] = ptb_coef * ulim * gaussrand();
		//d->qvel[8] = delta_x1[13] + state_nominal[index][17];
		for (i = 0; i < ctrl_num; i++)
		{
			delta_x1[i + 12] = ptb_coef * ulim * gaussrand();
			d->ctrl[i] = ctrl_nominal[index * ctrl_num + i] + delta_x1[i + 12];
		}

		for (i = 0; i < stepite; i++) mj_step(m, d);

		for (int y = 0; y < 3; y++)
		{
			delta_x2[2 * y] = d->qpos[y] - state_nominal[index + 1][2 * y];
		}

		quat2smpl(temp, &(d->qpos[3]));
		for (int k = 0; k < 3; k++) delta_x2[2 * k + 6] = temp[k] - state_nominal[index][2 * k + 6];

		//for (int k = 0; k < 3; k++) delta_x2[2 * k + 6] = d->qpos[k+4] - state_nominal[index][2 * k + 6];

		//delta_x2[12] = d->qpos[9] - state_nominal[index + 1][16];
		for (int y = 0; y < 6; y++)
		{
			delta_x2[2 * y + 1] = d->qvel[y] - state_nominal[index + 1][2 * y + 1];
		}
		//delta_x2[13] = d->qvel[8] - state_nominal[index + 1][17];
		// iteration method
		for (y = 0; y < 12; y++)
		{
			for (i = 0; i < 17; i++)
			{
				mat[index][y][i] = mat[index][y][i] * (1 - 1 / (trial + 1.0)) + delta_x2[y] * delta_x1[i] / ((trial + 1.0) * ptb_coef * ptb_coef * ulim * ulim);
			}
		}

		if (index == 0)
		{
			str3 = glstr;
			sprintf(str3, "%3.3f", mat[index][0][1]);
			fwrite(str3, 5, 1, fop);
			fputs(" ", fop);
		}
		index++;
	}
	index = 0;
	trial++;
}

// main function
int main(int argc, const char** argv)
{
    // activate MuJoCo Pro license (this must be *your* activation key)
	strcpy(kfilename, kfilepre);
	strcat(kfilename, "mjkeysmall.txt");
    mj_activate(kfilename);

    // get filename, determine file type
    std::string filename(mname);
    bool binary = (filename.find(".mjb")!=std::string::npos);
	strcpy(mfilename, mfilepre);
	strcat(mfilename, mname);

    // load model
    char error[1000] = "Could not load binary model";
    if( binary )
        m = mj_loadModel(mfilename, 0);
    else
        m = mj_loadXML(mfilename, 0, error, 1000);
    if( !m )
        return finish(error);

    // make data
    d = mj_makeData(m);
    if( !d )
        return finish("Could not allocate mjData", m);

	if (STATE_LINEAR == true)
	{
		#if defined(CART2POLE)||defined(CART3POLE)||defined(CART7POLE)
				state_nominal[0][2] = PI;
		#elif defined(PENDULUM)
				state_nominal[0][0] = 0;
		#elif defined(CARTPOLE)
				state_nominal[0][2] = -PI;
		#elif defined(ACROBOT)
				state_nominal[0][0] = 0;
				state_nominal[0][2] = -2 * PI;
		#endif
	}

	srand((unsigned)time(NULL));
	strcpy(dfilename, dfilepre);
	strcat(dfilename, "converge.txt");
	if ((fop = fopen(dfilename, "wt+")) == NULL) {
		return 0;
	}
	strcpy(dfilename, dfilepre);
	strcat(dfilename, "result.txt");
	if ((fp = fopen(dfilename, "r")) != NULL)
	{
		fscanf(fp, "%s", ctrl_buff);
		if (ctrl_buff[0] == 'C') {
			for (int i = 0; i < step_max * ctrl_num; i++)
			{
				fscanf(fp, "%s", ctrl_buff);
				ctrl_nominal[i] = atof(ctrl_buff);
				if (fabs(ctrl_nominal[i]) > ulim) ulim = fabs(ctrl_nominal[i]);

				#if CTRL_LIMITTED == true
					if (ctrl_nominal[i] > m->actuator_ctrlrange[1]) ctrl_nominal[i] = m->actuator_ctrlrange[1];
					else if (ctrl_nominal[i] < m->actuator_ctrlrange[0]) ctrl_nominal[i] = m->actuator_ctrlrange[0];
				#endif
				#if STATE_LINEAR == true
					ctrl_nominal[i] = 0;
				#endif
			}
			fclose(fp);
		}
	}

    // time simulation
    int steps = 0, contacts = 0, constraints = 0;
    double printfraction = 0.1;
    printf("\nSimulation ");
    double start = gettm();
	get_nominal(m, d);
	while (trial < roll_max)
    {
		setcontrol(m, d);

        // accumulate statistics
        steps = steps + stepite;
        contacts += d->ncon;
        constraints += d->nefc;

        // print '.' every 10% of duration
        if( trial >= roll_max*printfraction )
        {
            printf(".");
            printfraction += 0.1;
        }
    }
    double end = gettm();

    // print results
    printf("\n Simulation time      : %.2f s\n", end-start);
    printf(" Time per step        : %.3f ms\n", 1000.0*(end-start)/mjMAX(1,steps));
    printf(" Contacts per step    : %d\n", contacts/mjMAX(1,steps));
    printf(" Constraints per step : %d\n", constraints/mjMAX(1,steps));
    printf(" Degrees of freedom   : %d\n\n", m->nv);

	sysidcheck(m, d);

	// print result
	#if STATE_LINEAR == true
		strcpy(dfilename, dfilepre);
		strcat(dfilename, "linearization_top.txt");
		if ((fop2 = fopen(dfilename, "wt+")) != NULL)
		{
			str3 = glstr;
			for (int t = 0; t < 1; t++)
			{
				for (int h = 0; h < NS; h++)
				{
					for (int d = 0; d < NS + ctrl_num; d++)
					{
						sprintf(str3, "%4.8f", mat[t][h][d]);
						fwrite(str3, 10, 1, fop2);
						fputs(" ", fop2);
					}
					fputs("\n", fop2);
				}
				fputs("\n", fop2);
			}

			// for result checking
			for (int t = 0; t < NS; t++)
			{
				for (int h = 1; h <= 1; h++)
				{
					sprintf(str3, "%2.4f", dx[h][t]);
					fwrite(str3, 6, 1, fop2);
					fputs(" ", fop2);
				}
				fputs("\n", fop2);
				for (int d = 0; d < 1; d++)
				{
					sprintf(str3, "%2.4f", delta_xcs[d][t]);
					fwrite(str3, 6, 1, fop2);
					fputs(" ", fop2);
				}
				fputs("\n", fop2);
				fputs("\n", fop2);
			}
			fputs("\nptb_coef: ", fop2);
			sprintf(str3, "%2.4f\n", ptb_coef);
			fwrite(str3, 6, 1, fop2);
			fclose(fop2);
		}
	#else
		strcpy(dfilename, dfilepre);
		strcat(dfilename, "lnr.txt");
		if ((fop2 = fopen(dfilename, "wt+")) != NULL)
		{
			str3 = glstr;
			for (int t = 0; t < step_max; t++)
			{
				for (int h = 0; h < 12; h++)
				{
					for (int d = 0; d < 12 + ctrl_num; d++)
					{
						sprintf(str3, "%4.8f", mat[t][h][d]);
						fwrite(str3, 10, 1, fop2);
						fputs(" ", fop2);
					}
					fputs("\n", fop2);
				}
				fputs("\n", fop2);
			}

			//// for result checking
			//for (int t = 0; t < NS; t++)
			//{
			//	for (int h = 1; h <= step_max; h++)
			//	{
			//		sprintf(str3, "%2.4f", dx[h][t]);
			//		fwrite(str3, 6, 1, fop2);
			//		fputs(" ", fop2);
			//	}
			//	fputs("\n", fop2);
			//	for (int d = 0; d < step_max; d++)
			//	{
			//		sprintf(str3, "%2.4f", delta_xcs[d][t]);
			//		fwrite(str3, 6, 1, fop2);
			//		fputs(" ", fop2);
			//	}
			//	fputs("\n", fop2);
			//	fputs("\n", fop2);
			//}
			fputs("sysiderr: ", fop2);
			sprintf(str3, "%2.4f\n", sysiderr);
			fwrite(str3, 6, 1, fop2);
			fputs("\nptb_coef: ", fop2);
			sprintf(str3, "%2.4f\n", ptb_coef);
			fwrite(str3, 6, 1, fop2);

			fclose(fop2);
		}

		//// state output
		//strcpy(dfilename, dfilepre);
		//strcat(dfilename, "states.txt");
		//if ((fp1 = fopen(dfilename, "at+")) != NULL)
		//{
		//	str3 = glstr;
		//	for (int h = 0; h < NS; h++)
		//	{
		//		for (int d = 1; d < step_max; d++)
		//		{
		//			sprintf(str3, "%4.8f", state_nominal[d][h]);
		//			fwrite(str3, 10, 1, fp1);
		//			fputs(" ", fp1);
		//		}
		//		fputs("\n", fp1);
		//	}
		//	fclose(fp1);
		//}
	#endif
	// hold cmd
	system("pause");
    // finalize
    return finish(0, m, d);
}

backup/some old code and slides/sample/sysidpartial.cpp