#include <sstream>
#include <fstream>
#include <iostream>
#include <random>
#include <memory>
#include <array>

#include <Eigen/Core>
#include <gflags/gflags.h>
#include <ceres/ceres.h>

template<typename T>
class Ellipse
{
public:
    Ellipse() : Ellipse(0, 0, 1, 1)
    {
    }

    explicit Ellipse(T h, T k, T a, T b)
    {
        m_data[0] = h;
        m_data[1] = k;
        m_data[2] = a;
        m_data[3] = b;
    }

    explicit Ellipse(const T* data)
    {
        std::copy(data, data + 4, m_data.begin());
    }

    explicit Ellipse(const Ellipse& other) : Ellipse(other.params())
    {
    }

    Ellipse& operator= (const Ellipse &other)
    {
        std::copy(other.m_data.begin(), other.m_data.end(), m_data.begin());
        return *this;
    }

    const T* params() const
    {
        return m_data.data();
    }

    T* params()
    {
        return m_data.data();
    }

    T h() const
    {
        return m_data[0];
    }

    T k() const
    {
        return m_data[1];
    }

    T a() const
    {
        return m_data[2];
    }

    T b() const
    {
        return m_data[3];
    }

    void set_bounds(ceres::Problem& problem)
    {
        problem.SetParameterLowerBound(params(), 2, 1e-5);
        problem.SetParameterLowerBound(params(), 3, 1e-5);
    }

    std::string to_string()
    {
        std::stringstream buff;
        buff << "(h=" << h() << ", k=" << k() << ", a=" << a() << ", b=" << b() << ")";
        return buff.str();
    }

private:
    std::array<T, 4> m_data;
};

/** Sample functor which simply computes the residual.
 *  Used for numeric differentiation.
 */
struct NumericEllipseCostFunctor
{
    NumericEllipseCostFunctor(const Eigen::Vector2d &observed_point) : observed_point(observed_point) {}
    bool operator()(const double *const parameters, double *residuals) const
    {
        Ellipse<double> ellipse(parameters);

        // compute the cost
        const double dx = observed_point.x() - ellipse.h();
        const double dy = observed_point.y() - ellipse.k();
        const double a2 = ellipse.a() * ellipse.a();
        const double b2 = ellipse.b() * ellipse.b();
        residuals[0] = (dx * dx) / a2 + (dy * dy) / b2 - 1;
        return true;
    }

    Eigen::Vector2d observed_point;
};

/** Slightly more advanced functor which is templated, allowing
 *  Ceres to automatically compute the Jacobian using
 *  templates.
 */
struct AutoEllipseCostFunctor
{
    AutoEllipseCostFunctor(const Eigen::Vector2d &observed_point) : observed_point(observed_point) {}

    /** Ceres will create a version of this with a special
     *  autodiff type for determining the Jacobian
     *  and another with doubles for residual computation
     */
    template <typename T>
    bool operator()(const T *const parameters, T *residuals) const
    {
        Ellipse<T> ellipse(parameters);

        T dx = T(observed_point.x()) - ellipse.h();
        T dy = T(observed_point.y()) - ellipse.k();
        T a2 = ellipse.a() * ellipse.a();
        T b2 = ellipse.b() * ellipse.b();
        residuals[0] = (dx * dx) / a2 + (dy * dy) / b2 - T(1.0);
        return true;
    }

    Eigen::Vector2d observed_point;
};

/** If the analytic gradient is simple to compute or if perfomance is a concern,
 *  it can be best to compute the Jacobians by hand, as shown here. The template
 *  arguments indicate to Ceres the number of residuals, and the number of
 *  parameters. This can also be determined dynamically using the base
 *  class `CostFunction`.
 */
struct AnalyticEllipseCostFunction : public ceres::SizedCostFunction<1, 4>
{
    AnalyticEllipseCostFunction(const Eigen::Vector2d &observed_point) : observed_point(observed_point) {}
    virtual ~AnalyticEllipseCostFunction() {}

    /** This function performs double duty: it both computes the residuals and,
     *  at other times, will also compute the Jacobians. This is communicated
     *  via potential `nullptr` values in `jacobians`. While somewhat awkward,
     *  this allows for re-use of sub-expressions for increased efficiency.
     *  The sizes of the arrays are indicated via the template argument
     *  above.
     *
     *  \param parameters an array of parameter arrays
     *  \param residuals an array of residuals values
     *  \param jacobians an array of Jacobian matrices. Each matrix is in row-major order.
     *  \return whether the evaluation was successful
     */
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
        Ellipse<double> ellipse(parameters[0]);

        // We can re-use all of these later
        const double dx = observed_point.x() - ellipse.h();
        const double dy = observed_point.y() - ellipse.k();
        const double dx2 = dx * dx;
        const double dy2 = dy * dy;
        const double a2 = ellipse.a() * ellipse.a();
        const double b2 = ellipse.b() * ellipse.b();
        residuals[0] = dx2 / a2 + dy2 / b2 - 1;

        // will be null if only evaluating residuals
        if (jacobians != nullptr)
        {
            // if some parameters are being held constant,
            // then individual Jacobian matrices will also be null
            // to avoid unneeded computation
            if (jacobians[0] != nullptr)
            {
                using jacobian_t = Eigen::Matrix<double, 1, 4, Eigen::RowMajor>;
                Eigen::Map<jacobian_t> jac(jacobians[0]);
                jac(0, 0) = (-2 * dx) / a2;
                jac(0, 1) = (-2 * dy) / b2;
                jac(0, 2) = (-2 * dx2) / (a2 * ellipse.a());
                jac(0, 3) = (-2 * dy2) / (b2 * ellipse.b());
            }
        }

        return true;
    }

    Eigen::Vector2d observed_point;
};

/** We can inject our own code into the optimization process to do
 *  custom logging and the like. This class writes intermediate values
 *  to a CSV file.
 */
class CSVCallback : public ceres::IterationCallback
{
public:
    explicit CSVCallback(const std::string &path, const double *params, int num_observations)
        : m_params(params), m_num_observations(num_observations), m_output(path)
    {
        m_output << "Cost,h,k,a,b" << std::endl;
    }

    ~CSVCallback() {}

    ceres::CallbackReturnType operator()(const ceres::IterationSummary &summary)
    {
        Ellipse<double> ellipse(m_params);
        m_output << summary.cost / m_num_observations << "," << ellipse.h() << "," << ellipse.k() << "," << ellipse.a() << "," << ellipse.b() << std::endl;
        return ceres::CallbackReturnType::SOLVER_CONTINUE;
    }

private:
    const double *m_params;
    const int m_num_observations;
    std::ofstream m_output;
};

/** Creates a dataset consisting of noisy samples from an arc of an
 *  axis-aligned ellipse.
 *
 *  \param num_observations the number of observations to sample
 *  \param params the ellipse parameters
 *  \param start_angle the starting angle of the arc in radians
 *  \param end_angle the ending angle of the arc in radians
 *  \param noise_sigma the sigma of the Gaussian used for noise
 *  \return a matrix of points
 */
Eigen::Matrix2Xd create_dataset(int num_observations,
                                const Ellipse<double> &ellipse,
                                double start_angle = -0.5,
                                double end_angle = 2.0,
                                double noise_sigma = 0.05)
{
    Eigen::RowVectorXd angles = Eigen::RowVectorXd::LinSpaced(num_observations, start_angle, end_angle);
    Eigen::Matrix2Xd data(2, num_observations);
    data.row(0) = (ellipse.a() * angles.array().cos()) + ellipse.h();
    data.row(1) = (ellipse.b() * angles.array().sin()) + ellipse.k();
    std::random_device rd{};
    std::mt19937 gen{rd()};
    std::normal_distribution<> d{0, noise_sigma};
    for (auto i = 0; i < data.cols(); ++i)
    {
        data(0, i) += d(gen);
        data(1, i) += d(gen);
    }

    return data;
}

// Setup the problem using numeric differentiation.
void setup_numeric(ceres::Problem &problem, const Eigen::Matrix2Xd &dataset, double *params)
{
    std::cout << "Numeric differentiation: " << std::endl;
    using cost_t = ceres::NumericDiffCostFunction<NumericEllipseCostFunctor,
                                                  ceres::CENTRAL, // method to use
                                                  1,              // # residuals
                                                  4>;             // # params
    for (auto i = 0; i < dataset.cols(); ++i)
    {
        ceres::CostFunction *cost_function =
            new cost_t(new NumericEllipseCostFunctor(dataset.col(i)));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
}

// Setup the problem using automatic differentiation.
void setup_autodiff(ceres::Problem &problem, const Eigen::Matrix2Xd &dataset, double *params)
{
    std::cout << "Automatic differentiation: " << std::endl;
    using cost_t = ceres::AutoDiffCostFunction<AutoEllipseCostFunctor,
                                               1,   // # residuals
                                               4>;  // # params
    for (auto i = 0; i < dataset.cols(); ++i)
    {
        ceres::CostFunction *cost_function =
            new cost_t(new AutoEllipseCostFunctor(dataset.col(i)));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
}

// Setup the problem using analytic differentiation.
void setup_analytic(ceres::Problem &problem, const Eigen::Matrix2Xd &dataset, double *params)
{
    std::cout << "Analytic differentiation: " << std::endl;
    for (auto i = 0; i < dataset.cols(); ++i)
    {
        ceres::CostFunction *cost_function = new AnalyticEllipseCostFunction(dataset.col(i));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
}

// Perform a gradient check
int check_gradients(const Eigen::Matrix2Xd &dataset, double *params, double tolerance)
{
    // First we create an instance of the cost function we want to check
    Eigen::Vector2d observed_point = dataset.col(0);
    auto cost_function = std::make_shared<AnalyticEllipseCostFunction>(observed_point);

    const double *parameters[] = {params};

    // We can use this object to customise the checking process
    ceres::NumericDiffOptions numeric_diff_options;
    ceres::GradientChecker gradient_checker(cost_function.get(), nullptr, numeric_diff_options);

    // We perform a probe. If unsuccessful, we can view the erroneous
    // gradients by writing the error log to the console
    ceres::GradientChecker::ProbeResults results;
    if (!gradient_checker.Probe(parameters, tolerance, &results))
    {
        std::cerr << "An error has occurred:\n"
                  << results.error_log;
        return EXIT_FAILURE;
    }

    std::cout << "Gradients correct!" << std::endl;
    return EXIT_SUCCESS;
}

DEFINE_string(mode, "numeric", "Mode for the program (one of 'numeric', 'autodiff', 'analytic', 'check_grad')");
DEFINE_int32(num_observations, 100, "Number of observations");
DEFINE_bool(verbose, false, "Output a verbose summary of the optimization");
DEFINE_bool(dump_data, false, "Whether to dump the data to a csv");

int main(int argc, char **argv)
{
    gflags::SetUsageMessage("Ceres Example");
    gflags::ParseCommandLineFlags(&argc, &argv, true);

    Ellipse<double> initial(0.1, 0.3, 0.9, 1.2);
    Ellipse<double> target(-0.3, 0.5, 4.3, 2.1);
    Ellipse<double> ellipse;
    Eigen::Matrix2Xd dataset = create_dataset(FLAGS_num_observations, target);

    if (FLAGS_dump_data)
    {
        std::ofstream output(FLAGS_mode + "_data.csv");
        output << "x,y" << std::endl;
        for (auto i = 0; i < dataset.cols(); ++i)
        {
            output << dataset(0, i) << "," << dataset(1, i) << std::endl;
        }
    }

    ceres::Problem problem;
    ellipse = initial;

    if ("autodiff" == FLAGS_mode)
    {
        setup_autodiff(problem, dataset, ellipse.params());
    }
    else if ("numeric" == FLAGS_mode)
    {
        setup_numeric(problem, dataset, ellipse.params());
    }
    else if ("analytic" == FLAGS_mode)
    {
        setup_analytic(problem, dataset, ellipse.params());
    }
    else if ("check_grad" == FLAGS_mode)
    {
        return check_gradients(dataset, ellipse.params(), 1e-9);
    }
    else
    {
        std::cout << "Unrecognized mode: " << FLAGS_mode << std::endl;
        return 1;
    }

    // we can set upper and lower bounds for all parameters
    ellipse.set_bounds(problem);

    // The solver has a wide variety customization options
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    options.num_threads = 8;

    // Here we add our own custom callback for logging to a file
    options.update_state_every_iteration = true;
    std::shared_ptr<CSVCallback> callback = std::make_shared<CSVCallback>(FLAGS_mode + "_fit.csv", ellipse.params(), FLAGS_num_observations);
    options.callbacks.push_back(callback.get());

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    if (FLAGS_verbose)
    {
        std::cout << summary.FullReport() << std::endl;
    }
    else
    {
        std::cout << summary.BriefReport() << std::endl;
    }

    std::cout << "Initial: " << initial.to_string() << std::endl
              << "Final: " << ellipse.to_string() << std::endl
              << "Target: " << target.to_string() << std::endl;

    return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/concept/value.hpp>
#include <eve/constant/valmin.hpp>
#include <eve/constant/valmax.hpp>
#include <eve/function/all.hpp>
#include <eve/function/erfc_inv.hpp>
#include <eve/function/diff/erfc_inv.hpp>
#include <eve/function/is_negative.hpp>
#include <eve/function/is_positive.hpp>
#include <type_traits>
#include <cmath>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/constant/smallestposval.hpp>
#include <eve/platform.hpp>
#include <boost/math/special_functions/erf.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
// EVE_TEST_TYPES( "Check return types of erfc_inv"
//             , eve::test::simd::ieee_reals
//             )
// <typename T>(eve::as<T>)
// {
//   using v_t = eve::element_type_t<T>;

//   TTS_EXPR_IS( eve::erfc_inv(T())  , T);
//   TTS_EXPR_IS( eve::erfc_inv(v_t()), v_t);
// };

//==================================================================================================
// erfc_inv  tests
//==================================================================================================
EVE_TEST( "Check behavior of erfc_inv on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.1, 2.0))
        )
<typename T>(T const&  a0)
{
  using v_t = eve::element_type_t<T>;
  using eve::erfc_inv;
  using eve::as;
  TTS_ULP_EQUAL( erfc_inv(a0),  map([](auto e){return boost::math::erfc_inv(e);}, a0), 2);
  auto derfc_inv = [](auto e){return v_t(-0.886226925452758013649)*std::exp(eve::sqr(erfc_inv(e)));};
  TTS_ULP_EQUAL( eve::diff(erfc_inv)(a0),  map(derfc_inv, a0), 2);

  TTS_ULP_EQUAL(erfc_inv(T(0.5)), T(boost::math::erfc_inv(v_t(0.5))), 1. );

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(erfc_inv(eve::nan(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(erfc_inv(eve::inf(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(erfc_inv(eve::minf(eve::as<T>())) , eve::nan(eve::as<T>()) );
  }

  TTS_ULP_EQUAL(erfc_inv(T(35)), eve::nan(eve::as<T>()), 0.5);
  TTS_ULP_EQUAL(erfc_inv(T(-35)), eve::nan(eve::as<T>()), 0.5);

  TTS_IEEE_EQUAL(erfc_inv(T( 0 )), eve::inf(eve::as<T>()));
  TTS_IEEE_EQUAL(erfc_inv(T(-0.)), eve::inf(eve::as<T>()));
  TTS_ULP_EQUAL(erfc_inv(T( 0.1 )), T( boost::math::erfc_inv(0.1)), 0.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 0.2 )), T( boost::math::erfc_inv(0.2)), 1 );
  TTS_ULP_EQUAL(erfc_inv(T( 0.3 )), T( boost::math::erfc_inv(0.3)),  1 );
  TTS_ULP_EQUAL(erfc_inv(T( 0.5 )), T( boost::math::erfc_inv(0.5)),  1 );
  TTS_ULP_EQUAL(erfc_inv(T( 0.15)), T( boost::math::erfc_inv(0.15)), 0.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 0.75)), T( boost::math::erfc_inv(0.75)), 0.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.0 )), T( boost::math::erfc_inv(1.0 )), 0.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.1 )), T( boost::math::erfc_inv(1.1)), 1.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.2 )), T( boost::math::erfc_inv(1.2)), 1.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.3 )), T( boost::math::erfc_inv(1.3)), 0.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.5 )), T( boost::math::erfc_inv(1.5)),  1 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.15)), T( boost::math::erfc_inv(1.15)), 1.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.75)), T( boost::math::erfc_inv(1.75)), 0.5 );
  TTS_ULP_EQUAL(erfc_inv(T( 1.45984)), T( boost::math::erfc_inv(1.45984)), 1.5 );

};


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class PoissonLogTestRig : public VectorIntRNGTestRig {
 public:
  PoissonLogTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6},
                            {-0.5, 0.0, 0.1, 1.7}, {-2, 0, 1, 2}, {}, {}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2&, const T3&,
                        T_rng& rng) const {
    return stan::math::poisson_log_rng(alpha, rng);
  }

  template <typename T1>
  double pmf(int y, T1 alpha, double, double) const {
    return std::exp(stan::math::poisson_log_lpmf(y, alpha));
  }
};

TEST(ProbDistributionsPoissonLog, errorCheck) {
  check_dist_throws_all_types(PoissonLogTestRig());
}

TEST(ProbDistributionsPoissonLog, distributionCheck) {
  check_counts_real(PoissonLogTestRig());
}


#ifndef GUARD_CIRCUIT_SIMULATOR_HPP
#define GUARD_CIRCUIT_SIMULATOR_HPP

#include <sstream>
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
	// Information that tells the caller the numeric type (eg. double) and size (input / output dim)
	typedef _Scalar Scalar;
	enum
	{ // Required by numerical differentiation module
		InputsAtCompileTime = NX,
		ValuesAtCompileTime = NY
	};
	// Tell the caller the matrix sizes associated with the input, output, and jacobian
	typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
	typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
	typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

	// Local copy of the number of inputs
	int m_inputs, m_values;

	// Two constructors:
	Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
	Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

	// Get methods for users to determine function input and output dimensions
	int inputs() const { return m_inputs; }
	int values() const { return m_values; }
};
struct ConductanceFunc : Functor<double>
{
	// Simple constructor
	double time = 0;
	Circuit::Schematic *schem;
	Circuit::ParamTable *param;
	double timestep;
	int NUM_NODES = 0;
	ConductanceFunc(Circuit::Schematic *schem, Circuit::ParamTable *param, double time, double timestep, int NUM_NODES) : Functor<double>(schem->nonLinearComps.size(), schem->nonLinearComps.size())
	{
		this->schem = schem;
		this->param = param;
		this->timestep = timestep;
		this->time = time;
		this->NUM_NODES = NUM_NODES;
	}

	int operator()(const Eigen::VectorXd &vDiff, Eigen::VectorXd &fvec) const
	{
		Eigen::VectorXd voltage(NUM_NODES);
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);
		for (int i = 0; i < vDiff.size(); i++)
		{
			schem->nonLinearComps[i]->setConductance(param, timestep, vDiff(i));
		}
		Circuit::Math::getConductanceTRAN(schem, conductance, param, time, timestep);
		Circuit::Math::getCurrentTRAN(schem, current, conductance, param, time, timestep);
		Circuit::Math::solveMatrix(conductance, voltage, current);

		for (int i = 0; i < vDiff.size(); i++)
		{
			double vPos = (schem->nonLinearComps[i]->getPosNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getPosNode()->getId()) : 0;
			double vNeg = (schem->nonLinearComps[i]->getNegNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getNegNode()->getId()) : 0;
			fvec(i) = vPos - vNeg - vDiff(i);
		}

		return 0;
	}
	int getVdif(const Eigen::VectorXd &vDiff, Eigen::VectorXd &fvec) const
	{

		Eigen::VectorXd voltage(NUM_NODES);
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);

		for (int i = 0; i < vDiff.size(); i++)
		{
			schem->nonLinearComps[i]->setConductance(param, timestep, vDiff(i));
		}
		Circuit::Math::getConductanceTRAN(schem, conductance, param, time, timestep);
		Circuit::Math::getCurrentTRAN(schem, current, conductance, param, time, timestep);
		Circuit::Math::solveMatrix(conductance, voltage, current);

		for (int i = 0; i < vDiff.size(); i++)
		{
			double vPos = (schem->nonLinearComps[i]->getPosNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getPosNode()->getId()) : 0;
			double vNeg = (schem->nonLinearComps[i]->getNegNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getNegNode()->getId()) : 0;
			fvec(i) = vPos - vNeg;
		}

		return 0;
	}
	void getVoltageVector(const Eigen::VectorXd &vDiff, Eigen::VectorXd &fvec)
	{
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);

		for (int i = 0; i < vDiff.size(); i++)
		{
			schem->nonLinearComps[i]->setConductance(param, timestep, vDiff(i));
		}

		Circuit::Math::getConductanceTRAN(schem, conductance, param, time, timestep);
		Circuit::Math::getCurrentTRAN(schem, current, conductance, param, time, timestep);
		Circuit::Math::solveMatrix(conductance, fvec, current);
	}
};

class Circuit::Simulator
{
private:
	Schematic *schem;
	double tranStopTime;
	double tranSaveStart;
	double tranStepTime;
	std::stringstream spiceStream;
	std::stringstream csvStream;

	void spicePrintTitle()
	{
		spiceStream << "Time";
		for (auto node_pair : schem->nodes)
		{
			spiceStream << "\tV(" << node_pair.first << ")";
		}
		for (auto comp_pair : schem->comps)
		{
			spiceStream << "\tI(" << comp_pair.first << ")";
		}
		spiceStream << "\n";
	}
	void csvPrintTitle()
	{
		csvStream << "Time";
		for (auto node_pair : schem->nodes)
		{
			csvStream << ",V(" << node_pair.first << ")";
		}
		for (auto comp_pair : schem->comps)
		{
			csvStream << ",I(" << comp_pair.first << ")";
		}
		csvStream << "\n";
	}

	void printStep(int n)
	{
		ParamTable *param = schem->tables[n];
		if (param->lookup.size() == 0)
		{
			return;
		}
		for (auto x : param->lookup)
		{
			spiceStream << "Step Information:";
			csvStream << "Step Information:";
			for (std::pair<std::string, double> var : param->lookup)
			{
				csvStream << " " << var.first << "=" << var.second;
				spiceStream << " " << var.first << "=" << var.second;
			}
			csvStream << " Run: " << n + 1 << "/" << schem->tables.size() << std::endl;
			spiceStream << " Run: " << n + 1 << "/" << schem->tables.size() << std::endl;
		}
	}
	void spicePrint(ParamTable *param, double time, double timestep)
	{
		spiceStream << time;
		for (auto node_pair : schem->nodes)
		{
			spiceStream << "\t" << node_pair.second->voltage;
		}
		for (auto comp_pair : schem->comps)
		{
			spiceStream << "\t" << comp_pair.second->getCurrent(param, time, timestep);
		}
		spiceStream << "\n";
	}
	void csvPrint(ParamTable *param, double time, double timestep)
	{
		csvStream << time;
		for (auto node_pair : schem->nodes)
		{
			csvStream << "," << node_pair.second->voltage;
		}
		for (auto comp_pair : schem->comps)
		{
			csvStream << "," << comp_pair.second->getCurrent(param, time, timestep);
		}
		csvStream << "\n";
	}

public:
	enum SimulationType
	{
		OP,
		TRAN,
		DC,
		SMALL_SIGNAL
	};

	const SimulationType type;

	enum OutputFormat
	{
		CSV,
		SPACE // actually tab separated
	};

	using enumPair = std::pair<SimulationType, std::string>;

	std::map<SimulationType, std::string> simulationTypeMap = {
		enumPair(OP, "OP"),
		enumPair(TRAN, "TRAN"),
		enumPair(DC, "DC"),
		enumPair(SMALL_SIGNAL, "SMALL_SIGNAL"),
	};

	Simulator(Schematic *schem, SimulationType type) : schem(schem), type(type) {}
	Simulator(Schematic *schem, SimulationType type, double tranStopTime, double tranSaveStart = 0, double tranStepTime = 0) : Simulator(schem, type)
	{
		if (tranStepTime == 0)
		{
			tranStepTime = tranStopTime / 1000.0; // default of a thousand cycles
		}
		this->tranStopTime = tranStopTime;
		this->tranSaveStart = tranSaveStart;
		this->tranStepTime = tranStepTime;
	}

	void run(std::ostream &dst, OutputFormat format)
	{
		const unsigned int NUM_NODES = schem->nodes.size() - 1;
		const unsigned int NUM_V_GUESS = schem->nonLinearComps.size();

		Eigen::VectorXd voltage(NUM_NODES);
		Eigen::VectorXd vGuess(NUM_V_GUESS);
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);

		if (format == SPACE)
		{
			spiceStream.str("");
			spicePrintTitle();
		}
		else if (format == CSV)
		{
			csvStream.str("");
			csvPrintTitle();
		}
		ParamTable *param;
		for (size_t i = 0; i < schem->tables.size(); i++)
		{
			param = schem->tables[i];
			printStep(i);

			for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
				if (node_pair.second->getId() != -1)
				{
					node_pair.second->voltage = 0.0;
				}
			});
			if (type == OP)
			{
				Circuit::Math::getConductanceOP(schem, conductance, param);
				Circuit::Math::getCurrentOP(schem, current, conductance, param);
				Circuit::Math::solveMatrix(conductance, voltage, current);

				dst << "\t-----Operating Point-----\t\n";
				if (param->lookup.size() > 0)
				{
					dst << "Step Information: ";
					for (std::pair<std::string, double> var : param->lookup)
					{
						dst << " " << var.first << "=" << var.second;
					}
					dst << " Run: " << i + 1 << "/" << schem->tables.size() << std::endl;
				}
				dst << std::endl;
				for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
					if (node_pair.second->getId() != -1)
					{
						node_pair.second->voltage = voltage[node_pair.second->getId()];
						dst << "V(" << node_pair.first << ")\t\t" << node_pair.second->voltage << "\t\tnode_voltage\n";
					}
				});

				for_each(schem->comps.begin(), schem->comps.end(), [&](const auto comp_pair) {
					dst << "I(" << comp_pair.first << ")\t\t" << comp_pair.second->getCurrent(param, 0, -1) << "\t\tdevice_current\n";
				});
			}
			else if (type == TRAN)
			{
				if (!schem->nonLinear)
				{
					Eigen::SparseMatrix<double> sparse;

					for (double t = 0; t <= tranStopTime; t += tranStepTime)
					{
						Math::progressBar(t / tranStopTime, i, schem->tables.size());
						Math::getConductanceTRAN(schem, conductance, param, t, tranStepTime);
						Math::getCurrentTRAN(schem, current, conductance, param, t, tranStepTime);

						try
						{
							Circuit::Math::solveMatrix(conductance, voltage, current);
						}
						catch (const std::exception &e)
						{
							std::cerr << "error solving skipping timestep" << std::endl;
							continue;
						}

						for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
							if (node_pair.second->getId() != -1)
							{
								node_pair.second->voltage = voltage[node_pair.second->getId()];
							}
						});
						if (t >= tranSaveStart)
						{
							if (format == SPACE)
							{
								spicePrint(param, t, tranStepTime);
							}
							else if (format == CSV)
							{
								csvPrint(param, t, tranStepTime);
							}
						}
					}
				}
				else
				{
					for (double t = 0; t <= tranStopTime; t += tranStepTime)
					{
						//Math::progressBar(t / tranStopTime, i, schem->tables.size());
						Math::init_vector(vGuess);
						ConductanceFunc functor(schem, param, t, tranStepTime, NUM_NODES);
						Eigen::NumericalDiff<ConductanceFunc> numDiff(functor);

						if (schem->itType == Schematic::IterationType::Levenberg)
						{
							Eigen::LevenbergMarquardt<Eigen::NumericalDiff<ConductanceFunc>, double> lm(numDiff);
							lm.parameters.maxfev = 1000;
							lm.parameters.xtol = 1.0e-10;
							lm.minimize(vGuess);
						}
						else if (schem->itType == Schematic::IterationType::Newton)
						{
							for (size_t i = 0; i < 1000; i++)
							{
								Eigen::MatrixXd jaq(NUM_V_GUESS, NUM_V_GUESS);
								numDiff.df(vGuess, jaq);
								Eigen::VectorXd vErrVec(NUM_V_GUESS);
								functor(vGuess, vErrVec);
								Eigen::MatrixXd inverseJaq = jaq.transpose().inverse();
								for (size_t x = 0; x < NUM_V_GUESS; x++)
								{
									for (size_t y = 0; y < NUM_V_GUESS; y++)
									{
										if (std::isnan(inverseJaq(x, y)))
										{
											inverseJaq(x, y) = 1e-200;
										}
										if (!std::isfinite(inverseJaq(x, y)))
										{
											inverseJaq(x, y) = 1e200;
										}
									}
								}
								std::cerr<<t<<","<<i<<","<<vGuess[0]<<","<<vGuess[1]<<","<<vErrVec.norm()<<std::endl;
								vGuess = vGuess - 0.005 * (inverseJaq * vErrVec);
							}
						}
						else
						{
							std::cerr << "unknown iteration type" << std::endl;
							std::terminate();
						}

						Eigen::VectorXd vErrVec(NUM_V_GUESS);
						functor.getVoltageVector(vGuess, voltage);
						for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
							if (node_pair.second->getId() != -1)
							{
								node_pair.second->voltage = voltage[node_pair.second->getId()];
							}
						});
						if (t >= tranSaveStart)
						{
							if (format == SPACE)
							{
								spicePrint(param, t, tranStepTime);
							}
							else if (format == CSV)
							{
								csvPrint(param, t, tranStepTime);
							}
						}
					}
				}
				std::cerr << std::endl;
			}
			if (format == SPACE && type != OP)
			{
				dst << spiceStream.str();
				spiceStream.str("");
			}
			else if (format == CSV && type != OP)
			{
				dst << csvStream.str();
				csvStream.str("");
			}
		}
	}
};

#endif


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      The Mathworks, Inc. RKF87, Symbolic Math Toolbox, 2012.
 *
 *    Notes
 *      All the test for this integrator are based on the data generated using the Symbolic Math
 *      Toolbox (MathWorks, 2012). Ideally, another source of data should be used to complete the
 *      testing.
 *
 *      The single step and full integration error tolerances were picked to be as small as
 *      possible, without causing the tests to fail. These values are not deemed to indicate any
 *      bugs in the code; however, it is important to take these discrepancies into account when
 *      using this numerical integrator.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/reinitializableNumericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTests.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"

#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"

#include <limits>
#include <string>

#include <Eigen/Core>

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_runge_kutta_fehlberg_78_integrator )

using linear_algebra::flipMatrixRows;

using numerical_integrators::NumericalIntegratorXdPointer;
using numerical_integrators::ReinitializableNumericalIntegratorXdPointer;
using numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd;
using numerical_integrators::RungeKuttaCoefficients;

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;
using numerical_integrator_test_functions::computeFehlbergLogirithmicTestODEStateDerivative ;
using numerical_integrator_test_functions::computeAnalyticalStateFehlbergODE;

//! Test Runge-Kutta-Fehlberg 78 integrator using benchmark ODE of Fehlberg (1968)
BOOST_AUTO_TEST_CASE( test_RungeKuttaFehlberg78_Integrator_Fehlberg_Benchmark )
{
    using namespace numerical_integrators;
    RungeKuttaCoefficients coeff78 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 );

    // Integrator settings
    double minimumStepSize   = std::numeric_limits<double>::epsilon( );
    double maximumStepSize   = std::numeric_limits<double>::infinity( );
    double initialStepSize   = 1E-6; // Error: 0.0521 for initialStepSize = 1 ?
    double relativeTolerance = 1E-16;
    double absoluteTolerance = 1E-16;

    // Initial conditions
    double initialTime = 0.0;
    double finalTime   = 5.0;
    Eigen::Vector2d initialState( exp( 1.0 ), 1.0);

    // Setup integrator
    numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd integrator78(
                coeff78, computeFehlbergLogirithmicTestODEStateDerivative,
                initialTime, initialState, minimumStepSize,
                maximumStepSize, relativeTolerance, absoluteTolerance );

    // Obtain numerical solution
    Eigen::Vector2d numericalSolution = integrator78.integrateTo( finalTime, initialStepSize );

    // Analytical solution
    Eigen::Vector2d analyticalSolution = computeAnalyticalStateFehlbergODE( finalTime, initialState );

    Eigen::Vector2d computedError = numericalSolution - analyticalSolution;
    BOOST_CHECK_SMALL( std::fabs(computedError( 0 )), 1E-13 );
    BOOST_CHECK_SMALL( std::fabs(computedError( 1 )), 1E-13 );
}

//! Test Runge-Kutta-Fehlberg 78 integrator using benchmark data from (The MathWorks, 2012).
BOOST_AUTO_TEST_CASE( testRungeKuttaFehlberg78IntegratorUsingMatlabData )
{
    using namespace numerical_integrator_tests;

    // Read in benchmark data (generated using Symbolic Math Toolbox in Matlab
    // (The MathWorks, 2012)). This data is generated using the RKF87 numerical integrator.
    const std::string pathToForwardIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKuttaFehlberg78Forward.txt";
    const std::string pathToDiscreteEventIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKuttaFehlberg78DiscreteEvent.txt";

    // Store benchmark data in matrix.
    const Eigen::MatrixXd matlabForwardIntegrationData =
            input_output::readMatrixFromFile( pathToForwardIntegrationOutputFile, "," );
    Eigen::MatrixXd matlabBackwardIntegrationData = matlabForwardIntegrationData;
    flipMatrixRows( matlabBackwardIntegrationData );
    const Eigen::MatrixXd matlabDiscreteEventIntegrationData =
            input_output::readMatrixFromFile( pathToDiscreteEventIntegrationOutputFile, "," );

    // Set integrator parameters.

    // All of the following parameters are set such that the input data is fully accepted by the
    // integrator, to determine the steps to be taken.
    const double zeroMinimumStepSize = std::numeric_limits< double >::epsilon( );
    const double infiniteMaximumStepSize = std::numeric_limits< double >::infinity( );
    const double infiniteRelativeErrorTolerance = std::numeric_limits< double >::infinity( );
    const double infiniteAbsoluteErrorTolerance = std::numeric_limits< double >::infinity( );

    // The following parameters set how the error control mechanism should work.
    const double relativeErrorTolerance = 1.0e-15;
    const double absoluteErrorTolerance = 1.0e-15;

    // Case 1: Execute integrateTo() to integrate one step forward in time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        executeOneIntegrateToStep( matlabForwardIntegrationData, 1.0e-15, integrator );
    }

    // Case 2: Execute performIntegrationStep() to perform multiple integration steps until final
    //         time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabForwardIntegrationData,
                                               1.0e-15, 1.0e-15, integrator );
    }

    // Case 3: Execute performIntegrationStep() to perform multiple integration steps until initial
    //         time (backwards).
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabBackwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabBackwardIntegrationData( FIRST_ROW,
                                                        STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabBackwardIntegrationData,
                                               1.0e-15, 1.0e-14, integrator );
    }

    // Case 4: Execute integrateTo() to integrate to specified time in one step.
    {
        // Note that this test has a strange issue that the if the absolute error tolerance is set
        // to 1.0e-15, the last step that the integrateTo() function takes does not result in the
        // expected final time of 1.0. As a temporary solution, the absolute error tolerance has
        // been multiplied by 10.0, which seems to solve the problem. This error indicated a
        // possible problem with the implementation of the integrateTo() function, which needs to
        // be investigated in future.

        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    relativeErrorTolerance,
                    absoluteErrorTolerance * 10.0 );

        executeIntegrateToToSpecifiedTime( matlabForwardIntegrationData, 1.0e-13, integrator,
                                           matlabForwardIntegrationData(
                                               matlabForwardIntegrationData.rows( ) - 1,
                                               TIME_COLUMN_INDEX ) );
    }

    // Case 5: Execute performIntegrationstep() to integrate to specified time in multiple steps,
    //         including discrete events.
    {
        // Declare integrator with all necessary settings.
        ReinitializableNumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTimeWithEvents( matlabDiscreteEventIntegrationData,
                                                         1.0e-15, 1.0e-13, integrator );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*======================================================================
Copyright 2019 Google LLC

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    https://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
======================================================================*/

#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "traj_circle.h"

using namespace std;
using namespace Eigen;

static double hz_;
static double r_,wr_;
static double a_,wa_;
static double ramp_,t_;

void
init_traj_circle(double r,
                 double wr,
                 double a,
                 double wa,
                 double hz,
                 double ramp)
{
  r_  = r;
  wr_ = wr;
  a_  = a;
  wa_ = wa;
  hz_ = hz;
  ramp_ = ramp;

  t_=0.0;
}

void
traj_circle(Vector3d &x, Vector3d &xd, Vector3d &xdd)
{
  double wr,wa;

   if( t_<ramp_ )
   { wr = t_/ramp_ * wr_;
     wa = t_/ramp_ * wa_;
   }
   else
   { wr = wr_;
     wa = wa_;
   }

    x[0] =  r_*      cos( wr*t_ ) - r_ ;
   xd[0] = -r_*wr*   sin( wr*t_ ) ;
  xdd[0] = -r_*wr*wr*cos( wr*t_ ) ;

    x[1] =  r_*      sin( wr*t_ ) ;
   xd[1] =  r_*wr*   cos( wr*t_ ) ;
  xdd[1] = -r_*wr*wr*sin( wr*t_ ) ;

    x[2] =  a_*   (1-cos( wa*t_ ));
   xd[2] =  a_*wa*   sin( wa*t_ ) ;
  xdd[2] =  a_*wa*wa*cos( wa*t_ ) ;

  t_ += 1.0/hz_;

}


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestAlignment
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <iomanip>
#include <cstdlib>
#include "minimath/rotation3d.hpp"
#include "minimath/transform3d.hpp"
#include "minimath/point3d.hpp"
#include "minimath/geom3d_ops.hpp"

#include "Defines.h"
#include <tr1/array>

using namespace minimath;

namespace
{
// check that results of calculated transformation and
// reference transformation are the same to within 64 epsilons.
template <typename T>
void checkTransformation(const T& transf)
{
  pointxyzd pA = transf*p100;
  pointxyzd pB = transf*p010;
  pointxyzd pC = transf*p001;
  bool success = true;

  typedef std::tr1::array<pointxyzd, 2> PointXYZDPair;

  PointXYZDPair p0 = { {p100, pA} };
  PointXYZDPair p1 = { {p010, pB} };
  PointXYZDPair p2 = { {p001, pC} };
  std::tr1::array<PointXYZDPair, 3> pointPairs = { {p0, p1, p2} };

  transform3d<double> transf1 = transformation<double>(pointPairs.begin(),
                                                       pointPairs.end(),
                                                       success);

  BOOST_CHECK(success);
  BOOST_CHECK(minimath::equal(transf1*p100, pA, 128u));
  BOOST_CHECK(minimath::equal(transf1*p010, pB, 128u));
  BOOST_CHECK(minimath::equal(transf1*p001, pC, 64u));

}

template <typename R>
void checkRotationAndTranslation()
{
  for (int i = 1; i < 9;  ++i) {
    double x = std::rand()%100;
    double y = std::rand()%100;
    double z = std::rand()%100;
    translation3d<double> transl(pointxyzd(x,y,z));
    rotation3d<double> rot(R(PI/i));
    transform3d<double> transf(rot, transl);
    checkTransformation(transf);
  }
}

template <typename R>
void checkTranslationAndRotation()
{
  for (int i = 1; i < 9;  ++i) {
    double x = std::rand()%100;
    double y = std::rand()%100;
    double z = std::rand()%100;
    translation3d<double> transl(pointxyzd(x,y,z));
    rotation3d<double> rot(R(PI/i));
    transform3d<double> transf(transl, rot);
    checkTransformation(transf);
  }
}

} // anonymous namespace

BOOST_AUTO_TEST_SUITE(test_awesome_grower)

BOOST_AUTO_TEST_CASE(testTranslation)
{
  // rotate reference points
  translation3d<double> transl(pointxyzd(100, 100, 100));
  checkTransformation(transl);
}

BOOST_AUTO_TEST_CASE(testRotation)
{
  // rotate reference points
  rotation3dzyx<double>  rot(PI/4, 0, PI/4);
  checkTransformation(rot);
}

BOOST_AUTO_TEST_CASE(testRotation3DXAndTranslation)
{
  checkRotationAndTranslation<rotation3dx<double> >();
}

BOOST_AUTO_TEST_CASE(testRotation3DYAndTranslation)
{
  checkRotationAndTranslation<rotation3dy<double> >();
}

BOOST_AUTO_TEST_CASE(testRotation3DZAndTranslation)
{
  checkRotationAndTranslation<rotation3dz<double> >();
}

BOOST_AUTO_TEST_CASE(testTranslationAndRotation3DX)
{
  checkTranslationAndRotation<rotation3dx<double> >();
}

BOOST_AUTO_TEST_CASE(testTranslationAndRotation3DY)
{
  checkTranslationAndRotation<rotation3dy<double> >();
}

BOOST_AUTO_TEST_CASE(testTranslationAndRotation3DZ)
{
  checkTranslationAndRotation<rotation3dz<double> >();
}

BOOST_AUTO_TEST_SUITE_END()