// ======================================================================
/*!
 * \file
 * \brief Implementation of namespace NFmiAreaFactory
 */
// ======================================================================
/*!
 * \namespace NFmiAreaFactory
 *
 * \brief Contains tools for creation NFmiArea objects
 *
 * The NFmiAreaFactory contains functions for creating NFmiArea
 * objects.
 *
 * The Create function takes as input a string describing the
 * desired projection, parses the description and returns
 * the desired area.
 *
 * The generic form for the string description is
 * \code
 * projection:area:grid
 * \endcode
 *
 * The possible values for the \c projection part are
 * \code
 * latlon
 * ykj
 * pkj
 * mercator
 * rotlatlon,polelatitude=-90,polelongitude=0
 * invrotlatlon,polelatitude=-90,polelongitude=0
 * orthographic,azimuth=0
 * stereographic,centrallongitude=0,centrallatitude=90,truelatitude=60
 * lambertequal,centrallongitude=10,centrallatitude=0,truelatitude=52
 * gnomonic,centrallongitude=0,centrallatitude=90,truelatitude=60
 * equidist,centrallongitude=0,centrallatitude=90
 * \endcode
 * The default values for optional parameters are as indicated above.
 * The \c invrotlatlon projection is a convenience id for creating
 * the \c rotlatlon projection based on rotated coordinates.
 *
 * The possible values for the \c area part are
 * \code
 * x1,y1,x2,y2
 * cx,cy,scale
 * cx,cy,scale/aspect
 * \endcode
 * where the first form requires the bottom left and top right
 * longitude and latitude. The latter form requires the center
 * longitude and latitude plus a scale factor.
 *
 * An optional aspect ratio may be used to compress or elongate the
 * Y-axis, which is useful for example for Meteosat images. The aspect
 * ratio is used only when the image center coordinate is defined, otherwise
 * the ratio is meaningless.
 *
 * If the projection is \c invrotlatlon, the latitude and longitude values
 * are measured in \c rotlatlon coordinates.
 *
 * The possible values for the \c grid part are
 * \code
 * x1,y1,x2,y2
 * y1,y2
 * xsize,ysize<units>,  where units may be 'm' or 'km'
 * \endcode
 * Comma may be replaced by "x".
 *
 * Also, the grid part may be omitted entirely, which is assumed
 * to imply values 0,0,1,1. The units version may not be used
 * if the center coordinate & scale are used to define the area.
 *
 * If also the area part is omitted, it is given the default
 * value of "6,51.3,49,70.2" which covers the entire Skandinavia
 * and coincides with the current editor area. This is mainly useful
 * for programs which do not care about XY-coordinates, but only
 * of world coordinates.
 *
 * For example, the following defines the stereographic projection
 * used by the forecasters:
 * \code
 * stereographic,20,90,60:6,51.3,49,70.2
 * \endcode
 * The projection is thus polar stereographic with central longitude 20,
 * true latitude 60, with bottom left corner at (5,51.3) and top right
 * corner at 49,70.2.
 *
 *
 * The CreateProj function takes a string containing a Proj4 projection
 * definition and the corner points of the desired area, and returns
 * the corresponding NFmiArea - object. If the projection described in
 * the Proj4-string is not supported, a runtime_error is thrown.
 *
 * The CreateProj function supports the following FMI-projections:
 * \code
 * latlon
 * ykj
 * mercator
 * orthographic
 * stereographic
 * lambertequal
 * gnomonic
 * equidist
 * \endcode
 *
 */
// ======================================================================

#include "NFmiAreaFactory.h"
#include "NFmiEquidistArea.h"
#include "NFmiGdalArea.h"
#include "NFmiGnomonicArea.h"
#include "NFmiLambertEqualArea.h"
#include "NFmiLatLonArea.h"
#include "NFmiMercatorArea.h"
#include "NFmiOrthographicArea.h"
#include "NFmiPKJArea.h"
#include "NFmiRotatedLatLonArea.h"
#include "NFmiStereographicArea.h"
#include "NFmiStringTools.h"
#include "NFmiYKJArea.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <deque>
#include <list>
#include <map>
#include <set>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

using namespace std;

namespace
{
double check_longitude(double theLongitude, bool usePacificView)
{
  std::string rangeStr;
  if (usePacificView)
  {
    if (theLongitude >= 0 && theLongitude <= 360) return theLongitude;
    rangeStr = "[0,360]";
  }
  else
  {
    if (theLongitude >= -180 && theLongitude <= 180) return theLongitude;
    rangeStr = "[-180,180]";
  }
  string msg = "Longitude value";
  msg += NFmiStringTools::Convert(theLongitude);
  msg += " is not in the required range " + rangeStr;
  throw runtime_error(msg);
}

double check_latitude(double theLatitude)
{
  if (theLatitude >= -90 && theLatitude <= 90) return theLatitude;
  string msg = "Latitude value";
  msg += NFmiStringTools::Convert(theLatitude);
  msg += " is not in the required range [-90,90]";
  throw runtime_error(msg);
}

double degrees_from_projparam(const string &inParam)
{
  // Trims the parameter containing proj degrees and returns double.
  size_t length = inParam.size();
  try
  {
    // North
    if (boost::iends_with(inParam, "n"))
    {
      return boost::lexical_cast<double>(inParam.substr(0, length - 1));
    }
    // South
    else if (boost::iends_with(inParam, "s"))
    {
      return -boost::lexical_cast<double>(inParam.substr(0, length - 1));
    }
    // East
    else if (boost::iends_with(inParam, "e"))
    {
      return boost::lexical_cast<double>(inParam.substr(0, length - 1));
    }
    // West
    else if (boost::iends_with(inParam, "w"))
    {
      return -boost::lexical_cast<double>(inParam.substr(0, length - 1));
    }
    else
    {
      return boost::lexical_cast<double>(inParam);
    }
  }
  catch (boost::bad_lexical_cast &e)
  {
    string errStr;
    errStr += "Bad cast to double in parameter: ";
    errStr += e.what();
    throw runtime_error(errStr);
  }
}

void print_unused_parameters(const map<string, string> &inMap, const set<string> &usedParams)
{
  cerr << "Unused parameters :" << endl;
  for (const auto &it : inMap)
  {
    if (usedParams.find(it.first) == usedParams.end())
    {
      cerr << it.first << "=" << it.second << endl;
    }
  }
  cerr << endl;
}
}

namespace NFmiAreaFactory
{
bool DoPossiblePacificFix(NFmiPoint &bottomLeftLatlon, NFmiPoint &topRightLatlon, bool &pacificView)
{
  if (pacificView)
  {
    // Fix top right longitude to Pacific view if it seems possible
    if (topRightLatlon.X() >= -180 && topRightLatlon.X() < 0)
    {
      topRightLatlon.X(topRightLatlon.X() + 360);
      return true;
    }
    else if (bottomLeftLatlon.X() < topRightLatlon.X() && bottomLeftLatlon.X() >= 180 &&
             topRightLatlon.X() > 180)
    {  // molemmat nurkkapisteet ovat pacific-alueella (180 - 360), tehdään niistä molemmista
       // atlantisia koordinaatteja
      // koska SmartMet toimii paremmin tälläisissa tilanteissa, jos kyseinen muutos tehdään. En
      // tiedä tarkemmin miksi, koska
      // debug-versio SmartMetista näytti toimivan täysin oikein joka tapauksessa oli data miten
      // hyvänsä.
      bottomLeftLatlon.X(bottomLeftLatlon.X() - 360);
      topRightLatlon.X(topRightLatlon.X() - 360);
      pacificView = false;
      return true;
    }
  }
  return false;
}

// ----------------------------------------------------------------------
/*!
 * \brief Create the desired projection
 *
 * Throws if there is an error in the projection description.
 *
 * \param theProjection String description of the projection
 * \return The created projection
 */
// ----------------------------------------------------------------------

boost::shared_ptr<NFmiArea> Create(const std::string &theProjection)
{
  boost::shared_ptr<NFmiArea> area;

  string projection = theProjection;
  NFmiStringTools::TrimAll(
      projection);  // siivotaan mahdolliset etu ja taka white spacet pois ettei sotke parserointia

  const char *separator = ":";
  if (projection.find('|') != std::string::npos) separator = "|";

  vector<string> parts = NFmiStringTools::Split<vector<string> >(projection, separator);

  try
  {
    if (parts.size() == 1) parts.emplace_back("6,51.3,49,70.2");

    if (parts.size() < 1 || parts.size() > 3)
      throw runtime_error("must have 1-3 parts separated by ':' or '|'");

    // extracts the parts separated by ','
    list<string> pparts = NFmiStringTools::Split<list<string> >(parts[0]);

    // Extract possible units
    bool units = false;
    double unitfactor = 1.0;
    if (parts.size() == 3)
    {
      // Allow both NxM and N,M
      std::replace(parts[2].begin(), parts[2].end(), 'x', ',');

      if (boost::iends_with(parts[2], "km"))
      {
        units = true;
        unitfactor = 1000;
        parts[2] = parts[2].substr(0, parts[2].size() - 2);
      }
      else if (boost::iends_with(parts[2], "m"))
      {
        units = true;
        parts[2] = parts[2].substr(0, parts[2].size() - 1);
      }
    }

    const vector<string> aparts = NFmiStringTools::Split<vector<string> >(parts[1], "/");
    const vector<double> avec = NFmiStringTools::Split<vector<double> >(aparts[0]);
    double aspect = (aparts.size() == 1 ? 1 : NFmiStringTools::Convert<double>(aparts[1]));

    deque<double> gvec =
        NFmiStringTools::Split<deque<double> >(parts.size() == 2 ? "0,0,1,1" : parts[2]);

    // intermediate validity checks
    if (pparts.size() < 1) throw runtime_error("projection part missing");
    if (avec.size() < 3 || avec.size() > 4)
      throw runtime_error("area specification must have 3-4 numbers");
    if (gvec.size() != 2 && gvec.size() != 4)
      throw runtime_error("grid specification must have 2 or 4 numbers");
    if (gvec.size() != 2 && units)
      throw runtime_error("grid specification must have 2 numbers when length units are used");

    string proj = pparts.front();
    pparts.pop_front();

    vector<double> pvec;

    for (list<string>::const_iterator it = pparts.begin(); it != pparts.end(); ++it)
      pvec.push_back(NFmiStringTools::Convert<double>(*it));

    // fixate gvec to size 4

    if (gvec.size() == 2)
    {
      gvec.push_front(0);
      gvec.push_front(0);
    }

    // extract the numeric parts

    const bool centered = (avec.size() == 3);

    NFmiPoint bottomleft = NFmiPoint(avec[0], avec[1]);
    NFmiPoint topright = (centered ? bottomleft : NFmiPoint(avec[2], avec[3]));

    bool usePacificView = NFmiArea::IsPacificView(bottomleft, topright);
    DoPossiblePacificFix(bottomleft, topright, usePacificView);

    check_longitude(bottomleft.X(), usePacificView);
    check_latitude(bottomleft.Y());
    check_longitude(topright.X(), usePacificView);
    check_latitude(topright.Y());

    // More validity checks

    if (units && centered)
      throw runtime_error(
          "Cannot use a centered projection speficiation with grid size of specific length");

    const NFmiPoint corner1(gvec[0], gvec[1]);
    const NFmiPoint corner2(gvec[2], gvec[3]);

    if (proj == "latlon")
    {
      if (pvec.size() != 0) throw runtime_error("latlon area does not require any parameters");
      area.reset(new NFmiLatLonArea(bottomleft, topright, corner1, corner2, usePacificView));
    }
    else if (proj == "ykj")
    {
      if (pvec.size() != 0) throw runtime_error("ykj area does not require any parameters");
      area.reset(new NFmiYKJArea(bottomleft, topright, corner1, corner2, usePacificView));
    }
    else if (proj == "pkj")
    {
      if (pvec.size() != 0) throw runtime_error("pkj area does not require any parameters");
      area.reset(new NFmiPKJArea(bottomleft, topright, corner1, corner2, usePacificView));
    }
    else if (proj == "mercator")
    {
      if (pvec.size() > 0) throw runtime_error("mercator area requires no parameters");

      area.reset(new NFmiMercatorArea(bottomleft, topright, corner1, corner2, usePacificView));
    }
    else if (proj == "rotlatlon")
    {
      if (pvec.size() > 2) throw runtime_error("rotlatlon area requires max 2 parameters");
      const double pole_lon = check_longitude(pvec.size() >= 2 ? pvec[1] : 0, usePacificView);
      const double pole_lat = check_latitude(pvec.size() >= 1 ? pvec[0] : -90);
      area.reset(new NFmiRotatedLatLonArea(
          bottomleft, topright, NFmiPoint(pole_lon, pole_lat), corner1, corner2, usePacificView));
    }
    else if (proj == "invrotlatlon")
    {
      if (pvec.size() > 2) throw runtime_error("invrotlatlon area requires max 2 parameters");
      const double pole_lon = check_longitude(pvec.size() >= 2 ? pvec[1] : 0, usePacificView);
      const double pole_lat = check_latitude(pvec.size() >= 1 ? pvec[0] : -90);

      // Invert the coordinates. Note that only the pole really matters in
      // the following constructor

      NFmiRotatedLatLonArea rot(bottomleft, topright, NFmiPoint(pole_lon, pole_lat));
      NFmiPoint bl = rot.ToRegLatLon(bottomleft);
      NFmiPoint tr = rot.ToRegLatLon(topright);

      area.reset(new NFmiRotatedLatLonArea(
          bl, tr, NFmiPoint(pole_lon, pole_lat), corner1, corner2, usePacificView));
    }
    else if (proj == "orthographic")
    {
      if (pvec.size() > 1) throw runtime_error("orthographi area requires max 1 parameter");
      const double azimuth = check_longitude(pvec.size() >= 1 ? pvec[0] : 0, usePacificView);
      area.reset(new NFmiOrthographicArea(
          bottomleft, topright, azimuth, corner1, corner2, usePacificView));
    }
    else if (proj == "stereographic")
    {
      if (pvec.size() > 3) throw runtime_error("stereographic area requires max 3 parameters");
      const double clon = check_longitude(pvec.size() >= 1 ? pvec[0] : 0, usePacificView);
      const double clat = check_latitude(pvec.size() >= 2 ? pvec[1] : 90);
      const double tlat = check_latitude(pvec.size() >= 3 ? pvec[2] : 60);
      area.reset(new NFmiStereographicArea(
          bottomleft, topright, clon, corner1, corner2, clat, tlat, usePacificView));
    }
    else if (proj == "lambertequal")
    {
      if (pvec.size() > 3) throw runtime_error("lambertequal area requires max 3 parameters");
      const double clon = check_longitude(pvec.size() >= 1 ? pvec[0] : 0, usePacificView);
      const double clat = check_latitude(pvec.size() >= 2 ? pvec[1] : 90);
      const double tlat = check_latitude(pvec.size() >= 3 ? pvec[2] : 60);
      area.reset(new NFmiLambertEqualArea(
          bottomleft, topright, clon, corner1, corner2, clat, tlat, usePacificView));
    }
    else if (proj == "gnomonic")
    {
      if (pvec.size() > 3) throw runtime_error("gnomonic area requires max 3 parameters");
      const double clon = check_longitude(pvec.size() >= 1 ? pvec[0] : 0, usePacificView);
      const double clat = check_latitude(pvec.size() >= 2 ? pvec[1] : 90);
      const double tlat = check_latitude(pvec.size() >= 3 ? pvec[2] : 60);
      area.reset(new NFmiGnomonicArea(
          bottomleft, topright, clon, corner1, corner2, clat, tlat, usePacificView));
    }
    else if (proj == "equidist")
    {
      if (pvec.size() > 2) throw runtime_error("equidist area requires max 2 parameters");
      const double clon = check_longitude(pvec.size() >= 1 ? pvec[0] : 0, usePacificView);
      const double clat = check_latitude(pvec.size() >= 2 ? pvec[1] : 90);
      area.reset(
          new NFmiEquidistArea(bottomleft, topright, clon, corner1, corner2, clat, usePacificView));
    }
#ifdef UNIX
    else
    {
      // Allow FMI: or WGS84: prefix to identify datum, default is WGS84
      std::string datum = "WGS84";
      if (proj.substr(0, 4) == "FMI:")
      {
        datum = "FMI";
        proj = proj.substr(4, std::string::npos);
      }
      else if (proj.substr(0, 6) == "WGS84:")
      {
        proj = proj.substr(6, std::string::npos);
      }
      area.reset(
          new NFmiGdalArea(datum, proj, bottomleft, topright, corner1, corner2, usePacificView));
    }
#endif  // UNIX

    // recalculate corners if center coordinate was given
    // instead of corners

    const double width = gvec[2] - gvec[0];
    const double height = gvec[3] - gvec[1];

    if (units && (width <= 0 || height <= 0))
      throw runtime_error("Cannot use negative lengths when specifying the grid size");

    if (centered)
    {
      if (width <= 0 || height <= 0)
        throw runtime_error("Width and height must be positive when center coordinate is given");

      const double scale = avec[2];
      const NFmiPoint c = area->LatLonToWorldXY(bottomleft);  // bottomleft = center here
      const NFmiPoint bl(c.X() - scale * 1000 * width, c.Y() - scale * 1000 * aspect * height);
      const NFmiPoint tr(c.X() + scale * 1000 * width, c.Y() + scale * 1000 * aspect * height);
      NFmiPoint BL = area->WorldXYToLatLon(bl);
      NFmiPoint TR = area->WorldXYToLatLon(tr);

      if (proj == "invrotlatlon")
      {
        BL = (static_cast<NFmiRotatedLatLonArea *>(area.get()))->ToRegLatLon(BL);
        TR = (static_cast<NFmiRotatedLatLonArea *>(area.get()))->ToRegLatLon(TR);
      }

      area.reset(area->NewArea(BL, TR));
    }
    else if (width < 0 && height > 0)
    {
      // calculate width to preserve aspect ratio
      double scale = area->WorldXYAspectRatio();
      double w = round(scale * height);
      NFmiRect rect(gvec[0], gvec[1], gvec[0] + w, gvec[3]);
      area->SetXYArea(rect);
    }
    else if (height < 0 && width > 0)
    {
      // calculate height to preserve aspect ratio
      double scale = area->WorldXYAspectRatio();
      double h = round(width / scale);
      NFmiRect rect(gvec[0], gvec[1], gvec[2], gvec[1] + h);
      area->SetXYArea(rect);
    }
    else if (width < 0 && height < 0)
      throw runtime_error("Width and height cannot both be negative");
    else if (units)
    {
      double w = area->WorldXYWidth();
      double h = area->WorldXYHeight();
      auto xsize = static_cast<int>(round(w / (unitfactor * width)));
      auto ysize = static_cast<int>(round(h / (unitfactor * height)));
      NFmiRect rect(0, 0, xsize, ysize);
      area->SetXYArea(rect);
    }
  }
  catch (std::runtime_error &e)
  {
    throw runtime_error("Projection specification '" + theProjection + "' is invalid: " + e.what());
  }

  return area;
}

// ----------------------------------------------------------------------
/*!
 * \brief Create the desired projection from Proj4 string
 *
 * Throws if there is an error in the projection description or when the projection is not supported
 *
 * \param projString String description of the projection
 * \return The created projection
 *
 * The following projections are not supported by the Proj4 factory: NFmiPKJArea,
 * NFmiRotatedLatLonArea, NFmiKKJArea
 */
// ----------------------------------------------------------------------

return_type CreateProj(const std::string &projString,
                       const NFmiPoint &bottomLeftLatLon,
                       const NFmiPoint &topRightLatLon,
                       const NFmiPoint &topLeftXY,
                       const NFmiPoint &bottomRightXY)
{
  // Map to hold the parameters
  map<string, string> projParams;

  // Map to hold parameters that are actually used in parsing, this can be used to print parameters
  // that are ignored
  set<string> usedParams;

  // Tokenize the input string
  vector<string> tokens, splitToken;
  boost::split(tokens, projString, boost::is_any_of(" \t\n"), boost::token_compress_on);

  // The result
  return_type result;

  // Build token map, while validating the proj-string
  for (auto &token : tokens)
  {
    // Split token into key->value pairs
    boost::split(splitToken, token, boost::is_any_of("="));

    if (!boost::starts_with(splitToken[0], "+"))
    {
      // Proj parameter keys start with a "+"

      throw runtime_error("Proj4 parameters must start with a '+'");
    }

    if (splitToken.size() < 2)
    {
      // Only key but no value, use empty string as value
      projParams.insert(make_pair(splitToken[0].erase(0, 1), string("")));
    }

    projParams.insert(make_pair(splitToken[0].erase(0, 1), splitToken[1]));
  }

  //	print_parameter_map(projParams);
  // Now parameter map is ready for processing
  map<string, string>::iterator map_it;
  map_it = projParams.find("proj");
  if (map_it == projParams.end())
  {
    throw runtime_error("No projection specified");
  }

  usedParams.insert(map_it->first);
  string projId = map_it->second;

  if (projId == "stere")
  {
    // Stereographic projection, find mandatory parameters

    // Central longitude
    map_it = projParams.find("lon_0");
    if (map_it == projParams.end())
    {
      throw runtime_error(
          "Central meridian 'lon_0' must be specified for stereographic projection");
    }

    double centralLon = degrees_from_projparam(map_it->second);

    usedParams.insert(map_it->first);

    // True scale latitude
    map_it = projParams.find("lat_ts");
    if (map_it == projParams.end())
    {
      throw runtime_error(
          "True scale latitude 'lat_ts' must be specified for stereographic projection");
    }
    double truescaleLat = degrees_from_projparam(map_it->second);

    usedParams.insert(map_it->first);
    // Center latitude
    map_it = projParams.find("lat_0");
    if (map_it == projParams.end())
    {
      throw runtime_error("Center latitude 'lat_0' must be specified for stereographic projection");
    }
    double centerLat = degrees_from_projparam(map_it->second);

    usedParams.insert(map_it->first);

    result = return_type(new NFmiStereographicArea(bottomLeftLatLon,
                                                   topRightLatLon,
                                                   centralLon,
                                                   topLeftXY,
                                                   bottomRightXY,
                                                   centerLat,
                                                   truescaleLat));
  }

  else if (projId == "aeqd")
  {
    // Azimuthal equidistant projection

    map_it = projParams.find("lat_0");
    if (map_it == projParams.end())
    {
      throw runtime_error(
          "Center latitude 'lat_0' must be specified for azimuthal equidistant projection");
    }
    double centerLat = degrees_from_projparam(map_it->second);
    usedParams.insert(map_it->first);

    map_it = projParams.find("lon_0");
    if (map_it == projParams.end())
    {
      throw runtime_error(
          "Center latitude 'lat_0' must be specified for azimuthal equidistant projection");
    }
    double centerLon = degrees_from_projparam(map_it->second);
    usedParams.insert(map_it->first);

    result = return_type(new NFmiEquidistArea(
        bottomLeftLatLon, topRightLatLon, centerLon, topLeftXY, bottomRightXY, centerLat));
  }

  else if ((projId == "latlon") || (projId == "lonlat"))
  {
    // Latlon area

    result =
        return_type(new NFmiLatLonArea(bottomLeftLatLon, topRightLatLon, topLeftXY, bottomRightXY));
  }

  else if (projId == "ortho")
  {
    // Orthographic projection
    result = return_type(
        new NFmiOrthographicArea(bottomLeftLatLon, topRightLatLon, 0, topLeftXY, bottomRightXY));
  }

  else if (projId == "gnom")
  {
    // Gnomonic projection
    map_it = projParams.find("lon_0");
    if (map_it == projParams.end())
    {
      throw runtime_error("Central meridian 'lon_0' must be specified for gnomonic projection");
    }
    double centralLon = degrees_from_projparam(map_it->second);
    usedParams.insert(map_it->first);

    // True scale latitude
    map_it = projParams.find("lat_ts");
    if (map_it == projParams.end())
    {
      throw runtime_error("True scale latitude 'lat_ts' must be specified for gnomonic projection");
    }
    double truescaleLat = degrees_from_projparam(map_it->second);
    usedParams.insert(map_it->first);

    // Center latitude
    map_it = projParams.find("lat_0");
    if (map_it == projParams.end())
    {
      throw runtime_error("Center latitude 'lat_0' must be specified for gnomonic projection");
    }
    double centerLat = degrees_from_projparam(map_it->second);
    usedParams.insert(map_it->first);

    result = return_type(new NFmiGnomonicArea(bottomLeftLatLon,
                                              topRightLatLon,
                                              centralLon,
                                              topLeftXY,
                                              bottomRightXY,
                                              centerLat,
                                              truescaleLat));
  }

  else if (projId == "merc")
  {
    // Mercator projection
    result = return_type(
        new NFmiMercatorArea(bottomLeftLatLon, topRightLatLon, topLeftXY, bottomRightXY));
  }

  else if (projId == "tmerc")
  {
    // The only supported Transverse Mercator projection is YKJ-projection (EPSG:2393), must check
    // that other proj-params are
    // according to http://spatialreference.org/ref/epsg/2393/proj4/. Fails otherwise.
    // Validates using string comparisons
    map_it = projParams.find("lon_0");
    if (map_it == projParams.end())
    {
      throw runtime_error("Central meridian 'lon_0' must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    if (map_it->second != "27")
    {
      string errStr;
      errStr += "Invalid lon_0 for YKJ projection: ";
      errStr += map_it->second;
      errStr += ". Should be 27";
      throw runtime_error(errStr);
    }
    usedParams.insert(map_it->first);

    map_it = projParams.find("lat_0");
    if (map_it == projParams.end())
    {
      throw runtime_error("Center latitude 'lat_0' must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    if (map_it->second != "0")
    {
      string errStr;
      errStr += "Invalid lat_0 for YKJ projection: ";
      errStr += map_it->second;
      errStr += ". Should be 0";
      throw runtime_error(errStr);
    }
    usedParams.insert(map_it->first);

    map_it = projParams.find("k");
    if (map_it == projParams.end())
    {
      throw runtime_error("k must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    if (map_it->second != "1")
    {
      string errStr;
      errStr += "Invalid k for YKJ projection: ";
      errStr += map_it->second;
      errStr += ". Should be 1";
      throw runtime_error(errStr);
    }
    usedParams.insert(map_it->first);

    map_it = projParams.find("x_0");
    if (map_it == projParams.end())
    {
      throw runtime_error("x_0 must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    if (map_it->second != "3500000")
    {
      string errStr;
      errStr += "Invalid k for YKJ projection: ";
      errStr += map_it->second;
      errStr += ". Should be 3500000";
      throw runtime_error(errStr);
    }
    usedParams.insert(map_it->first);

    // y_0 can be left out as it is 0
    map_it = projParams.find("y_0");
    if (!(map_it == projParams.end()))
    {
      if (map_it->second != "0")
      {
        string errStr;
        errStr += "Invalid y_0 for YKJ projection: ";
        errStr += map_it->second;
        errStr += ". Should be 0";
        throw runtime_error(errStr);
      }
    }
    usedParams.insert(map_it->first);

    map_it = projParams.find("ellps");
    if (map_it == projParams.end())
    {
      throw runtime_error("ellps must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    if (map_it->second != "intl")
    {
      string errStr;
      errStr += "Invalid ellps for YKJ projection: ";
      errStr += map_it->second;
      errStr += ". Should be intl";
      throw runtime_error(errStr);
    }
    usedParams.insert(map_it->first);

    map_it = projParams.find("units");
    if (map_it == projParams.end())
    {
      throw runtime_error("units must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    if (map_it->second != "m")
    {
      string errStr;
      errStr += "Invalid units for YKJ projection: ";
      errStr += map_it->second;
      errStr += ". Should be m";
      throw runtime_error(errStr);
    }
    usedParams.insert(map_it->first);

    map_it = projParams.find("no_defs");
    if (map_it == projParams.end())
    {
      throw runtime_error("no_defs must be specified for YKJ projection");
    }
    usedParams.insert(map_it->first);

    // If we are here the projection is valid YKJ (EPSG 2393)
    result =
        return_type(new NFmiYKJArea(bottomLeftLatLon, topRightLatLon, topLeftXY, bottomRightXY));
  }

  else
  {
    string errStr = "Unsupported projection: ";
    errStr += projId;
    throw runtime_error(errStr);
  }

  //	print_unused_parameters(projParams,usedParams);
  return result;
}

}  // namespace NFmiAreaFactory

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


/*
 * Copyright (C) 2015 Hamza Merzić
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TwoSegPrmTest

#include "two_seg_prm.h"

#include <vector>
#include <memory>
#include <cmath>
#include <iostream>

#include "environment/pqp_environment.h"
#include "random_generator/naive_generator.h"
#include <boost/test/unit_test.hpp>

typedef Eigen::VectorXd EVectorXd;

BOOST_AUTO_TEST_CASE(connect) {
  std::vector<std::pair<double, double>> limits;
  limits.emplace_back(0, 2 * M_PI);
  limits.emplace_back(0, 2 * M_PI);
  std::unique_ptr<RandomSpaceGeneratorInterface> generator (
    new NaiveGenerator(limits));
  std::unique_ptr<PqpEnvironment> pqp (new PqpEnvironment(
      {"models/two-seg/robot1_seg1.stl", "models/two-seg/robot1_seg2.stl"},
      "models/two-seg/dh_table_test.txt", "models/two-seg/obstacles_test.stl",
      generator.release()));

  EVectorXd start (2); start << 0, 0;
  EVectorXd end (2); end << 1, 1;
  TwoSegPrm two_seg_prm (pqp.release(), start, end, 15, 0.01);
  EVectorXd qbegin (2); qbegin << 0, 0;
  EVectorXd qend (2); qend << 1, 1;
  int qbegin_ind (two_seg_prm.InsertPoint(qbegin)),
    qend_ind (two_seg_prm.InsertPoint(qend));
  BOOST_CHECK_EQUAL(two_seg_prm.ConnectPoints(qbegin_ind, qend_ind), true);
}

BOOST_AUTO_TEST_CASE(connect1) {
  std::vector<std::pair<double, double>> limits;
  limits.emplace_back(0, 2 * M_PI);
  limits.emplace_back(0, 2 * M_PI);
  std::unique_ptr<RandomSpaceGeneratorInterface> generator (
    new NaiveGenerator(limits));
  std::unique_ptr<PqpEnvironment> pqp (new PqpEnvironment(
      {"models/two-seg/robot1_seg1.stl", "models/two-seg/robot1_seg2.stl"},
      "models/two-seg/dh_table_test.txt", "models/two-seg/obstacles_test.stl",
      generator.release(), 10000));

  EVectorXd start (2); start << 0, 0;
  EVectorXd end (2); end << 1, 1;
  TwoSegPrm two_seg_prm (pqp.release(), start, end, 15, 0.01);
  EVectorXd qbegin (2); qbegin << M_PI_2 - 0.1, M_PI_2 - 0.1;
  EVectorXd qend (2); qend << M_PI_2 + 0.1, M_PI_2 + 0.1;
  int qbegin_ind (two_seg_prm.InsertPoint(qbegin)),
    qend_ind (two_seg_prm.InsertPoint(qend));
  BOOST_CHECK_EQUAL(two_seg_prm.ConnectPoints(qbegin_ind, qend_ind), false);
  BOOST_CHECK_EQUAL(two_seg_prm.BuildTree(""), true);
  two_seg_prm.GeneratePath();
}


#include <boost/accumulators/statistics/weighted_p_square_cumul_dist.hpp>


#ifndef __M_UTILITY_
#define __M_UTILITY_

#include <random>
#include <cassert>
#include <cstdint>
#include <typeindex>
#include <functional>
#include <vector>
#include <cctype>

#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>

#include "zl_entry.hpp"
#include "config.hpp"

NAMESPACE_BEGIN(monoco)

class random_machine
{
public:
	static size_t gen_uint(const size_t& start, const size_t& end)
		{
			assert(start < end);
			std::random_device rd;
			std::mt19937_64 gen(rd());
			std::uniform_int_distribution<> dis(start, end);
			return dis(gen);
		}

	static double gen_real(const double& start, const double& end)
		{
			assert(start < end);
			std::random_device rd;
			std::mt19937 gen(rd());
			std::uniform_real_distribution<> dis(start, end);
			return dis(gen);
		}
};

NAMESPACE_BEGIN(utility)

int
check_arg(string& arg)
{
	for (auto && ch : arg) {
		if (!(std::isdigit(ch) ||
			  ch == '.'))
			return 1;
	}

	if (arg.empty())
		return 1;
	return 0;
}

template <typename T>
static int
_try_convert(const string & str, T* res)
{
	try {
	    *res = boost::lexical_cast<T>(str);
	}
	catch (...)
	{
		return 1;
	}
	return 0;
}

template <typename T>
int try_convert(std::vector<zl_entry>& res,
				string& arg)
{
	int fail = 0;
	T val;

	fail = _try_convert(arg, &val);

	if (fail == 0) {
		res.emplace_back(val);
	}

	return fail;
}

template <>
int
try_convert<string>(std::vector<zl_entry>& res,
					string& arg)
{
	return 1;

	try {
		if (arg.front() == '\'' ||
			arg.front() == '"') {
			arg.erase(arg.begin());
			arg.erase(arg.begin() + (arg.size() - 1));
		}
		res.emplace_back(arg);
		return 0;
	}
	catch (...) {
		return 1;
	}
	return 1;
}

int
args_to_zls(std::vector<string>::iterator first,
			std::vector<string>::iterator last,
			std::vector<zl_entry>& res)
{
	while(first != last) {
		auto arg = *first;
		std::advance(first, 1);

		if (arg.size() >= 3 &&
			((arg.front() == '"' && arg.back() == '"') ||
			 (arg.front() == '\'' && arg.back() == '\'')))
		{
			arg.erase(0, 1);
			arg.erase(arg.size() - 1);
			res.push_back(zl_entry(arg));
			continue;
		}

		if(try_convert<int16_t>(res, arg) == 0) continue;
		if(try_convert<int32_t>(res, arg) == 0) continue;
		if(try_convert<int64_t>(res, arg) == 0) continue;

		if(try_convert<long double>(res, arg) == 0) continue;
		if (try_convert<string>(res, arg) == 0) continue;

		res.emplace_back(arg);
	}

	return 0;
}

NAMESPACE_END(utility)

NAMESPACE_END(monoco)

#endif // __M_UTILITY_


#include <limits>
#include <vector>

#include <boost/test/unit_test.hpp>

#include "layout/interval.hpp"
#include "layout/overlap.hpp"
#include "layout/reuse.hpp"
#include "layout/source.hpp"

BOOST_AUTO_TEST_SUITE(overlap)

BOOST_AUTO_TEST_CASE(overlap_intervals) {
  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 1,
        .min_start_time = 0,
        .max_end_time = 1,
        .trim_intervals = {{1, std::numeric_limits<double>::max()}},
    };

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 1}),
                },
            .reuse = hisui::layout::Reuse::None,
        }));
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 1}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 1}),
                },
            .reuse = hisui::layout::Reuse::ShowNewest,
        }));
  }

  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 2,
        .min_start_time = 0,
        .max_end_time = 2,
        .trim_intervals = {{2, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 1, .end_time = 2}),
                },
            .reuse = hisui::layout::Reuse::None,
        }));

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 1, .end_time = 2}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 1, .end_time = 2}),
                },
            .reuse = hisui::layout::Reuse::ShowNewest,
        }));
  }

  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 2,
        .min_start_time = 0,
        .max_end_time = 3,
        .trim_intervals = {{3, std::numeric_limits<double>::max()}},
    };

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 2, .end_time = 3}),
                },
            .reuse = hisui::layout::Reuse::None,
        }));
  }
  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 1,
        .max_end_time = 3,
        .trim_intervals = {{3, std::numeric_limits<double>::max()}},
    };

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 2, .end_time = 3}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 2, .end_time = 3}),
                },
            .reuse = hisui::layout::Reuse::ShowNewest,
        }));
  }

  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 2,
        .min_start_time = 0,
        .max_end_time = 6,
        .trim_intervals = {{6, std::numeric_limits<double>::max()}},
    };

    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 2, .end_time = 4}),
                    hisui::layout::Interval({.start_time = 3, .end_time = 6}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));
  }

  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 3,
        .min_start_time = 0,
        .max_end_time = 8,
        .trim_intervals = {{8, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 0, .end_time = 8}),
                    hisui::layout::Interval({.start_time = 2, .end_time = 5}),
                    hisui::layout::Interval({.start_time = 5, .end_time = 6}),
                    hisui::layout::Interval({.start_time = 3, .end_time = 7}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));
  }

  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 1,
        .min_start_time = 1,
        .max_end_time = 7,
        .trim_intervals = {{0, 1},
                           {4, 5},
                           {7, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 1, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 2, .end_time = 4}),
                    hisui::layout::Interval({.start_time = 5, .end_time = 6}),
                    hisui::layout::Interval({.start_time = 6, .end_time = 7}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));
  }

  {
    auto expected = hisui::layout::OverlapIntervalsResult{
        .max_number_of_overlap = 2,
        .min_start_time = 1,
        .max_end_time = 7,
        .trim_intervals = {{0, 1},
                           {2, 3},
                           {4, 5},
                           {7, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_intervals({
            .intervals =
                {
                    hisui::layout::Interval({.start_time = 1, .end_time = 2}),
                    hisui::layout::Interval({.start_time = 3, .end_time = 4}),
                    hisui::layout::Interval({.start_time = 5, .end_time = 7}),
                    hisui::layout::Interval({.start_time = 6, .end_time = 7}),
                },
            .reuse = hisui::layout::Reuse::ShowOldest,
        }));
  }
}

BOOST_AUTO_TEST_CASE(overlap_trim_intervals) {
  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{0, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::overlap_trim_intervals(
                            {.list_of_trim_intervals = {
                                 {{0, std::numeric_limits<double>::max()}}}}));
  }
  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{0, 100},
                           {200, 300},
                           {400, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::overlap_trim_intervals(
                      {.list_of_trim_intervals = {
                           {{0, 100},
                            {200, 300},
                            {400, std::numeric_limits<double>::max()}}}}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{0, 100},
                           {200, 300},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::overlap_trim_intervals(
                            {.list_of_trim_intervals = {
                                 {{0, 100},
                                  {200, 300},
                                  {500, std::numeric_limits<double>::max()}},
                                 {{0, 200},
                                  {200, 400},
                                  {450, std::numeric_limits<double>::max()}},
                             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{0, 100}, {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{0, 100},
                  {200, 300},
                  {500, std::numeric_limits<double>::max()}},
                 {{0, 200}, {400, std::numeric_limits<double>::max()}},
                 {{0, 400}, {450, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{0, 100},
                  {200, 300},
                  {500, std::numeric_limits<double>::max()}},
                 {{0, 200}, {500, std::numeric_limits<double>::max()}},
                 {{500, std::numeric_limits<double>::max()}},
                 {{0, 400}, {500, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{0, 100},
                           {250, 300},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{0, 100},
                  {200, 300},
                  {500, std::numeric_limits<double>::max()}},
                 {{0, 200},
                  {250, 300},
                  {500, std::numeric_limits<double>::max()}},
                 {{0, 400}, {500, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{200, 300},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{0, 100},
                  {200, 350},
                  {500, std::numeric_limits<double>::max()}},
                 {{200, 300}, {500, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{200, 300},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{200, 300}, {500, std::numeric_limits<double>::max()}},
                 {{0, 100},
                  {200, 350},
                  {500, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{200, 300},
                           {325, 350},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{200, 300}, {325, std::numeric_limits<double>::max()}},
                 {{0, 100},
                  {200, 350},
                  {500, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{250, 300},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{0, 100},
                  {200, 350},
                  {500, std::numeric_limits<double>::max()}},
                 {{200, 300}, {500, std::numeric_limits<double>::max()}},
                 {{250, 400}, {500, std::numeric_limits<double>::max()}},
             }}));
  }

  {
    auto expected = hisui::layout::TrimIntervals{
        .trim_intervals = {{0, 2},
                           {3, 5},
                           {500, std::numeric_limits<double>::max()}},
    };
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::overlap_trim_intervals(
            {.list_of_trim_intervals = {
                 {{0, 2}, {3, std::numeric_limits<double>::max()}},
                 {{0, 2}, {3, 5}, {500, std::numeric_limits<double>::max()}},
             }}));
  }
}

BOOST_AUTO_TEST_SUITE_END()


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

//
// Created by Phillip Keldenich on 2019-09-23.
//

#pragma once

#include <gmpxx.h>
#include <mpfr.h>

#include <type_traits>
#include <iostream>
#include <sstream>
#include <fstream>
#include <utility>
#include <limits>
#include <climits>
#include <vector>

#include <cassert>
#include <climits>
#include <cstddef>
#include <cstdint>
#include <cmath>

#include <boost/io/ios_state.hpp>
#include <boost/config.hpp>

#include "ivarp/rounding.hpp"
#include "ivarp/bool.hpp"
#include "ivarp/metaprogramming.hpp"
#include "number/fwd.hpp"
#include "number/device_compat.hpp"
#include "number/exact_less_than.hpp"

namespace ivarp {
    /// A traits template that defines methods for getting lower and
    /// upper bounds as well as possible/definite undefinedness.
    template<typename NT> struct NumberTraits {
        static constexpr bool is_number = false;
    };

    /// A type and an object that can be passed to intervals to set the corresponding bound to infinity;
    /// needed for intervals if the underlying number type has no infinities.
    struct InfinityType {
        constexpr IVARP_HD InfinityType operator-() const noexcept { return InfinityType{}; }
    };

    static IVARP_CUDA_DEVICE_OR_CONSTEXPR InfinityType infinity{};

    template<typename Number> IVARP_HD constexpr static inline std::enable_if_t<!std::is_floating_point<Number>::value, bool>
        is_finite(const Number&) noexcept
    {
        return true;
    }

    template<typename Number> IVARP_HD static inline std::enable_if_t<std::is_floating_point<Number>::value, bool>
        is_finite(Number n) noexcept
    {
        return IVARP_NOCUDA_USE_STD isfinite(n);
    }

    template<typename Number> IVARP_HD static inline bool possibly_undefined(const Number& n) {
        return NumberTraits<Number>::possibly_undefined(n);
    }

    template<typename Number> IVARP_HD static inline bool definitely_defined(const Number& n) {
        return !NumberTraits<Number>::possibly_undefined(n);
    }

    template<typename Number>
        static inline IVARP_HD std::enable_if_t<NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> lb(const Number& n)
    {
        return NumberTraits<Number>::lb(n);
    }

    template<typename Number>
        static inline IVARP_H std::enable_if_t<!NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> lb(const Number& n)
    {
        return NumberTraits<Number>::lb(n);
    }

    template<typename Number>
        static inline IVARP_HD std::enable_if_t<NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> ub(const Number& n)
    {
        return NumberTraits<Number>::ub(n);
    }

    template<typename Number>
        static inline IVARP_H std::enable_if_t<!NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> ub(const Number& n)
    {
        return NumberTraits<Number>::ub(n);
    }

    template<typename IntType> using IsIntegral = std::integral_constant<bool,
        std::is_integral<BareType<IntType>>::value || std::is_same<BareType<IntType>, BigInt>::value
    >;

    template<typename T> struct IsRationalImpl : std::false_type {};
    template<typename A> struct IsRationalImpl<__gmp_expr<mpq_t, A>> : std::true_type {};

    template<typename NumType> using IsNumber = std::integral_constant<bool,
        NumberTraits<BareType<NumType>>::is_number>;

    template<bool IsNumber, typename NumType> struct IsCUDANumberImpl : std::false_type {};
    template<typename NumType> struct IsCUDANumberImpl<true, NumType> : std::integral_constant<bool, NumberTraits<NumType>::allows_cuda> {};
    template<typename NumType> using IsCUDANumber = IsCUDANumberImpl<IsNumber<NumType>::value, NumType>;

    template<typename T> using IsIntOrRational = std::integral_constant<bool,
            IsIntegral<T>::value || IsRational<T>::value>;

    template<typename T, bool B = IsNumber<T>::value> struct IsIntervalType : std::false_type {};
    template<typename T> struct IsIntervalType<T,true> : std::integral_constant<bool, NumberTraits<T>::is_interval> {};

    /// An interval class that also keeps track of definedness issues.
    template<typename NumberType> class Interval;

    // Intervals of float, double and Rational type.
    using IFloat  = Interval<float>;
    using IDouble = Interval<double>;
    using IRational = Interval<Rational>;

    /**
     * @brief Generate a (canonicalized) rational number from one or two integers.
     * Compared to the native GMP interface, this provides two benefits:
     * - It canonicalizes the number; failure to do so can results in difficult-to-diagnose bugs.
     * - It works with _all_ integral types including [unsigned] long long and std::[u]int64_t.
     * There also are versions where one or both of the arguments are BigInt.
     * @return The rational
     */
    template<typename IntegralType1, std::enable_if_t<IsIntegral<IntegralType1>::value,int> = 0>
        static inline Rational rational(IntegralType1 num);
    template<typename IntegralType1, typename IntegralType2>
        static inline std::enable_if_t<IsIntegral<IntegralType1>::value && IsIntegral<IntegralType2>::value, Rational>
            rational(IntegralType1 num, IntegralType2 denom);

    // a metafunction that computes the promoted number type for a set of argument types:
    // computations are done, unless additional promotions to intervals occur due to some operations, using
    // the number type with the highest rank between all arguments.
    // If a promotion to intervals occurs, it uses the interval version of the basic number type.
    // Rank is determined as follows:
    //  - Any interval type has higher rank than any number type. In particular, passing one float interval and
    //    a rational parameter leads to the computation being done on float intervals.
    //  - More precise number types have higher rank.
    template<typename Arg1, typename... Args> struct NumberTypePromotion;
    template<typename... Args> using Promote = typename NumberTypePromotion<BareType<Args>...>::type;
//  template<typename TargetType, typename SourceType> static inline
//      TargetType convert_number(const SourceType& source);
}

#include "number/mpfr.hpp"
#include "number/interval.hpp"
#include "number/traits.hpp"
#include "number/rational.hpp"
#include "number/fixed_point_bounds.hpp"
#include "number/fixed_point_bounds_sqrt.hpp"
#include "number/fixed_point_bounds_sin.hpp"
#include "number/fixed_point_bounds_cos.hpp"
#include "number/float_interval_ops.hpp"
#include "number/interval_comparisons.hpp"
#include "number/bounded_rational.hpp"
#include "number/type_conversions.hpp"
#include "number/minmax.hpp"
#include "number/literals.hpp"
#include "number/factorize.hpp"


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

/*
 * File:   Chain.cpp
 * Author: Mohammad Ali Sharpasand
 *
 * Created on March 17, 2016
 */

#include "Chain.hpp"
#include "Angle.hpp"
#include "Utility.hpp"

#include <stdexcept>
#include <vector>
#include <armadillo>
#include <ceres/ceres.h>
using namespace BipedLibrary;
using namespace BipedLibrary::Utility;
using ceres::CostFunction;
using ceres::SizedCostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

Chain::Chain(vec3 const posi_body_body, mat33 const ori_body) :
posi_body_body_(posi_body_body),
ori_body_(ori_body)
{

}

vec3 Chain::posi_body_body(int frame) const
{
    return ori_body_ * position_base_base(frame) + posi_body_body_;
}

mat33 const& Chain::ori_body() const
{
    return ori_body_;
}

vec3 Chain::posi_end_last_last() const
{
	return posi_end_last_last_;
}
mat33 Chain::ori_end_last() const
{
	return ori_end_last_;
}

Chain& Chain::setPosi_body_body(vec3 const& posi_body_body)
{

    posi_body_body_ = posi_body_body;
    return *this;
}

Chain& Chain::setOri_body(mat33 const& ori_body)
{
    ori_body_ = ori_body;
    return *this;
}

Chain& Chain::setPosi_end_last_last(vec3 const& posi_end_last_last)
{
	posi_end_last_last_ = posi_end_last_last;
	return *this;
}

Chain& Chain::setOri_end_last(mat33 const& ori_end_last)
{
	ori_end_last_ = ori_end_last;
	return *this;
}

vec3 Chain::position_end_body_body() const
{
	mat33 const ori_last_base = orientation_base();
	vec3 const posi_end_base_base = position_base_base() + ori_last_base * posi_end_last_last_;
	return posi_body_body_ + ori_body_ * posi_end_base_base;
}

vec3 Chain::position_base_base(int frame) const
{
    if(frame == -1)
        frame = size() - 1;

    if(frame < 0 || (size_t)frame >= size())
        throw(std::out_of_range("Index out of range in BipedLibrary::Chain::position_base_base(int)"));

    if(frame == 0)
        return position_pre_pre(frame);

    return position_base_base(frame - 1) + orientation_base(frame - 1) * position_pre_pre(frame);
}

vec3 Chain::position_body_body(int frame) const
{
	return posi_body_body_ + ori_body_ * position_base_base(frame);
}

vec3 Chain::position_pre_pre(int i) const
{
    vec3 output;

    try
    {
        output <<   at(i).r()                       << endr
               << - at(i).d() * at(i).alpha().sin() << endr
               <<   at(i).d() * at(i).alpha().cos() << endr;
    }
    catch(std::out_of_range &)
    {
        throw(std::out_of_range("Out of range index in Chain::position_pre_pre(int)."));
    }

    return output;
}

vec3 Chain::position_com(int frame) const
{
    // FIXME: This is not actually COM! Be warned!
    return vec3({at(frame).r() / 2.f, 0.f, 0.f});
}

mat33 Chain::orientation_end_body() const
{
	return ori_body_ * orientation_base() * ori_end_last_;

}

mat33 Chain::orientation_base(int frame) const
{
    if(frame == -1)
        frame = size() - 1;

    if(frame < 0 || (size_t)frame >= size())
        throw(std::out_of_range("Index out of range in BipedLibrary::Chain::orientation_base(int)"));

    if (frame == 0)
        return orientation_pre(frame);
    else
        return orientation_base(frame-1) * orientation_pre(frame);

}

mat33 Chain::orientation_body(int frame) const
{
	return ori_body_ * orientation_base(frame);
}

// TODO: This should be in DHFrame class
mat33 Chain::orientation_pre(int i) const
{
    mat33 output;
    Angle theta,alpha;
    theta = at(i).theta();
    alpha = at(i).alpha();
    try
    {
        output  << theta.cos()               << -theta.sin()              << 0            << endr
                << theta.sin() * alpha.cos() << theta.cos() * alpha.cos() << -alpha.sin() << endr
	            << theta.sin() * alpha.sin() << theta.cos() * alpha.sin() <<  alpha.cos() << endr;
    }
    catch(std::out_of_range &)
    {
        throw(std::out_of_range("Out of range index in Chain::orientation_pre(int)."));
    }

    return output;

}

mat66 Chain::jacobi_base(int frame, vec3 target) const
{
    mat66 jacobi = zeros(6, 6);
    vec3 const z = {0, 0, 1};
    for (int i=0; i<=frame; i++)
    {
        vec3 const z_i_base = orientation_base(i) * z;
        vec3 const r_frame_i_base = position_base_base(frame) - position_base_base(i) + orientation_base(frame) * target;
        jacobi.col(i).rows(0, 2) = crossProductMatrix(z_i_base) * r_frame_i_base;
        jacobi.col(i).rows(3, 5) = z_i_base;
    }

    return jacobi;
}

mat66 Chain::jacobi_body(int frame, vec3 target) const
{
	mat66 doubleOrientation = zeros(6, 6);
	doubleOrientation(span(0, 2), span(0, 2)) = ori_body();
	doubleOrientation(span(3, 5), span(3, 5)) = ori_body();

	return doubleOrientation * jacobi_base(frame, target);
}

bool Chain::solveForJointAngles(vec3 const& posi_end_body, mat33 ori_end_body)
{
//	google::InitGoogleLogging("");

	class IKCostFunction : public CostFunction
	{
	 public:
		IKCostFunction(Chain& chain, vec3 const& posi_end_body, mat33 ori_end_body)
	 	 : chain_(chain),
		   posi_end_body_(posi_end_body),
		   ori_end_body_(ori_end_body)
	 {
		mutable_parameter_block_sizes()->push_back(chain.size());

	   set_num_residuals(6);
	 }

	  virtual ~IKCostFunction() {}

	  virtual bool Evaluate(double const* const* parameters,
	                        double* residuals,
	                        double** jacobians) const
	  {
		  int i = 0;
		  for(DHFrame& frame : chain_)
		  {
			  frame.setTheta(parameters[0][i]);
			  i++;
		  }

		  vec3 const posiRes = chain_.position_end_body_body() - posi_end_body_;
		  for(int i = 0; i < 3; i++)
			  residuals[i] = posiRes(i);

		  mat33 const  oriResMat = ori_end_body_.t() * chain_.orientation_end_body();
		  vec3 const oriRes = Utility::rotationMatrixToAxisAngle(oriResMat).asAVector();

//		  vec3 const oriRes = Utility::rotationMatrixToAxisAngle(chain_.orientation_end_body()).asAVector() - Utility::rotationMatrixToAxisAngle(ori_end_body_).asAVector();
		  std::cout << "oriRes: " << oriRes << std::endl;

		  for(int i = 0; i < 3; i++)
			  residuals[i + 3] = oriRes(i);

			if (jacobians != NULL && jacobians[0] != NULL)
			{
				mat66 jacob = chain_.jacobi_body(5, chain_.posi_end_last_last());
				std::cout << "yagjouni: " << jacob << std::endl;
				for(int i = 0; i < 6; i++)
					for(int j = 0; j < 6; j++)
						jacobians[0][i * 6 + j] = jacob(i, j);

			}
			return true;
	  }
	 private:
	  Chain& chain_;
	  vec3 posi_end_body_;
	  mat33 ori_end_body_;
	};

	double joints[size()];
	std::vector<double*> parameters;
	parameters.push_back(joints);
	int i = 0;
	for(iterator ii = begin(); ii < end(); ii++, i++)
		joints[i] = ii->theta().toFloat();

	Problem problem;
	CostFunction *costFunction = new IKCostFunction(*this, posi_end_body, ori_end_body);

	problem.AddResidualBlock(costFunction, NULL, parameters);

	Solver::Options options;
	options.minimizer_progress_to_stdout = true;
	options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
	options.jacobi_scaling = true;
	options.check_gradients = true;
	Solver::Summary summary;
	Solve(options, &problem, &summary);
	std::cout << summary.BriefReport() << "\n";

	for(unsigned i = 0; i < size(); i++)
		at(i).setTheta(parameters.at(0)[i]);

	return true;
}


/*
 *              Copyright Andrey Semashev 2016.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file   bit_tools.hpp
 * \author Andrey Semashev
 * \date   16.01.2016
 *
 * \brief  This header is the Boost.Log library implementation, see the library documentation
 *         at http://www.boost.org/doc/libs/release/libs/log/doc/html/index.html.
 */

#ifndef BOOST_LOG_BIT_TOOLS_HPP_INCLUDED_
#define BOOST_LOG_BIT_TOOLS_HPP_INCLUDED_

#include <boost/log/detail/config.hpp>
#include <cstddef>
#include <boost/cstdint.hpp>
#include <boost/log/detail/header.hpp>

namespace boost {

BOOST_LOG_OPEN_NAMESPACE

namespace aux {

//! Checks that the integer is a power of 2.
template< typename T >
inline BOOST_CONSTEXPR bool is_power_of_2(T n) BOOST_NOEXCEPT
{
    return n != (T)0 && (n & (n - (T)1)) == (T)0;
}

//! Returns an integer comprising the four characters
inline BOOST_CONSTEXPR uint32_t make_fourcc(unsigned char c1, unsigned char c2, unsigned char c3, unsigned char c4) BOOST_NOEXCEPT
{
    return (static_cast< uint32_t >(c1) << 24) | (static_cast< uint32_t >(c2) << 16) | (static_cast< uint32_t >(c3) << 8) | static_cast< uint32_t >(c4);
}

} // namespace aux

BOOST_LOG_CLOSE_NAMESPACE // namespace log

} // namespace boost

#include <boost/log/detail/footer.hpp>

#endif // BOOST_LOG_BIT_TOOLS_HPP_INCLUDED_


/*
 * Copyright (c) 2011-2016, Graphics Lab, Georgia Tech Research Corporation
 * Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation
 * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

// Algorithm details and publications: http://www.golems.org/node/1570

#include "dart/planning/Path.hpp"

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

namespace dart {
namespace planning {

class LinearPathSegment : public PathSegment
{
public:
	LinearPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &end) :
    PathSegment((end-start).norm()),
		start(start),
    end(end)
	{
	}

	Eigen::VectorXd getConfig(double s) const {
		s /= length;
		s = std::max(0.0, std::min(1.0, s));
		return (1.0 - s) * start + s * end;
	}

	Eigen::VectorXd getTangent(double /* s */) const {
		return (end - start) / length;
	}

	Eigen::VectorXd getCurvature(double /* s */) const {
		return Eigen::VectorXd::Zero(start.size());
	}

	list<double> getSwitchingPoints() const {
		return list<double>();
	}

	LinearPathSegment* clone() const {
		return new LinearPathSegment(*this);
	}

private:
	Eigen::VectorXd start;
	Eigen::VectorXd end;
};

class CircularPathSegment : public PathSegment
{
public:
	CircularPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &intersection, const Eigen::VectorXd &end, double maxDeviation) {
		if((intersection - start).norm() < 0.000001 || (end - intersection).norm() < 0.000001) {
			length = 0.0;
			radius = 1.0;
			center = intersection;
			x = Eigen::VectorXd::Zero(start.size());
			y = Eigen::VectorXd::Zero(start.size());
			return;
		}

		const Eigen::VectorXd startDirection = (intersection - start).normalized();
		const Eigen::VectorXd endDirection = (end - intersection).normalized();

		if((startDirection - endDirection).norm() < 0.000001) {
			length = 0.0;
			radius = 1.0;
			center = intersection;
			x = Eigen::VectorXd::Zero(start.size());
			y = Eigen::VectorXd::Zero(start.size());
			return;
		}

    // const double startDistance = (start - intersection).norm();
    // const double endDistance = (end - intersection).norm();

		double distance = std::min((start - intersection).norm(), (end - intersection).norm());
		const double angle = acos(startDirection.dot(endDirection));

		distance = std::min(distance, maxDeviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle)));  // enforce max deviation

		radius = distance / tan(0.5 * angle);
		length = angle * radius;

		center = intersection + (endDirection - startDirection).normalized() * radius / cos(0.5 * angle);
		x = (intersection - distance * startDirection - center).normalized();
		y = startDirection;

		//debug
		double dotStart = startDirection.dot((intersection - getConfig(0.0)).normalized());
		double dotEnd = endDirection.dot((getConfig(length) - intersection).normalized());
    if(std::abs(dotStart - 1.0) > 0.0001 || std::abs(dotEnd - 1.0) > 0.0001) {
			std::cout << "Error\n";
		}
	}

	Eigen::VectorXd getConfig(double s) const {
		const double angle = s / radius;
		return center + radius * (x * cos(angle) + y * sin(angle));
	}

	Eigen::VectorXd getTangent(double s) const {
		const double angle = s / radius;
		return - x * sin(angle) + y * cos(angle);
	}

	Eigen::VectorXd getCurvature(double s) const {
		const double angle = s / radius;
		return - 1.0 / radius * (x * cos(angle) + y * sin(angle));
	}

	list<double> getSwitchingPoints() const {
		list<double> switchingPoints;
		const double dim = x.size();
		for(unsigned int i = 0; i < dim; i++) {
			double switchingAngle = atan2(y[i], x[i]);
			if(switchingAngle < 0.0) {
				switchingAngle += M_PI;
			}
			const double switchingPoint = switchingAngle * radius;
			if(switchingPoint < length) {
				switchingPoints.push_back(switchingPoint);
			}
		}
		switchingPoints.sort();
		return switchingPoints;
	}

	CircularPathSegment* clone() const {
		return new CircularPathSegment(*this);
	}

private:
	double radius;
	Eigen::VectorXd center;
	Eigen::VectorXd x;
	Eigen::VectorXd y;
};

Path::Path(const list<VectorXd> &path, double maxDeviation) :
	length(0.0)
{
	if(path.size() < 2)
		return;
	list<VectorXd>::const_iterator config1 = path.begin();
	list<VectorXd>::const_iterator config2 = config1;
  ++config2;
	list<VectorXd>::const_iterator config3;
	VectorXd startConfig = *config1;
	while(config2 != path.end()) {
		config3 = config2;
    ++config3;
		if(maxDeviation > 0.0 && config3 != path.end()) {
			CircularPathSegment* blendSegment = new CircularPathSegment(0.5 * (*config1 + *config2), *config2, 0.5 * (*config2 + *config3), maxDeviation);
			VectorXd endConfig = blendSegment->getConfig(0.0);
			if((endConfig - startConfig).norm() > 0.000001) {
				pathSegments.push_back(new LinearPathSegment(startConfig, endConfig));
			}
			pathSegments.push_back(blendSegment);

			startConfig = blendSegment->getConfig(blendSegment->getLength());

			//debug
			if(((endConfig - *config1).norm() > 0.000001 && (*config2 - endConfig).norm() > 0.000001
        && std::abs((endConfig - *config1).normalized().dot((*config2 - endConfig).normalized()) - 1.0) > 0.000001)
				|| ((startConfig - *config2).norm() > 0.000001 && (*config3 - startConfig).norm() > 0.000001
        && std::abs((startConfig - *config2).normalized().dot((*config3 - startConfig).normalized()) - 1.0) > 0.000001)) {
					cout << "error" << endl;
			}
		}
		else {
			pathSegments.push_back(new LinearPathSegment(startConfig, *config2));
			startConfig = *config2;
		}
		config1 = config2;
    ++config2;
	}

	// create list of switching point candidates, calculate total path length and absolute positions of path segments
  for(list<PathSegment*>::iterator segment = pathSegments.begin(); segment != pathSegments.end(); ++segment) {
		(*segment)->position = length;
		list<double> localSwitchingPoints = (*segment)->getSwitchingPoints();
    for(list<double>::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); ++point) {
			switchingPoints.push_back(make_pair(length + *point, false));
		}
		length += (*segment)->getLength();
		switchingPoints.push_back(make_pair(length, true));
	}
	switchingPoints.pop_back();
}

Path::Path(const Path &path) :
	length(path.length),
	switchingPoints(path.switchingPoints)
{
  for(list<PathSegment*>::const_iterator it = path.pathSegments.begin(); it != path.pathSegments.end(); ++it) {
		pathSegments.push_back((*it)->clone());
	}
}

Path::~Path() {
  for(list<PathSegment*>::iterator it = pathSegments.begin(); it != pathSegments.end(); ++it) {
		delete *it;
	}
}

double Path::getLength() const {
	return length;
}

PathSegment* Path::getPathSegment(double &s) const {
	list<PathSegment*>::const_iterator it = pathSegments.begin();
	list<PathSegment*>::const_iterator next = it;
  ++next;
	while(next != pathSegments.end() && s >= (*next)->position) {
		it = next;
    ++next;
	}
	s -= (*it)->position;
	return *it;
}

VectorXd Path::getConfig(double s) const {
	const PathSegment* pathSegment = getPathSegment(s);
	return pathSegment->getConfig(s);
}

VectorXd Path::getTangent(double s) const {
	const PathSegment* pathSegment = getPathSegment(s);
	return pathSegment->getTangent(s);
}

VectorXd Path::getCurvature(double s) const {
	const PathSegment* pathSegment = getPathSegment(s);
	return pathSegment->getCurvature(s);
}

double Path::getNextSwitchingPoint(double s, bool &discontinuity) const {
	list<pair<double, bool> >::const_iterator it = switchingPoints.begin();
	while(it != switchingPoints.end() && it->first <= s) {
    ++it;
	}
	if(it == switchingPoints.end()) {
		discontinuity = true;
		return length;
	}
	else {
		discontinuity = it->second;
		return it->first;
	}
}

list<pair<double, bool> > Path::getSwitchingPoints() const {
	return switchingPoints;
}

} // namespace planning
} // namespace dart


#define BOOST_MPL11_NO_ASSERTIONS
#include <boost/mpl11/detail/left_folds/bounded.hpp>
#include <boost/mpl11/list.hpp>

<%= render('_main.erb') do |f, state, xs|
    list = "boost::mpl11::list<#{xs.join(', ')}>"
    <<-EOS
    boost::mpl11::detail::left_folds::bounded<
        #{xs.size}, #{f}::type::apply, #{state}, #{list}
    >
    EOS
end %>


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  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_FUNCTION_SCALAR_NEGS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_NEGS_HPP_INCLUDED

#include <boost/simd/function/definition/negs.hpp>
#include <boost/simd/arch/common/scalar/function/negs.hpp>

#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns  -x if c is @ref True and x otherwise.

    @par Header <boost/simd/function/if_neg.hpp>

    @par Example:

      @snippet negif.cpp negif

    @par Possible output:

      @snippet negif.txt negif

  **/
  Value1 if_neg(Value0 const& c, Value1 const& x);
} }
#endif

#include <boost/simd/function/scalar/if_neg.hpp>
#include <boost/simd/function/simd/if_neg.hpp>

#endif


#include <boost/program_options.hpp>
#include <chrono>
#include <cmath>
#include <map>
#include <random>
#include <set>

#include "../tools/util.hpp"
#include "bases.hpp"
#include "bilinear_form.hpp"

int bsd_rnd() {
  static unsigned int seed = 0;
  int a = 1103515245;
  int c = 12345;
  unsigned int m = 2147483648;
  return (seed = (a * seed + c) % m);
}

using namespace Time;
using namespace datastructures;

namespace po = boost::program_options;

template <typename I>
long long MaxIndex(int level);

template <>
long long MaxIndex<OrthonormalWaveletFn>(int level) {
  return std::pow(2, level);
}

template <>
long long MaxIndex<ThreePointWaveletFn>(int level) {
  return std::pow(2, level - 1);
}

template <typename I>
TreeVector<I> GradedTree(I *meta_root, int max_lvl, bool towards_origin,
                         double theta = 0.5) {
  TreeVector<I> result(meta_root);
  result.DeepRefine(/* call_filter */
                    [&](auto psi) {
                      if (towards_origin)
                        return (psi->level() <= max_lvl &&
                                psi->index() <=
                                    std::pow(1 + theta, psi->level()));
                      else
                        return (psi->level() <= max_lvl &&
                                psi->index() >=
                                    MaxIndex<I>(psi->level()) -
                                        std::pow(1 + theta, psi->level()));
                    }, /* call_postprocess */
                    [&](auto nv) {
                      nv->set_random();
                      nv->node()->Refine();
                    });
  return result;
}
auto Now() { return std::chrono::high_resolution_clock::now(); }
double Duration(std::chrono::high_resolution_clock::time_point start) {
  return std::chrono::duration<double>(Now() - start).count();
}

template <template <typename, typename> class Operator, typename WaveletBasisIn,
          typename WaveletBasisOut>
void TimeBilForm(std::string name,
                 const datastructures::TreeVector<WaveletBasisIn> &vec_in,
                 const datastructures::TreeVector<WaveletBasisOut> &vec_out) {
  double time_create = 0, time_apply = 0, time_apply_upp = 0,
         time_apply_low = 0;
  size_t iters = 0;
  while (time_apply < 20 && iters < 30) {
    ++iters;
    auto time_start = Now();
    auto bilform = CreateBilinearForm<Operator>(vec_in, vec_out);
    time_create += Duration(time_start);

    time_start = Now();
    bilform.Apply();
    time_apply += Duration(time_start);

    time_start = Now();
    bilform.ApplyUpp();
    time_apply_upp += Duration(time_start);

    time_start = Now();
    bilform.ApplyLow();
    time_apply_low += Duration(time_start);
  }
  std::cout << "\n\ttime-" << name << "-create: " << time_create / iters;
  std::cout << "\n\ttime-" << name << "-apply: " << time_apply / iters;
  std::cout << "\n\ttime-" << name << "-apply-upp: " << time_apply_upp / iters;
  std::cout << "\n\ttime-" << name << "-apply-low: " << time_apply_low / iters;
  std::cout << std::flush;
}

int main(int argc, char *argv[]) {
  double theta = 0.3;
  bool print_mesh = false;
  size_t max_iter = 999;
  boost::program_options::options_description adapt_optdesc("Refine options");
  adapt_optdesc.add_options()("theta", po::value<double>(&theta))(
      "print_mesh", po::value<bool>(&print_mesh))("max_iter",
                                                  po::value<size_t>(&max_iter));
  boost::program_options::options_description cmdline_options;
  cmdline_options.add(adapt_optdesc);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(),
            vm);
  po::notify(vm);
  std::cout << "Adaptive options:" << std::endl;
  std::cout << "\ttheta: " << theta << std::endl << std::endl;
  std::cout << std::endl;

  Bases B;
  int iter = 0;
  bool uniform_refine = theta >= 1;
  while (iter < max_iter) {
    std::cout << "iter: " << ++iter;

    if (uniform_refine) {
      TreeVector<OrthonormalWaveletFn> ortho_tree(B.ortho_tree.meta_root());
      TreeVector<ThreePointWaveletFn> threept_tree(
          B.three_point_tree.meta_root());
      ortho_tree.UniformRefine(std::array{iter}, /* grow_tree */ true);
      threept_tree.UniformRefine(std::array{iter}, /* grow_tree */ true);
      std::cout << "\n\tortho-tree-size: " << ortho_tree.Bfs().size()
                << "\n\tthreept-tree-size: " << threept_tree.Bfs().size()
                << "\n\ttotal-memory-kB: " << getmem() << std::flush;
      TimeBilForm<MassOperator>("M-o-o", ortho_tree, ortho_tree);
      TimeBilForm<MassOperator>("M-t-o", threept_tree, ortho_tree);
      TimeBilForm<TransportOperator>("T-t-o", threept_tree, ortho_tree);
    } else {
      // Local refine.
      auto ortho_tree_0 =
          GradedTree(B.ortho_tree.meta_root(), iter, true, theta);
      auto ortho_tree_1 =
          GradedTree(B.ortho_tree.meta_root(), iter, false, theta);
      auto threept_tree_0 =
          GradedTree(B.three_point_tree.meta_root(), iter, true, theta);
      auto threept_tree_1 =
          GradedTree(B.three_point_tree.meta_root(), iter, false, theta);
      std::cout << "\n\tortho-tree-0-size: " << ortho_tree_0.Bfs().size()
                << "\n\tortho-tree-1-size: " << ortho_tree_1.Bfs().size()
                << "\n\tthreept-tree-0-size: " << threept_tree_0.Bfs().size()
                << "\n\tthreept-tree-1-size: " << threept_tree_1.Bfs().size()
                << "\n\ttotal-memory-kB: " << getmem() << std::flush;

      TimeBilForm<MassOperator>("M-o0-o0", ortho_tree_0, ortho_tree_0);
      // TimeBilForm<MassOperator>("M-o0-o1", ortho_tree_0, ortho_tree_1);
      // TimeBilForm<MassOperator>("M-t0-t0", threept_tree_0, threept_tree_0);
      // TimeBilForm<MassOperator>("M-t0-t1", threept_tree_0, threept_tree_1);
      // TimeBilForm<MassOperator>("M-o0-t0", ortho_tree_0, threept_tree_0);
      // TimeBilForm<MassOperator>("M-o0-t1", ortho_tree_0, threept_tree_1);
      TimeBilForm<MassOperator>("M-t0-o0", threept_tree_0, ortho_tree_0);
      TimeBilForm<MassOperator>("M-t0-o1", threept_tree_0, ortho_tree_1);

      TimeBilForm<TransportOperator>("T-t0-o0", threept_tree_0, ortho_tree_0);
      TimeBilForm<TransportOperator>("T-t0-o1", threept_tree_0, ortho_tree_1);
      // TimeBilForm<TransportOperator>("T-t1-o0", threept_tree_1,
      // ortho_tree_0);

      if (print_mesh) {
        std::cout << "\n\tortho-tree-0: [";
        for (auto nv : ortho_tree_0.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
        std::cout << "\n\tortho-tree-1: [";
        for (auto nv : ortho_tree_1.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
        std::cout << "\n\tthreept-tree-0: [";
        for (auto nv : threept_tree_0.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
        std::cout << "\n\tthreept-tree-1: [";
        for (auto nv : threept_tree_1.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
      }
    }
    std::cout << std::endl;
  }
  return 0;
}


/*
Copyright 2012 Rogier van Dalen.

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

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

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

/** \file
Test floating-point parser.
*/

#define BOOST_TEST_MODULE float_parser
#include "utility/test/boost_unit_test.hpp"

#include "parse_ll/number/float.hpp"

#include <string>

#include "range/core.hpp"
#include "range/std/container.hpp"

#include <boost/math/special_functions/sign.hpp>

#include "../helper/fuzz_parser.hpp"
#include "parse_ll/core/skip.hpp"
#include "parse_ll/core/literal.hpp"

BOOST_AUTO_TEST_SUITE(test_parse_float)

BOOST_AUTO_TEST_CASE (test_float) {
    using range::empty; using range::first; using range::drop;

    using parse_ll::parse;
    using parse_ll::success;
    using parse_ll::output;
    using parse_ll::rest;
    {
        std::string r ("e+5");
        {
            auto parser = parse_ll::float_exponent;
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), 5);

            // Error message contains type:
            //int i = parser;
        }
    }
    {
        std::string r ("52.41");
        {
            auto parser = parse_ll::skip (parse_ll::literal (' ')) [
                parse_ll::mantissa_parser <double>()];
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (std::get <0>(output (result)), 5241);
        }
    }
    {
        std::string r ("52.41 44");
        {
            // Don't consume the space.
            auto parser = parse_ll::skip (parse_ll::literal (' ')) [
                parse_ll::mantissa_parser <double>()];
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (std::get <0>(output (result)), 5241);
        }
    }
    {
        std::string r ("52.41e50");
        {
            auto parser = parse_ll::mantissa_parser <double>();
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (std::get <0>(output (result)), 5241);

            // Error message contains type:
            //int i = parser;
        }
    }

    {
        std::string r ("");
        {
            auto parser = fuzz (parse_ll::float_);
            auto result = parse (parser, r);
            BOOST_CHECK (!success (result));
        }
    }
    {
        std::string r ("a");
        {
            auto parser = parse_ll::float_;
            auto result = parse (parser, r);
            BOOST_CHECK (!success (result));
        }
    }
    {
        std::string r ("0");
        {
            auto parser = fuzz (parse_ll::float_);
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), 0);
            BOOST_CHECK (empty (rest (result)));
        }
    }
    {
        std::string r ("-0");
        {
            auto parser = fuzz (parse_ll::float_as <float>());
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), -0);
            BOOST_CHECK (boost::math::signbit (output (result)));
            BOOST_CHECK (empty (rest (result)));
        }
    }
    {
        std::string r ("+6.25 99");
        {
            auto parser = parse_ll::skip (parse_ll::literal (' ')) [
                parse_ll::float_];
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), 6.25);
            BOOST_CHECK_EQUAL (first (rest (result)), ' ');
        }
    }
    {
        std::string r ("+6.25e50");
        {
            auto parser = fuzz (parse_ll::float_);
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), 6.25e50);
            BOOST_CHECK (empty (rest (result)));
        }
        {
            auto parser = fuzz (parse_ll::float_);
            auto o = output (parse (parser, r));
            BOOST_CHECK_EQUAL (o, 6.25e50);
        }
    }
    {
        std::string r ("-6371");
        {
            auto parser = fuzz (parse_ll::float_);
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), -6371);
            BOOST_CHECK (empty (rest (result)));
        }
    }
    {
        std::string r ("-.3654");
        {
            auto parser = parse_ll::float_as <float>();
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), -.3654f);
        }
    }
    {
        std::string r ("-0.3654");
        {
            auto parser = parse_ll::float_as <float>();
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), -.3654f);
        }
    }
    {
        std::string r ("1984756.2345952301358780564e50");
        {
            auto parser = fuzz (parse_ll::float_as <double>());
            auto result = parse (parser, r);
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result), 1984756.2345952301358780564e50);
        }
    }

    // \todo Test overflow errors?
    /*
    // Overflow when the exponent gets added
    {
        std::string r ("1984756.2345952301358780564e50");
        {
            auto parser = parse_ll::float_as <float>();
            auto result = output (parse (parser, r));
        }
    }
    // Overflow while constructing the float should not happen.
    {
        std::string r ("1984756.23459523013587805644531854651321354651321");
        {
            auto parser = parse_ll::float_as <float>();
            auto result = output (parse (parser, r));
            BOOST_CHECK (success (result));
            BOOST_CHECK_EQUAL (output (result),
                1984756.23459523013587805644531854651321354651321f);
        }
    }*/

}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <boost/graph/adjacency_list.hpp>

struct VertexProp{
  unsigned index;
  VertexProp(const unsigned _idx = 0): index(_idx) {};
};
typedef boost::adjacency_list<
      boost::vecS, // OutEdgeList
      boost::vecS,     // VertexList
      boost::undirectedS, // (bi)directed ?
      VertexProp
    > RawGraph;

// in the graph, vertices are pairs of (int,char): the index of the character and the state
class Graph: public RawGraph
{
  using RawGraph::RawGraph;

public:
  typedef typename boost::graph_traits<RawGraph>::vertex_descriptor Vertex;
  typedef typename boost::graph_traits<RawGraph>::edge_descriptor   Edge;
  typedef std::pair<unsigned, char> VertexName;
  typedef typename boost::graph_traits<Graph>::adjacency_iterator   AdjIter;
  typedef typename std::pair<AdjIter, AdjIter>                      AdjIterRange;
  typedef typename boost::graph_traits<Graph>::vertex_iterator      VertexIter;
  typedef typename std::pair<VertexIter, VertexIter>                VertexIterRange;

protected:
  boost::unordered_map<VertexName, Vertex> name_to_vertex;

public:

  size_t num_vertices() const
  {
    return boost::num_vertices(*this);
  }

  AdjIterRange adjacent_vertices(const Vertex& u) const
  {
    return boost::adjacent_vertices(u, *this);
  }

  VertexIterRange vertices() const
  {
    return boost::vertices(*this);
  }

  size_t num_edges() const
  {
    return boost::num_edges(*this);
  }

  unsigned get_index(const Vertex& u) const
  {
    return (*this)[u].index;
  }

  Vertex add_vertex(const unsigned _index)
  {
    return boost::add_vertex(VertexProp(_index), *this);
  }

  // return the vertex with the specified name or create one if the name does not exist
  const Vertex& emplace_vertex_by_name(const VertexName& vname)
  {
    const auto n2v_iter = name_to_vertex.find(vname);
    if(n2v_iter == name_to_vertex.end())
      return name_to_vertex.emplace_hint(n2v_iter, vname, add_vertex(boost::num_vertices(*this)))->second;
    else
      return n2v_iter->second;
  }

  template<typename Container = std::vector<Vertex>>
  void make_clique(const Container& clique)
  {
    // translate vertex names to vertices
    for(auto u_iter = clique.begin(); u_iter != clique.end(); ++u_iter)
      for(auto v_iter = std::next(u_iter); v_iter != clique.end(); ++v_iter)
        boost::add_edge(*u_iter, *v_iter, *this);
  }
};


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "mismatch_fraction.hpp"

#include <iterator>
#include <algorithm>
#include <cassert>

#include <boost/variant.hpp>

#include "io/variant/vcf_record.hpp"
#include "utils/concat.hpp"

namespace octopus { namespace csr {

const std::string MismatchFraction::name_ = "MF";

MismatchFraction::MismatchFraction()
: mismatch_count_ {}
, depth_ {true, false}
{}

std::unique_ptr<Measure> MismatchFraction::do_clone() const
{
    return std::make_unique<MismatchFraction>(*this);
}

Measure::ResultType MismatchFraction::get_default_result() const
{
    return std::vector<double> {};
}

Measure::ResultType MismatchFraction::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto depths = boost::get<std::vector<std::size_t>>(depth_.evaluate(call, facets));
    const auto mismatch_counts = boost::get<std::vector<int>>(mismatch_count_.evaluate(call, facets));
    assert(depths.size() == mismatch_counts.size());
    std::vector<double> result(depths.size());
    std::transform(std::cbegin(mismatch_counts), std::cend(mismatch_counts), std::cbegin(depths), std::begin(result),
                   [] (auto mismatches, auto depth) { return depth > 0 ? static_cast<double>(mismatches) / depth : 0.0; });
    return result;
}

Measure::ResultCardinality MismatchFraction::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& MismatchFraction::do_name() const
{
    return name_;
}

std::string MismatchFraction::do_describe() const
{
    return "Fraction of reads with mismatches at variant position";
}

std::vector<std::string> MismatchFraction::do_requirements() const
{
    return concat(depth_.requirements(), mismatch_count_.requirements());
}

} // namespace csr
} // namespace octopus


/* RevKit: A Toolkit for Reversible Circuit Design (www.revkit.org)
 * Copyright (C) 2009-2014  The RevKit Developers <revkit@informatik.uni-bremen.de>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "truth_table_widget.hpp"

#include <boost/range/algorithm.hpp>
#include <boost/range/irange.hpp>

#include <reversible/truth_table.hpp>
#include <reversible/functions/circuit_to_truth_table.hpp>
#include <reversible/simulation/simple_simulation.hpp>

using namespace cirkit;

TruthTableWidget::TruthTableWidget( const circuit& circ, QWidget * parent )
  : QTableWidget( (1u << circ.lines() ), 2u * circ.lines(), parent )
{
  binary_truth_table spec;
  circuit_to_truth_table( circ, spec, simple_simulation_func() );

  QStringList hlabels, vlabels;

  boost::for_each( spec.inputs(), [&hlabels]( const std::string& i ) { hlabels.append( QString::fromStdString( i ) ); } );
  boost::for_each( spec.outputs(), [&hlabels]( const std::string& o ) { hlabels.append( QString::fromStdString( o ) ); } );
  boost::for_each( boost::irange( 0u, 1u << spec.num_inputs() ), [&vlabels]( unsigned n ) { vlabels.append( QString::number( n ) ); } );

  setHorizontalHeaderLabels( hlabels );
  setVerticalHeaderLabels( vlabels );
  setAlternatingRowColors( true );
  setShowGrid( false );

  unsigned row = 0u;
  for ( const auto& line : spec )
  {
    bool valid = true;
    for ( unsigned i = 0u; i < spec.num_inputs(); ++i )
    {
      // TODO add circuit as reference
    }

    unsigned col = 0u;
    for ( const auto& in : boost::make_iterator_range( line.first ) )
    {
      addItem( row, col, *in, !circ.constants()[col] || *circ.constants()[col] == *in );
      col++;
    }
    for ( const auto& out : boost::make_iterator_range( line.second ) )
    {
      addItem( row, col++, *out, !circ.garbage()[col - circ.lines()] );
    }

    ++row;
  }

  resizeColumnsToContents();
}

void TruthTableWidget::addItem( unsigned row, unsigned column, bool value, bool enabled )
{
  QTableWidgetItem * item = new QTableWidgetItem( value ? "1" : "0" );
  int flags = Qt::ItemIsSelectable;
  if ( enabled )
  {
    flags |= Qt::ItemIsEnabled;
  }
  item->setFlags( (Qt::ItemFlag)flags );
  setItem( row, column, item );
}

#include "src/truth_table_widget.moc"

// Local Variables:
// c-basic-offset: 2
// End: