/*
  Copyright (C) 2016 Ahmed Riza

  This file is part of MathFin.

  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/>.
*/

/*
  Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
  Copyright (C) 2003, 2004, 2005, 2006 StatPro Italia srl
  Copyright (C) 2004, 2005, 2006 Ferdinando Ametrano
  Copyright (C) 2006 Katiuscia Manzoni
  Copyright (C) 2006 Toyin Akin
  Copyright (C) 2015 Klaus Spanderen

  QuantLib is free software: you can redistribute it and/or modify it
  under the terms of the QuantLib license.  You should have received a
  copy of the license along with this program; if not, please email
  <quantlib-dev@lists.sf.net>. The license is also available online at
  <http://quantlib.org/license.shtml>.

  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 license for more details.
*/

#include <iostream>
#include <time/date.hpp>
#include <base/error.hpp>
#include <base/conversion.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>

using boost::posix_time::ptime;
using boost::posix_time::time_duration;

namespace MathFin {

  // Helper functions used in other parts of the code.
  namespace {
    const boost::gregorian::date& serialNumberDateReference() {
      static const boost::gregorian::date dateReference(
        1899, boost::gregorian::Dec, 30);
      return dateReference;
    }

#define compatibleEnums \
    (int(boost::date_time::Monday)      +1 == as_integer(Weekday::Monday) \
     && int(boost::date_time::Tuesday)  +1 == as_integer(Weekday::Tuesday) \
     && int(boost::date_time::Wednesday)+1 == as_integer(Weekday::Wednesday) \
     && int(boost::date_time::Thursday) +1 == as_integer(Weekday::Thursday) \
     && int(boost::date_time::Friday)   +1 == as_integer(Weekday::Friday) \
     && int(boost::date_time::Saturday) +1 == as_integer(Weekday::Saturday) \
     && int(boost::date_time::Sunday)   +1 == as_integer(Weekday::Sunday) \
     && int(boost::date_time::Jan) == as_integer(Month::January)        \
     && int(boost::date_time::Feb) == as_integer(Month::February)       \
     && int(boost::date_time::Mar) == as_integer(Month::March)          \
     && int(boost::date_time::Apr) == as_integer(Month::April)          \
     && int(boost::date_time::May) == as_integer(Month::May)            \
     && int(boost::date_time::Jun) == as_integer(Month::June)           \
     && int(boost::date_time::Jul) == as_integer(Month::July)           \
     && int(boost::date_time::Aug) == as_integer(Month::August)         \
     && int(boost::date_time::Sep) == as_integer(Month::September)      \
     && int(boost::date_time::Oct) == as_integer(Month::October)        \
     && int(boost::date_time::Nov) == as_integer(Month::November)       \
     && int(boost::date_time::Dec) == as_integer(Month::December))

    template <bool compatible>
    Weekday mapBoostDateType2MF(boost::gregorian::greg_weekday d) {
      if (compatible) {
        return Weekday(d.as_number() + 1);
      } else {
        switch (d) {
        case boost::date_time::Monday   : return Weekday::Monday;
        case boost::date_time::Tuesday  : return Weekday::Tuesday;
        case boost::date_time::Wednesday: return Weekday::Wednesday;
        case boost::date_time::Thursday : return Weekday::Thursday;
        case boost::date_time::Friday   : return Weekday::Friday;
        case boost::date_time::Saturday : return Weekday::Saturday;
        case boost::date_time::Sunday   : return Weekday::Sunday;
        default:
          MF_FAIL("Unknown boost date_time day of week given");
        }
      }
    }

    template <bool compatible>
    Month mapBoostDateType2MF(boost::gregorian::greg_month m) {
      if (compatible) {
        return Month(m.as_number());
      } else {
        switch (m) {
        case boost::date_time::Jan : return Month::January;
        case boost::date_time::Feb : return Month::February;
        case boost::date_time::Mar : return Month::March;
        case boost::date_time::Apr : return Month::April;
        case boost::date_time::May : return Month::May;
        case boost::date_time::Jun : return Month::June;
        case boost::date_time::Jul : return Month::July;
        case boost::date_time::Aug : return Month::August;
        case boost::date_time::Sep : return Month::September;
        case boost::date_time::Oct : return Month::October;
        case boost::date_time::Nov : return Month::November;
        case boost::date_time::Dec : return Month::December;
        default:
          MF_FAIL("Unknown boost date_time month of week given");
        }
      }
    }

    template <bool compatible>
    boost::gregorian::greg_month mapMFDateType2Boost(Month m) {
      if (compatible) {
        return boost::gregorian::greg_month(as_integer(m));
      } else {
        switch (m) {
        case Month::January  : return boost::date_time::Jan;
        case Month::February : return boost::date_time::Feb;
        case Month::March    : return boost::date_time::Mar;
        case Month::April    : return boost::date_time::Apr;
        case Month::May      : return boost::date_time::May;
        case Month::June     : return boost::date_time::Jun;
        case Month::July     : return boost::date_time::Jul;
        case Month::August   : return boost::date_time::Aug;
        case Month::September: return boost::date_time::Sep;
        case Month::October  : return boost::date_time::Oct;
        case Month::November : return boost::date_time::Nov;
        case Month::December : return boost::date_time::Dec;
        default:
          MF_FAIL("Unknown boost date_time month of week given");
        }
      }
    }

    boost::gregorian::date gregorianDate(Year y, Month m, Day d) {
      MF_REQUIRE(y > 1900 && y < 2200,
                 "year " << y << " out of bound. It must be in [1901,2199]");
      MF_REQUIRE(Integer(m) > 0 && Integer(m) < 13,
                 "month " << Integer(m)
                 << " outside January-December range [1,12]");

      const boost::gregorian::greg_month gregorianMonth =
        mapMFDateType2Boost<compatibleEnums>(m);

      const Day endOfMonthDay =
        boost::gregorian::gregorian_calendar::end_of_month_day(y, gregorianMonth);
      MF_REQUIRE(d <= endOfMonthDay && d > 0,
                 "Day outside month (" << m << ") day-range "
                 << "[1," << endOfMonthDay << "]");

      return boost::gregorian::date(y, gregorianMonth, d);
    }
  }

  // ---------------------------------------------------------------------------

  Date::Date() : dateTime_(serialNumberDateReference()) {}

  Date::Date(Date::serial_type serialNumber)
    : dateTime_(serialNumberDateReference() + boost::gregorian::days(serialNumber)) {
    checkSerialNumber(serialNumber);
  }

  Date::Date(Day d, Month m, Year y) : dateTime_(gregorianDate(y, m, d)) {}

  Date::Date(const ptime& localTime) : dateTime_(localTime) {}

  Date::Date(Day d,
             Month m,
             Year y,
             Hour hours,
             Minute minutes,
             Second seconds,
             Millisecond millisec,
             Microsecond microsec)
    : dateTime_(
      gregorianDate(y, m, d),
      boost::posix_time::time_duration(
        hours, minutes, seconds,
        millisec * (time_duration::ticks_per_second()/1000)
        + microsec*(time_duration::ticks_per_second()/1000000)))
  {}

  // ---------------------------------------------------------------------------

  Weekday Date::weekday() const {
    return mapBoostDateType2MF<compatibleEnums>(dateTime_.date().day_of_week());
  }

  Day Date::dayOfMonth() const {
    return dateTime_.date().day();
  }

  Day Date::dayOfYear() const {
    return dateTime_.date().day_of_year();
  }

  Month Date::month() const {
    return mapBoostDateType2MF<compatibleEnums>(dateTime_.date().month());
  }

  Year Date::year() const {
    return dateTime_.date().year();
  }

  Date::serial_type Date::serialNumber() const {
    const Date::serial_type n = (
      dateTime_.date() - serialNumberDateReference()).days();
    checkSerialNumber(n);
    return n;
  }

  Hour Date::hours() const {
    return dateTime_.time_of_day().hours();
  }

  Minute Date::minutes() const {
    return dateTime_.time_of_day().minutes();
  }

  Second Date::seconds() const {
    return dateTime_.time_of_day().seconds();
  }

  Millisecond Date::milliseconds() const {
    return dateTime_.time_of_day().fractional_seconds()
      / (ticksPerSecond()/1000);
  }

  Microsecond Date::microseconds() const {
    return (dateTime_.time_of_day().fractional_seconds()
            - milliseconds()*(time_duration::ticks_per_second()/1000))
      / (ticksPerSecond()/1000000);
  }

  Time Date::fractionOfDay() const {
    const time_duration t = dateTime_.time_of_day();
    const Time seconds = (t.hours()*60.0 + t.minutes())*60.0 + t.seconds()
      + Real(t.fractional_seconds()) / ticksPerSecond();
    return seconds / 86400.0; // ignore any DST hocus-pocus
  }

  Time Date::fractionOfSecond() const {
    return dateTime_.time_of_day().fractional_seconds()
      / Real(ticksPerSecond());
  }

  Real Date::lengthOfYear() const {
    return isLeap(year()) ? 366.0 : 365.0;
  }

  // ---------------------------------------------------------------------------
  // static private members.

  Date::serial_type Date::minimumSerialNumber() {
    return 367;       // Jan 1st, 1901
  }

  Date::serial_type Date::maximumSerialNumber() {
    return 109574;    // Dec 31st, 2199
  }

  void Date::checkSerialNumber(Date::serial_type serialNumber) {
    // TODO
    // Need to implement std::ostream& operator<<(std::ostream&, const Date&);
    /*
    MF_REQUIRE(serialNumber >= minimumSerialNumber() &&
               serialNumber <= maximumSerialNumber(),
               "Date's serial number (" << serialNumber << ") outside "
               "allowed range [" << minimumSerialNumber() <<
               "-" << maximumSerialNumber() << "], i.e. [" <<
               minDate() << "-" << maxDate() << "]");
    */

    MF_REQUIRE(serialNumber >= minimumSerialNumber() &&
               serialNumber <= maximumSerialNumber(),
               "Date's serial number (" << serialNumber << ") outside "
               "allowed range [" << minimumSerialNumber() <<
               ", ... ," << maximumSerialNumber() << "]";
      );
  }

  // ---------------------------------------------------------------------------
  // static public methods

  Date Date::todaysDate() {
    boost::gregorian::date current_date(boost::gregorian::day_clock::local_day());
    return Date(
      Day(current_date.day()),
      Month(current_date.month().as_number()),
      Year(current_date.year())
      );
  }

  Date Date::minDate() {
    static const Date minimumDate(minimumSerialNumber());
    return minimumDate;
  }

  Date Date::maxDate() {
    static const Date maximumDate(maximumSerialNumber());
    return maximumDate;
  }

  bool Date::isLeap(Year y) {
    return boost::gregorian::gregorian_calendar::is_leap_year(y);
  }

  Date Date::endOfMonth(const Date& d) {
    const Month m = d.month();
    const Year y = d.year();
    const Day eoM = boost::gregorian::gregorian_calendar::end_of_month_day(
      d.year(), mapMFDateType2Boost<compatibleEnums>(d.month()));
    return Date(eoM, m, y);
  }

  bool Date::isEndOfMonth(const Date& d) {
    return d.dayOfMonth() ==
      boost::gregorian::gregorian_calendar::end_of_month_day(
        d.year(), mapMFDateType2Boost<compatibleEnums>(d.month()));
  }

  Date Date::nextWeekday(const Date& d, Weekday dayOfWeek) {
    Weekday wd = d.weekday();
    return d + ((wd > dayOfWeek ? 7 : 0) - as_integer(wd) + as_integer(dayOfWeek));
  }

  Date Date::nthWeekday(Size nth, Weekday dayOfWeek,
                        Month m, Year y) {
    MF_REQUIRE(nth>0,
               "zeroth day of week in a given (month, year) is undefined");
    MF_REQUIRE(nth<6,
               "no more than 5 weekday in a given (month, year)");
    Weekday first = Date(1, m, y).weekday();
    Size skip = nth - (dayOfWeek >= first ? 1 : 0);
    return Date((1 + as_integer(dayOfWeek) + skip * 7) - as_integer(first), m, y);
  }

  Date Date::localDateTime() {
    return Date(boost::posix_time::microsec_clock::local_time());
  }

  Date Date::universalDateTime() {
    return Date(boost::posix_time::microsec_clock::universal_time());
  }

  const Size Date::ticksPerSecond() {
    return time_duration::ticks_per_second();
  }

  // ---------------------------------------------------------------------------
  // date algebra

  namespace {
    void advance(ptime& dt, Integer n, TimeUnit units) {
      if (units == TimeUnit::Days) {
        dt += boost::gregorian::days(n);
      } else if (units == TimeUnit::Weeks) {
        dt += boost::gregorian::weeks(n);
      } else if (units == TimeUnit::Months || units == TimeUnit::Years) {
        const boost::gregorian::date date = dt.date();
        const Day endOfMonthDay =
          boost::gregorian::gregorian_calendar::end_of_month_day(
            date.year(), date.month());
        if (units == TimeUnit::Months) {
          dt += boost::gregorian::months(n);
        } else {
          dt += boost::gregorian::years(n);
        }
        if (date.day() == endOfMonthDay) {
          // avoid snap-to-end-of-month behavior of boost::date_time
          const Day newEndOfMonthDay
            = boost::gregorian::gregorian_calendar::end_of_month_day(
              dt.date().year(),
              dt.date().month()
              );
          if (newEndOfMonthDay > endOfMonthDay) {
            dt -= boost::gregorian::days(newEndOfMonthDay - endOfMonthDay);
          }
        }
      } else {
        MF_FAIL("Unsupported time units: " << units);
      }
    }
  } // end anonymous namespace

  Date Date::operator+(Date::serial_type days) const {
    return Date(dateTime_ + boost::gregorian::days(days));
  }

  Date Date::operator-(Date::serial_type days) const {
    return Date(dateTime_ - boost::gregorian::days(days));
  }

  Date Date::operator+(const Period& p) const {
    ptime dateTime = dateTime_;
    advance(dateTime, p.length(), p.units());
    return Date(dateTime);
  }

  Date Date::operator-(const Period& p) const {
    ptime dateTime = dateTime_;
    advance(dateTime, -p.length(), p.units());
    return Date(dateTime);
  }

  // ---------------------------------------------------------------------------

  // difference in days between dates
  Date::serial_type operator-(const Date& d1, const Date& d2) {
    return (d1.dateTime().date() - d2.dateTime().date()).days();
  }

  // Difference in days (including fraction of days) between dates
  Time daysBetween(const Date& d1, const Date& d2) {
    const Date::serial_type days = d2 - d1;
    return days + d2.fractionOfDay() - d1.fractionOfDay();
  }

  // ---------------------------------------------------------------------------

  bool operator==(const Date& d1, const Date& d2) {
    return (d1.dateTime() == d2.dateTime());
  }

  bool operator!=(const Date& d1, const Date& d2) {
    return (d1.dateTime() != d2.dateTime());
  }

  bool operator<(const Date& d1, const Date& d2) {
    return (d1.dateTime() < d2.dateTime());
  }

  bool operator<=(const Date& d1, const Date& d2) {
    return (d1.dateTime() <= d2.dateTime());
  }

  bool operator>(const Date& d1, const Date& d2) {
    return (d1.dateTime() > d2.dateTime());
  }

  bool operator>=(const Date& d1, const Date& d2) {
    return (d1.dateTime() >= d2.dateTime());
  }

  // ---------------------------------------------------------------------------

  std::ostream& operator<<(std::ostream& out, const Date& d) {
    out << d.year() << "-"
        << std::setw(2) << std::setfill('0') << as_integer(d.month()) << "-"
        << std::setw(2) << std::setfill('0') << d.dayOfMonth()
        << "T"
        << std::setw(2) << std::setfill('0') << d.hours() << ":"
        << std::setw(2) << std::setfill('0') << d.minutes() << ":"
        << std::setw(2) << std::setfill('0') << d.seconds() << ","
        << std::setw(3) << std::setfill('0') << d.milliseconds()
        << std::setw(3) << std::setfill('0') << d.microseconds();

    return out;
  }

  // ---------------------------------------------------------------------------

}


/**
 * @file
 * @brief Tests for RelativeOrientations() method
 * @author Raffael Casagrande
 * @date   2018-07-01 01:15:55
 * @copyright MIT License
 */

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

#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include "lf/mesh/test_utils/test_meshes.h"
#include "mesh_factory_test.h"

namespace lf::mesh::hybrid2d::test {
// Output orientations of edges for test mesh
TEST(lf_hybrid2d, lf_orientation) {
  std::cout << "### TEST: mesh orientation" << std::endl;
  // Building the test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(0);

  EXPECT_EQ(mesh_p->NumEntities(0), 9) << "Test mesh: 9 cells expected!";
  EXPECT_EQ(mesh_p->NumEntities(1), 18) << "Test mesh: 18 edges expected!";
  EXPECT_EQ(mesh_p->NumEntities(2), 10) << "Test mesh: 10 nodes expected!";

  std::vector<std::vector<lf::mesh::Orientation>> o{
      {lf::mesh::Orientation::positive, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::positive},
      {lf::mesh::Orientation::positive, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::positive},
      {lf::mesh::Orientation::positive, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::negative},
      {lf::mesh::Orientation::positive, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::negative},
      {lf::mesh::Orientation::positive, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::negative},
      {lf::mesh::Orientation::negative, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::negative, lf::mesh::Orientation::positive},
      {lf::mesh::Orientation::negative, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::positive, lf::mesh::Orientation::positive},
      {lf::mesh::Orientation::negative, lf::mesh::Orientation::positive,
       lf::mesh::Orientation::negative},
      {lf::mesh::Orientation::negative, lf::mesh::Orientation::negative,
       lf::mesh::Orientation::negative}};

  // Run through cells and output edge orientations
  for (const mesh::Entity &cell : mesh_p->Entities(0)) {
    const lf::base::glb_idx_t cell_idx(mesh_p->Index(cell));
    std::cout << cell << ' ' << cell_idx << ": ";
    // Array of edges
    lf::base::RandomAccessRange<const lf::mesh::Entity> edges(
        cell.SubEntities(1));
    // Array of orientations
    lf::base::RandomAccessRange<const lf::mesh::Orientation> oris(
        cell.RelativeOrientations());
    for (int ed_idx = 0; ed_idx < cell.RefEl().NumSubEntities(1); ed_idx++) {
      std::cout << ", edge " << mesh_p->Index(edges[ed_idx])
                << ": or = " << to_char(oris[ed_idx]) << ' ';
      EXPECT_EQ(o[cell_idx][ed_idx], oris[ed_idx])
          << "Orientation mismatch, cell " << cell_idx << ", edge " << ed_idx;
    }
    std::cout << std::endl;
  }
}

}  // namespace lf::mesh::hybrid2d::test


/*
 * rsxgltest - manycubes
 */

#define GL3_PROTOTYPES
#include <GL3/gl3.h>
#include <GL3/gl3ext.h>

#include "rsxgltest.h"
#include "math3d.h"
#include "sine_wave.h"

#include <stddef.h>
#include "cube_vert.h"
#include "cube_frag.h"

#include <io/pad.h>

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

const char * rsxgltest_name = "manycubes";

struct sine_wave_t rgb_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f
  },
  {
    0.5f,
    0.5f,
    1.5f
  },
  {
    0.5f,
    0.5f,
    2.5f
  }
};

struct sine_wave_t xyz_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f / 4.0f
  },
  {
    0.5f,
    0.5f,
    1.5f / 4.0f
  },
  {
    0.5f,
    0.5f,
    2.5f / 4.0f
  }
};

GLuint buffers[2] = { 0,0 };
GLuint texture = 0;

GLuint shaders[2] = { 0,0 };
GLuint program = 0;

GLuint query = 0;

GLint ProjMatrix_location = -1, TransMatrix_location = -1, color_location = -1, ncubes_location = -1, texture_location = -1;

#define DTOR(X) ((X)*0.01745329f)
#define RTOD(d) ((d)*57.295788f)

Eigen::Projective3f ProjMatrix(perspective(DTOR(54.3),1920.0 / 1080.0,0.1,1000.0));

Eigen::Affine3f ViewMatrixInv =
  Eigen::Affine3f(Eigen::Affine3f::Identity() *
		  (
		   Eigen::Translation3f(1.779,2.221,4.034) *
		   (
		    Eigen::AngleAxisf(DTOR(0),Eigen::Vector3f::UnitZ()) *
		    Eigen::AngleAxisf(DTOR(23.8),Eigen::Vector3f::UnitY()) *
		    Eigen::AngleAxisf(DTOR(-26.738),Eigen::Vector3f::UnitX())
		    )
		   )
		  ).inverse();

const GLuint ncubes = 100;

extern "C"
void
rsxgltest_pad(unsigned int,const padData * paddata)
{
  if(paddata -> BTN_UP) {
    tcp_printf("up\n");
  }
  else if(paddata -> BTN_DOWN) {
    tcp_printf("down\n");
  }
}

extern "C"
void
rsxgltest_init(int argc,const char ** argv)
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);

  // Set up us the program:
  shaders[0] = glCreateShader(GL_VERTEX_SHADER);
  shaders[1] = glCreateShader(GL_FRAGMENT_SHADER);

  program = glCreateProgram();

  glAttachShader(program,shaders[0]);
  glAttachShader(program,shaders[1]);

  // Supply shader binaries:
  //glShaderBinary(1,shaders,0,cube_vpo,cube_vpo_size);
  //glShaderBinary(1,shaders + 1,0,cube_fpo,cube_fpo_size);

  // Supply shader SOURCES!
  char szInfo[2048];

  const GLchar * shader_srcs[] = { (const GLchar *)cube_vert, (const GLchar *)cube_frag };
  GLint shader_srcs_lengths[] = { cube_vert_len, cube_frag_len };
  GLint compiled = 0;

  glShaderSource(shaders[0],1,shader_srcs,shader_srcs_lengths);
  glCompileShader(shaders[0]);

  glGetShaderiv(shaders[0],GL_COMPILE_STATUS,&compiled);
  tcp_printf("shader compile status: %i\n",compiled);

  glGetShaderInfoLog(shaders[0],2048,0,szInfo);
  tcp_printf("%s\n",szInfo);

  glShaderSource(shaders[1],1,shader_srcs + 1,shader_srcs_lengths + 1);
  glCompileShader(shaders[1]);

  glGetShaderiv(shaders[1],GL_COMPILE_STATUS,&compiled);
  tcp_printf("shader compile status: %i\n",compiled);

  glGetShaderInfoLog(shaders[1],2048,0,szInfo);
  tcp_printf("%s\n",szInfo);

  // Link the program for real:
  glLinkProgram(program);
  glValidateProgram(program);

  summarize_program("draw",program);

  GLint
    vertex_location = glGetAttribLocation(program,"position");

  color_location = glGetAttribLocation(program,"color");

  ProjMatrix_location = glGetUniformLocation(program,"ProjMatrix");
  TransMatrix_location = glGetUniformLocation(program,"TransMatrix");
  ncubes_location = glGetUniformLocation(program,"ncubes");
  texture_location = glGetUniformLocation(program,"texture");

  tcp_printf("vertex_location: %i\n",vertex_location);
  tcp_printf("color_location: %i\n",color_location);
  tcp_printf("ProjMatrix_location: %i TransMatrix_location: %i ncubes_location: %i texture_location: %i\n",
	     ProjMatrix_location,TransMatrix_location,ncubes_location,texture_location);

  glUseProgram(program);

  glUniformMatrix4fv(ProjMatrix_location,1,GL_FALSE,ProjMatrix.data());

  glUniform1f(ncubes_location,(float)ncubes);
  glUniform1i(texture_location,0);

  // Set up us the vertex data:
  const float geometry[] = {
    // -X
    -0.5,-0.5,-0.5,
    1,0,0,

    -0.5,0.5,-0.5,
    1,0,0,

    -0.5,0.5,0.5,
    1,0,0,

    -0.5,-0.5,0.5,
    1,0,0,

    // +X
    0.5,-0.5,-0.5,
    1,0,0,

    0.5,-0.5,0.5,
    1,0,0,

    0.5,0.5,0.5,
    1,0,0,

    0.5,0.5,-0.5,
    1,0,0,

    // -Y
    -0.5,-0.5,-0.5,
    0,1,0,

    -0.5,-0.5,0.5,
    0,1,0,

    0.5,-0.5,0.5,
    0,1,0,

    0.5,-0.5,-0.5,
    0,1,0,

    // +Y
    -0.5,0.5,-0.5,
    0,1,0,

    0.5,0.5,-0.5,
    0,1,0,

    0.5,0.5,0.5,
    0,1,0,

    -0.5,0.5,0.5,
    0,1,0,

    // -Z
    -0.5,-0.5,-0.5,
    0,0,1,

    -0.5,0.5,-0.5,
    0,0,1,

    0.5,0.5,-0.5,
    0,0,1,

    0.5,-0.5,-0.5,
    0,0,1,

    // +Z
    -0.5,-0.5,0.5,
    0,0,1,

    -0.5,0.5,0.5,
    0,0,1,

    0.5,0.5,0.5,
    0,0,1,

    0.5,-0.5,0.5,
    0,0,1
  };

  const GLuint indices[] = {
    // -X
    0, 1, 2,
    2, 3, 0,

    // +X
    4, 5, 6,
    6, 7, 4,

    // -Y
    8, 9, 10,
    10, 11, 8,

    // +Y
    12, 13, 14,
    14, 15, 12,

    // -Z
    16, 17, 18,
    18, 19, 16,

    // +Z
    20, 21, 22,
    22, 23, 20
  };

  glGenBuffers(2,buffers);

  glBindBuffer(GL_ARRAY_BUFFER,buffers[0]);

  glBufferData(GL_ARRAY_BUFFER,sizeof(float) * 6 * 4 * 6,geometry,GL_STATIC_DRAW);
  glEnableVertexAttribArray(vertex_location);
  glEnableVertexAttribArray(color_location);
  glVertexAttribPointer(vertex_location,3,GL_FLOAT,GL_FALSE,sizeof(float) * 6,0);
  glVertexAttribPointer(color_location,3,GL_FLOAT,GL_FALSE,sizeof(float) * 6,(const GLvoid *)(sizeof(float) * 3));

  glBindBuffer(GL_ARRAY_BUFFER,0);

  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,buffers[1]);
  glBufferData(GL_ELEMENT_ARRAY_BUFFER,sizeof(GLuint) * 6 * 2 * 3,indices,GL_STATIC_DRAW);

  // the texture:
  glGenTextures(1,&texture);
  glBindTexture(GL_TEXTURE_1D,texture);

  glTexStorage1D(GL_TEXTURE_1D,1,GL_RGBA32F,ncubes * 4);

  {
    float * tmp = new float[ncubes * 16];
    float * ptmp = tmp;

    for(unsigned int i = 0;i < ncubes;++i,ptmp += 16) {
      const float x = (float)(i % 10) / 10.0 * 30.0 - 15.0;
      const float y = (float)(i / 10) / 10.0 * 30.0 - 15.0;
      const float z = 0.0;

      Eigen::Affine3f m = Eigen::Affine3f::Identity() * Eigen::Translation3f(x,y,z);
      memcpy(ptmp,m.data(),sizeof(float) * 16);
    }

    glTexSubImage1D(GL_TEXTURE_1D,0,0,ncubes * 4,GL_RGBA,GL_FLOAT,tmp);

    delete [] tmp;
  }

  glTexParameteri(GL_TEXTURE_1D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
  glTexParameteri(GL_TEXTURE_1D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);

  glGenQueries(1,&query);
}

extern "C"
int
rsxgltest_draw()
{
  float rgb[3] = {
    compute_sine_wave(rgb_waves,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 2,rsxgltest_elapsed_time)
  };

  glClearColor(rgb[0],rgb[1],rgb[2],1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  float xyz[3] = {
    compute_sine_wave(xyz_waves,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 2,rsxgltest_elapsed_time)
  };

  {
    glBeginQuery(GL_SAMPLES_PASSED,query);

    Eigen::Affine3f rotmat =
      Eigen::Affine3f::Identity() *
      Eigen::AngleAxisf(DTOR(xyz[2]) * 360.0f,Eigen::Vector3f::UnitZ()) *
      Eigen::AngleAxisf(DTOR(xyz[1]) * 360.0f,Eigen::Vector3f::UnitY()) *
      Eigen::AngleAxisf(DTOR(xyz[0]) * 360.0f,Eigen::Vector3f::UnitX());

    Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * rotmat);
    glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());
    glDrawElementsInstanced(GL_TRIANGLES,36,GL_UNSIGNED_INT,0,ncubes);

    glEndQuery(GL_SAMPLES_PASSED);

    GLuint samples = 1357;
    glGetQueryObjectuiv(query,GL_QUERY_RESULT,&samples);
    tcp_printf("samples: %u\n",(unsigned int)samples);
  }

  GLsync s = glFenceSync(GL_SYNC_GPU_COMMANDS_COMPLETE,0);
  glClientWaitSync(s,GL_SYNC_FLUSH_COMMANDS_BIT,1e9);

  {
    //glBeginQuery(GL_SAMPLES_PASSED,query);

    Eigen::Affine3f rotmat =
      Eigen::Affine3f::Identity() *
      Eigen::AngleAxisf(DTOR(xyz[2]) * 360.0f,Eigen::Vector3f::UnitZ()) *
      Eigen::AngleAxisf(DTOR(xyz[1]) * 360.0f,Eigen::Vector3f::UnitY()) *
      Eigen::AngleAxisf((DTOR(xyz[0] + 180.0f)) + 180.0f,Eigen::Vector3f::UnitX());

    Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * rotmat);
    glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());
    glDrawElementsInstanced(GL_TRIANGLES,36,GL_UNSIGNED_INT,0,ncubes);

    //glEndQuery(GL_SAMPLES_PASSED);
  }

  return 1;
}

extern "C"
void
rsxgltest_exit()
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  glDeleteShader(shaders[0]);
  glDeleteProgram(program);
  glDeleteShader(shaders[1]);
}


#define BOOST_TEST_MODULE Misc.Coords
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "Helpers.h"
#include "Misc/Coords.h"
#include "Misc/Direction.h"
#include "Misc/Exceptions.h"
#include <algorithm>
#include <sstream>

BOOST_AUTO_TEST_CASE(Constructors)
{
    // Check that constructors assign the correct values
    Coords defaultCoords;
    BOOST_CHECK_EQUAL(defaultCoords, Coords(0, 0));

    Coords nonDefaultCoords(2, 4);
    BOOST_CHECK_EQUAL(nonDefaultCoords, Coords(2, 4));

    Coords copy(nonDefaultCoords);
    BOOST_CHECK_EQUAL(copy, Coords(2, 4));
}

BOOST_AUTO_TEST_CASE(Adjacent)
{
    Coords coords(2, 4);
    Coords adjacent = coords.Adjacent(Direction::Up);
    BOOST_CHECK_EQUAL(adjacent.X, coords.X);
    BOOST_CHECK_EQUAL(adjacent.Y, coords.Y - 1);
}

BOOST_AUTO_TEST_CASE(Move)
{
    Coords coords(2, 4);
    coords.Move(Direction::Up);
    BOOST_CHECK_EQUAL(coords.X, 2);
    BOOST_CHECK_EQUAL(coords.Y, 3);
}

BOOST_AUTO_TEST_CASE(SharesAxis)
{
    // Check both axes for correct results
    Coords coords(2, 4);
    Coords verticallyAlignedCoords(2, 6);
    Coords horizontallyAlignedCoords(5, 4);
    Coords unalignedCoords(5, 6);

    BOOST_CHECK(coords.SharesAxis(verticallyAlignedCoords));
    BOOST_CHECK(coords.SharesAxis(horizontallyAlignedCoords));
    BOOST_CHECK(!coords.SharesAxis(unalignedCoords));
}

BOOST_AUTO_TEST_CASE(StraightPath)
{
    Coords from(2, 4);
    Coords to(7, 4);
    auto path = from.StraightPath(to);
    // Check size
    BOOST_CHECK_EQUAL(path.size(), 6);

    // Check all elements of the path are present
    for (int x = 2; x <= 7; x++)
    {
        BOOST_CHECK(std::find(path.begin(), path.end(), Coords(x, 4)) != path.end());
    }

    // Check that a non-straight path throws an InvalidPositionException
    Coords invalidTo(7, 5);
    BOOST_CHECK_THROW(from.StraightPath(invalidTo), InvalidPositionException);
}

BOOST_AUTO_TEST_CASE(Distance)
{
    // Check the distance is correct when from and to share an axis
    Coords from(2, 4);
    Coords toSharesAxis(2, 8);
    BOOST_CHECK_EQUAL(from.Distance(toSharesAxis), 4);

    // Check combined distances also work
    Coords to(5, 8);
    BOOST_CHECK_EQUAL(from.Distance(to), 7);
}

BOOST_AUTO_TEST_CASE(Operators)
{
    // Check operator==
    Coords a(2, 4);
    Coords a2(2, 4);
    BOOST_CHECK(a == a2);
    // Check operator!=
    Coords b(1, 1);
    BOOST_CHECK(a != b);
    // Check operator+
    BOOST_CHECK_EQUAL(a + b, Coords(3, 5));
    // Check operator-
    BOOST_CHECK_EQUAL(a - b, Coords(1, 3));

    // Check operator+=
    a += b;
    BOOST_CHECK_EQUAL(a, Coords(3, 5));
    // Check operator-=
    a -= b;
    BOOST_CHECK_EQUAL(a, a2);

    // Check operator<<
    a = {2, 4};
    CheckStringConversion(a, "[2, 4]");
}


#include "knapsacksolver/algorithms/algorithms.hpp"

#include <boost/program_options.hpp>

using namespace knapsacksolver;

int main(int argc, char *argv[])
{
    namespace po = boost::program_options;

    // Parse program options

    std::string algorithm = "bellman_array";
    std::string instance_path = "";
    std::string output_path = "";
    std::string certificate_path = "";
    std::string log_path = "";
    std::string format = "standard";
    int loglevelmax = 999;
    int seed = 0;
    double time_limit = std::numeric_limits<double>::infinity();

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("algorithm,a", po::value<std::string>(&algorithm), "set algorithm")
        ("input,i", po::value<std::string>(&instance_path)->required(), "set input path (required)")
        ("format,f", po::value<std::string>(&format), "set input file format (default: standard)")
        ("output,o", po::value<std::string>(&output_path), "set JSON output path")
        ("certificate,c", po::value<std::string>(&certificate_path), "set certificate path")
        ("time-limit,t", po::value<double>(&time_limit), "set time limit (in s)")
        ("seed,s", po::value<int>(&seed), "set seed")
        ("verbose,v", "enable verbosity")
        ("log,l", po::value<std::string>(&log_path), "set log path")
        ("loglevelmax", po::value<int>(&loglevelmax), "set log max level")
        ("log2stderr", "write log to stderr")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
        std::cout << desc << std::endl;;
        return 1;
    }
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        return 1;
    }

    // Run algorithm

    std::mt19937_64 gen(seed);
    Instance instance(instance_path, format);

    Info info = Info()
        .set_verbose(vm.count("verbose"))
        .set_time_limit(time_limit)
        .set_certificate_path(certificate_path)
        .set_json_output_path(output_path)
        .set_only_write_at_the_end(true)
        .set_log_path(log_path)
        .set_log2stderr(vm.count("log2stderr"))
        .set_maximum_log_level(loglevelmax)
        ;

    auto output = run(algorithm, instance, gen, info);

    if (instance.optimal_solution() != NULL) {
        if (output.solution.feasible() && output.solution.profit() > instance.optimum()) {
            std::cerr << "\033[31m" << "ERROR, computed solution strictly better than provided optimum." << "\033[0m" << std::endl;
            return 1;
        }
        if (output.upper_bound != -1 && output.upper_bound < instance.optimum()) {
            std::cerr << "\033[31m" << "ERROR, computed upper bound strictly lower than provided optimum." << "\033[0m" << std::endl;
            return 1;
        }
    }

    return 0;
}


// Copyright 2020 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     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.

#include <boost/math/special_functions/beta.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <stdexcept>

// Check values of Boost's `ibeta_derivative` implementation using double
// precision (to match JavaScript as close as possible).
// https://www.boost.org/doc/libs/1_73_0/libs/math/doc/html/math_toolkit/sf_beta/beta_derivative.html

int main(int argc, char** argv) {
  // Parameters for the derivative of the regularized incomplete beta function.
  double a = 0;
  double b = 0;
  double x = 0;

  try {
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()("help,h", "print help message")(
        "a,a", po::value<double>(&a)->required(), "parameter a")(
        "b,b", po::value<double>(&b)->required(), "parameter b")(
        "x,x", po::value<double>(&x)->required(), "parameter x");

    po::positional_options_description pos_options;
    pos_options.add("a", 1).add("b", 1).add("x", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv)
                  .options(desc)
                  .positional(pos_options)
                  .run(),
              vm);

    if (vm.count("help")) {
      std::cout << "Calculate ∂/∂x I_x(a, b) in double precision.\n"
                << "Usage:\n"
                << "  " << argv[0] << " <a> <b> <x>\n"
                << "  " << argv[0] << " --help\n";
      return 1;
    }

    // Notify about missing arg(s) after --help.
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  std::cerr << "ibeta_derivative(" << a << ", " << b << ", " << x << ")\n";

  // Most (all?) JS engines use SSE. In order to match, don't allow Boost to
  // auto-promote to extended (80 bit) precision.
  typedef boost::math::policies::policy<
      boost::math::policies::promote_double<false> >
      double_policy;

  double ibeta_value = boost::math::ibeta_derivative(a, b, x, double_policy());

  std::cout
      // Make sure enough digits are printed.
      << std::setprecision(std::numeric_limits<double>::max_digits10)
      << ibeta_value << "\n";
}


/*=============================================================================
Copyright 2018 Pranam Lashkari <plashkari628@gmail.com>
Copyright 2019 Sarthak Singhal <singhalsarthak2007@gmail.com>
Copyright 2020 Rohit Ranjan    <rohitrjn629@gmail.com>

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

#ifndef BOOST_ASTRONOMY_COORDINATE_SPHERICAL_COSLAT_DIFFERENTIAL_HPP
#define BOOST_ASTRONOMY_COORDINATE_SPHERICAL_COSLAT_DIFFERENTIAL_HPP

#include <tuple>
#include <type_traits>

#include <math.h>
#include <boost/geometry/strategies/strategy_transform.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/static_assert.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

#include <boost/astronomy/detail/is_base_template_of.hpp>
#include <boost/astronomy/coordinate/diff/base_differential.hpp>
#include <boost/astronomy/coordinate/diff/cartesian_differential.hpp>
#include <boost/astronomy/coordinate/diff/spherical_differential.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

//!Represents the differential in spherical representation including cos(latitude) term
//!Uses three components to represent a differential (dlatitude, dlongitude_coslat, ddistance)

template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename DistQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>
>
struct spherical_coslat_differential : public base_differential
    <3, geometry::cs::spherical<radian>, CoordinateType>
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG(
        ((std::is_same<typename bu::get_dimension<LatQuantity>::type,
            bu::plane_angle_dimension>::value) &&
            (std::is_same<typename bu::get_dimension<LonQuantity>::type,
            bu::plane_angle_dimension>::value)),
        "Latitude and Longitude must be of plane angle type");
    BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
        "CoordinateType must be a floating-point type");
    ///@endcond

public:
    typedef LatQuantity quantity1;
    typedef LonQuantity quantity2;
    typedef DistQuantity quantity3;

    //default constructor no initialization
    spherical_coslat_differential() {}

    //!constructs object from provided components of differential
    spherical_coslat_differential
    (
        LatQuantity const& dlat,
        LonQuantity const& dlon_coslat,
        DistQuantity const& ddistance
    )
    {
        this->set_dlat_dlon_coslat_ddist(dlat, dlon_coslat, ddistance);
    }

    //!constructs object from boost::geometry::model::point object
    template
    <
        std::size_t OtherDimensionCount,
        typename OtherCoordinateSystem,
        typename OtherCoordinateType
    >
    spherical_coslat_differential
    (
        bg::model::point
        <
            OtherCoordinateType,
            OtherDimensionCount,
            OtherCoordinateSystem
        > const& pointObject
    )
    {
        bg::model::point<OtherCoordinateType, 3, bg::cs::cartesian> temp;
        bg::transform(pointObject, temp);
        bg::transform(temp, this->diff);
    }

    //copy constructor
    spherical_coslat_differential
    (
        spherical_coslat_differential
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        > const& other
    )
    {
        this->diff = other.get_differential();
    }

    // !constructs object from any type of differential
    template <typename Differential>
    spherical_coslat_differential(Differential const& other)
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_differential, Differential>::value),
            "No constructor found with given argument type");

        BOOST_STATIC_ASSERT_MSG(
        ((std::is_same<typename bu::get_dimension<DistQuantity>::type,
        typename bu::get_dimension<typename Differential::quantity3>::type>::value)),
        "Two differentials must have same dimensions");

        auto tempDiff = make_spherical_coslat_differential(other);
        bg::model::point
        <
            typename std::conditional
            <
                sizeof(CoordinateType) >= sizeof(typename Differential::type),
                CoordinateType,
                typename Differential::type
            >::type,
            3,
            bg::cs::spherical<radian>
        > tempPoint;

        bg::set<0>(tempPoint,
            static_cast<
            bu::quantity<bu::si::plane_angle, CoordinateType>
            >(tempDiff.get_dlat()).value());
        bg::set<1>(tempPoint,
            static_cast<
            bu::quantity<bu::si::plane_angle, CoordinateType>
            >(tempDiff.get_dlon_coslat()).value());
        bg::set<2>(tempPoint,
            static_cast<DistQuantity>(tempDiff.get_ddist()).value());

        this->diff = tempPoint;
    }

    //! returns the (dlat, dlon_coslat, ddistance) in the form of tuple
    std::tuple<LatQuantity, LonQuantity, DistQuantity> get_dlat_dlon_coslat_ddist() const
    {
        return std::make_tuple(this->get_dlat(), this->get_dlon_coslat(),
            this->get_ddist());
    }

    //!returns the dlat component of differential
    LatQuantity get_dlat() const
    {
        return static_cast<LatQuantity>
            (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<0>(this->diff))
            );
    }

    //!returns the dlon_coslat component of differential
    LonQuantity get_dlon_coslat() const
    {
        return static_cast<LonQuantity>
            (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<1>(this->diff))
            );
    }

    //!returns the ddistance component of differential
    DistQuantity get_ddist() const
    {
        return DistQuantity::from_value(bg::get<2>(this->diff));
    }

    //!set value of (dlat, dlon_coslat, ddistance) in current object
    void set_dlat_dlon_coslat_ddist
    (
        LatQuantity const& dlat,
        LonQuantity const& dlon_coslat,
        DistQuantity const& ddistance
    )
    {
        this->set_dlat(dlat);
        this->set_dlon_coslat(dlon_coslat);
        this->set_ddist(ddistance);
    }

    //!set value of dlat component of differential
    void set_dlat(LatQuantity const& dlat)
    {
        bg::set<0>
            (
            this->diff,
            static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(dlat).value()
            );
    }

    //!set value of dlon_coslat component of differential
    void set_dlon_coslat(LonQuantity const& dlon_coslat)
    {
        bg::set<1>
            (
            this->diff,
            static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>
                (dlon_coslat).value()
            );
    }

    //!set value of ddistance component of differential
    void set_ddist(DistQuantity const& ddistance)
    {
        bg::set<2>(this->diff, ddistance.value());
    }

    //!operator for addition of differential
    template
    <
        typename Addend
    >
    spherical_coslat_differential
    <
        CoordinateType,
        LatQuantity,
        LonQuantity,
        DistQuantity
    >
    operator +(Addend const& addend) const
    {
        auto cartesian1 = make_cartesian_differential
            <CoordinateType, DistQuantity, DistQuantity, DistQuantity>(this->diff);
        auto cartesian2 = make_cartesian_differential(addend);

        auto temp = cartesian1 + cartesian2;

        spherical_coslat_differential
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        > result = make_spherical_coslat_differential(temp);

        return result;
    }

    //!operator for multiplication of differential
    template
    <
        typename OtherQuantity
    >
    auto operator *(OtherQuantity const& dt) const
    {

        spherical_coslat_differential
            <CoordinateType, LatQuantity, LonQuantity, DistQuantity> temp(this->diff);

        spherical_coslat_differential
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            bu::quantity<typename bu::multiply_typeof_helper
            <
                typename DistQuantity::unit_type,
                typename OtherQuantity::unit_type>::type,
                CoordinateType
            >
        > product
        (
            temp.get_dlat(),
            temp.get_dlon_coslat(),
            temp.get_ddist() * dt
        );

        return product;
    }
}; //spherical_coslat_differential

//!constructs object from provided components of differential
template
<
    typename CoordinateType,
    template <typename Unit1, typename CoordinateType_> class LatQuantity,
    template <typename Unit2, typename CoordinateType_> class LonQuantity,
    template <typename Unit3, typename CoordinateType_> class DistQuantity,
    typename Unit1,
    typename Unit2,
    typename Unit3
>
spherical_coslat_differential
<
    CoordinateType,
    LatQuantity<Unit1, CoordinateType>,
    LonQuantity<Unit2, CoordinateType>,
    DistQuantity<Unit3, CoordinateType>
>
make_spherical_coslat_differential
(
    LatQuantity<Unit1, CoordinateType> const& dlat,
    LonQuantity<Unit2, CoordinateType> const& dlon_coslat,
    DistQuantity<Unit3, CoordinateType> const& ddist
)
{
    return spherical_coslat_differential
        <
            CoordinateType,
            LatQuantity<Unit1, CoordinateType>,
            LonQuantity<Unit2, CoordinateType>,
            DistQuantity<Unit3, CoordinateType>
        >(dlat, dlon_coslat, ddist);
}

//!constructs object from provided components of differential with different units
template
<
    typename ReturnCoordinateType,
    typename ReturnLatQuantity,
    typename ReturnLonQuantity,
    typename ReturnDistQuantity,
    typename CoordinateType,
    typename LatQuantity,
    typename LonQuantity,
    typename DistQuantity
>
spherical_coslat_differential
<
    ReturnCoordinateType,
    ReturnLatQuantity,
    ReturnLonQuantity,
    ReturnDistQuantity
>
make_spherical_coslat_differential
(
    spherical_coslat_differential
    <
        CoordinateType,
        LatQuantity,
        LonQuantity,
        DistQuantity
    > const& other
)
{
    return make_spherical_coslat_differential(
        static_cast<ReturnLatQuantity>(other.get_dlat()),
        static_cast<ReturnLonQuantity>(other.get_dlon_coslat()),
        static_cast<ReturnDistQuantity>(other.get_ddist())
    );
}

//!constructs object from provided differential
template
<
    typename CoordinateType,
    typename LatQuantity,
    typename LonQuantity,
    typename DistQuantity
>
spherical_coslat_differential
<
    CoordinateType,
    LatQuantity,
    LonQuantity,
    DistQuantity
>
make_spherical_coslat_differential
(
    spherical_coslat_differential
    <
        CoordinateType,
        LatQuantity,
        LonQuantity,
        DistQuantity
    > const& other
)
{
    return spherical_coslat_differential
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        >(other);
}

//!constructs object from boost::geometry::model::point object
template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename DistQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    std::size_t OtherDimensionCount,
    typename OtherCoordinateSystem,
    typename OtherCoordinateType
>
spherical_coslat_differential
<
    CoordinateType,
    LatQuantity,
    LonQuantity,
    DistQuantity
>
make_spherical_coslat_differential
(
    bg::model::point
    <
        OtherCoordinateType,
        OtherDimensionCount,
        OtherCoordinateSystem
    > const& pointObject
)
{
    return spherical_coslat_differential
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        >(pointObject);
}

//!constructs object from any type of differential
template
<
    typename OtherDifferential
>
auto make_spherical_coslat_differential
(
    OtherDifferential const& other
)
{
    auto temp = make_spherical_differential(other);
    typedef decltype(temp) spherical_type;

    temp.set_dlon(temp.get_dlon() * cos(static_cast<bu::quantity
        <bu::si::plane_angle, typename spherical_type::type>>(temp.get_dlat()).value()));

    return spherical_coslat_differential
        <
            typename spherical_type::type,
            bu::quantity<bu::si::plane_angle, typename spherical_type::type>,
            bu::quantity<bu::si::plane_angle, typename spherical_type::type>,
            typename spherical_type::quantity3
        >(temp.get_differential());
}

//!constructs cartesian object from spherical_coslat_differential
//function placed in this file to avoid circular dependency of header files
template
<
    typename CoordinateType,
    template <typename Unit1, typename CoordinateType_> class LatQuantity,
    template <typename Unit2, typename CoordinateType_> class LonQuantity,
    template <typename Unit3, typename CoordinateType_> class DistQuantity,
    typename Unit1,
    typename Unit2,
    typename Unit3
>
auto
make_cartesian_differential
(
    spherical_coslat_differential
    <
        CoordinateType,
        LatQuantity<Unit1, CoordinateType>,
        LonQuantity<Unit2, CoordinateType>,
        DistQuantity<Unit3, CoordinateType>
    > const& other
)
{
    typedef spherical_coslat_differential
    <
        CoordinateType,
        LatQuantity<Unit1, CoordinateType>,
        LonQuantity<Unit2, CoordinateType>,
        DistQuantity<Unit3, CoordinateType>
    > spherical_type;
    bg::model::point<typename spherical_type::type, 3, bg::cs::spherical<radian>> temp;

    temp = other.get_differential();

    bg::set<1>(temp, bg::get<1>(temp) / cos(bg::get<0>(temp)));

    return cartesian_differential
    <
        typename spherical_type::type,
        typename spherical_type::quantity3,
        typename spherical_type::quantity3,
        typename spherical_type::quantity3
    >(temp);
}

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_SPHERICAL_COSLAT_DIFFERENTIAL_HPP


#include "covreg/CovarianceManager.h"

#include "argus_utils/utils/MatrixUtils.h"
#include "argus_utils/utils/YamlUtils.h"
#include "argus_utils/utils/ParamUtils.h"

#include <boost/foreach.hpp>
#include <sstream>

using namespace covreg;
using namespace argus;

namespace percepto
{

CovarianceManager::CovarianceManager()
: _estimator( nullptr ) {}

// TODO Parse from info -> Msg instead so we can have nicer error messages
void CovarianceManager::Initialize( const std::string& sourceName,
                                    ros::NodeHandle& ph )
{
	YAML::Node estimatorYaml;
	GetParamRequired( ph, "estimator", estimatorYaml );

	_sourceName = sourceName;
	_estimator = std::make_shared<CovarianceEstimator>( _sourceName, estimatorYaml );

	ros::NodeHandle subh( ph.resolveName( "input_streams" ) );
	_receiver.Initialize( subh );

	if( _receiver.GetDim() != _estimator->InputDim() )
	{
		std::stringstream ss;
		ss << "Estimator: " << sourceName << " has input dim "
		   << _estimator->InputDim() << " but features have total dim: "
		   << _receiver.GetDim();
		throw std::runtime_error( ss.str() );
	}

	// Load params if we have any to load
	bool initialized = false;
	if( HasParam( estimatorYaml, "load_path" ) )
	{
		std::string paramFilePath;
		GetParam( estimatorYaml, "load_path", paramFilePath );
		CovarianceEstimatorInfo estInfo;
		if( !ReadInfo<CovarianceEstimatorInfo>( paramFilePath,
		                                        estInfo ) )
		{
			ROS_WARN_STREAM( "Could not read params from: " << paramFilePath );
		}
		else
		{
			ROS_INFO_STREAM( "Loaded parameters for: " << _sourceName <<
			                 " from " << paramFilePath );
			_estimator->SetParamsMsg( estInfo );
			initialized = true;
		}
	}
	if( !initialized )
	{
		ROS_WARN_STREAM( "No parameters specified for: " << _sourceName <<
		                 ". Using defaults." );
		_estimator->RandomizeVarianceParams();
		_estimator->ZeroCorrelationParams();
	}

	_queryServer = ph.advertiseService( "query_covariance",
                                    &CovarianceManager::QueryCallback,
                                    this );
}

void CovarianceManager::SetUpdateTopic( const std::string& topic )
{
	_paramSub = _nodeHandle.subscribe( topic,
	                                  10,
	                                  &CovarianceManager::ParamCallback,
	                                  this );
}

bool CovarianceManager::IsReady() const
{
	if( !_estimator ) { return false; }
	return _receiver.IsReady();
}

unsigned int CovarianceManager::OutputDim() const
{
	if( !_estimator ) { return 0; }
	return _estimator->OutputDim();
}

MatrixType CovarianceManager::EstimateCovariance( const ros::Time& time )
{
	StampedFeatures f;
	if( !_receiver.ReadStream( time, f ) )
	{
		throw std::runtime_error( "CovarianceManager: Could not read stream." );
	}
	return _estimator->Evaluate( f.features );
}

void CovarianceManager::ParamCallback( const CovarianceEstimatorInfo::ConstPtr& msg )
{
	WriteLock lock( _estimatorMutex );
	if( msg->source_name != _sourceName )
	{
		ROS_WARN_STREAM( "Received params for: " << msg->source_name <<
		                 " but expected " << _sourceName );
	}
	ROS_INFO_STREAM( "Updating parameters for: " << _sourceName );
	_estimator->SetParamsMsg( *msg );
}

bool CovarianceManager::QueryCallback( QueryCovariance::Request& request,
	                                   QueryCovariance::Response& response )
{
	WriteLock lock( _estimatorMutex );
	VectorType feats = GetVectorView( request.features );
	if( feats.size() != _estimator->InputDim() )
	{
		ROS_WARN_STREAM( "Covariance query has feature dim: " << + feats.size() <<
		                 " but estimator input dim: " << _estimator->InputDim() );
		return false;
	}
	MatrixType cov = _estimator->Evaluate( feats );
	response.covariance = MatrixToMsg( cov );
	return true;
}

}


/*
 * @file winner_determination_in_MUCA.hpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-1-7
 */
#ifndef PAAL_WINNER_DETERMINATION_IN_MUCA_HPP
#define PAAL_WINNER_DETERMINATION_IN_MUCA_HPP

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/auction_traits.hpp"
#include "paal/auctions/auction_utils.hpp"
#include "paal/utils/concepts.hpp"
#include "paal/utils/property_map.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/concept/requires.hpp>
#include <boost/optional/optional.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/concepts.hpp>
#include <boost/range/iterator.hpp>

#include <iterator>
#include <memory>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

namespace paal {
namespace auctions {

namespace detail {
   template <class Value, class ItemSet>
   struct bidder_info {
      Value m_best_items_val;
      ItemSet m_best_items;
   };

   template <
      class GammaOracleAuction,
      class Base = gamma_oracle_auction_traits<GammaOracleAuction>
   >
   struct determine_winners_in_gamma_oracle_auction_traits : Base {
         using value = promote_with_double_t<typename Base::value_t>;
         using item_set = typename Base::items_t;
         using bidder_info = detail::bidder_info<value, item_set>;
         using result = std::pair<typename Base::bidder_t, item_set>;
   };
}//!detail

/**
 * @brief This is determine winners in gamma oracle auction and return assignment of items to bidders.
 *
 * Example:
 *  \snippet winner_determination_in_MUCA_example.cpp Winner Determination In MUCA Example
 *
 * Complete example is winner_determination_in_MUCA_example.cpp.
 *
 * @tparam GammaOracleAuction
 * @tparam OutputIterator
 * @tparam PriceMap
 * @tparam Epsilon
 * @param auction
 * @param result
 * @param price Stores the current mapping of items to prices.
 * These are prices just for the working purposes of the algorithm,
 * not the prices to be paid by the bidders.
 * @param epsilon Used for floating point comparison to ensure feasibility.
 */
template<
   class GammaOracleAuction,
   class OutputIterator,
   class PriceMap,
   class Epsilon
>
BOOST_CONCEPT_REQUIRES(
   ((concepts::gamma_oracle_auction<GammaOracleAuction>))

   ((boost::ForwardRangeConcept<
     typename gamma_oracle_auction_traits<GammaOracleAuction>::bidders_universe_t
   >))

   ((utils::concepts::move_constructible<
     typename gamma_oracle_auction_traits<GammaOracleAuction>::items_t
   >))

   ((utils::concepts::output_iterator<
      OutputIterator,
      std::pair<
         typename gamma_oracle_auction_traits<GammaOracleAuction>::bidder_t,
         typename gamma_oracle_auction_traits<GammaOracleAuction>::items_t
      >
   >))

   ((boost::ReadWritePropertyMapConcept<
      PriceMap,
      typename gamma_oracle_auction_traits<GammaOracleAuction>::item_t
   >))

   ((utils::concepts::floating_point<Epsilon>)),

(void))
determine_winners_in_gamma_oracle_auction(
   GammaOracleAuction&& auction,
   OutputIterator result,
   PriceMap price,
   Epsilon epsilon
) {
   using Price = typename boost::property_traits<PriceMap>::value_type;

   using Traits =
      detail::determine_winners_in_gamma_oracle_auction_traits<GammaOracleAuction>;
   using Value = typename Traits::value;
   using BidderInfo = typename Traits::bidder_info;
   using BidderIter = typename Traits::bidder_iterator_t;
   using Frac = typename Traits::frac_t;
   using Res = typename Traits::result_t;

   Price items_num = 0;
   for (auto item = std::begin(auction.template get<items>());
         item != std::end(auction.template get<items>());
         ++item
   ) {
      ++items_num;
      auto const copies = auction.template call<get_copies_num>(*item);
      put(price, *item, 1.0 / copies);
   }
   Price price_sum = items_num;

   // TODO allow to change the allocator
   std::vector<BidderInfo> bidders_info_vec(bidders_number(auction));

   auto last_assigned_bidder_info = bidders_info_vec.end();
   BidderIter last_assigned_bidder;
   Value total_value = 0, last_value{};
   auto const b = get_minimum_copies_num(auction);
   auto const multiplier = std::exp(Value(b) + 1) * items_num;
   auto const gamma_ = auction.template get<gamma>();
   auto get_threshold = [=](const BidderInfo& b)
   {
      return (1 + 2 * gamma_) * b.m_best_items_val;
   };
   do {
      Res best = boost::none;
      auto get_frac = [](Res r) { return r->second; };
      auto bidder_info = bidders_info_vec.begin();
      auto bidder = std::begin(auction.template get<bidders>());
      for (; bidder_info != bidders_info_vec.end(); ++bidder_info, ++bidder) {
         auto const threshold = get_threshold(*bidder_info);
         auto result = auction.template call<gamma_oracle>(
            *bidder, utils::make_property_map_get(price), threshold
         );
         if (!result) continue;
         if (!best || get_frac(result) < get_frac(best)) {
            best = std::move(result);
            last_assigned_bidder_info = bidder_info;
            last_assigned_bidder = bidder;
            last_value = get_frac(result).den + threshold;
         }
      }
      if (!best) break;
      auto& best_items = best->first;
      for (auto item = std::begin(best_items); item != std::end(best_items); ++item) {
         auto const copies = auction.template call<get_copies_num>(*item);
         auto const old_price = get(price, *item);
         auto const new_price =
            old_price * std::pow(multiplier, 1.0 / (copies+ 1));
         put(price, *item, new_price);
         price_sum += copies* (new_price - old_price);
      }
      total_value += last_value - last_assigned_bidder_info->m_best_items_val;
      last_assigned_bidder_info->m_best_items = std::move(best_items);
      last_assigned_bidder_info->m_best_items_val = last_value;
   } while (price_sum + epsilon < multiplier);

   const bool nothing_assigned =
      last_assigned_bidder_info == bidders_info_vec.end();
   if (nothing_assigned) return;

   auto output = [&](
      puretype(last_assigned_bidder_info) bidder_info,
      BidderIter bidder
   ) {
      *result = std::make_pair(*bidder, std::move(bidder_info->m_best_items));
      ++result;
   };
   if (last_value > total_value - last_value) {
      output(last_assigned_bidder_info, last_assigned_bidder);
      return;
   }
   auto bidder_info = bidders_info_vec.begin();
   auto bidder = std::begin(auction.template get<bidders>());
   for (; bidder_info != bidders_info_vec.end(); ++bidder_info, ++bidder)
      if (bidder != last_assigned_bidder)
         output(bidder_info, bidder);
}

/**
 * @brief This is determine winners in gamma oracle auction and return assignment of bidders to items.
 * This is version with default PriceMap using std::unordered_map and
 * default epsilon.
 *
 * @tparam GammaOracleAuction
 * @tparam OutputIterator
 * @tparam Epsilon
 * @param auction
 * @param result
 * @param epsilon Used for floating point comparison to ensure feasibility.
 */
template <
   class GammaOracleAuction,
   class OutputIterator,
   class Epsilon = double
>
void determine_winners_in_gamma_oracle_auction(
   GammaOracleAuction&& auction,
   OutputIterator result,
   Epsilon epsilon = 1e-8
) {
   using Traits = gamma_oracle_auction_traits<GammaOracleAuction>;
   using Value = promote_with_double_t<typename Traits::value_t>;
   using ItemVal = typename Traits::item_val_t;

   std::unordered_map<ItemVal, Value> umap;
   return determine_winners_in_gamma_oracle_auction(
      std::forward<GammaOracleAuction>(auction),
      result,
      boost::make_assoc_property_map(umap),
      epsilon
   );
}

}//!auctions
}//!paal

#endif /* PAAL_WINNER_DETERMINATION_IN_MUCA_HPP */


#include <fstream>
#include <ros/ros.h>
#include <ros/package.h>
#include <Eigen/Dense>

Eigen::MatrixXf load_SDF(std::string sdf_path){
  // Open file
  std::ifstream sdf_file(sdf_path.c_str());

  std::vector<std::vector<float>> pre_mat;
  std::string line;
  if (sdf_file.is_open()){
    while (std::getline(sdf_file, line)){
      std::stringstream ss;
      ss << line;
      float dist;
      std::vector<float> temp_vec;
      while (ss >> dist){
        temp_vec.push_back(dist);
      }
      pre_mat.push_back(temp_vec);
    }
  }
  else {
    ROS_INFO("failed");
  }

  int width = pre_mat.size();
  int height = pre_mat[0].size();
  Eigen::MatrixXf sdf_mat(width, height);
  for (int i = 0; i < width; i++){
    for (int j = 0; j < height; j++){
      sdf_mat(i, j) = pre_mat[i][j];
    }
  }
  return sdf_mat;
}

int main(int argc, char **argv)
{
  // Get path and file name
  std::string package_path = ros::package::getPath("crowdbot_active_slam");
  std::string directory_path = package_path + "/" + argv[1];

  std::string general_results_path = directory_path + "/general_results.txt";
  std::string test_sdf_path = directory_path + "/sdf_map.txt";
  std::string ref_sdf_path = package_path + "/worlds/sdf_maps/sdf_map_" + argv[2]
                             + "_2000x2000.txt";

  Eigen::MatrixXf test_sdf_mat = load_SDF(test_sdf_path);
  Eigen::MatrixXf ref_sdf_mat = load_SDF(ref_sdf_path);

  // Calculate score
  double sdf_score_full = 0;
  double sdf_score_easy = 0;

  for (int i = 0; i < test_sdf_mat.rows(); i++){
    for (int j = 0; j < test_sdf_mat.cols(); j++){
      sdf_score_full += pow(std::abs(test_sdf_mat(i, j) - ref_sdf_mat(i, j)), 2);
      if (ref_sdf_mat(i, j) >= 0){
        if (std::abs(test_sdf_mat(i, j) - ref_sdf_mat(i, j)) > 1){
          double score_increment = std::abs(test_sdf_mat(i, j) - ref_sdf_mat(i, j)) - 1;
          sdf_score_easy += pow(score_increment, 2);
        }
      }
    }
  }

  std::ofstream result_file(general_results_path.c_str(), std::ofstream::app);
  if (result_file.is_open()){
    // Add information to file
    result_file << std::endl;
    result_file << "sdf_score_full: " << sdf_score_full << std::endl;
    result_file << "sdf_score_easy: " << sdf_score_easy << std::endl;

    // Close file
    result_file.close();
  }
  else{
    ROS_INFO("Could not open general_results.txt!");
  }

  std::cout << "sdf_score_full: " << sdf_score_full << std::endl;
  std::cout << "sdf_score_easy: " << sdf_score_easy << std::endl;
  return 0;
}


#include <CMDParser.hpp>
#include <FileBuffer.hpp>
#include <calibvolume.hpp>
#include <rgbdsensor.hpp>
#include <DataTypes.hpp>
#include <NearestNeighbourSearch.hpp>
#include <udpconnection.hpp>
#include <squish.h>
#include <clock.hpp>
#include <timevalue.hpp>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <iostream>
#include <fstream>
#include <sstream>

namespace{

  class voxel{
  public:
    voxel()
      : xyz(),
	rgbi()
    {
      xyz[0] = 0;
      xyz[1] = 0;
      xyz[2] = 0;
      rgbi[0] = 0;
      rgbi[1] = 0;
      rgbi[2] = 0;
      rgbi[3] = 0;
    }
    unsigned short xyz[3];
    unsigned char rgbi[4];
  };

  std::ostream& operator<< (std::ostream& os, const voxel& v){
    os << "voxel: " << v.xyz[0] << ", " << v.xyz[1] << ", " << v.xyz[2] << " color: " << (int) v.rgbi[0] << ", " << (int) v.rgbi[1] << ", " << (int) v.rgbi[2];
    return os;
  }

  glm::vec3 bbx_min = glm::vec3(-1.2, -0.05, -1.2);
  glm::vec3 bbx_max = glm::vec3( 1.2, 2.4,  1.2);

  bool clip(const glm::vec3& p){
    if(p.x < bbx_min.x ||
       p.y < bbx_min.y ||
       p.z < bbx_min.z ||
       p.x > bbx_max.x ||
       p.y > bbx_max.y ||
       p.z > bbx_max.z){
      return true;
    }
    return false;
  }

  float voxelsize_inv = 1.0f/0.008;
  unsigned volume_width  = (bbx_max[0] - bbx_min[0])*voxelsize_inv;
  unsigned volume_height = (bbx_max[1] - bbx_min[1])*voxelsize_inv;

}

void decomp_per_thread(const unsigned s_num, std::vector<unsigned char*>& tmp_rgba, std::vector<unsigned char*>& tmp_rgb, const RGBDConfig& cfg, RGBDSensor& sensor, const unsigned colorsize_tmp){
  squish::DecompressImage (tmp_rgba[s_num], cfg.size_rgb.x, cfg.size_rgb.y,
			   (unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), squish::kDxt1);
  // copy back rgbsensor
  unsigned buffida = 0;
  unsigned buffid = 0;
  for(unsigned y = 0; y < cfg.size_rgb.y; ++y){
    for(unsigned x = 0; x < cfg.size_rgb.x; ++x){
      tmp_rgb[s_num][buffid++] = tmp_rgba[s_num][buffida++];
      tmp_rgb[s_num][buffid++] = tmp_rgba[s_num][buffida++];
      tmp_rgb[s_num][buffid++] = tmp_rgba[s_num][buffida++];
      buffida++;
    }
  }
  memcpy((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), tmp_rgb[s_num], colorsize_tmp);
}

void recon_per_thread(const unsigned tid, std::vector<CalibVolume*>& cvs, RGBDSensor& sensor, std::map<size_t, std::vector<voxel>>* recon_volume, const unsigned start_y, const unsigned end_y){
  // do 3D reconstruction for each depth pixel

  for(unsigned y = start_y; y < end_y; ++y){
    for(unsigned x = 0; x < (sensor.config.size_d.x - 3); ++x){

      const unsigned d_idx = y* sensor.config.size_d.x + x;
      float d = tid == 0 ? sensor.frame_d[d_idx] : sensor.slave_frames_d[tid - 1][d_idx];
      if(d < cvs[tid]->min_d || d > cvs[tid]->max_d){
	continue;
      }

      glm::vec3 pos3D;
      glm::vec2 pos2D_rgb;

      pos3D = cvs[tid]->lookupPos3D( x * 1.0/sensor.config.size_d.x,
				     y * 1.0/sensor.config.size_d.y, d);

      if(clip(pos3D)){
	continue;
      }

      glm::vec2 pos2D_rgb_norm = cvs[tid]->lookupPos2D_normalized( x * 1.0/sensor.config.size_d.x,
								   y * 1.0/sensor.config.size_d.y, d);
      pos2D_rgb = glm::vec2(pos2D_rgb_norm.x * sensor.config.size_rgb.x,
			    pos2D_rgb_norm.y * sensor.config.size_rgb.y);

      glm::vec3 rgb = sensor.get_rgb_bilinear_normalized(pos2D_rgb, tid);

      // discretize voxel here
      voxel vxl;
      vxl.xyz[0] = (unsigned short)((pos3D.x - bbx_min[0])*voxelsize_inv);
      vxl.xyz[1] = (unsigned short)((pos3D.y - bbx_min[1])*voxelsize_inv);
      vxl.xyz[2] = (unsigned short)((pos3D.z - bbx_min[2])*voxelsize_inv);
      // compute grid_loc of surface point
      const size_t grid_loc = vxl.xyz[0] + volume_width * vxl.xyz[1] + volume_width * volume_height * vxl.xyz[2];
      vxl.rgbi[0] = (unsigned char) std::max(0.0f , std::min(255.0f, rgb.x * 255.0f));
      vxl.rgbi[1] = (unsigned char) std::max(0.0f , std::min(255.0f, rgb.y * 255.0f));
      vxl.rgbi[2] = (unsigned char) std::max(0.0f , std::min(255.0f, rgb.z * 255.0f));
      (*recon_volume)[grid_loc].push_back(vxl);
    }
  }
}

// ./send_recording_as_point_cloud /mnt/pitoti/kinect_recordings/25.8b/23.cv /mnt/pitoti/kinect_recordings/25.8b/24.cv /mnt/pitoti/kinect_recordings/25.8b/25.cv /mnt/pitoti/kinect_recordings/25.8b/26.cv -s /mnt/pitoti/kinect_recordings/25.8b/session_1.stream -c -bbx -0.5 0.0 -0.5 0.5 2.0 0.5 -u 127.0.0.1 7002 -p1k 20 -p2k 3
int main(int argc, char* argv[]){

  bool rgb_is_compressed = false;
  std::string stream_filename;
  unsigned wait_ms = 0;
  udpconnection* sender = 0;
  udpconnection* feedback = 0;

  CMDParser p("basefilename_cv .... ");
  p.addOpt("s",1,"stream_filename", "specify the stream filename which should be converted");
  p.addOpt("c",-1,"rgb_is_compressed", "enable compressed support for rgb stream, default: false (not compressed)");

  p.addOpt("bbx",6,"bounding_box", "specify the bounding box x_min y_min z_min x_max y_max z_max in meters, default -1.2 -0.05 -1.2 1.2 2.4 1.2");

  p.addOpt("u",2,"udpconnection", "specify hostname and port of sending udpconnection (for sending to client)");
  p.addOpt("f",1,"feedbackport", "specify port of receiving socket (for receiving from client)");

  p.addOpt("w",1,"wait", "specify how many milliseconds to wait between packets: default 0");
  p.addOpt("v",1,"voxelsize", "specify the voxel size in meter: default 0.008");

  p.init(argc,argv);

  if(p.isOptSet("u")){
    sender = new udpconnection;
    sender->open_sending_socket(p.getOptsString("u")[0].c_str(), p.getOptsInt("u")[1]);
  }
  else{
    p.showHelp();
    return 0;
  }

  if(p.isOptSet("f")){
    feedback = new udpconnection;
    feedback->open_receiving_socket(p.getOptsInt("f")[0]);
  }

  if(p.isOptSet("bbx")){
    bbx_min = glm::vec3(p.getOptsFloat("bbx")[0], p.getOptsFloat("bbx")[1], p.getOptsFloat("bbx")[2]);
    bbx_max = glm::vec3(p.getOptsFloat("bbx")[3], p.getOptsFloat("bbx")[4], p.getOptsFloat("bbx")[5]);
    std::cout << "setting bounding box to min: " << bbx_min << " -> max: " << bbx_max << std::endl;
  }

  if(p.isOptSet("s")){
    stream_filename = p.getOptsString("s")[0];
  }
  else{
    std::cerr << "ERROR, please specify stream filename with flag -s, see " << argv[0] << " -h for help" << std::endl;
    return 0;
  }

  if(p.isOptSet("c")){
    rgb_is_compressed = true;
  }

  if(p.isOptSet("w")){
    wait_ms = p.getOptsInt("w")[0];
  }

  if(p.isOptSet("v")){
    voxelsize_inv = 1.0f/p.getOptsFloat("v")[0];
  }

  const unsigned num_streams(p.getArgs().size());

  std::vector<CalibVolume*> cvs;
  for(unsigned i = 0; i < num_streams; ++i){
    std::string basefilename = p.getArgs()[i];
    std::string filename_xyz(basefilename + "_xyz");
    std::string filename_uv(basefilename + "_uv");
    cvs.push_back(new CalibVolume(filename_xyz.c_str(), filename_uv.c_str()));
  }

  RGBDConfig cfg;
  cfg.size_rgb = glm::uvec2(1280, 1080);
  cfg.size_d   = glm::uvec2(512, 424);
  RGBDSensor sensor(cfg, num_streams - 1);

  const unsigned colorsize = rgb_is_compressed ? 691200 : cfg.size_rgb.x * cfg.size_rgb.y * 3;
  const unsigned depthsize = cfg.size_d.x * cfg.size_d.y * sizeof(float);

  std::vector<unsigned char*> tmp_rgb;
  std::vector<unsigned char*> tmp_rgba;
  const unsigned colorsize_tmp = cfg.size_rgb.x * cfg.size_rgb.y * 3;
  const unsigned colorsize_tmpa = cfg.size_rgb.x * cfg.size_rgb.y * 4;
  if(rgb_is_compressed){
    for(unsigned s_num = 0; s_num < num_streams; ++s_num){
      tmp_rgb.push_back(new unsigned char [colorsize_tmp]);
      tmp_rgba.push_back(new unsigned char [colorsize_tmpa]);
    }
  }

  FileBuffer fb(stream_filename.c_str());
  if(!fb.open("r")){
    std::cerr << "ERROR, while opening " << stream_filename << " exiting..." << std::endl;
    return 1;
  }
  const unsigned num_frames = fb.calcNumFrames(num_streams * (colorsize + depthsize));
  double curr_frame_time = 0.0;

  unsigned frame_num = 0;

  sensor::timevalue ts = sensor::clock::time();

  while(frame_num < num_frames){
    ++frame_num;
    std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! start of frame" << std::endl;
    sensor::timevalue ts_now = sensor::clock::time();
    std::cout << "frame time: " << (ts_now - ts).msec() << " ms" << std::endl;
    ts = ts_now;

    if(feedback != 0){
      float voxelsize_want = 0.008;
      size_t bytes_received = feedback->recv(&voxelsize_want, sizeof(voxelsize_want), false /*do no block*/);
      if(bytes_received == sizeof(voxelsize_want)){
	std::cout << "received new voxelsize_want: " << voxelsize_want << std::endl;
	voxelsize_inv = 1.0f/voxelsize_want;
	// recompute volume dimensions
	volume_width  = (bbx_max[0] - bbx_min[0])*voxelsize_inv;
	volume_height = (bbx_max[1] - bbx_min[1])*voxelsize_inv;
      }
    }

    unsigned bytes_per_frame = 0;
    // start reading frames from file
    for(unsigned s_num = 0; s_num < num_streams; ++s_num){
      fb.read((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), colorsize);
      bytes_per_frame += colorsize;
      if(s_num == 0){
	memcpy((char*) &curr_frame_time, (const char*) sensor.frame_rgb, sizeof(double));
	//std::cout << "curr_frame_time: " << curr_frame_time << std::endl;
      }
      fb.read((unsigned char*) (s_num == 0 ? sensor.frame_d : sensor.slave_frames_d[s_num - 1]), depthsize);
      bytes_per_frame += depthsize;
    }

    if(rgb_is_compressed){
      boost::thread_group threadGroup;
      for(unsigned s_num = 0; s_num < num_streams; ++s_num){
	threadGroup.create_thread([s_num, &tmp_rgba, &tmp_rgb, &cfg, &sensor, colorsize_tmp](){
				    decomp_per_thread(s_num, tmp_rgba, tmp_rgb, cfg, sensor, colorsize_tmp);
				  }
				  );
      }
      threadGroup.join_all();
    }

    sensor::timevalue ts_now_read = sensor::clock::time();
    std::cout << "frame time_read: " << (ts_now_read - ts_now).msec() << " ms" << std::endl;

    const unsigned num_threads = num_streams * 2;
    std::vector< std::map<size_t, std::vector<voxel>>> recon_volumes(num_threads);
    boost::thread_group threadGroup;
    unsigned tid = 0;
    for(unsigned s_num = 0; s_num != num_streams; ++s_num){
      threadGroup.create_thread([s_num, &cvs, &sensor, &recon_volumes, tid](){
				  recon_per_thread(s_num, cvs, sensor, &(recon_volumes[tid]), 0, sensor.config.size_d.y/2);
						   }
				);
      ++tid;
      threadGroup.create_thread([s_num, &cvs, &sensor, &recon_volumes, tid](){
				  recon_per_thread(s_num, cvs, sensor, &(recon_volumes[tid]), sensor.config.size_d.y/2, sensor.config.size_d.y);
						   }
				);
      ++tid;

    }
    threadGroup.join_all();

    // filter recon_volume
    std::vector<voxel> voxels;
    for(const auto& recon_volume : recon_volumes){
      for(const auto& vxls : recon_volume){
	voxels.push_back(vxls.second.front());
      }
    }
    unsigned voxel_count = voxels.size();
    std::cout << "voxel_count: " << voxel_count << std::endl;

    sensor::timevalue ts_now_recon = sensor::clock::time();
    std::cout << "frame time_recon: " << (ts_now_recon - ts_now_read).msec() << " ms" << std::endl;
    // finished of 3D reconstruction

    size_t timestamp = curr_frame_time * 1000;
    std::cout << "timestamp: " << timestamp << std::endl;
    /*
      layout of udp packet:
      timestamp_8 packetNumber_4 voxelCount_4 x_2 y_2 z_2 r_1 g_1 b_1 i_1 ...
      8 4 4 2 2 2 1 1 1 1
      memcpy(&message[0], &timeStamp, 8);
      memcpy(&message[8], &packetNumber, 4);
      memcpy(&message[12], &voxelCount, 4);
    */
    const unsigned max_voxels_per_packet = 6000;
    const unsigned byte_per_voxel = 10;
    const unsigned byte_of_header = 16;
    const unsigned max_bufflen = max_voxels_per_packet * byte_per_voxel + byte_of_header;

    unsigned char buff[max_bufflen];

    size_t buff_index = 0;
    unsigned packet_number = 0;
    unsigned voxel_number = 0;
    unsigned bytes_sent_per_frame = 0;
    for(const auto& s : voxels){

      if(voxel_number == max_voxels_per_packet){
	std::cout << "sending packet number: " << packet_number
		  << " -> " << voxel_number
		  << " (" << buff_index << " bytes)" << std::endl;
	sender->send(buff, buff_index);
	bytes_sent_per_frame += buff_index;
	if(wait_ms)
	  sleep(sensor::timevalue::const_100_us * wait_ms);

	voxel_number = 0;
	buff_index = 0;
	++packet_number;
      }

      // fill header
      if(0 == voxel_number){
	if(feedback != 0){
	  double voxelsize_sent = 1.0f/voxelsize_inv;
	  memcpy(&buff[buff_index], &voxelsize_sent, sizeof(voxelsize_sent));
	  buff_index += sizeof(voxelsize_sent);
	}
	else{
	  memcpy(&buff[buff_index], &timestamp, sizeof(timestamp));
	  buff_index += sizeof(timestamp);
	}
	memcpy(&buff[buff_index], &packet_number, sizeof(packet_number));
	buff_index += sizeof(packet_number);
	memcpy(&buff[buff_index], &voxel_count, sizeof(voxel_count));
	buff_index += sizeof(voxel_count);
      }

      unsigned char r = s.rgbi[0];
      unsigned char g = s.rgbi[1];
      unsigned char b = s.rgbi[2];
      unsigned short x = s.xyz[0];
      unsigned short y = s.xyz[1];
      unsigned short z = s.xyz[2];

      // copy "voxel into buff"
      memcpy(&buff[buff_index], &x, sizeof(x));
      buff_index += sizeof(x);
      memcpy(&buff[buff_index], &y, sizeof(y));
      buff_index += sizeof(y);
      memcpy(&buff[buff_index], &z, sizeof(z));
      buff_index += sizeof(z);

      memcpy(&buff[buff_index], &r, sizeof(r));
      buff_index += sizeof(r);
      memcpy(&buff[buff_index], &g, sizeof(g));
      buff_index += sizeof(g);
      memcpy(&buff[buff_index], &b, sizeof(b));
      buff_index += sizeof(b);

      unsigned char joint_id = 0; // not used but needed by protocol
      memcpy(&buff[buff_index], &joint_id, sizeof(joint_id));
      buff_index += sizeof(joint_id);

      ++voxel_number;
    }

    if(voxel_number > 0){
      std::cout << "sending packer number: " << packet_number
		<< " -> " << voxel_number
		<< " (" << buff_index << " bytes)" << std::endl;
      sender->send(buff, buff_index);
      bytes_sent_per_frame += buff_index;
    }

    sensor::timevalue ts_now_send = sensor::clock::time();
    std::cout << "frame time_send: " << (ts_now_send - ts_now_recon).msec() << " ms" << std::endl;
    // finished sending

    // print some additional stats
    std::cout << "kbytes_sent_per_frame (kbytes_per_frame): " << bytes_sent_per_frame/1024 << " (" << bytes_per_frame/1024 << ") -> " << bytes_sent_per_frame * 100.0f/bytes_per_frame << "%" << std::endl;

    if(frame_num == num_frames){
      frame_num = 0;
      fb.rewindFile();
    }

  }

  if(rgb_is_compressed){
    for(unsigned s_num = 0; s_num < num_streams; ++s_num){
      delete [] tmp_rgb[s_num];
      delete [] tmp_rgba[s_num];
    }
  }

  return 0;
}


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.

#ifndef poplibs_test_ctc_loss_hpp
#define poplibs_test_ctc_loss_hpp

#include "CTCUtil.hpp"

#include <boost/multi_array.hpp>

namespace poplibs_test {
namespace ctc {

template <typename FPType>
boost::multi_array<FPType, 2>
alpha(const boost::multi_array<FPType, 2> &sequence,
      const std::vector<unsigned> &paddedSequence, unsigned blankIndex,
      unsigned validTimesteps);

template <typename FPType>
boost::multi_array<FPType, 2>
beta(const boost::multi_array<FPType, 2> &sequence,
     const std::vector<unsigned> &paddedSequence, unsigned blankIndex,
     unsigned validTimesteps);

template <typename FPType>
FPType loss(const boost::multi_array<FPType, 2> &sequence,
            const std::vector<unsigned> &paddedSequence, unsigned blankIndex,
            unsigned validTimesteps);

template <typename FPType>
boost::multi_array<FPType, 2>
expandedGrad(const boost::multi_array<FPType, 2> &sequence,
             const boost::multi_array<FPType, 2> &alpha,
             const boost::multi_array<FPType, 2> &beta,
             const std::vector<unsigned> &paddedSequence, unsigned blankIndex,
             unsigned validTimesteps);

// TODO: Not really CTC grad, as this returns Prob * ctcGrad
template <typename FPType>
boost::multi_array<FPType, 2>
ctcGrad(const boost::multi_array<FPType, 2> &sequence,
        const boost::multi_array<FPType, 2> &alpha,
        const boost::multi_array<FPType, 2> &beta,
        const std::vector<unsigned> &paddedSequence, unsigned symbolsIncBlank,
        unsigned blankIndex, unsigned validTimesteps);

template <typename FPType>
boost::multi_array<FPType, 2>
grad(const boost::multi_array<FPType, 2> &sequence,
     const boost::multi_array<FPType, 2> &logProbs,
     const boost::multi_array<FPType, 2> &alpha,
     const boost::multi_array<FPType, 2> &beta,
     const std::vector<unsigned> &paddedSequence, unsigned symbolsIncBlank,
     unsigned blankIndex, unsigned validTimesteps,
     bool testReducedCodeletGradient);

} // namespace ctc
} // namespace poplibs_test

#endif // poplibs_test_ctc_loss_hpp


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_MAXIMUM_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_MAXIMUM_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/include/constants/maxinit.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief maximum generic tag

     Represents the maximum function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct max_;
    struct Maxinit;

    struct maximum_ : ext::reduction_<maximum_, tag::max_, tag::Maxinit>
    {
      /// @brief Parent hierarchy
      typedef ext::reduction_<maximum_, tag::max_, tag::Maxinit> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_maximum_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::maximum_, Site> dispatching_maximum_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::maximum_, Site>();
   }
   template<class... Args>
   struct impl_maximum_;
  }
  /*!
    Returns the greatest element of the SIMD vector

    @par Semantic:

    For every parameter of type T0

    @code
    scalar_of<T0> r = maximum(a0);
    @endcode

    is similar to:

    @code
      scalar_of<T0> r = Minf;
      for(std::size_t i=0;i<cardinal_of<T0>;++i)
        r =  r < a0[i] ? a0[i] : r;
    @endcode

    @param a0

    @return a value of the scalar type associated to the parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::maximum_, maximum, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::maximum_, maximum, 2)
} }
#endif


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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)

#include <pch.hpp>

#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/array.hpp>

#define BOOST_CHECK_CLOSE_EX(a, b, prec, i) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK_CLOSE(a, b, prec); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " , " << data[i][5] << " } " << std::endl;\
      }\
   }

//
// Implement various versions of inverse of the incomplete beta
// using different root finding algorithms, and deliberately "bad"
// starting conditions: that way we get all the pathological cases
// we could ever wish for!!!
//

template <class T, class Policy>
struct ibeta_roots_1   // for first order algorithms
{
   ibeta_roots_1(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   T operator()(const T& x)
   {
      return boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
   }
private:
   T a, b, target;
   bool invert;
};

template <class T, class Policy>
struct ibeta_roots_2   // for second order algorithms
{
   ibeta_roots_2(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   boost::math::tuple<T, T> operator()(const T& x)
   {
      typedef typename boost::math::lanczos::lanczos<T, Policy>::type L;
      T f = boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
      T f1 = invert ?
         -boost::math::detail::ibeta_power_terms(b, a, 1 - x, x, L(), true, Policy())
               : boost::math::detail::ibeta_power_terms(a, b, x, 1 - x, L(), true, Policy());
      T y = 1 - x;
      if(y == 0)
         y = boost::math::tools::min_value<T>() * 8;
      f1 /= y * x;

      // make sure we don't have a zero derivative:
      if(f1 == 0)
         f1 = (invert ? -1 : 1) * boost::math::tools::min_value<T>() * 64;

      return boost::math::make_tuple(f, f1);
   }
private:
   T a, b, target;
   bool invert;
};

template <class T, class Policy>
struct ibeta_roots_3   // for third order algorithms
{
   ibeta_roots_3(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   boost::math::tuple<T, T, T> operator()(const T& x)
   {
      typedef typename boost::math::lanczos::lanczos<T, Policy>::type L;
      T f = boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
      T f1 = invert ?
               -boost::math::detail::ibeta_power_terms(b, a, 1 - x, x, L(), true, Policy())
               : boost::math::detail::ibeta_power_terms(a, b, x, 1 - x, L(), true, Policy());
      T y = 1 - x;
      if(y == 0)
         y = boost::math::tools::min_value<T>() * 8;
      f1 /= y * x;
      T f2 = f1 * (-y * a + (b - 2) * x + 1) / (y * x);
      if(invert)
         f2 = -f2;

      // make sure we don't have a zero derivative:
      if(f1 == 0)
         f1 = (invert ? -1 : 1) * boost::math::tools::min_value<T>() * 64;

      return boost::math::make_tuple(f, f1, f2);
   }
private:
   T a, b, target;
   bool invert;
};

double inverse_ibeta_bisect(double a, double b, double z)
{
   typedef boost::math::policies::policy<> pol;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   boost::math::tools::eps_tolerance<double> tol(precision);
   return boost::math::tools::bisect(ibeta_roots_1<double, pol>(a, b, z, invert), min, max, tol).first;
}

double inverse_ibeta_newton(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   return boost::math::tools::newton_raphson_iterate(ibeta_roots_2<double, boost::math::policies::policy<> >(a, b, z, invert), guess, min, max, precision);
}

double inverse_ibeta_halley(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   return boost::math::tools::halley_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(a, b, z, invert), guess, min, max, precision);
}

double inverse_ibeta_schroeder(double a, double b, double z)
{
   double guess = 0.5;
   bool invert = false;
   int bits = std::numeric_limits<double>::digits;

   //
   // special cases, we need to have these because there may be other
   // possible answers:
   //
   if(z == 1) return 1;
   if(z == 0) return 0;

   //
   // We need a good estimate of the error in the incomplete beta function
   // so that we don't set the desired precision too high.  Assume that 3-bits
   // are lost each time the arguments increase by a factor of 10:
   //
   using namespace std;
   int bits_lost = static_cast<int>(ceil(log10((std::max)(a, b)) * 3));
   if(bits_lost < 0)
      bits_lost = 3;
   else
      bits_lost += 3;
   int precision = bits - bits_lost;

   double min = 0;
   double max = 1;
   return boost::math::tools::schroeder_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(a, b, z, invert), guess, min, max, precision);
}

template <class T>
void test_inverses(const T& data)
{
   using namespace std;
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   value_type precision = static_cast<value_type>(ldexp(1.0, 1-boost::math::policies::digits<value_type, boost::math::policies::policy<> >()/2)) * 100;
   if(boost::math::policies::digits<value_type, boost::math::policies::policy<> >() < 50)
      precision = 1;   // 1% or two decimal digits, all we can hope for when the input is truncated

   for(unsigned i = 0; i < data.size(); ++i)
   {
      //
      // These inverse tests are thrown off if the output of the
      // incomplete beta is too close to 1: basically there is insuffient
      // information left in the value we're using as input to the inverse
      // to be able to get back to the original value.
      //
      if(data[i][5] == 0)
      {
         BOOST_CHECK_EQUAL(inverse_ibeta_halley(data[i][0], data[i][1], data[i][5]), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_schroeder(data[i][0], data[i][1], data[i][5]), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_newton(data[i][0], data[i][1], data[i][5]), value_type(0));
         BOOST_CHECK_EQUAL(inverse_ibeta_bisect(data[i][0], data[i][1], data[i][5]), value_type(0));
      }
      else if((1 - data[i][5] > 0.001)
         && (fabs(data[i][5]) > 2 * boost::math::tools::min_value<value_type>())
         && (fabs(data[i][5]) > 2 * boost::math::tools::min_value<double>()))
      {
         value_type inv = inverse_ibeta_halley(data[i][0], data[i][1], data[i][5]);
         BOOST_CHECK_CLOSE_EX(data[i][2], inv, precision, i);
         inv = inverse_ibeta_schroeder(data[i][0], data[i][1], data[i][5]);
         BOOST_CHECK_CLOSE_EX(data[i][2], inv, precision, i);
         inv = inverse_ibeta_newton(data[i][0], data[i][1], data[i][5]);
         BOOST_CHECK_CLOSE_EX(data[i][2], inv, precision, i);
         inv = inverse_ibeta_bisect(data[i][0], data[i][1], data[i][5]);
         BOOST_CHECK_CLOSE_EX(data[i][2], inv, precision, i);
      }
      else if(1 == data[i][5])
      {
         BOOST_CHECK_EQUAL(inverse_ibeta_halley(data[i][0], data[i][1], data[i][5]), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_schroeder(data[i][0], data[i][1], data[i][5]), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_newton(data[i][0], data[i][1], data[i][5]), value_type(1));
         BOOST_CHECK_EQUAL(inverse_ibeta_bisect(data[i][0], data[i][1], data[i][5]), value_type(1));
      }

   }
}

#ifndef SC_
#define SC_(x) static_cast<T>(BOOST_JOIN(x, L))
#endif

template <class T>
void test_beta(T, const char* /* name */)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // five items, input value a, input value b, integration limits x, beta(a, b, x) and ibeta(a, b, x):
   //
#  include "ibeta_small_data.ipp"

   test_inverses(ibeta_small_data);

#  include "ibeta_data.ipp"

   test_inverses(ibeta_data);

#  include "ibeta_large_data.ipp"

   test_inverses(ibeta_large_data);
}

int test_main(int, char* [])
{
   test_beta(0.1, "double");
   return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_DEFINITION_REMFIX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_REMFIX_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/function/definition/rem.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    using remfix_ = rem_;
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::remfix_,remfix);
} }

#endif