// ======================================================================
/*!
 * \file NFmiAzimuthalArea.cpp
 * \brief Implementation of class NFmiAzimuthalArea
 */
// ======================================================================
/*!
 * \class NFmiAzimuthalArea
 *
 * Abstract class for azimuthal projections.
 * Partially based on masterpiece "npolster.cpp" (Originaali 1.  3. 1996/Persa ja Kari)
 *
 * Azimuthal projections are based on refs.
 *
 *	-# Peter Richardus, Ron K.Adler: "Map Projections", North-Holland, 2nd printing 1974.
 *	-# "Navigation Facility Reference Manual" rev. 2.0 3/93
 *	-# "An Album of Map Projections" USGS Paper 1453
 *
 * SOME BASIC AZIMUTHAL PROJECTION CONCEPTS
 *
 * POLAR or NORMAL CASE means the central latitude is 90 degrees North,
 * that is the North Pole
 *
 * TANGENTIAL CASE means the true latitude is 90 degrees North, otherwise we talk
 * about NON-TANGENTIAL or SECANT CASE
 *
 *
 * NOTE:
 * Applying non-polar case (central latitude other than 90 degrees North) is
 * possible but not tested in this program. Applying both non-tangential projection
 * plane AND non-polar case together is NOT ALLOWED in this program! FMI defaults to
 * following values: central latitude 90 deg North, true latitude 60 deg North
 *
 *
 * The y axis lies along the central meridian (lon0), y increasing north;
 * The x axis is perpendicular to the y axis at (lon0,lat0), x increasing east.
 *
 * RECTANGULAR LOCAL COORDINATES are relative unitless cartesian XY-coordinates.
 * The range for these coordinates will be derived directly from the size of the
 * local rectangle used. The upper left corner XY-point and the opposite lower right
 * corner XY-point will define the LOCAL RECTANGLE.
 *
 * RECTANGULAR WORLD COORDINATES are cartesian XY-coordinates measured in meters
 * on "true world" map plane. The upper left corner XY-point and the opposite lower
 * right corner XY-point (in meters) will define the WORLD RECTANGLE.
 *
 * GEODETIC COORDINATES are latitude-longitude coordinates in degrees.
 *
 *
 * \see NFmiStereographicArea, NFmiGnomonicArea and NFmiEquidistArea
 *
 * INPUT PARAMETERS/ARGUMENT LISTS
 *
 * theBottomLeftLatLon = lower left corner point of the rectangle in GEODETIC
 *								 latitude-longitude coordinates
 *
 * theTopRightLatLon =	 upper right corner point of the rectangle in GEODETIC
 *								 latitude-longitude coordinates for
 *the
 *rectangle
 *
 * theCentralLongitude = central longitude (also called central meridian)
 *
 * theTopLeftXY = upper left corner of the rectangle in rectangular LOCAL coordinates
 *
 * theBottomRightXY = lower right corner of the rectangle in rectangular LOCAL coordinates
 *
 *
 * theCenterLatitude = See definition in ref [1]. For example, center latitude
 *					 for the north POLAR stereographic projection is always 90
 *degrees.
 *					 This is also called a "normal" projection case.
 *                   If the central latitude is other than 90 degrees the projection is NOT
 *					 a polar (or normal case of) projection.
 *
 * theTrueLatitude = latitude, where the Earth globe intersects the rectangular world
 *					 XY plane. For true angle 90 degrees North the intersecting
 *XY
 *plane
 *gently
 *					 touches the north pole. For details, see ref [2].
 *
 * theRadialRange = radius of the circle bounded by corner points forming a square.
 *                  Radius is assumed to be given in meters
 *
 */
// ======================================================================

#include "NFmiAzimuthalArea.h"
#include <boost/functional/hash.hpp>
#include <macgyver/Exception.h>
#include <cassert>
#include <cmath>
#include <iostream>

using namespace std;

#include "NFmiVersion.h"

// ----------------------------------------------------------------------
/*!
 * Void constructor
 */
// ----------------------------------------------------------------------

NFmiAzimuthalArea::NFmiAzimuthalArea()
    : itsTopRightLatLon(),
      itsBottomLeftLatLon(),
      itsBottomLeftWorldXY(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect(),
      itsRadialRange(),
      itsCentralLongitude(),
      itsCentralLatitude(),
      itsTrueLatitude(),
      itsTrueLatScaleFactor()
{
}

// ----------------------------------------------------------------------
/*!
 * Copy constructor
 *
 * \param theAzimuthalArea The other area being copied
 */
// ----------------------------------------------------------------------

NFmiAzimuthalArea::NFmiAzimuthalArea(const NFmiAzimuthalArea &theAzimuthalArea)

    = default;

/*!
 * Constructor
 *
 * \param theCenterLatLon Undocumented
 * \param theRadialRangeInMeters Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
NFmiAzimuthalArea::NFmiAzimuthalArea(double theRadialRangeInMeters,
                                     const NFmiPoint &theCenterLatLon,
                                     const NFmiPoint &theTopLeftXY,
                                     const NFmiPoint &theBottomRightXY,
                                     bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(),
      itsBottomLeftWorldXY(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect(),
      itsRadialRange(theRadialRangeInMeters),
      itsCentralLongitude(theCenterLatLon.X(), usePacificView),
      itsCentralLatitude(theCenterLatLon.Y()),
      itsTrueLatitude(90.),
      itsTrueLatScaleFactor(1.0)
{
}

// ----------------------------------------------------------------------
/*!
 * This constructor creates a XY world rectangle based on the given bottom
 * left and top right latlons.
 *
 * \param theBottomLeftLatLon The bottom left corner coordinates
 * \param theTopRightLatLon The top right corner coordinates
 * \param theCentralLongitude The central longitude of the projection
 * \param theTopLeftXY The top left view coordinates
 * \param theBottomRightXY The bottom right view coordinates
 * \param theCentralLatitude The projection center latitude
 * \param theTrueLatitude The tangential plane latitude
 */
// ----------------------------------------------------------------------

NFmiAzimuthalArea::NFmiAzimuthalArea(const NFmiPoint &theBottomLeftLatLon,
                                     const NFmiPoint &theTopRightLatLon,
                                     const double theCentralLongitude,
                                     const NFmiPoint &theTopLeftXY,
                                     const NFmiPoint &theBottomRightXY,
                                     const double theCentralLatitude,
                                     const double theTrueLatitude,
                                     bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsTopRightLatLon(theTopRightLatLon),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsBottomLeftWorldXY(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect(),
      itsRadialRange(0.),
      itsCentralLongitude(theCentralLongitude, usePacificView),
      itsCentralLatitude(theCentralLatitude),
      itsTrueLatitude(theTrueLatitude),
      itsTrueLatScaleFactor(0.0)
{
}

// ----------------------------------------------------------------------
/*!
 * This constructor creates a XY world rectangle based on the given bottom
 * left latlon and the given width and height of the world rectangle
 * in X and Y directions, respectively.
 *
 * \param theBottomLeftLatLon The bottom left corner coordinates
 * \param theCentralLongitude The central longitude of the projection
 * \param theTopLeftXY The top left view coordinates
 * \param theBottomRightXY The bottom right view coordinates
 * \param theCentralLatitude The projection center latitude
 * \param theTrueLatitude The tangential plane latitude
 */
// ----------------------------------------------------------------------

NFmiAzimuthalArea::NFmiAzimuthalArea(const NFmiPoint &theBottomLeftLatLon,
                                     const double theCentralLongitude,
                                     const NFmiPoint &theTopLeftXY,
                                     const NFmiPoint &theBottomRightXY,
                                     const double theCentralLatitude,
                                     const double theTrueLatitude,
                                     bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsBottomLeftWorldXY(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect(),
      itsRadialRange(0.),
      itsCentralLongitude(theCentralLongitude, usePacificView),
      itsCentralLatitude(theCentralLatitude),
      itsTrueLatitude(theTrueLatitude),
      itsTrueLatScaleFactor(0.0)
{
}

// ----------------------------------------------------------------------
/*!
 * This constructor creates a XY world square based on the radial range.
 * This range can be taken as the radius of the circle bounded by corner
 * points forming the square. Radius is assumed to be given in meters
 *
 * \param theRadialRange The redius of the view area in meters.
 * \param theCentralLongitude The central longitude of the projection
 * \param theTopLeftXY The top left view coordinates
 * \param theBottomRightXY The bottom right view coordinates
 * \param theCentralLatitude The projection center latitude
 * \param theTrueLatitude The tangential plane latitude
 */
// ----------------------------------------------------------------------

NFmiAzimuthalArea::NFmiAzimuthalArea(const double theRadialRange,
                                     const double theCentralLongitude,
                                     const NFmiPoint &theTopLeftXY,
                                     const NFmiPoint &theBottomRightXY,
                                     const double theCentralLatitude,
                                     const double theTrueLatitude,
                                     bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsTopRightLatLon(),
      itsBottomLeftLatLon(),
      itsBottomLeftWorldXY(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect(),
      itsRadialRange(theRadialRange),
      itsCentralLongitude(theCentralLongitude, usePacificView),
      itsCentralLatitude(theCentralLatitude),
      itsTrueLatitude(theTrueLatitude),
      itsTrueLatScaleFactor(0.0)
{
}

// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented, unused
 */
// ----------------------------------------------------------------------

void NFmiAzimuthalArea::Init(bool /* fKeepWorldRect */)
{
  try
  {
    itsXScaleFactor = Width() / itsWorldRect.Width();
    itsYScaleFactor = Height() / itsWorldRect.Height();

    itsTrueLatScaleFactor =
        (DistanceFromPerspectivePointToCenterOfEarth() + kRearth * itsTrueLatitude.Sin()) /
        (DistanceFromPerspectivePointToCenterOfEarth() + kRearth);

    itsTopRightLatLon = TopRightLatLon();
    itsBottomLeftLatLon = BottomLeftLatLon();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Returns the geodetic latitude-longitude point corresponding the
 * rectangular LOCAL point theXYPoint. Point will be returned in degrees.
 *
 * \param theXYPoint The local xy-coordinates to be converted
 * \return The respective geodetic coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::ToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xWorld, yWorld;

    // Transform local xy-coordinates into world xy-coordinates (meters).

    xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

    // Transform world xy-coordinates into geodetic coordinates.

    return WorldXYToLatLon(NFmiPoint(xWorld, yWorld));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Returns the world XY coordinates corresponding the
 * rectangular LOCAL point theXYPoint.
 *
 * \param theXYPoint The local xy-coordinates to be converted
 * \return The respective world coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::XYToWorldXY(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

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

// ----------------------------------------------------------------------
/*!
 * Returns the XY coordinates corresponding the
 * rectangular LOCAL point theWorldXYPoint.
 *
 * \param theWorldXYPoint The local world xy-coordinates to be converted
 * \return The respective XY coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::WorldXYToXY(const NFmiPoint &theWorldXYPoint) const
{
  try
  {
    double x = itsXScaleFactor * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - itsYScaleFactor * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Transforms input world xy-coordinates into geodetic coordinates
 * (longitude,latitude) on globe. This proceeds in two steps:
 *
 *  -# transform world xy-coordinates into tangential world xy coordinates
 *  -# transform tangential world xy-coordinates into geodetic coordinates
 *
 * Some basic projection concepts:
 *
 *  - POLAR or NORMAL CASE means the central latitude is 90 degrees North,
 *    that is the North Pole
 *  - TANGENTIAL CASE means the true latitude is 90 degrees North,
 *    otherwise we talk about NON-TANGENTIAL or SECANT CASE
 *
 * \note
 * Applying non-polar case (central latitude other than 90 degrees North)
 * is possible but not tested in this program. Applying both non-tangential
 * projection plane -AND- non-polar case together is NOT ALLOWED in this program!
 * FMI defaults to following values: central latitude 90 deg North,
 * true latitude 60 deg North.
 *
 * \param theXYPoint The world xy-coordinates to be converted
 * \return The geodetic coordinates.
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::WorldXYToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double lat0, lat, dlon, sinlat0, coslat0, coslat, delta;
    double sinDelta, cosDelta, B, cosB, xWorld, yWorld, xWorldTangential, yWorldTangential, xyDist;
    double dlonDeg, lonDeg, latDeg, trueLat, centralLat, centralLon;

    trueLat = itsTrueLatitude.Value();
    centralLat = itsCentralLatitude.Value();
    centralLon = itsCentralLongitude.Value();

    // See the second NOTE above
    if ((trueLat != 90.) && (centralLat != 90.))
      return NFmiPoint(kFloatMissing, kFloatMissing);

    // ----------------------------------------------------------------------------------------
    // STEP 1.
    // Transform world xy-coordinates (defined by itsTrueLatitude) onto the tangential xy-plane
    // ----------------------------------------------------------------------------------------

    DistanceFromPerspectivePointToCenterOfEarth();

    xWorld = theXYPoint.X();
    yWorld = theXYPoint.Y();

    xWorldTangential = 0.0;
    yWorldTangential = 0.0;

    if (trueLat == 90.)
    {
      // TANGENTIAL case

      xWorldTangential = xWorld;
      yWorldTangential = yWorld;
    }
    else
    {
      // NON-TANGENTIAL "secant" case

      // (D + kRearth)*xWorld/(D + kRearth*itsTrueLatitude.Sin());
      if (xWorld != 0.0)
        xWorldTangential = xWorld / itsTrueLatScaleFactor;

      // (D + kRearth)*yWorld/(D + kRearth*itsTrueLatitude.Sin());
      if (yWorld != 0.0)
        yWorldTangential = yWorld / itsTrueLatScaleFactor;
    }

    // ----------------------------------------------------------------------
    // STEP 2.
    // Transform tangential xy world coordinates into geodetic coordinates
    // ----------------------------------------------------------------------

    // XY distance measured between current point and the map center
    xyDist = sqrt(xWorldTangential * xWorldTangential + yWorldTangential * yWorldTangential);

    lat0 = FmiRad(centralLat);  // = Reference latitude = central latitude

    if (xyDist == 0.0)
      return NFmiPoint(centralLon, lat0);

    // Delta angle for the tangential plane depends on the tangential XY distance
    delta = CalcDelta(xyDist);

    if (centralLat == 90.)
    {
      // POLAR (=NORMAL) case

      // Compute absolute latitude

      // Fakta: lat = asin(cosdelta) = FmiRad(90) - delta
      lat = FmiRad(90) - delta;  // = asin(cosDelta);
      latDeg = FmiDeg(lat);

      // Compute longitude difference relative to central longitude

      if (yWorldTangential == 0.)
        dlonDeg = Sign(xWorldTangential) * 90.;
      else
      {
        // Fakta: dlon = asin((sinDelta*cosB)/cos(lat)) = atan(xWorldTangential/yWorldTangential)

        Sign(yWorldTangential) < 0
            ? dlonDeg =
                  Sign(xWorldTangential) * FmiDeg(atan(fabs(xWorldTangential / yWorldTangential)))
            : dlonDeg = Sign(xWorldTangential) *
                        (FmiDeg(atan(fabs(yWorldTangential / xWorldTangential))) + 90.);
      }

      lonDeg = NFmiLongitude(centralLon + dlonDeg, PacificView()).Value();
    }
    else
    {
      // HUOM! HUOM! Tätä "ei-polaaria" tapausta ei ole testattu!

      // NON-POLAR case

      // Azimuthal angle for the XY point ON THE TANGENTIAL MAP PLANE
      B = asin(yWorldTangential / xyDist);
      cosB = cos(B);

      sinlat0 = sin(lat0);
      coslat0 = cos(lat0);
      cosDelta = cos(delta);
      sinDelta = sin(delta);

      // Compute absolute latitude
      lat = asin(coslat0 * sinDelta * sin(B) + sinlat0 * cosDelta);
      coslat = cos(lat);
      latDeg = FmiDeg(lat);

      // Compute longitude difference relative to CentralLongitude
      dlon = asin((sinDelta * cosB) / coslat);
      dlonDeg = FmiDeg(dlon);

      // Quadrant check
      // VOI OLLA ETTEI TÄMÄ TARKASTELU OLE RIITTÄVÄ!
      if (xWorld > 0.0)
        lonDeg = NFmiLongitude(centralLon + dlonDeg, PacificView()).Value();
      else
        lonDeg = NFmiLongitude(centralLon - dlonDeg, PacificView()).Value();
    }

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

// ----------------------------------------------------------------------
/*!
 * Transforms input geodetic coordinates (longitude,latitude) into world coordinates
 * (meters) on xy-plane cutting the Earth globe. This proceeds in two steps:
 *
 *  -# transform geodetic coordinates into TANGENTIAL world xy-coordinates
 *  -# transform tangential world xy-coordinates into final world xy coordinates
 *
 *
 * Some basic projection concepts:
 *
 *  - POLAR or NORMAL CASE means the central latitude is 90 degrees North, that
 *    is the North Pole
 *  - TANGENTIAL CASE means the true latitude is 90 degrees North, otherwise we talk
 *    about NON-TANGENTIAL or SECANT CASE
 *
 * \note
 * Aapplying non-polar case (central latitude other than 90 degrees North) is possible
 * but not tested in this program. Applying both non-tangential projection plane
 * -AND- non-polar case together is NOT ALLOWED in this program!
 *  FMI defaults to following values: central latitude 90 deg North, true
 * latitude 60 deg North.
 *
 * \param theLatLonPoint The geodetic coordinates to be converted
 * \return the world xy-coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::LatLonToWorldXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    double k, lat0, lat, dlon, sinlat0, sinlat, coslat0, coslat, delta;
    double xWorldTangential, yWorldTangential;
    double xWorld, yWorld, trueLat, centralLat, centralLon;

    trueLat = itsTrueLatitude.Value();
    centralLat = itsCentralLatitude.Value();
    centralLon = itsCentralLongitude.Value();

    if ((trueLat != 90.) && (centralLat != 90.))
    {
      // Jos tullaan t�h�n, projektion parametrit eiv�t ole sallittuja, muuta centralLat arvoon
      // 90!!!!!!!
      assert(!((trueLat != 90.) && (centralLat != 90.)));
      return NFmiPoint::gMissingLatlon;
    }

    lat0 = FmiRad(centralLat);
    lat = FmiRad(theLatLonPoint.Y());
    dlon = FmiRad(theLatLonPoint.X() - centralLon);
    sinlat = sin(lat);
    coslat = cos(lat);

    DistanceFromPerspectivePointToCenterOfEarth();

    // ----------------------------------------------------------------------
    // STEP 1.
    // Transform geodetic coordinates into world coordinates on a xy-plane tangential to
    // the surface of Earth globe.
    // ----------------------------------------------------------------------

    if (centralLat == 90.)
    {
      // Polar (=normal) case, that is, lat0 = FmiRad(90 deg).
      // NOTE! This polar case COULD be computed just the way non-polar case is computed
      // by just assigning lat0 = FmiRad(90).
      // Howewer, since sin(lat0) == 1 and cos(lat0) == 0,
      // this is the reduced and somewhat faster way of computing the normal case:

      delta = sinlat;
      k = K(delta);
      if (k == kFloatMissing)
        return NFmiPoint::gMissingLatlon;

      // Fakta:  k*coslat*sin(dlon) = 2*kRearth*tan(0.5*(kPii/2 - lat))*sin(dlon)
      xWorldTangential = k * coslat * sin(dlon);
      // Fakta:  k*(-coslat*cos(dlon)) = -2*kRearth*tan(0.5*(kPii/2 - lat))*cos(dlon)
      yWorldTangential = k * (-coslat * cos(dlon));
    }
    else
    {
      // Non-polar case

      sinlat0 = sin(lat0);
      coslat0 = cos(lat0);

      delta = sinlat0 * sinlat + coslat0 * coslat * cos(dlon);
      k = K(delta);
      if (k == kFloatMissing)
        return NFmiPoint::gMissingLatlon;

      xWorldTangential = k * coslat * sin(dlon);
      yWorldTangential = k * (coslat0 * sinlat - sinlat0 * coslat * cos(dlon));
    }

    // ----------------------------------------------------------------------
    // STEP 2.
    // Transform tangential world xy-coordinates onto the xy-plane cutting Earth
    // globe. This cutting xy-plane is parallel to tangential xy-plane and is
    // defined by itsTrueLatitude
    // ----------------------------------------------------------------------

    xWorld = 0.0;
    yWorld = 0.0;

    if (trueLat == 90.)
    {
      // Cutting plane is already tangential to north pole - nothing to do.
      if (xWorldTangential != 0.0)
        xWorld = xWorldTangential;

      if (yWorldTangential != 0.0)
        yWorld = yWorldTangential;
    }
    else
    {
      // xWorldTangential*(D + kRearth*itsTrueLatitude.Sin())/(D + kRearth)
      if (xWorldTangential != 0.0)
        xWorld = xWorldTangential * itsTrueLatScaleFactor;

      // yWorldTangential*(D + kRearth*itsTrueLatitude.Sin())/(D + kRearth)
      if (yWorldTangential != 0.0)
        yWorld = yWorldTangential * itsTrueLatScaleFactor;
    }

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

// ----------------------------------------------------------------------
/*!
 * Transforms input geodetic coordinates (longitude,latitude) into local
 * (relative) coordinates on xy-plane.
 *
 * \param theLatLonPoint The geodetic coordinates to be converted.
 * \return The converted local xy-coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::ToXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    double xLocal, yLocal;

    // Transform input geodetic coordinates into world coordinates (meters) on xy-plane.
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint xyWorld(LatLonToWorldXY(latlon));

    if (xyWorld == NFmiPoint::gMissingLatlon)
      return xyWorld;

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

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

// ----------------------------------------------------------------------
/*!
 * Turns the input azimuth angle (in degrees) and radius (in meters)
 * into corresponding RELATIVE XY coordinates.
 *
 * \param theAzimuth The azimuth angle in degrees
 * \param theRadius The radius in meters
 * \return The respective relative xy-coordinates.
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::RadialXYPoint(double theAzimuth, double theRadius) const
{
  try
  {
    double startAngle;
    double radianAngle;

    // Start angle defaults to 0 deg. = North
    startAngle = 0.5 * kPii;

    radianAngle = startAngle - kPii * theAzimuth / 180.;

    return NFmiPoint(theRadius * cos(radianAngle), theRadius * sin(radianAngle));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Transforms input azimuth angle and radius into world coordinates on xy-plane.
 *
 * \param theAzimuth The azimuth angle
 * \param theRadius The radius
 * \return The respective world coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::LatLonToWorldXY(double theAzimuth, double theRadius) const
{
  try
  {
    return RadialXYPoint(theAzimuth, theRadius);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Transforms input azimuth angle and radius from world coordinates into
 * local (relative) coordinates on xy-plane.
 *
 * \param theAzimuth The azimuth angle
 * \param theRadius The radius
 * \return The respective local relative xy-coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::ToXY(double theAzimuth, double theRadius) const
{
  try
  {
    double xLocal, yLocal;

    // First, transform azimuth and radius into world coordinates (meters) on xy-plane.
    NFmiPoint xyWorld(LatLonToWorldXY(theAzimuth, theRadius));

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

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

// ----------------------------------------------------------------------
/*!
 * Transforms input azimuth angle (in degrees) and radius (in meters)
 * from world coordinates into geodetic (latitude-longitude) coordinates.
 *
 * \param theAzimuth The azimuth angle in degrees
 * \param theRadius The radius in meters
 * \return The respective geodetic coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::ToLatLon(double theAzimuth, double theRadius) const
{
  try
  {
    return WorldXYToLatLon(RadialXYPoint(theAzimuth, theRadius));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 *  Returns the geodetic center point
 *
 * \return The geodetic center point
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiAzimuthalArea::CurrentCenter() const
{
  try
  {
    return NFmiPoint(itsCentralLongitude.Value(), itsCentralLatitude.Value());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Assignment operator
 *
 * \param theArea The other area being copied
 * \return Referencee to the assigned value.
 * \todo Should protect from self assignment, since NFmiArea::operator=
 *       cannot be trusted here.
 */
// ----------------------------------------------------------------------

NFmiAzimuthalArea &NFmiAzimuthalArea::operator=(const NFmiAzimuthalArea &theArea)
{
  try
  {
    NFmiArea::operator=(theArea);

    itsBottomLeftLatLon = theArea.itsBottomLeftLatLon;
    itsTopRightLatLon = theArea.itsTopRightLatLon;
    itsCentralLongitude.SetValue(theArea.itsCentralLongitude.Value());
    itsCentralLatitude.SetValue(theArea.itsCentralLatitude.Value());
    itsTrueLatitude.SetValue(theArea.itsTrueLatitude.Value());
    itsXScaleFactor = theArea.itsXScaleFactor;
    itsYScaleFactor = theArea.itsYScaleFactor;
    itsBottomLeftWorldXY = theArea.itsBottomLeftWorldXY;
    itsWorldRect = theArea.itsWorldRect;
    itsRadialRange = theArea.itsRadialRange;
    itsTrueLatScaleFactor = theArea.itsTrueLatScaleFactor;

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

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other area being compared to
 * \return True, if the areas are equivalent
 * \todo Investigate whether NFmiArea::operator== should also be called.
 */
// ----------------------------------------------------------------------

bool NFmiAzimuthalArea::operator==(const NFmiAzimuthalArea &theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsCentralLongitude.Value() == theArea.itsCentralLongitude.Value()) &&
        (itsCentralLatitude.Value() == theArea.itsCentralLatitude.Value()) &&
        (itsTrueLatitude.Value() == theArea.itsTrueLatitude.Value()) &&
        (itsXScaleFactor == theArea.itsXScaleFactor) &&
        (itsYScaleFactor == theArea.itsYScaleFactor) &&
        (itsBottomLeftWorldXY == theArea.itsBottomLeftWorldXY) &&
        (itsWorldRect == theArea.itsWorldRect) && (itsRadialRange == theArea.itsRadialRange))
      return true;

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

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The other area being compared to
 * \return True, if the areas are not equivalent
 */
// ----------------------------------------------------------------------

bool NFmiAzimuthalArea::operator!=(const NFmiAzimuthalArea &theArea) const
{
  try
  {
    return (!(*this == theArea));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison with an area derived from NFmiAzimuthalArea.
 *
 * \param theArea The other area being compared to
 * \return True, if the NFmiAzimuthalArea parts are equivalent
 * \todo Use static_cast instead of C-style cast
 */
// ----------------------------------------------------------------------

bool NFmiAzimuthalArea::operator==(const NFmiArea &theArea) const
{
  try
  {
    return *this == static_cast<const NFmiAzimuthalArea &>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison with an area derived from NFmiAzimuthalArea.
 *
 * \param theArea The other area being compared to
 * \return True, if the NFmiAzimuthalArea parts are not equivalent
 */
// ----------------------------------------------------------------------

bool NFmiAzimuthalArea::operator!=(const NFmiArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Write the projection to the given stream
 *
 * \param file The output stream to write to.
 * \return The output stream written to.
 */
// ----------------------------------------------------------------------

std::ostream &NFmiAzimuthalArea::Write(std::ostream &file) const
{
  try
  {
    NFmiArea::Write(file);

    file << itsBottomLeftLatLon << itsTopRightLatLon << itsCentralLongitude.Value() << endl
         << itsCentralLatitude.Value() << endl
         << itsTrueLatitude.Value() << endl;
    int oldPrec = file.precision();
    file.precision(15);

    // We trust everything to be at least version 6 by now
    if (DefaultFmiInfoVersion >= 5)
    {
      file << itsRadialRange << " 0"
           << " 0" << endl;
      file << itsWorldRect << ' ';
    }
    else
      file << itsWorldRect << endl;

    file.precision(oldPrec);

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

// ----------------------------------------------------------------------
/*!
 * Replace the projection with the one in the given input stream.
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream &NFmiAzimuthalArea::Read(std::istream &file)
{
  try
  {
    double centralLatitude, trueLatitude, centralLongitude;

    NFmiArea::Read(file);

    file >> itsBottomLeftLatLon;
    file >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));
    file >> centralLongitude;
    file >> centralLatitude;
    file >> trueLatitude;
    // We trust everything to be at least version 6 by now
    if (DefaultFmiInfoVersion >= 5)
    {
      unsigned long dummy;
      file >> itsRadialRange >> dummy >> dummy;
    }

    itsCentralLongitude.SetValue(centralLongitude);
    itsCentralLatitude.SetValue(centralLatitude);
    itsTrueLatitude.SetValue(trueLatitude);

    file >> itsWorldRect;

    Init();

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

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiAzimuthalArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsBottomLeftWorldXY.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsXScaleFactor));
    boost::hash_combine(hash, boost::hash_value(itsYScaleFactor));
    boost::hash_combine(hash, itsWorldRect.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsRadialRange));
    boost::hash_combine(hash, itsCentralLongitude.HashValue());
    boost::hash_combine(hash, itsCentralLatitude.HashValue());
    boost::hash_combine(hash, itsTrueLatitude.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsTrueLatScaleFactor));

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

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


//==================================================================================================
/**
  Copyright 201 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ERF_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ERF_HPP_INCLUDED

#include <boost/simd/function/std.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/arch/common/detail/generic/erf_kernel.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/signnz.hpp>
#endif

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( erf_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    inline A0 operator() (A0 x) const
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(x)) return x;
      #endif
      A0 y =  bs::abs(x);
      if (y <= Ratio<A0, 15, 32>()) // 0.46875
      {
        return detail::erf_kernel1<A0>::erf1(x, y);
      }
      else if (y <= 4)
      {
        A0 res = detail::erf_kernel1<A0>::erf2(x, y);
        res =    detail::erf_kernel1<A0>::finalize2(res, y);
        res = (Half<A0>() - res) + Half<A0>();
        if (is_ltz(x)) res = -res;
        return res;
      }
      else if  (y <= 26.543)
      {
        A0 res = detail::erf_kernel1<A0>::erf3(x, y);
        res =    detail::erf_kernel1<A0>::finalize2(res, y);
        res = (Half<A0>() - res) + Half<A0>();
        if (is_ltz(x)) res = -res;
        return res;
      }
      else return sign(x);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( erf_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    inline A0 operator()(A0 a0) const
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0)) return a0;
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      if (bs::is_inf(a0)) return signnz(a0);
      #endif

      A0 x =  bs::abs(a0);
      if (x < Ratio<A0, 2, 3>())
      {
        return a0*detail::erf_kernel<A0>::erf1(sqr(x));
      }
      else
      {
        A0 z = x/inc(x)-Ratio<A0, 2, 5>();
        A0 r2 =   oneminus(exp(-sqr(x))*detail::erf_kernel<A0>::erfc2(z));
        if (is_ltz(a0)) r2 = -r2;
        return r2;
      }
   }
  };

  BOOST_DISPATCH_OVERLOAD ( erf_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::erf(a0);
    }
  };
} } }

#endif


//
// Created by Can on 9/2/15.
//

#include <Eigen/Dense>
#include <TeColor/Illuminant.h>

namespace tecolor {

Illuminant::Illuminant(IlluminantName ill)
    : spectra_(new Spectra()) {

  auto num_wavelengths = &detail::ILL_D65_NUM_WAVELENGTHS;
  auto wavelengths = &detail::ILL_D65_WAVELENGTHS;
  auto data = &detail::ILL_D65_SPECTRUM;

  switch (ill) {
    case ILLUMINANT_D50:
      break;
    case ILLUMINANT_D65:
      num_wavelengths = &detail::ILL_D65_NUM_WAVELENGTHS;
      wavelengths = &detail::ILL_D65_WAVELENGTHS;
      data = &detail::ILL_D65_SPECTRUM;
      break;
    case ILLUMINANT_E:
      break;
  }

  EigenMatrix1ColumnInitializer(spectra_->spectra(), *num_wavelengths, data);
  EigenVectorInitializer(spectra_->wavelengths(), *num_wavelengths, wavelengths);
}

const Eigen::VectorXd Illuminant::m() const {
  return spectra_->spectrum_at(0);
}

const Eigen::VectorXd &Illuminant::wavelengths() const {
  return spectra_->wavelengths();
}

}


#ifndef PYTHONIC_INCLUDE_NUMPY_SINH_HPP
#define PYTHONIC_INCLUDE_NUMPY_SINH_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/sinh.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> sinh(std::complex<T> const &val)
    {
      return std::sinh(val);
    }
    template <class T>
    auto sinh(T const &val) -> decltype(boost::simd::sinh(val))
    {
      return boost::simd::sinh(val);
    }
  }
#define NUMPY_NARY_FUNC_NAME sinh
#define NUMPY_NARY_FUNC_SYM wrapper::sinh
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include <boost/test/unit_test.hpp>

#include "defines.hpp"

#include <scorum/utils/extra_high_bit_operations.hpp>

#include "performance_common.hpp"

namespace multiply_by_fractional_tests {

using performance_common::cpu_profiler;

BOOST_AUTO_TEST_SUITE(multiply_by_fractional_tests)

SCORUM_TEST_CASE(diff_with_optimized_check)
{
    const size_t cycles = 100'000;

    size_t case1 = 0u;
    {
        cpu_profiler prof;

        for (size_t ci = 0; ci < cycles; ++ci)
        {
            scorum::utils::multiply_by_fractional(999 + ci, 20 + ci, 100);
        }

        case1 = prof.elapsed();
        BOOST_TEST_MESSAGE("multiply_by_fractional' with division use: " << case1 << "ms");
    }

    size_t case2 = 0u;
    {
        cpu_profiler prof;

        for (size_t ci = 0; ci < cycles; ++ci)
        {
            scorum::utils::multiply_by_fractional(999 + ci, 20 + ci, 1);
        }

        case2 = prof.elapsed();
        BOOST_TEST_MESSAGE("multiply_by_fractional' without division use: " << case2 << "ms");
    }

    BOOST_REQUIRE_LT(case2, case1);
}

BOOST_AUTO_TEST_SUITE_END()
}


#include <gentl/inference/mcmc.h>
#include <gentl/util/randutils.h>
#include <gentl/types.h>

#include <utility>
#include <vector>
#include <stdexcept>
#include <memory>
#include <array>
#include <iostream>

#include <Eigen/Dense>

// TODO support parameters
// TODO support subset of addresses

using gentl::SimulateOptions;
using gentl::GenerateOptions;
using gentl::UpdateOptions;

// ****************************
// *** Model implementation ***
// ****************************

constexpr size_t latent_dimension = 2;
typedef Eigen::Vector<double,latent_dimension> mean_t;
typedef Eigen::Matrix<double,latent_dimension,latent_dimension> cov_t;

// Selection types

class LatentsSelection {};

// Choice buffer types

class EmptyChoiceBuffer {};

typedef Eigen::Array<double,latent_dimension,1> latent_choices_t;

// model change types

struct ModelChange {
    mean_t new_mean;
    cov_t new_cov;
};

// return value change types

class RetvalChange {};

// learnable parameters

class Parameters { };
class GradientAccumulator { };

class Trace;

class Model {
    typedef int return_type;
    friend class Trace;

private:
    typedef Eigen::LLT<Eigen::Matrix<double,latent_dimension,latent_dimension>> Chol;
    mean_t mean_;
    cov_t cov_;
    cov_t precision_;
    Chol chol_;

public:
    template <typename RNGType>
    void exact_sample(latent_choices_t& latents, RNGType& rng) const {
        static std::normal_distribution<double> standard_normal_dist(0.0, 1.0);
        for (auto& x : latents)
            x = standard_normal_dist(rng);
        latents = (mean_ + (chol_.matrixL() * latents.matrix())).array();
    }

    [[nodiscard]] double logpdf(const latent_choices_t& latents) const {
        static double logSqrt2Pi = 0.5*std::log(2*M_PI);
        double quadform = chol_.matrixL().solve(latents.matrix() - mean_).squaredNorm();
        return std::exp(-static_cast<double>(latent_dimension)*logSqrt2Pi - 0.5*quadform) / chol_.matrixL().determinant();
    }

    template <typename RNGType>
    [[nodiscard]] std::pair<double,double> importance_sample(latent_choices_t& latents, RNGType& rng) const {
        exact_sample(latents, rng);
        double log_weight = 0.0;
        return {logpdf(latents), log_weight};
    }

    void logpdf_grad(latent_choices_t& latent_gradient, const latent_choices_t& latents) const {
        // gradient wrt value x is -x
        latent_gradient = (-precision_ * (latents.matrix() - mean_)).array();
    }

public:
    Model(mean_t mean, cov_t cov) :
            mean_{std::move(mean)}, cov_{std::move(cov)}, chol_{cov}, precision_{cov.inverse()} {
        if (chol_.info() != Eigen::Success)
            throw std::logic_error("decomposition failed!");
    }

    // simulate into a new trace object
    template <typename RNGType>
    std::unique_ptr<Trace> simulate(RNGType& rng, Parameters& parameters, const SimulateOptions&) const;

    // simulate into an existing trace object (overwriting existing contents)
    template <typename RNGType>
    void simulate(RNGType& rng, Parameters& parameters, const SimulateOptions&, Trace& trace) const;

    // generate into a new trace object
    template <typename RNGType>
    std::pair<std::unique_ptr<Trace>,double> generate(const EmptyChoiceBuffer& constraints, RNGType& rng,
                                                      Parameters& parameters, const GenerateOptions&) const;

    // generate into an existing trace object (overwriting existing contents)
    template <typename RNGType>
    double generate(Trace& trace, const EmptyChoiceBuffer& constraints,
                    RNGType& rng, Parameters& parameters, const GenerateOptions&) const;

    // equivalent to generate but without returning a trace

    template <typename RNG>
    std::pair<int, double> assess(RNG&, Parameters&, const latent_choices_t& constraints) const;

    template <typename RNG>
    std::pair<int, double> assess(RNG&, Parameters&, const EmptyChoiceBuffer& constraints) const;

};

class Trace {
    friend class Model;
private:
    Model model_;
    double score_;
    latent_choices_t latents_;
    latent_choices_t alternate_latents_;
    latent_choices_t latent_gradient_;
    bool can_be_reverted_;
    bool gradients_computed_;
    RetvalChange diff_;
private:
    // initialize trace without precomputed gradient
    Trace(Model model, double score, latent_choices_t&& latents) :
        model_{std::move(model)}, score_{score}, latents_{latents},
        can_be_reverted_{false}, gradients_computed_{false} {}
    // initialize trace with gradient precomputed
    Trace(Model model, double score, latent_choices_t&& latents, latent_choices_t&& latent_gradient) :
    model_{std::move(model)}, score_{score}, latents_{latents}, latent_gradient_{latent_gradient},
        can_be_reverted_{false}, gradients_computed_{true} {}
public:
    Trace() = delete;
    Trace(const Trace& other) = delete;
    Trace(Trace&& other) = delete;
    Trace& operator=(const Trace& other) = delete;
    Trace& operator=(Trace&& other) noexcept = delete;

    [[nodiscard]] double score() const;
    [[nodiscard]] const latent_choices_t& choices() const;
    [[nodiscard]] const latent_choices_t& choices(const LatentsSelection& selection) const;
    const latent_choices_t& choice_gradient(const LatentsSelection& selection);
    template <typename RNG>
    double update(RNG&, const gentl::change::NoChange&, const latent_choices_t& constraints, const UpdateOptions& options);
    const latent_choices_t& backward_constraints();

    void revert();
};

// ****************************
// *** Model implementation ***
// ****************************

template <typename RNGType>
std::unique_ptr<Trace> Model::simulate(RNGType& rng, Parameters& parameters, const SimulateOptions& options) const {
    latent_choices_t latents;
    exact_sample(latents, rng);
    auto log_density = logpdf(latents);
    if (options.precompute_gradient()) {
        latent_choices_t latent_gradient;
        logpdf_grad(latent_gradient, latents);
        // note: this copies the model
        return std::unique_ptr<Trace>(new Trace(*this, log_density, std::move(latents), std::move(latent_gradient)));
    } else {
        // note: this copies the model
        return std::unique_ptr<Trace>(new Trace(*this, log_density, std::move(latents)));
    }
}

template <typename RNGType>
void Model::simulate(RNGType& rng, Parameters& parameters, const SimulateOptions& options, Trace& trace) const {
    exact_sample(trace.latents_, rng);
    trace.score_ = logpdf(trace.latents_);
    if (options.precompute_gradient()) {
        logpdf_grad(trace.latent_gradient_, trace.latents_);
    }
    trace.gradients_computed_ = options.precompute_gradient();
    trace.can_be_reverted_ = false;
}

template <typename RNGType>
std::pair<std::unique_ptr<Trace>,double> Model::generate(const EmptyChoiceBuffer& constraints, RNGType& rng,
                                                         Parameters& parameters, const GenerateOptions& options) const {
    latent_choices_t latents;
    auto [log_density, log_weight] = importance_sample(latents, rng);
    std::unique_ptr<Trace> trace = nullptr;
    if (options.precompute_gradient()) {
        latent_choices_t latent_gradient;
        logpdf_grad(latent_gradient, latents);
        trace = std::unique_ptr<Trace>(new Trace(*this, log_density, std::move(latents), std::move(latent_gradient)));
    } else {
        trace = std::unique_ptr<Trace>(new Trace(*this, log_density, std::move(latents)));
    }
    return {std::move(trace), log_weight};
}

template <typename RNGType>
double Model::generate(Trace& trace, const EmptyChoiceBuffer& constraints, RNGType& rng, Parameters& parameters,
                       const GenerateOptions& options) const {
    trace.model_ = *this;
    auto [log_density, log_weight] = importance_sample(trace.latents_, rng);
    trace.score_ = log_density;
    double score = logpdf(trace.latents_);
    if (options.precompute_gradient()) {
        logpdf_grad(trace.latent_gradient_, trace.latents_);
    }
    trace.gradients_computed_ = options.precompute_gradient();
    trace.can_be_reverted_ = false;
    return log_weight;
}

template <typename RNG>
std::pair<int, double> Model::assess(RNG&, Parameters& parameters, const latent_choices_t& constraints) const {
    return {-1, logpdf(constraints)};
}

template <typename RNG>
std::pair<int, double> Model::assess(RNG&, Parameters& parameters, const EmptyChoiceBuffer& constraints) const {
    return {-1, 0.0};
}

// ****************************
// *** Trace implementation ***
// ****************************

double Trace::score() const {
    return score_;
}

const latent_choices_t& Trace::choices(const LatentsSelection& selection) const {
    return latents_;
}

const latent_choices_t& Trace::choices() const {
    return latents_;
}

void Trace::revert() {
    if (!can_be_reverted_)
        throw std::logic_error("log_weight is only available between calls to update and revert");
    can_be_reverted_ = false;
    std::swap(latents_, alternate_latents_);
    gradients_computed_ = false;
}

const latent_choices_t& Trace::backward_constraints() {
    return alternate_latents_;
}

const latent_choices_t& Trace::choice_gradient(const LatentsSelection& selection) {
    if (!gradients_computed_) {
        model_.logpdf_grad(latent_gradient_, latents_);
    }
    gradients_computed_ = true;
    return  latent_gradient_;
}

template <typename RNG>
double Trace::update(RNG&, const gentl::change::NoChange&, const latent_choices_t& latents, const UpdateOptions& options) {
    if (options.save()) {
        std::swap(latents_, alternate_latents_);
        latents_ = latents; // copy assignment
        can_be_reverted_ = true;
    } else {
        latents_ = latents; // copy assignment
        // can_be_reverted_ keeps its previous value
    };
    double new_log_density = model_.logpdf(latents_);
    double log_weight = new_log_density - score_;
    score_ = new_log_density;
    if (options.precompute_gradient()) {
        model_.logpdf_grad(latent_gradient_, latents_);
    }
    gradients_computed_ = options.precompute_gradient();
    return log_weight;
}

// TODO add implementation of update that accepts a change to the model

// *********************
// *** Example usage ***
// *********************

int main(int argc, char* argv[]) {
    using std::cout;
    using std::endl;
    using std::cerr;

    // TODO test multiple threads

    // parse arguments

    static const std::string usage = "Usage: ./mcmc"
                                     "<hmc_cycles_per_iter>"
                                     "<mala_cycles_per_iter>"
                                     "<mh_cycles_per_iter>"
                                     "<hmc_leapfrog_steps>"
                                     "<hmc_eps>"
                                     "<mala_tau>"
                                     "<num_threads>"
                                     "<num_iters>"
                                     "<seed>";
    if (argc != 10) {
        throw std::invalid_argument(usage);
    }
    size_t hmc_cycles_per_iter;
    size_t mala_cycles_per_iter;
    size_t mh_cycles_per_iter;
    size_t hmc_leapfrog_steps;
    double hmc_eps;
    double mala_tau;
    size_t num_threads;
    size_t num_iters;
    uint32_t seed;
    try {
        hmc_cycles_per_iter = std::atoi(argv[1]);
        mala_cycles_per_iter = std::atoi(argv[2]);
        mh_cycles_per_iter = std::atoi(argv[3]);
        hmc_leapfrog_steps = std::atoi(argv[4]);
        hmc_eps = std::atof(argv[5]);
        mala_tau = std::atof(argv[6]);
        num_threads = std::atoi(argv[7]);
        num_iters = std::atoi(argv[8]);
        seed = std::atoi(argv[9]);
    } catch (const std::invalid_argument& e) {
        throw std::invalid_argument(usage);
    }
    cerr << "hmc_cycles_per_iter: " << hmc_cycles_per_iter << endl;
    cerr << "mala_cycles_per_iter: " << mala_cycles_per_iter << endl;
    cerr << "mh_cycles_per_iter: " << mh_cycles_per_iter << endl;
    cerr << "hmc_leapfrog_steps: " << hmc_leapfrog_steps << endl;
    cerr << "hmc_eps: " << hmc_eps << endl;
    cerr << "mala_tau: " << mala_tau << endl;
    cerr << "num_threads: " << num_threads << endl;
    cerr << "num_iters: " << num_iters << endl;
    cerr << "seed: " << seed << endl;

    // initialize RNG

    gentl::randutils::seed_seq_fe128 seed_seq {seed};
    std::mt19937 rng(seed_seq);

    // define the model and proposal
    mean_t mean {0.0, 0.0};

    cov_t target_covariance {{1.0, 0.95},
                             {0.95, 1.0}};
    Model model {mean, target_covariance};

    cov_t proposal_covariance {{1.0, 0.0},
                               {0.0, 1.0}};
    Model proposal {mean, proposal_covariance};

    auto make_proposal = [&proposal](const Trace& trace) {
        return proposal;
    };

    // generate initial trace and choice buffers
    Parameters unused {};
    auto [trace, log_weight] = model.generate(EmptyChoiceBuffer{}, rng, unused, GenerateOptions().precompute_gradient(true));
    LatentsSelection hmc_selection;
    LatentsSelection mala_selection;
    auto proposal_trace = make_proposal(*trace).simulate(rng, unused, SimulateOptions().precompute_gradient(false));

    latent_choices_t hmc_momenta_buffer {trace->choices(hmc_selection)}; // copy constructor
    latent_choices_t hmc_values_buffer {trace->choices(hmc_selection)}; // copy constructor
    latent_choices_t mala_buffer_1 {trace->choices(mala_selection)}; // copy constructor
    latent_choices_t mala_buffer_2 {trace->choices(mala_selection)}; // copy constructor

    // do some MALA and HMC on the latent variables (without allocating any memory inside the loop)
    std::vector<mean_t> history(num_iters);
    size_t hmc_num_accepted = 0;
    size_t mala_num_accepted = 0;
    size_t mh_num_accepted = 0;
    for (size_t iter = 0; iter < num_iters; iter++) {
        history[iter] = trace->choices(LatentsSelection{}).matrix();
        for (size_t cycle = 0; cycle < hmc_cycles_per_iter; cycle++) {
            hmc_num_accepted += gentl::mcmc::hmc(
                    *trace, hmc_selection, hmc_leapfrog_steps, hmc_eps,
                    hmc_momenta_buffer, hmc_values_buffer, rng);
        }
        for (size_t cycle = 0; cycle < mala_cycles_per_iter; cycle++) {
            mala_num_accepted += gentl::mcmc::mala(
                    *trace, mala_selection, mala_tau, mala_buffer_1, mala_buffer_2, rng);
        }
        for (size_t cycle = 0; cycle < mh_cycles_per_iter; cycle++) {
            mh_num_accepted += gentl::mcmc::mh(
                    rng, *trace, make_proposal, unused, *proposal_trace, true);
        }
    }

    cerr << "hmc acceptance rate: " << static_cast<double>(hmc_num_accepted) / static_cast<double>(num_iters * hmc_cycles_per_iter) << endl;
    cerr << "mala acceptance rate: " << static_cast<double>(mala_num_accepted) / static_cast<double>(num_iters * mala_cycles_per_iter) << endl;
    cerr << "mh acceptance rate: " << static_cast<double>(mh_num_accepted) / static_cast<double>(num_iters * mh_cycles_per_iter) << endl;

    for (const auto& x : history)
        cout << x(0) << "," << x(1) << endl;

}


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_LDLT_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_LDLT_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/LDLT_factor.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>
#include <type_traits>

namespace stan {
namespace math {

/**
 * Returns the solution of the system Ax=b given an LDLT_factor of A
 * @param A LDLT_factor
 * @param b Right hand side matrix or vector.
 * @return x = b A^-1, solution of the linear system.
 * @throws std::domain_error if rows of b don't match the size of A.
 */

template <int R1, int C1, int R2, int C2, typename T1, typename T2>
inline Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                     R1, C2>
mdivide_left_ldlt(const LDLT_factor<T1, R1, C1> &A,
                  const Eigen::Matrix<T2, R2, C2> &b) {
  check_multiplicable("mdivide_left_ldlt", "A", A, "b", b);

  return A.solve(
      promote_common<Eigen::Matrix<T1, R2, C2>, Eigen::Matrix<T2, R2, C2> >(b));
}

}  // namespace math
}  // namespace stan
#endif


/*
 *  longest_path - main.cpp
 *
 *
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright (c) 2011-2013, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#include "chinese_remainder.hpp"
#include "graph_type.hpp"
#include "parse_graph.hpp"
#include "transfer.hpp"
#include "tree_decomposition/heuristics.hpp"
#include "tree_decomposition/tree_decomposition.hpp"
#include "longest_path.hpp"
#include "utility/gmp.hpp"
#include "utility/polynomial.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/algorithm/equal.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>

#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

/*
 * code to parse user-supplied elimination order
 */

template<class OutputIterator>
void parse_elimination_order(std::string const& s, OutputIterator out)
{
  boost::tokenizer<> tok(s);
  boost::tokenizer<>::iterator i;
  for (auto& c : tok)
    *out++ = boost::lexical_cast<unsigned>(c);
}

/*
 *  validate the ordering by checking if it's a permutation of [0..num_vertices)
 */
template<class Range, class Graph>
bool validate_elimination_order(Range range, Graph const& g)
{
  using namespace boost;
  return boost::equal(sort(range), irange((size_t) 0, num_vertices(g)));
}

/*
 *  The algorithm to run, dependent on the weight type
 */

template<typename T>
using algo = longest_path<polynomial<T>>;

int main (int argc, char *argv[])
{
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()
  ("help,h", "Produce help message")
  ("input-file", po::value<std::string>(), "Read the graph from a file.")
  // tree decomposition options
  ("degree", "Use greedy degree algorithm [default].")
  ("fill-in", "Use greedy fill-in algorithm.")
  ("local-degree", "Use 'local' greedy degree algorithm.")
  ("local-fill-in", "Use 'local' greedy fill-in algorithm.")
  ("elimination-order", po::value<std::string>(), "Specify a vertex elimination order.")
  ("print-tree", "Print tree decomposition.")
  ("tree-only", "Print tree decomposition and exit.")
  // longest_path options
  ("chinese-remainder", "Use the chinese remainder trick.")
  ;

  po::variables_map vm;

  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
  } catch (po::error& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (vm.count("help")) {
    std::cerr << desc << "\n";
    return 0;
  }

  int check = 0;
  check += vm.count("degree");
  check += vm.count("fill-in");
  check += vm.count("local-degree");
  check += vm.count("local-fill-in");
  check += vm.count("elimination-order");

  if (check > 1) {
    std::cerr <<
      "error: please specify at most one between degree, fill-in,"
      "local-degree, local-fill-in and elimination-order\n";
    return 1;
  }

  graph_type g;
  try {
    std::string s;
    if (vm.count("input-file")) {
      auto filename = vm["input-file"].as<std::string>();
      std::ifstream input(filename, std::ios_base::in);
      if (not input.is_open()) {
        std::cerr << "error: file " << filename << " not found\n";
        return 1;
      }
      input >> s;
    } else {
      std::cin >> s;
    }
    g = parse_graph(s);
  } catch (std::exception& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  std::cerr << "Graph with " << num_vertices(g) << " vertices and "
            << num_edges(g) << " edges.\n";

  // check for connectedness
  auto component = boost::make_vector_property_map<int>(
    get(boost::vertex_index, g));
  auto num = connected_components(g, component);
  if (num > 1) {
    std::cerr << "The input graph is not connected. A connected input is required\n";
    return 1;
  }

  std::vector<unsigned int> order(num_vertices(g));

  if (vm.count("fill-in")) {
    heuristics::greedy_fillin_order(g, order.begin());
  } else if (vm.count("local-degree")) {
    heuristics::greedy_local_degree_order(g, order.begin());
  } else if (vm.count("local-fill-in")) {
    heuristics::greedy_local_fillin_order(g, order.begin());
  } else if (vm.count("elimination-order")) {
    // parse the std::string
    std::string s = vm["elimination-order"].as<std::string>();
    parse_elimination_order(s, order.begin());
    if (not validate_elimination_order(order, g)) {
      std::cerr << "error: elimination order not valid\n";
      return 1;
    }
    std::cerr << "Vertex ordering: " << s << "\n";
  } else {
    heuristics::greedy_degree_order(g, order.begin());
  }

  auto td = tree_decomposition::build_tree_decomposition(order, g);

  if (vm.count("print-tree") or vm.count("tree-only")) {
    std::cerr << "Elimination order: ";
    for (auto x : order)
      std::cerr << x << " ";
    std::cerr << "\n";

    std::cerr << "Tree decomposition: " << td << "\n"
              << "Tree decomposition width: "
              << max_bag_size(td) - 1 << "\n";
  }

  if (vm.count("tree-only"))
    return 0;

  if (vm.count("chinese-remainder")) {
    chinese_remainder::chinese_remainder<algo>(td);
  } else {
    using gmp::mpz_int;
    auto result = transfer::transfer(algo<mpz_int>(), td);
    std::cout << result << "\n";
  }
}


/*
 * File:   serialize.hpp
 * Copyright (C) 2014  K M Masum Habib <masum.habib@gmail.com>
 *
 * Created on April 11, 2013, 5:34 PM
 *
 * Serialization for armadillo classes. The serialization is used by
 * the Boost::MPI to transmit and receive objects over the network.
 *
 */

#ifndef SERIALIZE_HPP
#define	SERIALIZE_HPP

#include "maths/svec.h"
#include "maths/arma.hpp"

#include <string>
#include <sstream>
#include <armadillo>
#include <boost/serialization/access.hpp>
#include <boost/serialization/string.hpp>
#include <boost/serialization/split_free.hpp>

#include <boost/serialization/complex.hpp>
#include <boost/serialization/utility.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/map.hpp>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

namespace boost {
namespace serialization {
using maths::armadillo::uint;
using std::stringstream;
using std::string;
using std::pair;
using std::complex;
using arma::Mat;
using arma::Col;
using arma::Row;
using namespace maths::spvec;

/*
 * Serialization for stringstream.
 * =============================================================================
 */

template<class Archive>
void save(Archive& ar, const stringstream& ss, const unsigned int version){
    string s = ss.str();
    ar & s;
};

template<class Archive>
void load(Archive& ar, stringstream& ss, const unsigned int version){
    string s;
    ar & s;
    ss.str(s);
};

template<class Archive>
inline void serialize(Archive& ar, stringstream& ss, const unsigned int file_version){
    split_free(ar, ss, file_version);
};

/*
 * Serialization for svec.
 * =============================================================================
 */

template<class Archive>
void serialize(Archive& ar, svec& sv, const unsigned int version){
    ar & sv(coord::X);
    ar & sv(coord::Y);
    ar & sv(coord::Z);
};

/*
 * Serialization for arma::Mat<T>: Armadillo matrices.
 * V2: Using raw data.
 * =============================================================================
 */
template<class Archive, class T>
void save(Archive& ar, const Mat<T>& mat, const unsigned int version){
    uint n_rows = mat.n_rows;
    uint n_cols = mat.n_cols;

     ar << n_rows;
     ar << n_cols;
    for(uint ir = 0; ir < n_rows; ++ ir){
        for(uint ic = 0; ic < n_cols; ++ ic){
            ar << mat(ir, ic);
        }
    }
};

template<class Archive, class T>
void load(Archive& ar, Mat<T>& mat, const unsigned int version){
    uint n_rows;
    uint n_cols;

    ar >> n_rows;
    ar >> n_cols;

    if (n_rows != mat.n_rows || n_cols != mat.n_cols){
        mat.set_size(n_rows, n_cols);
    }

    for(int ir = 0; ir < n_rows; ++ ir){
        for(int ic = 0; ic < n_cols; ++ ic){
            ar >> mat(ir, ic);
        }
    }
};

template<class Archive, class T>
inline void serialize(Archive& ar, Mat<T>& m, const unsigned int file_version){
    split_free(ar, m, file_version);
};

/*
 * Serialization for arma::Col<T>: Armadillo column vector.
 * V2: using raw data.
 * =============================================================================
 */
template<class Archive, class T>
void save(Archive& ar, const Col<T>& col, const unsigned int version){
    int n_rows = col.n_rows;
    ar << n_rows;
    for(int it = 0; it < col.n_rows; ++ it){
        ar << col(it);
    }
};

template<class Archive, class T>
void load(Archive& ar, Col<T>& col, const unsigned int version){
    uint n_rows;
    ar >> n_rows;
    if (n_rows != col.n_rows){
        col.set_size(n_rows);
    }
    for(uint it = 0; it < n_rows; ++ it){
        ar >> col(it);
    }
};

template<class Archive, class T>
inline void serialize(Archive& ar, Col<T>& col, const unsigned int file_version){
    split_free(ar, col, file_version);
};

/*
 * Serialization for arma::Row<T>: Armadillo row vector.
 * V2: using raw data.
 * =============================================================================
 */
template<class Archive, class T>
void save(Archive& ar, const Row<T>& row, const unsigned int version){
    uint n_cols = row.n_cols;
    ar << n_cols;
    for(uint it = 0; it < n_cols; ++ it){
        ar << row(it);
    }
};

template<class Archive, class T>
void load(Archive& ar, Row<T>& row, const unsigned int version){
    uint n_cols;
    ar >> n_cols;
    if (n_cols != row.n_cols){
        row.set_size(n_cols);
    }
    for(uint it = 0; it < n_cols; ++ it){
        ar >> row(it);
    }
};

template<class Archive, class T>
inline void serialize(Archive& ar, Row<T>& row, const unsigned int file_version){
    split_free(ar, row, file_version);
};

}}

#endif	/* SERIALIZE_HPP */


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * testDiscreteBayesNet.cpp
 *
 *  @date Feb 27, 2011
 *  @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/map.hpp>
using namespace boost::assign;

#include <iostream>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST(DiscreteBayesNet, Asia)
{
  DiscreteBayesNet asia;
//  DiscreteKey A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), B(
//      "Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea");
  DiscreteKey A(0,2), S(4,2), T(3,2), L(6,2), B(7,2), E(5,2), X(2,2), D(1,2);

  // TODO: make a version that doesn't use the parser
  add_front(asia, A % "99/1");
  add_front(asia, S % "50/50");

  add_front(asia, T | A = "99/1 95/5");
  add_front(asia, L | S = "99/1 90/10");
  add_front(asia, B | S = "70/30 40/60");

  add_front(asia, (E | T, L) = "F T T T");

  add_front(asia, X | E = "95/5 2/98");
  // next lines are same as add_front(asia, (D | E, B) = "9/1 2/8 3/7 1/9");
  DiscreteConditional::shared_ptr actual =
      boost::make_shared<DiscreteConditional>((D | E, B) = "9/1 2/8 3/7 1/9");
  asia.push_front(actual);
  //  GTSAM_PRINT(asia);

  // Convert to factor graph
  DiscreteFactorGraph fg(asia);
  //  GTSAM_PRINT(fg);
  LONGS_EQUAL(3,fg.front()->size());
  Potentials::ADT expected(B & D & E, "0.9 0.3 0.1 0.7 0.2 0.1 0.8 0.9");
  CHECK(assert_equal(expected,(Potentials::ADT)*actual));

  // Create solver and eliminate
  DiscreteSequentialSolver solver(fg);
  DiscreteBayesNet::shared_ptr chordal = solver.eliminate();
  //  GTSAM_PRINT(*chordal);
  DiscreteConditional expected2(B % "11/9");
  CHECK(assert_equal(expected2,*chordal->back()));

  // solve
  DiscreteFactor::sharedValues actualMPE = optimize(*chordal);
  DiscreteFactor::Values expectedMPE;
  insert(expectedMPE)(A.first, 0)(D.first, 0)(X.first, 0)(T.first, 0)(S.first,
      0)(E.first, 0)(L.first, 0)(B.first, 0);
  EXPECT(assert_equal(expectedMPE, *actualMPE));

  // add evidence, we were in Asia and we have Dispnoea
  fg.add(A, "0 1");
  fg.add(D, "0 1");
//  fg.product().dot("fg");

  // solve again, now with evidence
  DiscreteSequentialSolver solver2(fg);
  DiscreteBayesNet::shared_ptr chordal2 = solver2.eliminate();
//  GTSAM_PRINT(*chordal2);
  DiscreteFactor::sharedValues actualMPE2 = optimize(*chordal2);
  DiscreteFactor::Values expectedMPE2;
  insert(expectedMPE2)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(S.first,
      1)(E.first, 0)(L.first, 0)(B.first, 1);
  EXPECT(assert_equal(expectedMPE2, *actualMPE2));

  // now sample from it
  DiscreteFactor::Values expectedSample;
  SETDEBUG("DiscreteConditional::sample", false);
  insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(
      S.first, 1)(E.first, 0)(L.first, 0)(B.first, 1);
  DiscreteFactor::sharedValues actualSample = sample(*chordal2);
  EXPECT(assert_equal(expectedSample, *actualSample));
}

/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesNet, Sugar)
{
  DiscreteKey T(0,2), L(1,2), E(2,2), D(3,2), C(8,3), S(7,2);

  DiscreteBayesNet bn;

  // test some mistakes
  //  add(bn, D);
  //  add(bn, D | E);
  //  add(bn, D | E = "blah");

  // try logic
  add(bn, (E | T, L) = "OR");
  add(bn, (E | T, L) = "AND");

  //  // try multivalued
  add(bn, C % "1/1/2");
  add(bn, C | S = "1/1/2 5/2/3");
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================

#include <boost/simd/function/dec.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

template <typename T, std::size_t N, typename Env>
void pre_test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i+1) : T(2+i);
    b[i] = a1[i] - T(1);
  }

  p_t aa1(&a1[0], &a1[0]+N), aa2;
  p_t bb(&b[0], &b[0]+N);

  aa2 = --aa1;
  STF_EQUAL(aa2, bb);
  STF_EQUAL(aa1, bb);
}

STF_CASE_TPL("Check pre-decrement on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  static const std::size_t N = bs::pack<T>::static_size;

  pre_test<T, N>(runtime);
  pre_test<T, N/2>(runtime);
  pre_test<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void post_test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(1+i) : T(2+i);
    b[i] = a1[i] - T(1);
  }

  p_t aa1(&a1[0], &a1[0]+N), prev(aa1), aa2;
  p_t bb(&b[0], &b[0]+N);

  aa2 = aa1--;
  STF_EQUAL(aa2, prev);
  STF_EQUAL(aa1, bb);
}

STF_CASE_TPL (" dec signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::dec;
  using p_t = bs::pack<T>;

  // return type conformity test
  STF_EXPR_IS(dec(p_t()), p_t);

  // specific values tests
  STF_EQUAL(dec(bs::Mone<p_t>()), p_t(-2));
  STF_EQUAL(dec(bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(dec(bs::Zero<p_t>()), bs::Mone<p_t>());
}

STF_CASE_TPL (" decunsigned_uint",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::dec;
  using p_t = bs::pack<T>;

  // return type conformity test
  STF_EXPR_IS(dec(p_t()), p_t);

  // specific values tests
  STF_EQUAL(dec(bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(dec(p_t(2)), bs::One<p_t>());
  STF_EQUAL(dec(bs::Zero<p_t>()), bs::Valmax<p_t>());
}

STF_CASE_TPL(" dec floating", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::dec;
  using p_t = bs::pack<T>;

  // return type conformity test
  STF_EXPR_IS(dec(p_t()), p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(dec(bs::Inf<p_t>()), bs::Inf<p_t>());
  STF_IEEE_EQUAL(dec(bs::Nan<p_t>()), bs::Nan<p_t>());
  STF_EQUAL(dec(bs::Minf<p_t>()), bs::Minf<p_t>());
#endif
  STF_EQUAL(dec(bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(dec(p_t(2)), bs::One<p_t>());
  STF_EQUAL(dec(bs::Zero<p_t>()), bs::Mone<p_t>());
}


#include <catch2/catch.hpp>

#include <Eigen/Geometry>

#include <tbb/parallel_sort.h>
#include <tbb/global_control.h>
#include <tbb/task_scheduler_init.h>

#include <igl/predicates/segment_segment_intersect.h>

#include <ipc/ipc.hpp>
#include <ipc/broad_phase/brute_force.hpp>
#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/utils/intersection.hpp>

#include "test_utils.hpp"

using namespace ipc;

bool combine_meshes(
    const std::string& mesh1_name,
    const std::string& mesh2_name,
    const Eigen::Matrix3d& R1,
    const Eigen::Matrix3d& R2,
    int dim,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& E,
    Eigen::MatrixXi& F)
{
    Eigen::MatrixXd V1, V2;
    Eigen::MatrixXi E1, E2, F1, F2;

    bool success =
        load_mesh(mesh1_name, V1, E1, F1) && load_mesh(mesh2_name, V2, E2, F2);
    if (!success) {
        return false;
    }

    V1 = V1 * R1.transpose(); // (RVᵀ)ᵀ = VRᵀ
    V2 = V2 * R2.transpose();

    REQUIRE(dim <= V1.cols());
    REQUIRE(dim <= V2.cols());
    V = Eigen::MatrixXd(V1.rows() + V2.rows(), dim);
    V.topRows(V1.rows()) = V1.leftCols(dim);
    V.bottomRows(V2.rows()) = V2.leftCols(dim);

    REQUIRE(E1.cols() == E2.cols());
    E = Eigen::MatrixXi(E1.rows() + E2.rows(), E1.cols());
    E.topRows(E1.rows()) = E1;
    E.bottomRows(E2.rows()) = E2;
    E.bottomRows(E2.rows()).array() += V1.rows();

    REQUIRE(F1.cols() == F2.cols());
    F = Eigen::MatrixXi(F1.rows() + F2.rows(), F1.cols());
    F.topRows(F1.rows()) = F1;
    F.bottomRows(F2.rows()) = F2;
    F.bottomRows(F2.rows()).array() += V1.rows();

    return true;
}

TEST_CASE("Test HashGrid EF vs brute-force", "[intersection][brute_force]")
{
#ifdef NDEBUG
    std::string mesh1_name = GENERATE("cube.obj", "bunny.obj");
    std::string mesh2_name = GENERATE("cube.obj", "bunny.obj");
#else
    std::string mesh1_name = "cube.obj";
    std::string mesh2_name = "cube.obj";
#endif
    int dim = GENERATE(2, 3);
    bool perform_aabb_check = true;

    Eigen::Matrix3d R1 = GENERATE(take(4, RotationGenerator::create()));
    Eigen::Matrix3d R2 = GENERATE(take(4, RotationGenerator::create()));

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = combine_meshes(mesh1_name, mesh2_name, R1, R2, dim, V, E, F);
    REQUIRE(success);

    HashGrid hash_grid;
    hash_grid.resize(V, E);
    hash_grid.addEdges(V, E);
    if (V.cols() == 3) {
        // These are not needed for 2D
        hash_grid.addFaces(V, F);
    }

    if (V.cols() == 2) { // Need to check segment-segment intersections in 2D
        std::vector<EdgeEdgeCandidate> hg_ee_candidates;
        hash_grid.getEdgeEdgePairs(E, hg_ee_candidates);

        std::vector<EdgeEdgeCandidate> bf_ee_candidates;
        detect_edge_edge_collision_candidates_brute_force(
            V, E, bf_ee_candidates, perform_aabb_check);

        REQUIRE(hg_ee_candidates.size() <= bf_ee_candidates.size());

        tbb::parallel_sort(hg_ee_candidates.begin(), hg_ee_candidates.end());
        tbb::parallel_sort(bf_ee_candidates.begin(), bf_ee_candidates.end());

        // narrow-phase using igl
        igl::predicates::exactinit();
        int hg_ci = 0;
        for (int bf_ci = 0; bf_ci < bf_ee_candidates.size(); bf_ci++) {
            if (hg_ee_candidates.size() <= hg_ci
                || bf_ee_candidates[bf_ci] != hg_ee_candidates[hg_ci]) {
                long eai = bf_ee_candidates[bf_ci].edge0_index;
                long ebi = bf_ee_candidates[bf_ci].edge1_index;
                bool intersects = igl::predicates::segment_segment_intersect(
                    V.row(E(eai, 0)).head<2>(), V.row(E(eai, 1)).head<2>(),
                    V.row(E(ebi, 0)).head<2>(), V.row(E(ebi, 1)).head<2>());

                CHECK(!intersects); // Check for FN

            } else {
                hg_ci++;
            }
        }
        CHECK(hg_ci >= hg_ee_candidates.size());
    } else { // Need to check segment-triangle intersections in 3D
        assert(V.cols() == 3);

        std::vector<EdgeFaceCandidate> hg_ef_candidates;
        hash_grid.getEdgeFacePairs(E, F, hg_ef_candidates);

        std::vector<EdgeFaceCandidate> bf_ef_candidates;
        detect_edge_face_collision_candidates_brute_force(
            V, E, F, bf_ef_candidates, perform_aabb_check);

        REQUIRE(hg_ef_candidates.size() <= bf_ef_candidates.size());

        tbb::parallel_sort(hg_ef_candidates.begin(), hg_ef_candidates.end());
        tbb::parallel_sort(bf_ef_candidates.begin(), bf_ef_candidates.end());

        // narrow-phase using igl
        int hg_ci = 0;
        for (int bf_ci = 0; bf_ci < bf_ef_candidates.size(); bf_ci++) {
            if (hg_ef_candidates.size() <= hg_ci
                || bf_ef_candidates[bf_ci] != hg_ef_candidates[hg_ci]) {
                long ei = bf_ef_candidates[bf_ci].edge_index;
                long fi = bf_ef_candidates[bf_ci].face_index;
                bool intersects = is_edge_intersecting_triangle(
                    V.row(E(ei, 0)), V.row(E(ei, 1)), V.row(F(fi, 0)),
                    V.row(F(fi, 1)), V.row(F(fi, 2)));

                CHECK(!intersects); // Check for FN
            } else {
                hg_ci++;
            }
        }
        CHECK(hg_ci >= hg_ef_candidates.size());
    }

    CAPTURE(mesh1_name, mesh2_name, R1, R2);
}

TEST_CASE("Test has_intersections()", "[intersection][thisone]")
{
    std::string mesh1_name = GENERATE("cube.obj", "bunny.obj");
    std::string mesh2_name = GENERATE("cube.obj", "bunny.obj");
    int dim = GENERATE(2, 3);

#ifdef NDEBUG
    Eigen::Matrix3d R1 = GENERATE(take(4, RotationGenerator::create()));
    Eigen::Matrix3d R2 = GENERATE(take(4, RotationGenerator::create()));
#else
    Eigen::Matrix3d R1 = GENERATE(take(2, RotationGenerator::create()));
    Eigen::Matrix3d R2 = GENERATE(take(2, RotationGenerator::create()));
#endif

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = combine_meshes(mesh1_name, mesh2_name, R1, R2, dim, V, E, F);
    REQUIRE(success);

    CAPTURE(mesh1_name, mesh2_name, R1, R2);
    CHECK(has_intersections(V, E, F));
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

void test(int n)
{
    mtl::compressed2D<double> A(n*n, n*n);
    {
        mtl::mat::inserter<mtl::compressed2D<double> > ins(A, 5);
	for (int i= 0; i < n; i++) {
	    int r= i * n, re= r+n;
	    if (i > 0)
		for (int j= r; j < re; j++)
		    ins[j][j-n] << -1.0;
	    ins[r][r] << 4.0;
	    ins[r][r+1] << -1;
	    for (int j= r+1; j < re-1; j++) {
		ins[j][j-1] << -1.0;
		ins[j][j] << 4.0;
		ins[j][j+1] << -1.0;
	    }
	    ins[re-1][re-2] << -1.0;
	    ins[re-1][re-1] << 4.0;
	    if (i < n-1)
		for (int j= r; j < re; j++)
		    ins[j][j+n] << -1.0;
	}
    }
    if (n < 9) cout << "A is \n" << A;
    MTL_THROW_IF(A.nnz() != unsigned(5*n*n - 4*n), mtl::runtime_error("Wrong number of non-zeros."));
    for (int i= 0, ns= n*n; i < ns; i++) {
	MTL_THROW_IF(A[i][i] != 4.0, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF(i%n != 0 && A[i][i-1] != -1.0, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF((i+1)%n != 0 && A[i][i+1] != -1.0, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF(i-n >= 0 && A[i][i-n] != -1.0, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF(i+n < ns && A[i][i+n] != -1.0, mtl::runtime_error("Wrong value"));
    }

}

int main(int, char**)
{
    test(9);

    return 0;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_TEST_LAWS_MONAD_HPP
#define BOOST_HANA_TEST_LAWS_MONAD_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/chain.hpp>
#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/concept/monad.hpp>
#include <boost/hana/concept/sequence.hpp>
#include <boost/hana/core/make.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/flatten.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/functional/compose.hpp>
#include <boost/hana/functional/id.hpp>
#include <boost/hana/lift.hpp>
#include <boost/hana/monadic_compose.hpp>
#include <boost/hana/transform.hpp>

#include <laws/base.hpp>

namespace boost { namespace hana { namespace test {
    template <typename M, typename = when<true>>
    struct TestMonad : TestMonad<M, laws> {
        using TestMonad<M, laws>::TestMonad;
    };

    template <typename M>
    struct TestMonad<M, laws> {
        // Xs are Monads over something
        // XXs are Monads over Monads over something
        template <typename Xs, typename XXs>
        TestMonad(Xs xs, XXs xxs) {
            hana::for_each(xs, [](auto m) {
                static_assert(Monad<decltype(m)>{}, "");

                auto f = hana::compose(lift<M>, test::_injection<0>{});
                auto g = hana::compose(lift<M>, test::_injection<1>{});
                auto h = hana::compose(lift<M>, test::_injection<2>{});
                auto x = test::ct_eq<0>{};

                //////////////////////////////////////////////////////////////
                // Laws formulated with `monadic_compose`
                //////////////////////////////////////////////////////////////
                // associativity
                BOOST_HANA_CHECK(hana::equal(
                    hana::monadic_compose(h, hana::monadic_compose(g, f))(x),
                    hana::monadic_compose(hana::monadic_compose(h, g), f)(x)
                ));

                // left identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::monadic_compose(lift<M>, f)(x),
                    f(x)
                ));

                // right identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::monadic_compose(f, lift<M>)(x),
                    f(x)
                ));

                //////////////////////////////////////////////////////////////
                // Laws formulated with `chain`
                //
                // This just provides us with some additional cross-checking,
                // but the documentation does not mention those.
                //////////////////////////////////////////////////////////////
                BOOST_HANA_CHECK(hana::equal(
                    hana::chain(hana::lift<M>(x), f),
                    f(x)
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::chain(m, lift<M>),
                    m
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::chain(m, [f, g](auto x) {
                        return hana::chain(f(x), g);
                    }),
                    hana::chain(hana::chain(m, f), g)
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::transform(m, f),
                    hana::chain(m, hana::compose(lift<M>, f))
                ));

                //////////////////////////////////////////////////////////////
                // Consistency of method definitions
                //////////////////////////////////////////////////////////////
                // consistency of `chain`
                BOOST_HANA_CHECK(hana::equal(
                    hana::chain(m, f),
                    hana::flatten(hana::transform(m, f))
                ));

                // consistency of `monadic_compose`
                BOOST_HANA_CHECK(hana::equal(
                    hana::monadic_compose(f, g)(x),
                    hana::chain(g(x), f)
                ));
            });

            // consistency of `flatten`
            hana::for_each(xxs, [](auto mm) {
                BOOST_HANA_CHECK(hana::equal(
                    hana::flatten(mm),
                    hana::chain(mm, hana::id)
                ));
            });
        }
    };

    template <typename S>
    struct TestMonad<S, when<Sequence<S>::value>>
        : TestMonad<S, laws>
    {
        template <typename Xs, typename XXs>
        TestMonad(Xs xs, XXs xxs)
            : TestMonad<S, laws>{xs, xxs}
        {
            constexpr auto list = make<S>;

            //////////////////////////////////////////////////////////////////
            // flatten
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::flatten(list(list(), list())),
                list()
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::flatten(list(list(ct_eq<0>{}), list())),
                list(ct_eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::flatten(list(list(), list(ct_eq<0>{}))),
                list(ct_eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::flatten(list(list(ct_eq<0>{}), list(ct_eq<1>{}))),
                list(ct_eq<0>{}, ct_eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::flatten(list(
                    list(ct_eq<0>{}, ct_eq<1>{}),
                    list(),
                    list(ct_eq<2>{}, ct_eq<3>{}),
                    list(ct_eq<4>{})
                )),
                list(ct_eq<0>{}, ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{}, ct_eq<4>{})
            ));

            // just make sure we don't double move; this happened in hana::tuple
            hana::flatten(list(list(Tracked{1}, Tracked{2})));

            //////////////////////////////////////////////////////////////////
            // chain
            //////////////////////////////////////////////////////////////////
            {
                test::_injection<0> f{};
                auto g = hana::compose(list, f);

                BOOST_HANA_CONSTANT_CHECK(hana::equal(
                    hana::chain(list(), g),
                    list()
                ));

                BOOST_HANA_CONSTANT_CHECK(hana::equal(
                    hana::chain(list(ct_eq<1>{}), g),
                    list(f(ct_eq<1>{}))
                ));

                BOOST_HANA_CONSTANT_CHECK(hana::equal(
                    hana::chain(list(ct_eq<1>{}, ct_eq<2>{}), g),
                    list(f(ct_eq<1>{}), f(ct_eq<2>{}))
                ));

                BOOST_HANA_CONSTANT_CHECK(hana::equal(
                    hana::chain(list(ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{}), g),
                    list(f(ct_eq<1>{}), f(ct_eq<2>{}), f(ct_eq<3>{}))
                ));

                BOOST_HANA_CONSTANT_CHECK(hana::equal(
                    hana::chain(list(ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{}, ct_eq<4>{}), g),
                    list(f(ct_eq<1>{}), f(ct_eq<2>{}), f(ct_eq<3>{}), f(ct_eq<4>{}))
                ));
            }

            //////////////////////////////////////////////////////////////////
            // monadic_compose
            //////////////////////////////////////////////////////////////////
            {
                test::_injection<0> f{};
                test::_injection<1> g{};

                auto mf = [=](auto x) { return list(f(x), f(f(x))); };
                auto mg = [=](auto x) { return list(g(x), g(g(x))); };

                auto x = test::ct_eq<0>{};
                BOOST_HANA_CHECK(hana::equal(
                    hana::monadic_compose(mf, mg)(x),
                    list(f(g(x)), f(f(g(x))), f(g(g(x))), f(f(g(g(x)))))
                ));
            }
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_MONAD_HPP


#pragma once
#include <vizkit3d_debug_drawings/commands/DrawCommand.hpp>

#pragma once
#include <string>
#include <Eigen/Core>
#include <vizkit3d_debug_drawings/commands/BoostSerializationHelpers.hpp>

namespace osg
{
    class Node;
}

namespace vizkit3dDebugDrawings
{
class DrawRingCommand : public DrawCommand
{
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        // serialize base class information
        ar & boost::serialization::base_object<DrawCommand>(*this);
        ar & position;
        ar & orientation;
        ar & height;
        ar & thickness;
        ar & radius;
        ar & colorRGBA;
    }

public:
    DrawRingCommand();

    DrawRingCommand(const std::string& drawingChannel, const Eigen::Vector3d& position,
                    const Eigen::Quaterniond& orientation, double height,
                    double thickness, double radius, const Eigen::Vector4d& colorRGBA);

    virtual osg::ref_ptr<osgviz::Object> createPrimitive() const;

    virtual DrawRingCommand* clone() const;

private:
    Eigen::Vector3d position;
    Eigen::Quaterniond orientation;
    double height;
    double thickness;
    double radius;
    Eigen::Vector4d colorRGBA;

};
}