// ======================================================================
/*!
 * \brief Astronomical calculations
 *
 * Solar calculations
 *
 * Based on NOAA JavaScript at
 *       <http://www.srrb.noaa.gov/highlights/sunrise/azel.html>
 *       <http://www.srrb.noaa.gov/highlights/sunrise/sunrise.html>
 *
 * Reference:
 *       Solar Calculation Details
 *       <http://www.srrb.noaa.gov/highlights/sunrise/calcdetails.html>
 *
 * Transformation to C++ by Ilmatieteen Laitos, 2008.
 *
 * License:
 *       UNKNOWN (not stated in JavaScript)
 */
// ======================================================================

#include "AstronomyJulianTime.h"
#include "Exception.h"

#include <boost/date_time/local_time/local_time.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

using boost::gregorian::date;
using boost::local_time::local_date_time;
using boost::local_time::time_zone_ptr;
using boost::posix_time::not_a_date_time;
using boost::posix_time::ptime;

namespace Fmi
{
namespace Astronomy
{
/*
 * Construction from UTC date and time
 *
 * Note: -0.5 is required since Gregorian days start at midnight, Julian
 *       at noon. The beginning of a Gregorian day is -12h on the earlier
 *       Julian day.
 */
JulianTime::JulianTime(const ptime& utc)
{
  try
  {
    jd = (utc.date().julian_day() - 0.5 + utc.time_of_day().total_seconds() / (24.0 * 3600));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Convert to Gregorian UTC
 */
ptime JulianTime::ptime_utc() const
{
  try
  {
    if (jd == 0.0)
      return not_a_date_time;

    int z = lround(jd);
    double f = (jd + 0.5) - z;

    int A;
    if (z < 2299161)
    {
      A = z;
    }
    else
    {
      int alpha = (int)((z - 1867216.25) / 36524.25);
      A = z + 1 + alpha - INTDIV(alpha / 4);
    }

    int B = A + 1524;
    int C = (int)((B - 122.1) / 365.25);
    int D = (int)(365.25 * C);
    int E = (int)((B - D) / 30.6001);

    auto day = static_cast<std::uint16_t>(B - D - (int)floor(30.6001 * E));
    auto month = static_cast<std::uint16_t>((E < 14) ? E - 1 : E - 13);
    auto year = static_cast<std::uint16_t>((month > 2) ? C - 4716 : C - 4715);

    date date(year, month, day);

    auto ss = (unsigned)floor(f * (24 * 60 * 60));  // f: [0,1)

    return ptime(date, boost::posix_time::seconds(ss));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Convert to local Gregorian time
 */
local_date_time JulianTime::ldt(const time_zone_ptr& tz) const
{
  try
  {
    /*
     * When constructed this way, Boost takes 'ptime' as UTC.
     */
    return local_date_time(ptime_utc(), tz);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Calculation functions
 *
 * These are made static, 't' passed through since most calculations need it.
 */

/*
 * Julian century since J2000.0
 */
double JulianTime::T() const
{
  try
  {
    return (jd - 2451545.0) / 36525.0;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::GeomMeanAnomalySun(double t)
{
  try
  {
    return 357.52911 + t * (35999.05029 - 0.0001537 * t);  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::SunEqOfCenter(double t)
{
  try
  {
    double mrad = deg2rad(GeomMeanAnomalySun(t));
    double sinm = sin(mrad);
    double sin2m = sin(mrad * 2.0);
    double sin3m = sin(mrad * 3.0);
    return (sinm * (1.914602 - t * (0.004817 + 0.000014 * t)) + sin2m * (0.019993 - 0.000101 * t) +
            sin3m * 0.000289);  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::SunTrueAnomaly(double t)
{
  try
  {
    return GeomMeanAnomalySun(t) + SunEqOfCenter(t);  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::EccentricityEarthOrbit(double t)
{
  try
  {
    return 0.016708634 - t * (0.000042037 + 0.0000001267 * t);  // unitless
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::MeanObliquityOfEcliptic(double t)
{
  try
  {
    double secs = 21.448 - t * (46.8150 + t * (0.00059 - t * (0.001813)));
    return 23.0 + (26.0 + (secs / 60.0)) / 60.0;  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::ObliquityCorrection(double t)
{
  try
  {
    double e0 = MeanObliquityOfEcliptic(t);
    double omega = 125.04 - 1934.136 * t;
    return e0 + 0.00256 * cos_deg(omega);  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Distance to the Sun in AU
 */
double JulianTime::SunRadVector(double t)
{
  try
  {
    double v = SunTrueAnomaly(t);
    double e = EccentricityEarthOrbit(t);
    return (1.000001018 * (1 - e * e)) / (1 + e * cos_deg(v));  // AU
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::GeomMeanLongSun(double t)
{
  try
  {
    double L0 = 280.46646 + t * (36000.76983 + 0.0003032 * t);

    // NOTE: JavaScript has both ends inclusive: [0,360] range
    //
    while (L0 > 360.0)
      L0 -= 360.0;
    while (L0 < 0.0)
      L0 += 360.0;
    return L0;  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::SunTrueLong(double t)
{
  try
  {
    return GeomMeanLongSun(t) + SunEqOfCenter(t);  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double JulianTime::SunApparentLong(double t)
{
  try
  {
    double o = SunTrueLong(t);
    double omega = 125.04 - 1934.136 * t;
    return o - 0.00569 - 0.00478 * sin_deg(omega);  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Right ascension of the Sun
 */
double JulianTime::SunRtAscension(double t)
{
  try
  {
    double e = ObliquityCorrection(t);
    double lambda = SunApparentLong(t);
    double tananum = cos_deg(e) * sin_deg(lambda);
    double tanadenom = cos_deg(lambda);
    return rad2deg(atan2(tananum, tanadenom));  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Declination of the Sun
 */
double JulianTime::SunDeclination() const
{
  try
  {
    double t = T();

    double e = ObliquityCorrection(t);
    double lambda = SunApparentLong(t);
    double sint = sin_deg(e) * sin_deg(lambda);
    return rad2deg(asin(sint));  // degrees
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Difference between true solar time and mean solar time (in minutes of time)
 */
double JulianTime::EquationOfTime() const
{
  try
  {
    double t = T();

    double epsilon = ObliquityCorrection(t);
    double l0 = GeomMeanLongSun(t);
    double e = EccentricityEarthOrbit(t);
    double m = GeomMeanAnomalySun(t);

    double y = tan(deg2rad(epsilon) / 2.0);
    y *= y;

    double sin2l0 = sin_deg(2.0 * l0);
    double cos2l0 = cos_deg(2.0 * l0);
    double sin4l0 = sin_deg(4.0 * l0);
    double sinm = sin_deg(m);
    double sin2m = sin_deg(2.0 * m);

    double et = (y * sin2l0 - 2.0 * e * sinm + 4.0 * e * y * sinm * cos2l0 - 0.5 * y * y * sin4l0 -
                 1.25 * e * e * sin2m);

    return rad2deg(et) * 4.0;  // minutes of time
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

/*
 * Solar noon for the given day at the given location on Earth
 */
JulianTime SolNoon(const local_date_time& ldt, double lon_e)
{
  try
  {
    // Convert date to UTC date (normally they are the same, at local time
    // noon) but +-11..13hr time zones (with summer time) can be on the other
    // day's side.
    //
    // Note: We never hit the uncertain (duplicate/non-existing) cases, since
    //       we're at local noon.
    //

    local_date_time lnoon(ldt.local_time().date(),
                          boost::posix_time::time_duration(12, 0, 0),
                          ldt.zone(),
                          local_date_time::NOT_DATE_TIME_ON_ERROR);

    date date_utc = lnoon.utc_time().date();

    double JD = date_utc.julian_day() - 0.5;

    // First pass uses approximate solar noon to calculate eqtime
    //
    // '.julian_day()' gives us the noon (start of a Julian day),
    // 'lon_e/360.0' adjusts it -12..+12 hours.
    //
    auto jt = JulianTime(JD - lon_e / 360.0);

    // Second round
    //
    double diff = ((-lon_e) * 4.0) - jt.EquationOfTime();  // minutes
    jt = JulianTime(JD + diff / 1440.0);

    // Third round

    diff = ((-lon_e) * 4.0) - jt.EquationOfTime();
    jt = JulianTime(JD + diff / 1440.0);
    jt += 0.5;  // TBR: why is this needed?

    return jt;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

#ifdef _MSC_VER
#define isnan _isnan
#else
using std::isnan;
#endif

/*
 * UTC of sunrise/sunset for the given day at the given location on Earth
 *
 * Returns:  time in minutes from zero Z, "empty" JulianTime if no sunrise/set
 */
JulianTime Sunrise_or_set_UTC(const JulianTime& noon, double lon_e, double lat, bool rise)
{
  try
  {
    // *** First pass to approximate sunrise (using solar noon)
    //
    double eqtime = noon.EquationOfTime();
    double solarDec = noon.SunDeclination();
    double hourAngle = HourAngleSunrise_or_set(lat, solarDec, rise);
    if (isnan(hourAngle))
      return JulianTime();

    double delta = -lon_e - rad2deg(hourAngle);
    double timeDiff = 4 * delta;                   // minutes of time
    double timeUTC = 12 * 60 + timeDiff - eqtime;  // minutes

    // *** Second pass includes fractional day in gamma calc
    //

    /* 'date' has its time part (fractions) taken out; and adjusted to
     * midnight (N - 0.5).
     */

    double tmp = floor(noon.JulianDay() + 0.5) - 0.5;
    if (noon.JulianDay() - (tmp + timeUTC / 1440) > 0.5)
      ++tmp;
    JulianTime date(tmp);

    JulianTime jt(date.JulianDay() + timeUTC / 1440);
    eqtime = jt.EquationOfTime();
    solarDec = jt.SunDeclination();

#if 0
      std::cerr << "NOON: " << noon.ptime_utc() << '\t' << setprecision(16) << noon.JulianDay() << std::endl
          << "DATE: " << date.ptime_utc() << '\t' << setprecision(16) << date.JulianDay() << std::endl
          << "EQ: " << eqtime << std::endl
          << "DELTA: " << delta << std::endl
          << "DIFF: " << timeDiff << std::endl
          << "TIME: " << timeUTC << std::endl
          << "JT  : " << jt.ptime_utc() << std::endl
          << "-------" << std::endl;
#endif

    /* Use the same calculation for both; + for rise, - for set
     */
    hourAngle = HourAngleSunrise_or_set(lat, solarDec, rise);
    if (isnan(hourAngle))
      return JulianTime();

    assert(!isnan(hourAngle));

    delta = -lon_e - rad2deg(hourAngle);
    timeDiff = 4 * delta;
    timeUTC = 720 + timeDiff - eqtime;  // in minutes

    jt = JulianTime(date.JulianDay() + timeUTC / 1440.0);

#if 0
      std::cerr << "DELTA: " << delta << std::endl
          << "DIFF: " << timeDiff << std::endl
          << "TIME: " << timeUTC << std::endl
          << "JT  : " << jt.ptime_utc() << std::endl
          << std::endl;
#endif

    return jt;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Astronomy
}  // namespace Fmi

// ======================================================================


#define _USE_MATH_DEFINES

#include <cmath>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/SparseQR>
#include <hops/Polytope/MaximumVolumeEllipsoid.hpp>
#include <iostream>

template<typename Derived>
std::ostream &operator<<(std::ostream &out, const hops::MaximumVolumeEllipsoid<Derived> &maximumVolumeEllipsoid) {
    out << "Maximum Volume Ellipsoid:" << std::endl
        << maximumVolumeEllipsoid.maximumVolumeEllipsoid << std::endl
        << "center:" << std::endl
        << maximumVolumeEllipsoid.center << std::endl
        << "rounding transformation:" << std::endl
        << maximumVolumeEllipsoid.roundingTransformation << std::endl
        << "current error" << std::endl
        << maximumVolumeEllipsoid.currentError << std::endl;
    return out;
}

template<typename Derived>
void swap(hops::MaximumVolumeEllipsoid<Derived> &first, hops::MaximumVolumeEllipsoid<Derived> &second) {
    std::swap(first.roundingTransformation, second.roundingTransformation);
    std::swap(first.maximumVolumeEllipsoid, second.maximumVolumeEllipsoid);
    std::swap(first.center, second.center);
    std::swap(first.converged, second.converged);
    std::swap(first.iterations, second.iterations);
    std::swap(first.currentError, second.currentError);
    std::swap(first.tolerance, second.tolerance);
}

template<typename RealType>
Eigen::Matrix<RealType, Eigen::Dynamic, 1>
hops::MaximumVolumeEllipsoid<RealType>::applyRoundingTransformation(Eigen::Matrix<RealType, Eigen::Dynamic, 1> &x) {
    return roundingTransformation.template triangularView<Eigen::Lower>() * x;
}

template<typename RealType>
RealType hops::MaximumVolumeEllipsoid<RealType>::computeVolume() const {
    const RealType halfDim = static_cast<RealType>(roundingTransformation.cols() * 0.5);
    return std::pow(M_PI, halfDim) / std::tgamma(halfDim + 1.0) * roundingTransformation.diagonal().prod();
}

template<typename RealType>
const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> &
hops::MaximumVolumeEllipsoid<RealType>::getRoundingTransformation() const {
    return roundingTransformation;
}

template<typename RealType>
const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &hops::MaximumVolumeEllipsoid<RealType>::getCenter() const {
    return center;
}

template<typename RealType>
Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> hops::MaximumVolumeEllipsoid<RealType>::getEllipsoid() const {
    return maximumVolumeEllipsoid;
}

template<typename RealType>
bool hops::MaximumVolumeEllipsoid<RealType>::hasConverged() const {
    return converged;
}

template<typename RealType>
size_t hops::MaximumVolumeEllipsoid<RealType>::getNumberOfIterations() const {
    return iterations;
}

template<typename RealType>
RealType hops::MaximumVolumeEllipsoid<RealType>::getCurrentError() const {
    return currentError;
}

template<typename RealType>
RealType hops::MaximumVolumeEllipsoid<RealType>::getTolerance() const {
    return tolerance;
}

template<typename RealType>
hops::MaximumVolumeEllipsoid<RealType>
hops::MaximumVolumeEllipsoid<RealType>::construct(const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> &Ain,
                                                  const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &bin,
                                                  size_t maximumNumberOfIterationsToRun,
                                                  const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &startingPoint,
                                                  RealType tolerance) {
    if (((Ain * startingPoint - bin).array() > std::numeric_limits<RealType>::epsilon()).any()) {
        std::cerr << "staring point is not in polytope " << std::endl;
        throw std::runtime_error("starting point is not in polytope");
    }
    const RealType minmu = 1e-8;
    const RealType tau0 = 0.75;

    const typename Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>::Index m = Ain.rows();
    const typename Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>::Index n = Ain.cols();
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1> b = Eigen::Matrix<RealType, Eigen::Dynamic, 1>::Ones(m);
    const RealType bnrm = bin.norm();

    const long rank = Ain.colPivHouseholderQr().rank();
    if (rank < n) {
        throw std::runtime_error("Algorithm needs full column rank, because A^T * A has to be invertible");
    }

    const Eigen::Matrix<RealType, Eigen::Dynamic, 1> tempBmAx0 = bin - Ain * startingPoint;
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> bmAx0 = (tempBmAx0).cwiseInverse().asDiagonal();

    Eigen::SparseMatrix<RealType> A = (bmAx0 * Ain).sparseView();
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> x = Eigen::Matrix<RealType, Eigen::Dynamic, 1>::Zero(n);
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> y = Eigen::Matrix<RealType, Eigen::Dynamic, 1>::Ones(m);
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> bmAx = b;

    RealType t = 0.0;
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> z;
    Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> E2;

    RealType res = 1.0;
    MaximumVolumeEllipsoid maximumVolumeEllipsoid;
    maximumVolumeEllipsoid.converged = false;
    maximumVolumeEllipsoid.iterations = 0;
    while (maximumVolumeEllipsoid.iterations < maximumNumberOfIterationsToRun) {
        maximumVolumeEllipsoid.iterations++;

        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> Y = y.asDiagonal();

        E2 = Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>((A.transpose() * y.asDiagonal() * A)).inverse();
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> Q = A * E2 * A.transpose();

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> h = Q.diagonal().cwiseSqrt();

        if (maximumVolumeEllipsoid.iterations == 1) {
            t = (bmAx.cwiseQuotient(h)).minCoeff();
            const RealType t2 = t * t;
            y /= t2;
            h *= t;

            const Eigen::Matrix<RealType, Eigen::Dynamic, 1> temp = bmAx - h;
            z = temp.cwiseMax(1e-1);

            Q = t2 * Q;
            Y /= t2;
        }

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> yz = y.cwiseProduct(z);
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> yh = y.cwiseProduct(h);

        RealType gap = yz.sum() / m;
        RealType rmu = std::min(static_cast<RealType>(0.5), gap) * gap;
        rmu = std::max(rmu, minmu);

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> R1 = -A.transpose() * yh;
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> R2 = bmAx - h - z;

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> R3 = rmu - yz.array();

        const RealType r1 = R1.template lpNorm<Eigen::Infinity>();
        const RealType r2 = R2.template lpNorm<Eigen::Infinity>();
        const RealType r3 = R3.template lpNorm<Eigen::Infinity>();
        res = std::max(r1, std::max(r2, r3));

        if ((res < tolerance * (1 + bnrm)) && (rmu <= minmu)) {
            maximumVolumeEllipsoid.converged = true;
            x += startingPoint;
            maximumVolumeEllipsoid.center = x;
            break;
        }

        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> YQ = Y * Q;
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> YQQY = YQ.cwiseProduct(YQ.transpose());

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> y2h = 2.0 * yh;
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> YA = Y * A;

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> tempProd = y2h.cwiseProduct(z);
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> tempG = tempProd.cwiseMax(1e-12);
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> tempG2 = tempG.asDiagonal();
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> G = YQQY + tempG2;

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> hz = h + z;
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> tempHzYA = hz.asDiagonal() * YA;

        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> T = G.fullPivHouseholderQr().solve(tempHzYA);
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> ATP = (y2h.asDiagonal() * T - YA).transpose();

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> R3Dy = R3.cwiseQuotient(y);
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> R23 = R2 - R3Dy;

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> tempDxR = R1 + ATP * R23;
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> tempDxL = ATP * A;
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> dx = tempDxL.fullPivHouseholderQr().solve(tempDxR);

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> Adx = A * dx;
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> dyDy = G.fullPivHouseholderQr().solve(
                y2h.cwiseProduct(Adx - R23));

        Eigen::Matrix<RealType, Eigen::Dynamic, 1> dy = y.cwiseProduct(dyDy);
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> dz = R3Dy - z.cwiseProduct(dyDy);

        RealType minA = 1.0;
        RealType tempMin = -0.5;
        RealType ax = -1.0 / std::min(tempMin, (-Adx.cwiseProduct(bmAx)).minCoeff());
        if (ax <= minA) {
            minA = ax;
        }
        RealType ay = -1.0 / std::min(tempMin, dyDy.minCoeff());
        if (ay <= minA) {
            minA = ay;
        }
        RealType az = -1.0 / std::min(tempMin, (dz.cwiseQuotient(z)).minCoeff());
        if (az <= minA) {
            minA = az;
        }
        RealType tau = std::max(tau0, 1 - res);
        RealType astep = tau * minA;

        x += astep * dx;
        y += astep * dy;
        z += astep * dz;

        bmAx -= astep * Adx;
    }

    Eigen::LLT<Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>> chol(E2);
    maximumVolumeEllipsoid.roundingTransformation = chol.matrixL();
    maximumVolumeEllipsoid.maximumVolumeEllipsoid = E2;
    return maximumVolumeEllipsoid;
}

template<typename RealType>
hops::MaximumVolumeEllipsoid<RealType> hops::MaximumVolumeEllipsoid<RealType>::construct(
        const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> &A,
        const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &b,
        size_t maximumNumberOfIterationsToRun,
        RealType tolerance) {
    std::unique_ptr<LinearProgram> linearProgram;
    linearProgram = LinearProgramFactory::createLinearProgram(A.template cast<double>(),
                                                              b.template cast<double>());
    Eigen::VectorXd startingPoint = linearProgram->computeChebyshevCenter().optimalParameters;
    return MaximumVolumeEllipsoid<RealType>::construct(A,
                                                       b,
                                                       maximumNumberOfIterationsToRun,
                                                       startingPoint.template cast<RealType>(),
                                                       tolerance);
}

template
class hops::MaximumVolumeEllipsoid<float>;

template
class hops::MaximumVolumeEllipsoid<double>;


#include "erosion.h"
#include "../path.h"

#include <map>

#include <boost/log/trivial.hpp>

namespace WorldEngine
{

enum class Direction
{
   Center,
   North,
   East,
   South,
   West
};

typedef float     WaterFlowDataType;
typedef Direction WaterPathDataType;

typedef boost::multi_array<WaterFlowDataType, 2> WaterFlowArrayType;
typedef boost::multi_array<WaterPathDataType, 2> WaterPathArrayType;

static const std::unordered_map<Direction, Point> directionMap_ = {
   {Direction::Center, {0, 0}},
   {Direction::North, {0, -1}},
   {Direction::East, {1, 0}},
   {Direction::South, {0, 1}},
   {Direction::West, {-1, 0}}};

static const Direction dirNeighbors_[] = {Direction::North, //
                                          Direction::East,
                                          Direction::South,
                                          Direction::West};

static bool wrap_ = true;

static const float RIVER_THRESHOLD = 0.02f;

/**
 * @brief Try to find a lower elevation within a range of an increasing circle's
 * radius, try to find the best path, and return it
 * @param world
 * @param x
 * @param y
 * @return
 */
static std::tuple<bool, bool, Point>
FindLowerElevation(const World& world, int32_t x, int32_t y);

/**
 * @brief Water flows based on cost, seeking the highest elevation difference.
 * Highest positive number is the path of least resistance (lowest point).
 *
 * Cost
 * *** 1,0 ***
 * 0,1 *** 2,1
 * *** 1,2 ***
 *
 * @param world
 * @param x
 * @param y
 * @return
 */
static std::tuple<Direction, int32_t, int32_t>
FindQuickPath(const World& world, int32_t x, int32_t y);

/**
 * @brief Determine whether or not point (x, y) is in a circle with center
 * (centerX, centerY) and a given radius
 * @param radius
 * @param centerX
 * @param centerY
 * @param x
 * @param y
 * @return
 */
static bool InCircle(
   int32_t radius, int32_t centerX, int32_t centerY, int32_t x, int32_t y);

/**
 * @brief Find the flow direction for each cell in heightmap
 * @param world
 * @param waterPath
 */
static void FindWaterFlow(const World& world, WaterPathArrayType& waterPath);

/**
 * @brief Find places on map where sources of river can be found
 * @param world
 * @param waterPath
 * @param waterFlow
 * @return River sources
 */
static std::list<Point> RiverSources(const World&              world,
                                     const WaterPathArrayType& waterPath,
                                     WaterFlowArrayType&       waterFlow);

/**
 * @brief Simulate fluid dynamics by using starting point and flowing to the
 * lowest available point
 * @param world
 * @param source
 * @param riverList
 * @param lakeList
 * @return River flow path
 */
static std::list<Point> RiverFlow(const World&                       world,
                                  Point                              source,
                                  const std::list<std::list<Point>>& riverList,
                                  std::list<Point>&                  lakeList);

/**
 * @brief Validate that for each point in river is equal to or lower than the
 * last
 * @param world
 * @param river
 */
static void CleanUpFlow(World& world, std::list<Point>& river);

/**
 * @brief Simulate erosion in heightmap based on river path.
 * - Current location must be less than or equal to previous location
 * - Riverbed is carved out by % of volume/flow
 * - Sides of river are also eroded to slope into riverbed
 * @param world
 * @param river
 */
static void RiverErosion(World& world, std::list<Point>& river);

/**
 * @brief Update the river map with rainfall that is to become the waterflow
 * @param waterFlow
 * @param precipitations
 * @param river
 * @param riverMap
 */
static void RiverMapUpdate(const WaterFlowArrayType&     waterFlow,
                           const PrecipitationArrayType& precipitations,
                           const std::list<Point>&       river,
                           RiverMapArrayType&            riverMap);

void ErosionSimulation(World& world)
{
   BOOST_LOG_TRIVIAL(info) << "Erosion simulation start";

   uint32_t width  = world.width();
   uint32_t height = world.height();

   const PrecipitationArrayType& precipitations = world.GetPrecipitationData();

   WaterFlowArrayType waterFlow(precipitations);
   WaterPathArrayType waterPath(boost::extents[height][width]);

   RiverMapArrayType& riverMap = world.GetRiverMapData();
   LakeMapArrayType&  lakeMap  = world.GetLakeMapData();

   riverMap.resize(boost::extents[height][width]);
   lakeMap.resize(boost::extents[height][width]);

   std::list<Point> riverSources;

   std::list<std::list<Point>> riverList;
   std::list<Point>            lakeList;

   // Step 1: Water flow per cell based on rainfall
   FindWaterFlow(world, waterPath);

   // Step 2: Find river sources (seeds)
   riverSources = RiverSources(world, waterPath, waterFlow);

   // Step 3: For each source, find a path to sea
   for (Point source : riverSources)
   {
      std::list<Point> river = RiverFlow(world, source, riverList, lakeList);
      if (!river.empty())
      {
         riverList.push_back(river);
         CleanUpFlow(world, river);

         Point riverEnd = river.back();
         if (!world.IsOcean(riverEnd) && lakeList.back() != riverEnd)
         {
            lakeList.push_back(riverEnd);
         }

         BOOST_LOG_TRIVIAL(trace) << "Found river at (" << source.first << ", "
                                  << source.second << ")";
      }
   }

   // Step 4: Simulate erosion and update river map
   for (std::list<Point> river : riverList)
   {
      RiverErosion(world, river);
      RiverMapUpdate(waterFlow, precipitations, river, riverMap);
   }

   // Step 5: Rivers with no paths to sea form lakes
   for (Point lake : lakeList)
   {
      int32_t lx      = lake.first;
      int32_t ly      = lake.second;
      lakeMap[ly][lx] = 0.1f; // TODO: Make this based on rainfall/flow

      BOOST_LOG_TRIVIAL(trace) << "Found lake at (" << lx << ", " << ly << ")";
   }

   BOOST_LOG_TRIVIAL(debug) << "Number of rivers: " << riverList.size();
   BOOST_LOG_TRIVIAL(debug) << "Number of lakes:  " << lakeList.size();

   BOOST_LOG_TRIVIAL(info) << "Erosion simulation finish";
}

static std::tuple<bool, bool, Point>
FindLowerElevation(const World& world, int32_t x, int32_t y)
{
   const ElevationArrayType& e         = world.GetElevationData();
   const int32_t             maxRadius = 40;

   int32_t currentRadius   = 1;
   float   lowestElevation = e[y][x];
   bool    found           = false;
   bool    isWrapped       = false;
   int32_t destX = -1;
   int32_t destY = -1;

   std::vector<Point> wrapped;
   while (!found && currentRadius <= maxRadius)
   {
      for (int32_t cy = -currentRadius; cy <= currentRadius; cy++)
      {
         for (int32_t cx = -currentRadius; cx <= currentRadius; cx++)
         {
            int32_t rx = x + cx;
            int32_t ry = y + cy;

            // Are we within bounds?
            if (!wrap_ && !world.Contains(rx, ry))
            {
               continue;
            }

            // Are we within a circle?
            if (!InCircle(currentRadius, x, y, rx, ry))
            {
               continue;
            }

            rx %= world.width();
            ry %= world.height();

            float elevation = e[ry][rx];

            // Have we found a lower elevation?
            if (elevation < lowestElevation)
            {
               lowestElevation = elevation;
               destX           = rx;
               destY           = ry;
               found           = true;
               if (!world.Contains(x + cx, y + cy))
               {
                  wrapped.push_back({rx, ry});
               }
            }
         }
      }

      currentRadius++;
   }

   if (found && std::find(wrapped.begin(),
                          wrapped.end(),
                          std::make_pair(destX, destY)) != wrapped.end())
   {
      isWrapped = true;
   }

   Point dest(destX, destY);

   return std::tie(found, isWrapped, dest);
}

static std::tuple<Direction, int32_t, int32_t>
FindQuickPath(const World& world, int32_t x, int32_t y)
{
   const ElevationArrayType& elevation = world.GetElevationData();

   float     lowestElevation = elevation[y][x];
   Direction newDirection    = Direction::Center;
   int32_t   newPathX        = 0;
   int32_t   newPathY        = 0;

   for (Direction direction : dirNeighbors_)
   {
      int32_t dx;
      int32_t dy;
      std::tie(dx, dy) = directionMap_.at(direction);

      int32_t tx = x + dx;
      int32_t ty = y + dy;

      if (!wrap_ && !world.Contains(tx, ty))
      {
         continue;
      }

      tx %= world.width();
      ty %= world.height();

      float e = elevation[ty][tx];

      if (e < lowestElevation)
      {
         lowestElevation = e;
         newDirection    = direction;
         newPathX        = tx;
         newPathY        = ty;
      }
   }

   return std::tie(newDirection, newPathX, newPathY);
}

static bool
InCircle(int32_t radius, int32_t centerX, int32_t centerY, int32_t x, int32_t y)
{
   int32_t dx         = centerX - x;
   int32_t dy         = centerY - y;
   int32_t squareDist = (dx * dx + dy * dy);
   return squareDist <= radius * radius;
}

static void FindWaterFlow(const World& world, WaterPathArrayType& waterPath)
{
   for (uint32_t y = 0; y < world.height(); y++)
   {
      for (uint32_t x = 0; x < world.width(); x++)
      {
         std::tie(waterPath[y][x], std::ignore, std::ignore) =
            FindQuickPath(world, x, y);
      }
   }
}

static std::list<Point> RiverSources(const World&              world,
                                     const WaterPathArrayType& waterPath,
                                     WaterFlowArrayType&       waterFlow)
{
   const int32_t width  = world.width();
   const int32_t height = world.height();

   /*
    * Using the wind and rainfall data, create river "seeds" by flowing rainfall
    * along paths until a "flow" threshold is reached and we ahve a beginning of
    * a river.
    * Trickle -> Stream -> River -> Sea
    */
   const PrecipitationArrayType& precipitation = world.GetPrecipitationData();

   std::list<Point> riverSources;

   // Step 1: Using flow direction, follow the path for each cell adding the
   // previous cell's flow to the current cell's flow.

   // Step 2: We loop through the water flow map looking for cells above the
   // water flow threshold.  These are our river sources and we mark them as
   // rivers.  While looking, the cells with no outgoing flow, above water flow
   // threshold are still above sea level and marked as "sources".
   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         if (waterPath[y][x] == Direction::Center)
         {
            // Ignore cells without flow direction
            continue;
         }

         float rainfall = precipitation[y][x];

         // Begin with starting location
         int32_t cx = x;
         int32_t cy = y;

         bool neighborSeedFound = false;

         std::vector<Point> visited;

         // Follow flow path to where it may lead
         while (true)
         {
            // If we have been here before, break out of the loop
            if (std::find(visited.cbegin(), visited.cend(), Point(cx, cy)) !=
                visited.cend())
            {
               break;
            }

            // Mark point as visited
            visited.push_back(Point(cx, cy));

            // Have we found a seed?
            if (world.IsMountain(cx, cy) &&
                waterFlow[cy][cx] >= RIVER_THRESHOLD)
            {
               // Try not to create seeds around other seeds
               for (Point seed : riverSources)
               {
                  int32_t sx = seed.first;
                  int32_t sy = seed.second;

                  if (InCircle(9, cx, cy, sx, sy))
                  {
                     neighborSeedFound = true;
                     break;
                  }
               }

               if (neighborSeedFound)
               {
                  // We do not want seeds for neighbors
                  break;
               }

               riverSources.push_back({cx, cy});
               break;
            }

            // No path means dead end
            if (waterPath[cy][cx] == Direction::Center)
            {
               break;
            }

            // Follow path, add water flow from previous cell
            int32_t dx;
            int32_t dy;
            std::tie(dx, dy) = directionMap_.at(waterPath[y][x]);

            // Calculate next cell
            int32_t nx = cx + dx;
            int32_t ny = cy + dy;

            // Reached the end of the world, no path
            if (!wrap_ && !world.Contains(nx, ny))
            {
               break;
            }

            nx %= world.width();
            ny %= world.height();

            waterFlow[ny][nx] += rainfall;

            // Set current cell to next cell
            cx = nx;
            cy = ny;
         }
      }
   }

   return riverSources;
}

static std::list<Point> RiverFlow(const World&                       world,
                                  Point                              source,
                                  const std::list<std::list<Point>>& riverList,
                                  std::list<Point>&                  lakeList)
{
   Point            currentLocation = source;
   std::list<Point> path;

   path.push_back(source);

   // Start the flow
   while (true)
   {
      int32_t x = currentLocation.first;
      int32_t y = currentLocation.second;

      // If there is a nearby river, flow into it
      for (Direction direction : dirNeighbors_)
      {
         int32_t dx;
         int32_t dy;
         std::tie(dx, dy) = directionMap_.at(direction);

         int32_t ax = x + dx;
         int32_t ay = y + dy;

         if (wrap_)
         {
            ax %= world.width();
            ay %= world.height();
         }

         for (std::list<Point> river : riverList)
         {
            if (std::find(river.begin(), river.end(), std::make_pair(ax, ay)) !=
                river.end())
            {
               bool merge = false;

               for (Point r : river)
               {
                  if (r == std::make_pair(ax, ay) || merge)
                  {
                     merge = true;
                     path.push_back(r);
                  }
               }

               // Skip the rest, return path
               return path;
            }
         }
      }

      // Found at sea?
      if (world.IsOcean(x, y))
      {
         break;
      }

      // Find our immediate lowest elevationand flow there
      Direction quickDirection;
      int32_t   quickX;
      int32_t   quickY;
      std::tie(quickDirection, quickX, quickY) = FindQuickPath(world, x, y);
      if (quickDirection != Direction::Center)
      {
         path.push_back({quickX, quickY});
         currentLocation = {quickX, quickY};

         // Stop here and enter back into loop
         continue;
      }

      bool  foundLowerElevation;
      bool  isWrapped;
      Point lowerElevation;

      std::tie(foundLowerElevation, isWrapped, lowerElevation) =
         FindLowerElevation(world, x, y);
      if (foundLowerElevation && !isWrapped)
      {
         std::list<Point> lowerPath =
            FindPath(world.GetElevationData(), currentLocation, lowerElevation);
         if (!lowerPath.empty())
         {
            path.splice(path.end(), lowerPath);
            currentLocation = path.back();
         }
         else
         {
            break;
         }
      }
      else if (foundLowerElevation && isWrapped)
      {
         const int32_t maxRadius = 40;

         int32_t lx = lowerElevation.first;
         int32_t ly = lowerElevation.second;
         int32_t nx;
         int32_t ny;

         if (!InCircle(maxRadius, x, y, lx, y))
         {
            // Are we wrapping on X axis?
            if (x - lx < 0)
            {
               lx = 0;                 // Move to left edge
               nx = world.width() - 1; // Next step is wrapped around
            }
            else
            {
               lx = world.width() - 1; // Move to right edge
               nx = 0;                 // Next step is wrapped around
            }

            ly = ny = (y + ly) / 2; // Move halfway
         }
         else if (!InCircle(maxRadius, x, y, x, ly))
         {
            // Are we wrapping on Y axis?
            if (y - ly < 0)
            {
               ly = 0;                  // Move to top edge
               ny = world.height() - 1; // Next step is wrapped around;
            }
            else
            {
               ly = world.height() - 1; // Move to bottom edge
               ny = 0;                  // Next step is wrapped around
            }

            lx = nx = (x + lx) / 2; // Move halfway
         }
         else
         {
            BOOST_LOG_TRIVIAL(error)
               << "RiverFlow: Unexpected condition, unable to continue flow";
            break;
         }

         // Find our way to the edge
         std::list<Point> edgePath =
            FindPath(world.GetElevationData(), currentLocation, {lx, ly});
         if (edgePath.empty())
         {
            // Can't find a path, make it a lake
            lakeList.push_back(currentLocation);
            break;
         }
         path.splice(path.end(), edgePath); // Add our newly found path
         path.push_back({nx, ny}); // Finally add our overflow to the other side
         currentLocation = path.back();

         // Find our way to the lowest position originally found
         std::list<Point> lowerPath =
            FindPath(world.GetElevationData(), currentLocation, lowerElevation);
         path.splice(path.end(), edgePath);
         currentLocation = path.back();
      }
      else
      {
         // Can't find any other path, make it a lake
         lakeList.push_back(currentLocation);
         break; // End of river
      }

      if (!world.Contains(x, y))
      {
         BOOST_LOG_TRIVIAL(warning)
            << "RiverFlow: Out of bounds coordinates detected";
      }
   }

   return path;
}

static void CleanUpFlow(World& world, std::list<Point>& river)
{
   ElevationArrayType e = world.GetElevationData();

   float cElevation = 1.0;

   for (Point r : river)
   {
      int32_t rx         = r.first;
      int32_t ry         = r.second;
      float&  rElevation = e[ry][rx];
      if (rElevation <= cElevation)
      {
         cElevation = rElevation;
      }
      else
      {
         rElevation = cElevation;
      }
   }
}

static void RiverErosion(World& world, std::list<Point>& river)
{
   const int32_t radius = 2;

   ElevationArrayType& e = world.GetElevationData();

   // Erosion around river, create river valley
   for (Point r : river)
   {
      int32_t rx = r.first;
      int32_t ry = r.second;

      for (int32_t y = ry - radius; y <= ry + radius; y++)
      {
         for (int32_t x = rx - radius; x <= rx + radius; x++)
         {
            if (!wrap_ && !world.Contains(x, y))
            {
               // Ignore edges of map
               continue;
            }

            int32_t wx = x % world.width();
            int32_t wy = y % world.width();

            if (std::find(river.begin(), river.end(), std::make_pair(wx, wy)) !=
                river.end())
            {
               // Ignore points within the river
               continue;
            }

            if (e[wy][wx] <= e[ry][rx])
            {
               // Ignore points lower than the river
               continue;
            }

            if (!InCircle(radius, rx, ry, wx, wy))
            {
               // Ignore points outside a circle
               continue;
            }

            // Determine curve
            float   curve = 1.0f;
            int32_t adx   = std::abs(rx - wx);
            int32_t ady   = std::abs(ry - wy);
            if (adx == 1 || ady == 1)
            {
               curve = 0.2f;
            }
            else if (adx == 2 || ady == 2)
            {
               curve = 0.05f;
            }

            // Update elevation
            float diff         = e[ry][rx] - e[wy][wx];
            float newElevation = e[wy][wx] + diff * curve;

            if (newElevation < e[ry][rx])
            {
               BOOST_LOG_TRIVIAL(warning)
                  << "RiverErosion: New elevation is lower than river, fixing";
               newElevation = e[ry][rx];
            }

            e[wy][wx] = newElevation;
         }
      }
   }
}

static void RiverMapUpdate(const WaterFlowArrayType&     waterFlow,
                           const PrecipitationArrayType& precipitations,
                           const std::list<Point>&       river,
                           RiverMapArrayType&            riverMap)
{
   bool    isSeed = true;
   int32_t px     = 0;
   int32_t py     = 0;

   for (Point p : river)
   {
      int32_t x = p.first;
      int32_t y = p.second;

      if (isSeed)
      {
         riverMap[y][x] = waterFlow[y][x];
         isSeed         = false;
      }
      else
      {
         riverMap[y][x] = precipitations[y][x] + riverMap[py][px];
      }
      px = x;
      py = y;
   }
}

} // namespace WorldEngine


#include <stdexcept>

#include <Eigen/Geometry>
#include <gtest/gtest.h>

#include "drake/common/find_resource.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/examples/quadrotor/quadrotor_plant.h"
#include "drake/math/rigid_transform.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/constant_vector_source.h"

namespace drake {
namespace examples {
namespace quadrotor {
namespace {

using Eigen::Quaterniond;
using Eigen::VectorXd;
using Eigen::Vector3d;
using math::RigidTransformd;
using math::RollPitchYawd;
using multibody::Body;
using multibody::MultibodyPlant;
using multibody::Parser;
using multibody::SpatialVelocity;
using systems::ConstantVectorSource;
using systems::Context;
using systems::ContinuousState;
using systems::Diagram;
using systems::DiagramBuilder;
using systems::Simulator;

// This test program compares the behaviour of two plants:
//
// (#1) a GenericQuadrotor created from a QuadrotorPlant, and
//
// (#2) a MultibodyQuadrotor created from parsing a corresponding model URDF
// file into a MultibodyPlant.
//
// Both types of plant provide a method SetState whose argument is organized as
// [x, y, z, r, p, y, xdot, ydot, zdot, rdot, pdot, ydot], but the underlying
// State representation in the Context is NOT necessarily organized that way.
//
// Both types also provide GetPose.

// The models are integrated and compared for the duration specified by the
// following constant.
const double kSimulationDuration = 0.1;

// Case (#1): The Diagram that wraps a QuadrotorPlant.
class GenericQuadrotor: public Diagram<double> {
 public:
  GenericQuadrotor() {
    DiagramBuilder<double> builder;
    plant_ = builder.template AddSystem<QuadrotorPlant<double>>();
    auto* source = builder.template AddSystem<ConstantVectorSource<double>>(
        VectorXd::Zero(plant_->num_total_inputs()));
    builder.Cascade(*source, *plant_);
    builder.BuildInto(this);
  }

  // The `x` is a 12-element state vector per the file overview.
  void SetState(Context<double>* context, const VectorXd& x) const {
    DRAKE_DEMAND(x.size() == 12);
    Context<double>& plant_context =
        this->GetMutableSubsystemContext(*plant_, context);
    plant_context.SetContinuousState(x);
  }

  RigidTransformd GetPose(const Context<double>& context) const {
    const VectorXd state =
        context.get_continuous_state().get_vector().CopyToVector();
    const Vector3d xyz = state.segment(0, 3);
    const Vector3d rpy = state.segment(6, 3);
    return RigidTransformd(RollPitchYawd(rpy), xyz);
  }

 private:
  QuadrotorPlant<double>* plant_{};
};

// Case (#2): The Diagram that wraps a MultibodyPlant.
class MultibodyQuadrotor: public Diagram<double> {
 public:
  MultibodyQuadrotor() {
    auto owned_plant = std::make_unique<MultibodyPlant<double>>(0.0);
    plant_ = owned_plant.get();
    Parser(plant_).AddModelFromFile(FindResourceOrThrow(
        "drake/examples/quadrotor/quadrotor.urdf"));
    plant_->Finalize();
    body_ = &plant_->GetBodyByName("base_link");
    DiagramBuilder<double> builder;
    builder.AddSystem(std::move(owned_plant));
    builder.BuildInto(this);
  }

  // The `x` is a 12-element state vector per the file overview.
  void SetState(Context<double>* context, const VectorXd& x) const {
    // Unpack x into its elements as given by the file overview.
    const Vector3d xyz = x.segment(0, 3);
    const Vector3d rpy = x.segment(3, 3);
    const Vector3d xyz_dot = x.segment(6, 3);
    const Vector3d rpy_dot = x.segment(9, 3);

    // Convert the state values into our conventional multibody types.
    const Vector3d p_WB(xyz);
    const RollPitchYawd R_WB(rpy);
    const Vector3d v_WB(xyz_dot);
    const Vector3d w_WB = R_WB.CalcAngularVelocityInParentFromRpyDt(rpy_dot);
    const RigidTransformd X_WB(R_WB, p_WB);
    const SpatialVelocity<double> V_WB(w_WB, v_WB);

    // Set the state of the base_link.
    Context<double>& plant_context =
        this->GetMutableSubsystemContext(*plant_, context);
    plant_->SetFreeBodyPoseInWorldFrame(&plant_context, *body_, X_WB);
    plant_->SetFreeBodySpatialVelocity(&plant_context, *body_, V_WB);
  }

  RigidTransformd GetPose(const Context<double>& context) const {
    const Context<double>& plant_context =
        this->GetSubsystemContext(*plant_, context);
    return plant_->EvalBodyPoseInWorld(plant_context, *body_);
  }

 private:
  MultibodyPlant<double>* plant_{};
  const Body<double>* body_{};
};

void TestPassiveBehavior(const VectorXd& x0) {
  const GenericQuadrotor ge_model;
  const MultibodyQuadrotor mb_model;
  Simulator<double> ge_simulator(ge_model);
  Simulator<double> mb_simulator(mb_model);
  auto& ge_context = ge_simulator.get_mutable_context();
  auto& mb_context = mb_simulator.get_mutable_context();

  ge_model.SetState(&ge_context, x0);
  mb_model.SetState(&mb_context, x0);
  ge_simulator.AdvanceTo(kSimulationDuration);
  mb_simulator.AdvanceTo(kSimulationDuration);

  const RigidTransformd ge_pose = ge_model.GetPose(ge_context);
  const RigidTransformd mb_pose = mb_model.GetPose(mb_context);
  EXPECT_TRUE(CompareMatrices(
        ge_pose.translation(), mb_pose.translation(),
        1e-10, MatrixCompareType::absolute));
  EXPECT_TRUE(ge_pose.rotation().IsNearlyEqualTo(ge_pose.rotation(), 1e-10));
}

// Test comparing the state for of each kind of plant under passive behaviour.
// Each plant is dropped from rest at the origin.
GTEST_TEST(QuadrotorDynamicsTest, DropFromRest) {
  VectorXd x0 = VectorXd::Zero(12);
  TestPassiveBehavior(x0);
}

// Test comparing the state for of each kind of plant under passive behaviour.
// Each plant is dropped from the origin with an initial translational velocity.
GTEST_TEST(QuadrotorDynamicsTest, DropFromInitialVelocity) {
  VectorXd x0 = VectorXd::Zero(12);
  x0.segment(6, 3) << 1, 1, 1;  // Some translational velocity.
  TestPassiveBehavior(x0);
}

// Test comparing the state for of each kind of plant under passive behaviour.
// Each plant is dropped from the origin with an initial rotational velocity.
GTEST_TEST(QuadrotorDynamicsTest, DropFromInitialRotation) {
  VectorXd x0 = VectorXd::Zero(12);
  x0.segment(9, 3) << 1, 1, 1;  // Some rotary velocity.
  TestPassiveBehavior(x0);
}

// Test comparing the state for of each kind of plant under passive behaviour.
// Each plant is dropped from and arbitrary initial state.
GTEST_TEST(QuadrotorDynamicsTest, DropFromArbitraryState) {
  VectorXd x0 = VectorXd::Zero(12);
  x0 << 1, 2, 3, 4, 5, 6, 1, 2, 3, 4, 5, 6;  // Some initial state.
  TestPassiveBehavior(x0);
}

}  // namespace
}  // namespace quadrotor
}  // namespace examples
}  // namespace drake


/*    Copyright (c) 2010-2019, 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
 *      US Standard Atmosphere 1976,
 *          http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770009539_1977009539.pdf.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/unitConversions.h"

#include "tudat/astro/aerodynamics/customConstantTemperatureAtmosphere.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_custom_constant_temperature_atmosphere )

//! Test set- and get-functions of constants.
BOOST_AUTO_TEST_CASE( testCustomConstantTemperatureAtmosphereGetSet )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 288.16;
    std::vector< double > modelSpecificParameters;
    modelSpecificParameters.push_back( 0.0 ); // reference altitude
    modelSpecificParameters.push_back( 1.225 ); // density at reference altitude
    modelSpecificParameters.push_back( 7.050e3 ); // scale height

    // Create a custom atmosphere object.
    aerodynamics::CustomConstantTemperatureAtmosphere customAtmosphere(
                aerodynamics::exponential_atmosphere_model, constantTemperature,
                physical_constants::SPECIFIC_GAS_CONSTANT_AIR, 1.4,
                modelSpecificParameters );

    BOOST_CHECK_EQUAL( constantTemperature, customAtmosphere.getConstantTemperature( ) );
}

//! Check whether the atmosphere is calculated correctly at sea level.
// Values from (US Standard Atmosphere, 1976).
BOOST_AUTO_TEST_CASE( testCustomConstantTemperatureAtmosphereSeaLevel )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 288.16;
    std::vector< double > modelSpecificParameters;
    modelSpecificParameters.push_back( 0.0 ); // reference altitude
    modelSpecificParameters.push_back( 1.225 ); // density at reference altitude
    modelSpecificParameters.push_back( 7.050e3 ); // scale height
    const double altitude = 0.0;

    // Initialize the pressure at zero altitude.
    const double pressureAtZeroAltitude = 101325.0;

    // Create a custom atmosphere object.
    aerodynamics::CustomConstantTemperatureAtmosphere customAtmosphere(
                aerodynamics::exponential_atmosphere_model, constantTemperature,
                physical_constants::SPECIFIC_GAS_CONSTANT_AIR, 1.4,
                modelSpecificParameters );

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    BOOST_CHECK_CLOSE_FRACTION( constantTemperature, customAtmosphere.
                                getTemperature( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( modelSpecificParameters.at( 1 ), customAtmosphere.
                                getDensity( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( pressureAtZeroAltitude, customAtmosphere.
                                getPressure( altitude ), 0.002 * pressureAtZeroAltitude );
}

//! Test custom constant temperature atmosphere at 10 km altitude.
// The given value for pressure was calculated off-line with a calculator.
BOOST_AUTO_TEST_CASE( testCustomConstantTemperatureAtmosphereAt10km )
{
    {
        // Initialize constants that need to be set.
        const double constantTemperature = 288.16;
        std::vector< double > modelSpecificParameters;
        modelSpecificParameters.push_back( 0.0 ); // reference altitude
        modelSpecificParameters.push_back( 1.225 ); // density at reference altitude
        modelSpecificParameters.push_back( 7.050e3 ); // scale height
        const double altitude = 10.0e3;

        // Longitude, latitude and time to check overloading.
        double longitude = 0.0;
        double latitude = 0.0;
        double time = 0.0;

        // Create a custom atmosphere object.
        aerodynamics::CustomConstantTemperatureAtmosphere customAtmosphere(
                    aerodynamics::exponential_atmosphere_model, constantTemperature,
                    physical_constants::SPECIFIC_GAS_CONSTANT_AIR, 1.4,
                    modelSpecificParameters );

        // Declare and set expected density.
        const double expectedDensity  = modelSpecificParameters.at( 1 ) *
                std::exp( - altitude / modelSpecificParameters.at( 2 ) );
        const double expectedPressure = 24526.24934607106;

        // Declare tolerance used for Boost tests.
        const double tolerance = std::numeric_limits< double >::epsilon( );

        BOOST_CHECK_CLOSE_FRACTION( constantTemperature, customAtmosphere.
                                    getTemperature( altitude, longitude, latitude, time ), tolerance );
        BOOST_CHECK_CLOSE_FRACTION( expectedDensity, customAtmosphere.
                                    getDensity( altitude, longitude, latitude, time ), tolerance );
        BOOST_CHECK_CLOSE_FRACTION( expectedPressure, customAtmosphere.
                                    getPressure( altitude, longitude, latitude, time ), tolerance );
    }

    // Test three longitudinal waves atmosphere
    {
        // Initialize constants that need to be set.
        const double constantTemperature = 288.16;
        std::vector< double > modelSpecificParameters;
        modelSpecificParameters.push_back( 0.0 ); // reference altitude
        modelSpecificParameters.push_back( 1.225 ); // density at reference altitude
        modelSpecificParameters.push_back( 7.050e3 ); // scale height
        modelSpecificParameters.push_back( 1.0 ); // uncertainty factor
        modelSpecificParameters.push_back( 0.0 ); // dust storm factor
        const double altitude = 10.0e3;

        // Longitude, latitude and time to check overloading.
        double longitude = 0.0;
        double latitude = 0.0;
        double time = 0.0;

        // Create a custom atmosphere object.
        aerodynamics::CustomConstantTemperatureAtmosphere customAtmosphere(
                    aerodynamics::three_wave_atmosphere_model, constantTemperature,
                    physical_constants::SPECIFIC_GAS_CONSTANT_AIR, 1.4,
                    modelSpecificParameters );

        // Declare and set expected density.
        const double expectedDensity = modelSpecificParameters.at( 0 ) *
                std::exp( - altitude / modelSpecificParameters.at( 2 ) ) *
                ( modelSpecificParameters.at( 3 ) + modelSpecificParameters.at( 4 ) + // atmosphere parameters
                  0.1 * std::sin( 1.0 * longitude ) + // first longitudinal wave
                  0.2 * std::sin( 2.0 * ( longitude -
                                          unit_conversions::convertDegreesToRadians( 50.0 ) ) ) + // second longitudinal wave
                  0.1 * std::sin( 3.0 * ( longitude -
                                          unit_conversions::convertDegreesToRadians( 55.0 ) ) ) ); // third longitudinal wave
        const double expectedPressure = expectedDensity *
                physical_constants::SPECIFIC_GAS_CONSTANT_AIR * constantTemperature;

        // Declare tolerance used for Boost tests.
        const double tolerance = std::numeric_limits< double >::epsilon( );

        BOOST_CHECK_CLOSE_FRACTION( constantTemperature, customAtmosphere.
                                    getTemperature( altitude, longitude, latitude, time ), tolerance );
        BOOST_CHECK_CLOSE_FRACTION( expectedDensity, customAtmosphere.
                                    getDensity( altitude, longitude, latitude, time ), tolerance );
        BOOST_CHECK_CLOSE_FRACTION( expectedPressure, customAtmosphere.
                                    getPressure( altitude, longitude, latitude, time ), tolerance );
    }

    // Test three term atmosphere, where cosine and sine terms are zero (i.e., the model is the
    // conventional exponential atmosphere)
    {
        // Initialize constants that need to be set.
        const double constantTemperature = 288.16;
        std::vector< double > modelSpecificParameters;
        modelSpecificParameters.push_back( 0.0 ); // reference altitude
        modelSpecificParameters.push_back( 1.225 ); // density at reference altitude
        modelSpecificParameters.push_back( 7.050e3 ); // scale height
        modelSpecificParameters.push_back( -1.0 ); // weight for exponential term
        modelSpecificParameters.push_back( 0.0 ); // weight for cosine term
        modelSpecificParameters.push_back( 0.0 ); // weight for cosine term
        const double altitude = 10.0e3;

        // Longitude, latitude and time to check overloading.
        double longitude = 0.0;
        double latitude = 0.0;
        double time = 0.0;

        // Create a custom atmosphere object.
        aerodynamics::CustomConstantTemperatureAtmosphere customAtmosphere(
                    aerodynamics::three_term_atmosphere_model, constantTemperature,
                    physical_constants::SPECIFIC_GAS_CONSTANT_AIR, 1.4,
                    modelSpecificParameters );

        // Declare and set expected density.
        const double expectedDensity = modelSpecificParameters.at( 1 ) *
                std::exp( - altitude / modelSpecificParameters.at( 2 ) );
        const double expectedPressure = 24526.24934607106;

        // Declare tolerance used for Boost tests.
        const double tolerance = std::numeric_limits< double >::epsilon( );

        BOOST_CHECK_CLOSE_FRACTION( constantTemperature, customAtmosphere.
                                    getTemperature( altitude, longitude, latitude, time ), tolerance );
        BOOST_CHECK_CLOSE_FRACTION( expectedDensity, customAtmosphere.
                                    getDensity( altitude, longitude, latitude, time ), tolerance );
        BOOST_CHECK_CLOSE_FRACTION( expectedPressure, customAtmosphere.
                                    getPressure( altitude, longitude, latitude, time ), tolerance );
    }
}

//! Test if the position-independent functions work.
BOOST_AUTO_TEST_CASE( testCustomConstantTemperatureAtmospherePositionIndependentFunctions )
{
    // Initialize constants that need to be set.
    const double constantTemperature = 288.16;
    std::vector< double > modelSpecificParameters;
    modelSpecificParameters.push_back( 0.0 ); // reference altitude
    modelSpecificParameters.push_back( 1.225 ); // density at reference altitude
    modelSpecificParameters.push_back( 7.050e3 ); // scale height
    const double altitude = 10.0e3;

    // Longitude, latitude and time to check overloading.
    double longitude = 0.0;
    double latitude = 0.0;
    double time = 0.0;

    // Create a custom atmosphere object.
    aerodynamics::CustomConstantTemperatureAtmosphere customAtmosphere(
                aerodynamics::exponential_atmosphere_model, constantTemperature,
                physical_constants::SPECIFIC_GAS_CONSTANT_AIR, 1.4,
                modelSpecificParameters );

    const double density1 = customAtmosphere.getDensity( altitude );
    const double density2 = customAtmosphere.getDensity( altitude, longitude, latitude, time );

    const double pressure1 = customAtmosphere.getPressure( altitude );
    const double pressure2 = customAtmosphere.getPressure( altitude, longitude, latitude, time );

    const double temperature1 = customAtmosphere.getTemperature( altitude );
    const double temperature2 = customAtmosphere.getTemperature( altitude, longitude, latitude, time );

    BOOST_CHECK_EQUAL( density1, density2 );
    BOOST_CHECK_EQUAL( pressure1, pressure2 );
    BOOST_CHECK_EQUAL( temperature1, temperature2 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include "spectrumFitter.h"
#include <boost/iterator/iterator_concepts.hpp>

std::vector<TH1F*> globalHisto;

Double_t simulateFunction(Double_t* x, Double_t par[])
{
  Double_t xx = x[0];
  Int_t bin = globalHisto[0]->GetXaxis()->FindBin(xx);

  double br = 0;
  for( unsigned int i = 0; i < globalHisto.size(); i++)
  {
    br+= par[i]*globalHisto[i]->GetBinContent(bin);
  }
  return br;
}

spectrumFitter::spectrumFitter(const elementSpectrum& primarySpectrum)
:fPrimarySpectrum(primarySpectrum)
{
  gStyle->SetOptFit(1111);
}

void spectrumFitter::addNextSpectrum(const elementSpectrum& simSpectrum)
{
  fSpectraToFit.push_back(simSpectrum);
}

void spectrumFitter::fit(std::string out)
{
  for( unsigned int i = 0; i < fSpectraToFit.size(); i++)
    globalHisto.push_back( &fSpectraToFit[i].getHisto() );

  fFnc = new TF1("fFnc", simulateFunction, fStartOfRange, fEndOfRange, fSpectraToFit.size());
  for( unsigned int i = 0; i < fSpectraToFit.size(); i++)
  {
    fFnc->SetParameter(i, 0);
    std::string name = fSpectraToFit[i].getPath();
    int length = name.find_last_of("/") - (name.substr(0, name.find_last_of("/"))).find_last_of("/");
    name= name.substr( (name.substr(0, name.find_last_of("/"))).find_last_of("/")+1 , length-1);
    fFnc->SetParName(i,name.c_str());
//     fFnc->SetParLimits(i,0, 100 );
  }

  fFnc->SetNumberFitPoints(100000);

  TH1F* finalHisto = new TH1F("final", "final", fBins, fStart, fStop);
  TCanvas* c = new TCanvas();
  c->SetWindowSize(500,500);
  fPrimarySpectrum.getHisto().Draw("hist");
  if( fTypeOfFit == chi2 )
    fPrimarySpectrum.getHisto().Fit("fFnc", "0EMRs");
  else if ( fTypeOfFit == WL )
    fPrimarySpectrum.getHisto().Fit("fFnc", "0EMRWL");
  else
  {
    std::cout << "Unknown fit type " <<std::endl;
    std::exit(5);
  }

  std::cout << "Chi square: " << fFnc->GetChisquare()/fFnc->GetNDF() << std::endl;

  for( unsigned int i = 0; i < fSpectraToFit.size(); i++)
  {
    fSpectraToFit[i].getHisto().Scale( fFnc->GetParameter(i));
    fSpectraToFit[i].getHisto().SetLineColor(2+i);
    fSpectraToFit[i].getHisto().SetLineWidth(2);
    fSpectraToFit[i].getHisto().Draw("histsame");
    finalHisto->Add( &fSpectraToFit[i].getHisto());
  }

  finalHisto->SetLineColor(1);
  finalHisto->SetLineWidth(2);
  TLegend legend = buildLegend();
  legend.Draw();
  c->SetLogy();

  TString outName = out;
  c->SaveAs(outName+".png");
  c->SaveAs(outName+".root");

  finalHisto->SetLineColor(1);
  finalHisto->SetLineWidth(2);
  finalHisto->Draw("hist");
  finalHisto->SetMinimum(1e-8);
  fPrimarySpectrum.getHisto().Draw("histsame");
  c->SaveAs("fit.png");
  c->SaveAs("fit.root");

  for( unsigned int i = 0; i < fSpectraToFit.size(); i++)
  {
    fPrimarySpectrum.getHisto().Draw("hist");
    fSpectraToFit[i].getHisto().Draw("histsame");
    TLegend legend = buildLegend();
  legend.Draw();
  c->SetLogy();
    TString name;
    name += fFnc->GetParName(i);
    c->SaveAs(name+".png");
    c->SaveAs(name+".root");
  }

    fPrimarySpectrum.getHisto().Add(finalHisto, -1);
  fPrimarySpectrum.getHisto().Draw("hist");
  fPrimarySpectrum.getHisto().SetTitle("Experimental - fit");
  c->SaveAs("difference.png");
  c->SaveAs("difference.root");

  delete c;
  delete finalHisto;
}

TLegend spectrumFitter::buildLegend()
{
  TLegend leg(0.1,0.65,0.38,0.85);
  leg.AddEntry(&fPrimarySpectrum.getHisto(), "Experimental spectrum", "l");
  for(unsigned int i = 0; i < fSpectraToFit.size(); i++)
  {
    std::string name = fSpectraToFit[i].getPath();
    int length = name.find_last_of("/") - (name.substr(0, name.find_last_of("/"))).find_last_of("/");
    name= name.substr( (name.substr(0, name.find_last_of("/"))).find_last_of("/")+1 , length-1);
    leg.AddEntry(&fSpectraToFit[i].getHisto(), name.c_str(), "l");
  }
  return leg;
}

spectrumFitter::~spectrumFitter()
{

  for(unsigned int i = 0; i < globalHisto.size(); i++)
  {
    globalHisto.pop_back();
  }

  delete fFnc;
}

void spectrumFitter::setRangeOfFit(const double start, const double stop)
{
  fStartOfRange = start;
  fEndOfRange = stop;
}

void spectrumFitter::fitType(typeOfFit type)
{
  fTypeOfFit = type;
}


#include "decision_tree_unifier.h"
#include "config.h"
#include "glog/logging.h"
#include "util.h"
#include <queue>
#include <set>
#include <boost/dynamic_bitset.hpp>
#include <unordered_map>
#include <semantics_factory.h>
std::vector<boost::dynamic_bitset<> > cover;
std::vector<boost::dynamic_bitset<> > pred_val;
std::vector<Program*> terms;
std::vector<PointExample*> pts;
std::vector<Program*> preds;
namespace {

    double calc_entropy(const std::vector<int> &pts_id, const std::vector<int> &relate_term_id)
    {
        const double eps = 1e-7;
        double ans2 = 0;
        std::vector<std::vector<int>> ipts;
        std::unordered_map<int , int> icnt;
        for (int i = 0; i < relate_term_id.size(); ++i)
        {
            int tid = relate_term_id[i];
            std::vector<int> ipt;
            for(auto j : pts_id) if(cover[tid][j]) ipt.push_back(j);
            for(auto j : ipt) icnt[j] += ipt.size();
            ipts.push_back(ipt);
        }
        for (size_t i = 0; i < relate_term_id.size(); ++i)
        {
            size_t tid = relate_term_id[i];
            double prob = 0;
            double top = ipts[i].size();
            for (auto pid : ipts[i])
            {
                double bot = icnt[pid];
                prob += 1 / bot;
            }
            prob *= top;
            if (prob > eps)
            {
                prob /= pts_id.size();
                double v = -prob * log2(prob);
                ans2 += v;
            }
        }
        return ans2;
    }

    int choose_pred(const std::vector<int>& pts_id, const std::vector<int>& preds_id, std::pair<std::vector<int> ,std::vector<int> > &son_pts_id){

        double best_info_gain = 1e9;
        int res_id = -1;

        std::vector<int> relate_term_id;
        for(int i = 0; i < terms.size(); i++){
            int useful = 0;
            for(auto eid : pts_id) if (cover[i][eid]) {
                useful = 1;
                break;
            }
            if(useful) relate_term_id.push_back(i);
        }

        for(auto pid : preds_id){
            std::vector<int> pts_t,pts_f;
            for(auto eid : pts_id) {
                if (pred_val[pid][eid] == 1) pts_t.push_back(eid);else pts_f.push_back(eid);
            }
            double entrpy_true = calc_entropy(pts_t, relate_term_id);
            double entrpy_false = calc_entropy(pts_f, relate_term_id);
            double info_gain = (entrpy_true * pts_t.size() +
                                entrpy_false * pts_f.size()) /pts_id.size();
            if (info_gain < best_info_gain)
            {
                best_info_gain = info_gain;
                res_id = pid;
                son_pts_id = std::make_pair(pts_t, pts_f);
            }
        }
        return res_id;
    }

    Program* DTlearn(const std::vector<int>& pts_id, const std::vector<int>& preds_id){

        boost::dynamic_bitset<> need_cov = boost::dynamic_bitset<>(pts.size());
        for(auto i : pts_id) need_cov[i] = 1;

        for(int i = 0; i < terms.size(); i++)
            if ((need_cov & cover[i]) == need_cov) return terms[i];

        if (preds_id.empty()) return nullptr;

        std::pair<std:: vector<int> , std:: vector<int> > son_pts_id;
        int chosen_pred_id = choose_pred(pts_id, preds_id, son_pts_id);

        if(son_pts_id.first.size() == 0 || son_pts_id.second.size() == 0) return nullptr;

        std::vector<int> son_preds_id = preds_id;
        son_preds_id.erase(remove_if(son_preds_id.begin(), son_preds_id.end(),
                                    [chosen_pred_id](int x){return x == chosen_pred_id;}), son_preds_id.end());

        auto branch_true = DTlearn(son_pts_id.first , son_preds_id);
        if(nullptr == branch_true) return nullptr;
        auto branch_false = DTlearn(son_pts_id.second , son_preds_id);
        if(nullptr == branch_false) return nullptr;

        auto res = new Program({preds[chosen_pred_id], branch_true, branch_false}, semantics::getSemanticsFromName("ite"));
        return res;
    }
}

Program* DecisionTreeUnifier::unify(const std::vector<PointExample*>& example_list,  const std::vector<Program*>& term_list, const std::vector<Program*>& pred_list){
    std::vector<boost::dynamic_bitset<> > cov(term_list.size(),boost::dynamic_bitset<>(example_list.size()));

    for(int i = 0; i < term_list.size(); i++){
        for(int j = 0; j < example_list.size(); j++){
            auto term = term_list[i];
            auto exp = example_list[j];
            cov[i][j] = (term->run(exp->first) == exp->second);
        }
    }

    std::vector<boost::dynamic_bitset<> > pred_value(pred_list.size(),boost::dynamic_bitset<>(example_list.size()));
    for(int i = 0; i < pred_list.size(); i++){
        for(int j = 0; j < example_list.size(); j++){
            auto pred = pred_list[i];
            auto exp = example_list[j];
            pred_value[i][j] = (pred->run(exp->first) == Data(true));
        }

    }
    cover = cov;
    pred_val = pred_value;
    terms = term_list;
    preds = pred_list;
    pts = example_list;
    std::vector<int> pts_id, preds_id;
    for(int i = 0 ; i < example_list.size(); i ++) pts_id.push_back(i);
    for(int i = 0 ; i < pred_list.size(); i ++) preds_id.push_back(i);
    Program* res = DTlearn(pts_id, preds_id);

    return res;
}


#pragma once

#include <stdlib.h>
#include <math.h>

#include "vec.h"
#include "gnilk/engine/FaceHelper.hpp"
#include "gnilk/engine/core/Core.hpp"
#include "gnilk/engine/core/Edge.hpp"

#include <boost/unordered/unordered_map.hpp>

#include <vector>

namespace gnilk
{
	namespace engine
	{
		using namespace gnilk::engine::core;

		static float quadCoords[] = {
								-1.0f, 1.0f, 0.0f,
								1.0f, 1.0f, 0.0f,
								1.0f, -1.0f, 0.0f,
								-1.0f, -1.0f, 0.0f,

		};
		static int quadFaces[] = {
				3,2,1,0,
				-1,-1,-1,-1
		};

		static float cubeCoords[] = {
								-1.0f, 1.0f, 1.0f,
								1.0f, 1.0f, 1.0f,
								1.0f, -1.0f, 1.0f,
								-1.0f, -1.0f, 1.0f,

								-1.0f, 1.0f, -1.0f,
								1.0f, 1.0f, -1.0f,
								1.0f, -1.0f, -1.0f,
								-1.0f, -1.0f, -1.0f
		};

		/*     4---5
		 *    /   /|
		 *   0---1 6
		 *   |   |/
		 *   3---2
		 */
		static int cubeFaces[] = {
				3,2,1,0,
	//			-1,-1,-1,-1,
	//			-1,-1,-1,-1,
				4,5,6,7,
				1,5,4,0,
				2,3,7,6,
				5,1,2,6,
				0,4,7,3,
				-1,-1,-1,-1
		};

		static int extrudeCoordFaces[] = {
				 3,4,5,
				 0,3,5, 0,5,2,
				 0,4,3, 0,1,4,
				 5,4,1, 5,1,2,
				 -1, -1, -1,
				 -1, -1, -1, 1,3,0,
				 -1, -1, -1,
				 2,5,4, 2,4,1,
				 -1, -1, -1,

				// *  1,4,6,3 -> 1,4,6 ; 1,6,3
				// *  2,5,4,1 -> 2,5,4 ; 2,4,1
				// *  3,6,5,2 -> 3,6,5 ; 3,5,2

		};

		class Mesh
		{
		private:
			std::vector<Triangle *> triangles;
			std::vector<Edge *> edges;
			std::vector<Vertex3D> vertices;
			std::vector<Normal> faceNormals;
			std::vector<Normal> vertexNormals;
			std::vector<FaceGroup *> groups;

		public:
			float radius;
			Vertex3D mid;

			Mesh(std::vector<Triangle *> triangles, std::vector<Vertex3D> vertices, bool autoInit=true)
			{
				this->triangles = triangles;
				this->vertices = vertices;
				if (autoInit) {
					Initialize();
				}
			}
			virtual ~Mesh()
			{

			}

			void ClearData() {
				edges.clear();
			}

			// static gnilk::engine::Mesh *pMesh;
			static bool edgeLengthComparer(Edge *a, Edge *b) {

				return a->Length() < b->Length();
			}

			Mesh *Optimize() {

				std::vector<Triangle *> newTriangles;
				std::vector<Vertex3D> newVertices;
				// Triangles to be eliminated in new mesh
				std::vector<int> triDelete;
				std::vector<int> vtxDelete;

				int edgesBefore = edges.size();
				// remove 10%
				int numToDelete = 2000; // (int)(edges.size() * 0.10);
				int numFacesDeleted = 0;

				// TODO: need to recalc edges after this has been done
				std::sort (edges.begin(), edges.end(), edgeLengthComparer);

				int f1 = edges[0]->f1;
				int f2 = edges[0]->f2;
				int v1 = edges[0]->v1;
				int v2 = edges[0]->v2;

				vertices.push_back(edges[0]->Mid());
				int idxMid = vertices.size()-1;

				triDelete.push_back(f1);
				triDelete.push_back(f2);
				//
				// vIni(vertices[v1].data,0,0,0);
				// vIni(vertices[v2].data,0,0,0);

				for(auto tri : triangles) {
					tri->ReplaceVertex(v1, idxMid);
					tri->ReplaceVertex(v2, idxMid);
				}
				// Rebuild new triangle tables
				for(int i=0;i<triangles.size();i++) {
					bool bSkip = false;
					for(int j=0;j<triDelete.size();j++) {
						if (triDelete[j]==i) {
							bSkip = true;
							break;
						}
					}
					if (!bSkip) {
						newTriangles.push_back(triangles[i]);
					}
				}

				// Create the new mesh
				Mesh *newMesh = new Mesh(newTriangles, vertices);
				return newMesh;
			}
			void Initialize()
			{
				ClearData();
				CalculateEdges();
				CalculateFaceNormals();
				CalculateBoundingSphere();
				// AutoGroupFaces();
				// CalculateVertexNormals();

			}

			void CalculateBoundingSphere()
			{
				radius = FaceHelper::CalculateBoundingSphere(mid, vertices);
			}
			void AutoGroupFaces()
			{
				// Clear groups if already present
				if (groups.size() > 0) {
					groups.clear();
				}

				FaceHelper::CalculateFaceGroups(groups, edges, triangles, faceNormals);
			}
			void CalculateEdges()
			{
				FaceHelper::CalculateEdges(edges, triangles, vertices);
			}

			void CalculateVertexNormals()
			{
				FaceHelper::CalculateVertexNormals(vertices.size(), groups, triangles);
			}

			void CalculateFaceNormals()
			{
				//
				// calculate a vertex -> edge list
				// vertex normal is simply an iteration of all faces normals in the edge list
				//
				//FaceHelper::CalculateFaceNormals(faceNormals, triangles, vertices);
				FaceHelper::CalculateFaceNormals(triangles, vertices);
			}

			// Getters
			std::vector<Vertex3D> &GetVertices()
			{
				return this->vertices;
			}

			std::vector<Triangle *> &GetTriangles()
			{
				return this->triangles;
			}

			std::vector<Edge *> &GetEdges()
			{
				return this->edges;
			}

			// std::vector<Normal> &GetFaceNormals()
			// {
			// 	return this->faceNormals;
			// }

			std::vector<FaceGroup *> &GetGroups()
			{
				return this->groups;
			}

			// Stupid function that allows making of "spheres"
			// 1) Create a cube
			// 2) Subdivide X number of times
			// 3) Normalize the last mesh
			void NormalizeCoords() {
				for(int i=0;i<vertices.size();i++) {
					vNorm(vertices[i].data, vertices[i].data);
				}
				CalculateFaceNormals();
				CalculateBoundingSphere();
				AutoGroupFaces();
				CalculateVertexNormals();
			}

			Mesh *SplitFaces() {
				std::vector<Triangle *> newTris;
				std::vector<Vertex3D> newCoords (this->vertices);
				boost::unordered::unordered_map<int, int> edgemidmap;

				for(int i=0;i<edges.size();i++) {
					int idx = FaceHelper::SplitEdge(newCoords, vertices, edges[i]->v1, edges[i]->v2);
					edgemidmap.insert(std::pair<int, int>(edges[i]->HashCode(), idx));
				}

				for(int i=0;i<triangles.size();i++) {
					Triangle *pTri = triangles[i];

					int m1 = edgemidmap[Edge::HashCode(pTri->v[0],pTri->v[1])];
					int m2 = edgemidmap[Edge::HashCode(pTri->v[1],pTri->v[2])];
					int m3 = edgemidmap[Edge::HashCode(pTri->v[2],pTri->v[0])];

					newTris.push_back(new Triangle(pTri->v[0], m1, m3));
					newTris.push_back(new Triangle(pTri->v[1], m2, m1));
					newTris.push_back(new Triangle(pTri->v[2], m3, m2));
					newTris.push_back(new Triangle(m1, m2, m3));
				}
				return new Mesh(newTris, newCoords);
			}

			void Extrude(std::vector<Triangle *> triangles) {

			}

			void Extrude(int triIdx, float factor) {
				// Note:
				//  - Need to base extrusion on edges rather than triangles
				//  - Must avoid edges sharing two faces (no extrusion of such edges)

				Triangle *triangle = this->triangles[triIdx];
				int v1 = triangle->v[0];
				int v2 = triangle->v[1];
				int v3 = triangle->v[2];

				printf("Extruding\n");
				printf("Tri: %d,%d,%d\n",v1,v2,v3);
				printf("Triangles start: %d\n", this->triangles.size());
				printf("Vertices start: %d\n", this->vertices.size());
				printf("Facenormal: %f, %f, %f\n",triangle->normal.data[0],triangle->normal.data[1],triangle->normal.data[2]);
				printf("Factor: %f\n",factor);

				int v1New = this->vertices.size();
				// Duplicate the three vertices
				this->vertices.push_back(this->vertices[v1]);
				this->vertices.push_back(this->vertices[v2]);
				this->vertices.push_back(this->vertices[v3]);

				// Move vertices
				float tmp[3];
				vAdd(this->vertices[v1New+0].data, this->vertices[v1].data, vMul(tmp, triangle->normal.data, factor));
				vAdd(this->vertices[v1New+1].data, this->vertices[v2].data, vMul(tmp, triangle->normal.data, factor));
				vAdd(this->vertices[v1New+2].data, this->vertices[v3].data, vMul(tmp, triangle->normal.data, factor));

				printf("v1 (%d): %f, %f, %f\n",v1, this->vertices[v1].data[0],this->vertices[v1].data[1],this->vertices[v1].data[2]);
				printf("v2 (%d): %f, %f, %f\n",v2, this->vertices[v2].data[0],this->vertices[v2].data[1],this->vertices[v2].data[2]);
				printf("v3 (%d): %f, %f, %f\n",v3, this->vertices[v3].data[0],this->vertices[v3].data[1],this->vertices[v3].data[2]);

				printf("n1 (%d): %f, %f, %f\n",v1New+0, this->vertices[v1New+0].data[0],this->vertices[v1New+0].data[1],this->vertices[v1New+0].data[2]);
				printf("n2 (%d): %f, %f, %f\n",v1New+1, this->vertices[v1New+1].data[0],this->vertices[v1New+1].data[1],this->vertices[v1New+1].data[2]);
				printf("n3 (%d): %f, %f, %f\n",v1New+2, this->vertices[v1New+2].data[0],this->vertices[v1New+2].data[1],this->vertices[v1New+2].data[2]);

				// Construct new faces
				// 1) replace old face with new vertex information
//				triangle->v[0] = v1New+0;
//				triangle->v[1] = v1New+1;
//				triangle->v[2] = v1New+2;

				printf("Added vertices, new size: %d\n",this->vertices.size());
				printf("Ext.CreateFaces: %d, %d\n",v1, v1New);
				int idx = 0;
				int tbl[]={v1,v2,v3};
				while(extrudeCoordFaces[idx]!=-1) {

					Triangle *pNew = new Triangle();
					for(int i=0;i<3;i++) {
						pNew->v[i] = extrudeCoordFaces[idx+i];
						// Add old vertex index on top of relative vertex index
						if (pNew->v[i] >= 3) pNew->v[i]+=v1New-3;
						else {
							pNew->v[i] = tbl[extrudeCoordFaces[idx+i]];
						}
					}
					printf("t: %d,%d,%d\n",pNew->v[0], pNew->v[1], pNew->v[2]);
					this->triangles.push_back(pNew);
					idx+=3;
				}
				/*
				 *  1,4,6,3 -> 1,4,6 ; 1,6,3
				 *  2,4,4,1 -> 2,5,4 ; 2,4,1
				 *  3,6,5,2 -> 3,6,5 ; 3,5,2
				 *
				 *
				 *              1 --------- 4
				 *            / |          / |
				 *         3    |         6  |
				 *           \  |          \ |
				 *              2 ---------  5
				 *
				 */
			}

			static Mesh *CreateQuad() {
				std::vector<Vertex3D> coords;
				std::vector<Triangle *> triangles;

				for (int i=0;i<4;i++) {
					Vertex3D vertex;
					vDup(vertex.data, &quadCoords[i*3]);
					coords.push_back(vertex);

				}
				int i = 0;
				while(quadFaces[i] != -1) {

					Triangle *tri = new Triangle();
					tri->faceData = kFaceData_Vertex;
					tri->v[0] = quadFaces[i+0];
					tri->v[1] = quadFaces[i+1];
					tri->v[2] = quadFaces[i+2];
					triangles.push_back(tri);

					tri = new Triangle();
					tri->faceData = kFaceData_Vertex;
					tri->v[0] = quadFaces[i+0];
					tri->v[1] = quadFaces[i+2];
					tri->v[2] = quadFaces[i+3];

					triangles.push_back(tri);
					i+=4;
				}

				Mesh *pMesh = new Mesh(triangles, coords);
				return pMesh;
			}

			static Mesh *CreateCube() {
				std::vector<Vertex3D> coords;
				std::vector<Triangle *> triangles;
				for(int i=0;i<8;i++) {
					Vertex3D vertex;
					vDup(vertex.data, &cubeCoords[i*3]);
					coords.push_back(vertex);
				}

				int i = 0;
				while(cubeFaces[i] != -1) {

					Triangle *tri = new Triangle();
					tri->faceData = kFaceData_Vertex;
					tri->v[0] = cubeFaces[i+0];
					tri->v[1] = cubeFaces[i+1];
					tri->v[2] = cubeFaces[i+2];
					triangles.push_back(tri);

					tri = new Triangle();
					tri->faceData = kFaceData_Vertex;
					tri->v[0] = cubeFaces[i+0];
					tri->v[1] = cubeFaces[i+2];
					tri->v[2] = cubeFaces[i+3];

					triangles.push_back(tri);
					i+=4;
				}

				Mesh *pMesh = new Mesh(triangles, coords);
				return pMesh;
			}
			static Mesh *CreateSphere(float radius, int segments, int slices) {
				std::vector<Vertex3D> coords;
				std::vector<Triangle *> triangles;

				float mat[4*4];
				int nvps = segments - 1; // vertices per slices
				int nlps = segments * 2 - 1; // lines per slice

				Vertex3D top,bottom;
				vIni(top.data,0,radius,0);
				vIni(bottom.data,0,-radius,0);
				coords.push_back(top);
				coords.push_back(bottom);

				for(int i=0 ; i<slices ; i++)
				{
					vMatIdentity(mat);
					vMatFromAngles(mat, 0,2*M_PI*i/(float)slices,0);
					for(int j=0 ; j<nvps ; j++)
					{
						Vertex3D vertex;
						float v[3];
						v[0] = radius*sin(M_PI*(j+1)/(float)segments);
						v[1] = radius*cos(M_PI*(j+1)/(float)segments);
						v[2] = 0;
						vApplyMat(vertex.data, v, mat);
						coords.push_back(vertex);
					}
				}

				for(int i=0 ; i<slices ; i++)
				{
					Triangle *tri;
					// "vertikala" linjer och polygoner
					for(int j=0 ; j<nvps-1 ; j++)
					{
						tri = new Triangle();
						tri->faceData = kFaceData_Vertex;
						tri->v[0] = i*nvps+j+2+1;
						tri->v[1] = i*nvps+j+2;
						tri->v[2] = ((i+1)%slices)*nvps+j+2;
						triangles.push_back(tri);

						tri = new Triangle();
						tri->faceData = kFaceData_Vertex;
						tri->v[0] = ((i+1)%slices)*nvps+j+2;
						tri->v[1] = ((i+1)%slices)*nvps+j+2+1;
						tri->v[2] = i*nvps+j+2+1;
						triangles.push_back(tri);
					}

					tri = new Triangle();
					tri->faceData = kFaceData_Vertex;
					tri->v[0] = i*nvps+2;
					tri->v[1] = 0;
					tri->v[2] = ((i+1)%slices)*nvps+2;
					triangles.push_back(tri);

					tri = new Triangle();
					tri->faceData = kFaceData_Vertex;
					tri->v[0] = i*nvps+2+nvps-1;
					tri->v[1] = ((i+1)%slices)*nvps+2+nvps-1;
					tri->v[2] = 1;
					triangles.push_back(tri);
				}
				Mesh *pMesh = new Mesh(triangles, coords);
				return pMesh;
			}

		};
	}
}


#include <Eigen/Dense>
#include <string>

#include "TarjansAlgorithm.h"
#include "MLUtil.h"
#include "Clustering.h"
#include "Logger.h"

ConnectedComponentsEstimator::ConnectedComponentsEstimator(std::unique_ptr<ClusterPostProcessing> & cpp_, unsigned knnK_) : ClusterEstimator(cpp_), knnK(knnK_)
{

}

ConnectedComponentsEstimator::~ConnectedComponentsEstimator()
{

}

std::pair<unsigned, std::vector<ClusteringResult>> ConnectedComponentsEstimator::estimateK(const Eigen::MatrixXd & data)
{
	TarjansAlgorithm ta;
    DLOG << "CC: creating affinity matrix" << std::endl;
    Eigen::MatrixXd aff = MLUtil::knnAffinityMatrix(data, knnK, true);
    DLOG << "CC: running Tarjan's algorithm" << std::endl;
    auto res = ta.run(aff);
    DLOG << "CC: running cluster post-processing" << std::endl;
    cpp->run(res);

    std::vector<ClusteringResult> clusterings;
    clusterings.push_back(res);

    return std::make_pair(res.numClusters, clusterings);
}

std::string ConnectedComponentsEstimator::name()
{
	return "ConnectedComponentsEstimator";
}

std::map<std::string, std::string> ConnectedComponentsEstimator::parameters()
{
    std::map<std::string, std::string> params = {{"nn_graph_k", std::to_string(knnK)}};
    return params;
}