/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include "allaser.h"

#include <iostream>
#include <alcommon/alproxy.h>
#include <alproxies/almemoryproxy.h>
#include <boost/shared_ptr.hpp>
#include <alcommon/albroker.h>
#include <alcommon/almodule.h>
#include <sstream>

#include <pthread.h>
#include <signal.h>
#include <cmath>

#include <qi/log.hpp>

#if defined (__linux__)
#include <sys/prctl.h>
#endif

extern "C" {
# include "urg_ctrl.h"
# include "scip_handler.h" /* WARNING: private header not necessarly installed */
}

#define MODE_ON 0
#define MODE_OFF 1
#define SEND_MODE_ON 2
#define SEND_MODE_OFF 3

#define MIDDLE_ANGLE 384
#define DEFAULT_MIN_ANGLE 44
#define DEFAULT_MAX_ANGLE 725
#define MIN_ANGLE_LASER 44
#define MAX_ANGLE_LASER 725
#define RESOLUTION_LASER 1024
#define MIN_LENGTH_LASER 20
#define MAX_LENGTH_LASER 5600

#include <sys/time.h>

using namespace AL;
using namespace std;
pthread_t urgThreadId;
boost::shared_ptr<ALMemoryProxy> gSTM;
static int mode = MODE_ON;
static urg_t urg;
static int angle_min = DEFAULT_MIN_ANGLE;
static int angle_max = DEFAULT_MAX_ANGLE;
static int length_min = MIN_LENGTH_LASER;
static int length_max = MAX_LENGTH_LASER;

static void urg_exit(urg_t *urg, const char *message) {
  qiLogInfo("hardware.allaser", "%s: %s\n", message, urg_error(urg));
  urg_disconnect(urg);
  qiLogInfo("hardware.allaser", "urg_exit\n");
  pthread_exit((void *)NULL);
}

#define URG_DEFAULT_SPEED (19200)
#define URG_FAST_SPEED (115200)

void * urgThread(void * arg) {

#if defined (__linux__)
  // thread name
  prctl(PR_SET_NAME, "ALLaser::urgThread", 0, 0, 0);
#endif

  ALValue urgdata;
  long *data = NULL;
  int data_max;
  int ret;
  int n;
  int i,imemory,refTime,end,sampleTime, beginThread;
  std::string valueName="Device/Laser/Value";

  /* Connection */

  connectToLaser();

  /* Reserve the Receive data buffer */
  data_max = urg_dataMax(&urg);
  if (data_max <= 0) {
    perror("data_max is less than 0");
    pthread_exit((void *)NULL);
  }
  data = (long*)malloc(sizeof(long) * data_max);
  memset(data, 0, sizeof(long) * data_max);

  /* prepare ALValue for ALMemory*/
  urgdata.arraySetSize(data_max);
  for (i=0; i< data_max;i++)
  {
    urgdata[i].arraySetSize(4);
    urgdata[i][0]= (int)0;
    urgdata[i][1]= (double)0.0;
    urgdata[i][2]= (int)0;
    urgdata[i][3]= (int)0;
  }
  /* insert ALvalue in ALMemory*/

  gSTM->insertData(valueName,urgdata);

  gSTM->insertData("Device/Laser/LaserEnable", (bool) 1);
  gSTM->insertData("Device/Laser/MinAngle",(float)((2.0 * M_PI)
          * (DEFAULT_MIN_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MaxAngle",(float)((2.0 * M_PI)
          * (DEFAULT_MAX_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MinLength",(float)(length_min));
  gSTM->insertData("Device/Laser/MaxLength",(float)(length_max));

  stringstream ss;
  ss << "ALLaser running";
  qiLogInfo("hardware.laser") << ss.str() << std::endl;

  while(1)
  {
    if(mode==SEND_MODE_ON){
      ret = urg_laserOn(&urg);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }
      mode=MODE_ON;
      /* Connection */
      connectToLaser();
    }
    if(mode==SEND_MODE_OFF){
      scip_qt(&urg.serial_, NULL, ScipNoWaitReply);
      mode=MODE_OFF;
    }
    if(mode==MODE_ON){
      /* Request Data using GD-Command */
      ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }

      refTime = getLocalTime();
      /* Obtain Data */
      n = urg_receiveData(&urg, data, data_max);
      qiLogDebug("hardware.laser") << " n " << n << " expected " <<
            angle_max - angle_min << std::endl;
      if (n < 0) {
        urg_exit(&urg, "urg_receiveData()");
      }
      end= getLocalTime();
      sampleTime=end-refTime;

      imemory=0;
      for (i = 0; i < n; ++i) {
        int x, y;
        double angle = urg_index2rad(&urg, i);

        qiLogDebug("hardware.laser") << i << " angle " << angle <<
              " urgAngle " << urg_index2rad(&urg, i) <<
              " dist " << data[i] << std::endl;

        int length = data[i];

        if( length >= length_min && length <= length_max
            && i >= angle_min && i <= angle_max ){
          x = (int)((double)length * cos(angle));
          y = (int)((double)length * sin(angle));
          urgdata[imemory][0]= length;
          urgdata[imemory][1]= angle;
          urgdata[imemory][2]= x;
          urgdata[imemory][3]= y;
          imemory++;
        }
      }
      for(;imemory<data_max;imemory++){
        urgdata[imemory][0]= 0;
        urgdata[imemory][1]= 0;
        urgdata[imemory][2]= 0;
        urgdata[imemory][3]= 0;
      }
      gSTM->insertData(valueName,urgdata);
      usleep(1000);
    }else{
      usleep(10000);
    }
  }
  urg_disconnect(&urg);

  free(data);
}

//______________________________________________
// constructor
//______________________________________________
ALLaser::ALLaser(boost::shared_ptr<ALBroker> pBroker, const std::string& pName ): ALModule(pBroker, pName )
{
  setModuleDescription( "Allow control over Hokuyo laser when available on Nao's head." );

  functionName("laserOFF", "ALLaser", "Disable laser light");
  BIND_METHOD( ALLaser::laserOFF );

  functionName("laserON", "ALLaser", "Enable laser light and sampling");
  BIND_METHOD( ALLaser::laserON );

  functionName("setOpeningAngle", "ALLaser", "Set openning angle of the laser");
  addParam("angle_min_f", "float containing the min value in rad, this value must be upper than -2.35619449 ");
  addParam("angle_max_f", "float containing the max value in rad, this value must be lower than 2.092349795 ");
  addMethodExample( "python",
        "# Set the opening angle at -90/90 degres\n"
        "laser = ALProxy(\"ALLaser\",\"127.0.0.1\",9559)\n"
        "laser.setOpeningAngle(-1.570796327,1.570796327)\n"
      );
  BIND_METHOD( ALLaser::setOpeningAngle );

  functionName("setDetectingLength", "ALLaser", "Set detection threshold of the laser");
  addParam("length_min_l", "int containing the min length that the laser will detect(mm), this value must be upper than 20 mm");
  addParam("length_max_l", "int containing the max length that the laser will detect(mm), this value must be lower than 5600 mm");
  addMethodExample( "python",
        "# Set detection threshold at 500/3000 mm\n"
        "laser = ALProxy(\"ALLaser\",\"127.0.0.1\",9559)\n"
        "laser.setDetectingLength(500,3000)\n"
      );
  BIND_METHOD( ALLaser::setDetectingLength );

  // get broker on DCM and ALMemory
  try {
    gSTM = getParentBroker()->getMemoryProxy();
  } catch(ALError& e) {
    qiLogError("hardware.allaser")
        << "Could not connect to Memory. Error: " << e.what() << std::endl;
  }

  pthread_create(&urgThreadId, NULL, urgThread, NULL);
}

//______________________________________________
// destructor
//______________________________________________
ALLaser::~ALLaser()
{
  pthread_cancel(urgThreadId);
}

void ALLaser::laserOFF(void){
  mode = SEND_MODE_OFF;
  gSTM->insertData("Device/Laser/LaserEnable",(bool)0);
}

void ALLaser::laserON(void){
  mode = SEND_MODE_ON;
  gSTM->insertData("Device/Laser/LaserEnable",(bool)1);
}

void ALLaser::setOpeningAngle(const float& angle_min_f,
                              const float& angle_max_f){
  angle_min = MIDDLE_ANGLE + (int)(RESOLUTION_LASER
                                   * (float)angle_min_f / (2.0 * M_PI));
  angle_max = MIDDLE_ANGLE + (int)(RESOLUTION_LASER*(float)angle_max_f
                                   / (2.0 * M_PI));
  if(angle_min<MIN_ANGLE_LASER) angle_min = MIN_ANGLE_LASER;
  if(angle_max>MAX_ANGLE_LASER) angle_max = MAX_ANGLE_LASER;
  if(angle_min>=angle_max){
    angle_min = MIN_ANGLE_LASER;
    angle_max = MAX_ANGLE_LASER;
  }
  gSTM->insertData("Device/Laser/MinAngle",(float)((2.0 * M_PI)
          * (angle_min - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MaxAngle",(float)((2.0 * M_PI)
          * (angle_max - MIDDLE_ANGLE) / RESOLUTION_LASER));
}

void ALLaser::setDetectingLength(const int& length_min_l,
                                 const int& length_max_l){
  length_min=(int)length_min_l;
  length_max=(int)length_max_l;
  if(length_min<MIN_LENGTH_LASER) length_min = MIN_LENGTH_LASER;
  if(length_max>MAX_LENGTH_LASER) length_min = MAX_LENGTH_LASER;
  if(length_min>=length_max){
    length_min = MIN_LENGTH_LASER;
    length_max = MAX_LENGTH_LASER;
  }
  gSTM->insertData("Device/Laser/MinLength",(int)length_min);
  gSTM->insertData("Device/Laser/MaxLength",(int)length_max);
}

//_________________________________________________
// Service functions
//_________________________________________________

int connectToLaserViaUSB(void) {
  int success = -1;
  const char deviceUSB[] = "/dev/ttyUSB0"; /* Example when using Linux  */
  // Try to connect at low speed.
  success = urg_connect(&urg, deviceUSB, URG_DEFAULT_SPEED);
  if (success < 0) {
    std::stringstream ss;
    ss << "Connection failure to " << deviceUSB << " at "
        << URG_DEFAULT_SPEED << ". " << urg_error(&urg);
    qiLogDebug("hardware.allaser") << ss.str() << std::endl;
    return success;
  }
  else {
    // Try to switch to full speed.
    scip_ss(&urg.serial_, URG_FAST_SPEED);
    urg_disconnect(&urg);
    success = urg_connect(&urg, deviceUSB, URG_FAST_SPEED);
    if (success < 0) {
      std::stringstream ss;
      ss << "Connection failure to " << deviceUSB << " at "
          << URG_FAST_SPEED << ". " << urg_error(&urg);
      qiLogDebug("hardware.allaser") << ss.str() << std::endl;
      // Fall back to low speed.
      success = urg_connect(&urg, deviceUSB, URG_DEFAULT_SPEED);
      return success;
    }
    else {
      return success;
    }
  }
}

int connectToLaserViaACM(void) {
  int success = -1;
  const char deviceACM[] = "/dev/ttyACM0"; /* Example when using Linux  */
  success = urg_connect(&urg, deviceACM, URG_DEFAULT_SPEED);
  if (success < 0) {
    std::stringstream ss;
    ss << "Connection failure to " << deviceACM << " at "
        << URG_DEFAULT_SPEED << ". " << urg_error(&urg);
    qiLogDebug("hardware.allaser") << ss.str() << std::endl;
    return success;
  }
  else {
    // Try to switch to full speed.
    scip_ss(&urg.serial_, URG_FAST_SPEED);
    urg_disconnect(&urg);
    success = urg_connect(&urg, deviceACM, URG_FAST_SPEED);
    if (success < 0)
    {
      std::stringstream ss;
      ss << "Connection failure from " << deviceACM << " at "
          << URG_FAST_SPEED << ". " << urg_error(&urg);
      qiLogDebug("hardware.allaser") << ss.str() << std::endl;
      // Fall back to low speed.
      success = urg_connect(&urg, deviceACM, URG_DEFAULT_SPEED);
      return success;
    }
    else {
      return success;
    }
  }
}

void connectToLaser(void){
  int success = connectToLaserViaUSB();
  if (success < 0) {
    std::stringstream ss;
    ss << "Connection failure to USB, trying ACM... ";
    qiLogDebug("hardware.allaser") << ss.str() << std::endl;
    success = connectToLaserViaACM();
    if (success < 0) {
      std::stringstream ss;
      ss << "Connection to laser failed";
      qiLogDebug("hardware.allaser") << ss.str() << std::endl;
      pthread_exit((void *)NULL);
    }
    else {
      std::stringstream ss;
      ss << "Connection to laser via ACM";
      qiLogDebug("hardware.allaser") << ss.str() << std::endl;
    }
  }
  else {
    std::stringstream ss;
    ss << "Connection to laser via USB";
    qiLogDebug("hardware.allaser") << ss.str() << std::endl;
  }
}

unsigned int getLocalTime(void){
  struct timeval tv;
  unsigned int val;

  gettimeofday(&tv, NULL);

  val = (unsigned int)((unsigned int)(tv.tv_usec/1000)
                       + (unsigned int)(tv.tv_sec*1000));

  // Time in ms
  return val;
}

double index2rad(int index){

  double radian = (2.0 * M_PI) *
                  (index - MIDDLE_ANGLE) / RESOLUTION_LASER;

  return radian;
}


#include "profile3d.hpp"
#include <boost/python.hpp>
#include <pyublas/numpy.hpp>
#include "optimization.hpp"
#include "rootfinder.hpp"

namespace gd {
using namespace boost::python;

void py_export_profile_3d() {
	class_< ProfileModel3d, boost::noncopyable >("ProfileModel3d", no_init)
		;
	class_<ProfileModel3d1C, bases<ProfileModel3d>, boost::noncopyable  >("ProfileModel3d1C", init<Profile3d*>())
		.def("density",   (&ProfileModel3d1C::density))
		.def("potential", (&ProfileModel3d1C::potential))
		.def("dphidx", (&ProfileModel3d1C::dphidx))
		.def("dphidy", (&ProfileModel3d1C::dphidy))
		.def("dphidz", (&ProfileModel3d1C::dphidz))
		;
	class_<ProfileModel3d2C, bases<ProfileModel3d>, boost::noncopyable  >("ProfileModel3d2C", init<Profile3d*,Profile3d*>())
		.def("density",   (&ProfileModel3d2C::density))
		.def("potential", (&ProfileModel3d2C::potential))
		.def("dphidx", (&ProfileModel3d2C::dphidx))
		.def("dphidy", (&ProfileModel3d2C::dphidy))
		.def("dphidz", (&ProfileModel3d2C::dphidz))
		;
	class_< Profile3d, boost::noncopyable >("Profile3d", no_init)
		;
	class_< Density3d, boost::noncopyable >("Density3d", no_init)
		;

	class_<NFW3d, bases<Profile3d, Density3d> >("NFW3d", init<double, double, double, double>())
		.def("density",   (&NFW3d::density))
		.def("potential", (&NFW3d::potential))
		.def("dphidx", (&NFW3d::dphidx))
		.def("dphidy", (&NFW3d::dphidy))
		.def("dphidz", (&NFW3d::dphidz))
		.def_readonly("c", &NFW3d::c)
		.def_readonly("mass200", &NFW3d::mass200)
		.def_readonly("r200", &NFW3d::r200)
		.def_readonly("rs", &NFW3d::rs)
		.def_readonly("rho0", &NFW3d::rho0);
	;
	class_<NFW3dTriax, bases<Profile3d, Density3d> >("NFW3dTriax", init<double, double, double, double, double, double, double, double>())
		.def("density",   (&NFW3dTriax::density))
		.def("potential", (&NFW3dTriax::potential))
		.def("dphidx", (&NFW3dTriax::dphidx))
		.def("dphidy", (&NFW3dTriax::dphidy))
		.def("dphidz", (&NFW3dTriax::dphidz))
		.def_readonly("c", &NFW3dTriax::c)
		.def_readonly("mass200", &NFW3dTriax::mass200)
		.def_readonly("r200", &NFW3dTriax::r200)
		.def_readonly("rs", &NFW3dTriax::rs)
		.def_readonly("rho0", &NFW3dTriax::rho0);
	;
	class_<MiyamotoNagai3d, bases<Profile3d, Density3d> >("MiyamotoNagai3d", init<double, double, double, double>())
		.def("density",   (&MiyamotoNagai3d::density))
		.def("potential", (&MiyamotoNagai3d::potential))
		.def("dphidx", (&MiyamotoNagai3d::dphidx))
		.def("dphidy", (&MiyamotoNagai3d::dphidy))
		.def("dphidz", (&MiyamotoNagai3d::dphidz))
		.def_readonly("a", &MiyamotoNagai3d::a)
		.def_readonly("b", &MiyamotoNagai3d::b)
		.def_readonly("mass", &MiyamotoNagai3d::mass)
	;

}

}


#include "gtest/gtest.h"
#include <cstdio>
#include <thread>
#include <chrono>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <boost/filesystem.hpp>

#ifdef HAVE_JSON
#include "json.hxx"
#endif

#include "ray.hxx"
#include "image.hxx"
#include "camera.hxx"
#include "infiniteplane.hxx"
#include "sampler.hxx"
#include "scene.hxx"
#include "renderingalgorithms_simplebase.hxx"
#include "atmosphere.hxx"
#include "util.hxx"
#include "hashgrid.hxx"
#include "photonintersector.hxx"
#include "shader_util.hxx"
#include "shader_physics.hxx"
#include "lightpicker_ucb.hxx"
#include "memory_arena.hxx"
#include "ndarray.hxx"

using namespace materials;

TEST(TestRaySegment, ExprTemplates)
{
  auto e = RaySegment{}.EndPoint();
  ASSERT_NE(typeid(e), typeid(e.eval()));
  ASSERT_EQ(typeid(e.eval()), typeid(Eigen::Matrix<double, 3, 1>));
}

TEST(TestRaySegment, EndPointNormal)
{
  Double3 p = RaySegment{{Double3(1., 0., 0.), Double3(0., 1., 0.)}, 2.}.EndPoint();
  ASSERT_EQ(p[0], 1.);
  ASSERT_EQ(p[1], 2.);
  ASSERT_EQ(p[2], 0.);
}

TEST(TestRaySegment, Reversed)
{
  RaySegment s{{{1, 2, 3}, {42, 0, 0}}, 5.};
  RaySegment r = s.Reversed();
  ASSERT_NEAR(s.EndPoint()[0], r.ray.org[0], 1.e-8);
  ASSERT_NEAR(r.EndPoint()[0], s.ray.org[0], 1.e-8);
}

TEST(TestMath, TakeFromVectorByIndices)
{
  Eigen::Array<double, 10, 1> m; m << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;
  Eigen::Array<double, 3, 1> v = Take(m, Index3{3,4,9});
  ASSERT_EQ(v[0], 3);
  ASSERT_EQ(v[1], 4);
  ASSERT_EQ(v[2], 9);
}

TEST(TestMath, OrthogonalSystemZAligned)
{
  Double3 directions[] = {
    { 1., 0., 0. },
    { 0., 1., 0. },
    { 0., 0., 3. },
    { 1., 1., 0. }
  };
  for (auto dir : directions)
  {
    Eigen::Matrix<double, 3, 3> m = OrthogonalSystemZAligned(Normalized(dir));
    std::cout << "dir = " << dir << std::endl;
    std::cout << "m{dir} = " << m << std::endl;
    EXPECT_TRUE(m.isUnitary(1.e-6));
    ASSERT_NEAR(m.determinant(), 1., 1.e-6);
    auto mi = m.inverse().eval();
    auto dir_local = mi * dir;
    EXPECT_NEAR(dir_local[0], 0., 1.e-6);
    EXPECT_NEAR(dir_local[1], 0., 1.e-6);
    EXPECT_NEAR(dir_local[2], Length(dir), 1.e-6);
  }
}

TEST(TestMath, Quadratic)
{
  using util::Quadratic;
  float a = 1;
  float b = 100;
  float c = 1;
  float t0, t1, e0, e1;
  ASSERT_TRUE(Quadratic<float>(a, b, c, 0, 0, 0, t0, t1, e0, e1));
  std::cout << t0 << " " << t1 << " " << e0 << " " << e1 << std::endl;
  EXPECT_NEAR(t0, -99.99f, 1.e-4);
  EXPECT_NEAR(t1, -0.010001f, 1.e-7);
  EXPECT_NEAR(e0, 3.57604003511369e-5f, 1.e-10);
  EXPECT_NEAR(e1, 3.57675622453257e-9f, 1.e-15);
  ASSERT_TRUE(Quadratic<float>(a, b, c, 0.1, 0, 0, t0, t1, e0, e1));
  std::cout << e0 << " " << e1 << std::endl;
  EXPECT_NEAR(e0, 10.0000362396240, 1.e-4);
  EXPECT_NEAR(e1, 1.03616784485894e-7, 1.e-10);
  ASSERT_TRUE(Quadratic<float>(a, b, c, 0, 1, 0, t0, t1, e0, e1));
  std::cout << e0 << " " << e1 << std::endl;
  EXPECT_NEAR(e0, 1.00013589859009, 1.e-7);
  EXPECT_NEAR(e1, 0.000100033619673923, 1.e-10);
  ASSERT_TRUE(Quadratic<float>(a, b, c, 0, 0, 0.1, t0, t1, e0, e1));
  std::cout << e0 << " " << e1 << std::endl;
  EXPECT_NEAR(e0, 0.00103596039116383, 1.e-9);
  EXPECT_NEAR(e1, 0.00100020365789533, 1.e-9);
}

TEST(TestMath, Reflected)
{
  Double3 n{0., 1., 0.};
  auto in = Normalized(Double3{0., 1., 2.});
  Double3 out = Reflected(in, n);
  auto out_expected = Normalized(Double3{0., 1., -2.});
  ASSERT_GE(Dot(out, out_expected), 0.99);
  // Test normal flip
  n = Double3{0.,-1.,0.};
  out = Reflected(in, n);
  ASSERT_GE(Dot(out, out_expected), 0.99);
}

TEST(TestMath, Refracted)
{
  Double3 n{0., 1., 0.};
  auto w1 = Normalized(Double3{0., 1., 2.});
  double eta1 = 1.;
  double eta2 = 1.1;
  auto w2 = Refracted(w1, n, eta1/eta2);
  ASSERT_TRUE((bool)w2);
  auto w3 = Refracted(*w2, n, eta2/eta1);
  ASSERT_TRUE((bool)w3);
  EXPECT_NEAR((*w3)[0], w1[0], 1.e-3);
  EXPECT_NEAR((*w3)[1], w1[1], 1.e-3);
  EXPECT_NEAR((*w3)[2], w1[2], 1.e-3);
  // Test normal flip
  n = Double3{0.,-1.,0.};
  Double3 w4 = *Refracted(w1, n, eta1/eta2);
  EXPECT_NEAR(w4[0], (*w2)[0], 1.e-3);
  EXPECT_NEAR(w4[1], (*w2)[1], 1.e-3);
  EXPECT_NEAR(w4[2], (*w2)[2], 1.e-3);
  // Test total reflection.
  w1 = Normalized(Double3{0., 1., 100.});
  eta1 = 1.2;
  eta2 = 1.;
  w2 = Refracted(w1, n, eta1/eta2);
  ASSERT_FALSE((bool)w2);
}

TEST(TestMath, RefractedHalfVector)
{
  double eta_1_over_2 = 1.0/1.5;
  Double3 w1 = Normalized(Double3{0., 3., 1.});
  Double3 w2  = Normalized(Double3{0., -1., 0.});
  boost::optional<Double3> h = HalfVectorRefracted(w1, w2, eta_1_over_2);
  boost::optional<Double3> wr = Refracted(w1, *h, eta_1_over_2);
  ASSERT_TRUE((bool)wr);
  ASSERT_NEAR((*wr)[0], w2[0], 1.e-3);
  ASSERT_NEAR((*wr)[0], w2[0], 1.e-3);
  ASSERT_NEAR((*wr)[0], w2[0], 1.e-3);
}

TEST(TestMath, RefractedHalfVectorDegenerate)
{
  double eta_1_over_2 = 1.0/1.5;
  Double3 w1{0., 1., 0.};
  Double3 w2{0., -1., 0.};
  boost::optional<Double3> h = HalfVectorRefracted(w1, w2, eta_1_over_2);
  boost::optional<Double3> wr = Refracted(w1, *h, eta_1_over_2);
  ASSERT_TRUE((bool)wr);
  ASSERT_NEAR((*wr)[0], w2[0], 1.e-3);
  ASSERT_NEAR((*wr)[0], w2[0], 1.e-3);
  ASSERT_NEAR((*wr)[0], w2[0], 1.e-3);
}

TEST(TestMath, HalfVector)
{
  Double3 w1 = Normalized(Double3{0., 3., 1.});
  Double3 w2  = Normalized(Double3{0., -1., 0.});
  Double3 h = HalfVector(w1, w2);
  Double3 wr = Reflected(w1, h);
  ASSERT_NEAR(wr[0], w2[0], 1.e-3);
  ASSERT_NEAR(wr[0], w2[0], 1.e-3);
  ASSERT_NEAR(wr[0], w2[0], 1.e-3);
}

TEST(TestMath, HalfVectorDegenerate)
{
  Double3 w1{0., 1., 0.};
  Double3 w2{0., -1., 0.};
  Double3 h = HalfVector(w1, w2);
  Double3 wr = Reflected(w1, h);
  ASSERT_NEAR(wr[0], w2[0], 1.e-3);
  ASSERT_NEAR(wr[0], w2[0], 1.e-3);
  ASSERT_NEAR(wr[0], w2[0], 1.e-3);
}

TEST(TestMath, RaySphereClip)
{
  { // Start ray outside of the sphere
    Double3 org{1,0,-1};
    Double3 dir{0,0,1};
    Double3 p{1,0,2};
    double  r{2};
    auto [ok, tnear, tfar] = ClipRayToSphereInterior(org, dir, 0, LargeNumber, p, r);
    EXPECT_TRUE(ok);
    EXPECT_NEAR(tnear, 1., 1.e-6);
    EXPECT_NEAR(tfar, 5., 1.e-6);
  }
  { // Start ray inside
    Double3 org{1,0,-1};
    Double3 dir{0,0,1};
    Double3 p{1,0,1};
    double  r{3};
    auto [ok, tnear, tfar] = ClipRayToSphereInterior(org, dir, 0, LargeNumber, p, r);
    EXPECT_TRUE(ok);
    EXPECT_NEAR(tnear, 0., 1.e-6);
    EXPECT_NEAR(tfar, 5., 1.e-6);
  }
}

TEST(TestMath, Pow)
{
  EXPECT_EQ(util::Pow(5.,0), 1.);
  EXPECT_EQ(util::Pow(5.,1), 5.);
  EXPECT_NEAR(util::Pow(5.,27), std::pow(5.,27.), 1.e-9);
  EXPECT_NEAR(util::Pow(0.25,12), std::pow(0.25,12), 1.e-9);
}

TEST(Spectral, RGBConversion)
{
  using namespace Color;
  std::array<RGB, 10> trials = {{
    { 0._rgb, 0._rgb, 0._rgb },
    { 1._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 1._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 1._rgb },
    { 1._rgb, 1._rgb, 0._rgb },
    { 1._rgb, 0._rgb, 1._rgb },
    { 0._rgb, 1._rgb, 1._rgb },
    { 1._rgb, 1._rgb, 1._rgb },
    { 1._rgb, 0.5_rgb, 0.3_rgb },
    { 0.5_rgb, 0.8_rgb, 0.9_rgb },
  }};
  for (RGB trial : trials)
  {
    SpectralN s = RGBToSpectrum(trial);
    for (int i=0; i<s.size(); ++i)
    {
      EXPECT_GE(s[i], 0.);
      EXPECT_LE(s[i], 1.01);
    }
    RGB back = SpectrumToRGB(s);
    EXPECT_NEAR(value(trial[0]), value(back[0]), 1.3e-2);
    EXPECT_NEAR(value(trial[1]), value(back[1]), 1.3e-2);
    EXPECT_NEAR(value(trial[2]), value(back[2]), 1.3e-2);
  }
}

TEST(Spectral, RGBConversionLinearity)
{
  using namespace Color;
  SpectralN spectra[3] = {
    SpectralN::Zero(), SpectralN::Zero(), SpectralN::Zero()};
  spectra[0][0] = 1.;
  spectra[1][2] = 1.;
  spectra[2][NBINS-1] = 1.;
  auto CheckLinear = [](const SpectralN &a, const SpectralN &b)
  {
    RGB rgb_a = SpectrumToRGB(a);
    RGB rgb_b = SpectrumToRGB(b);
    RGB rgb_ab = SpectrumToRGB(a+b);
    EXPECT_LE(value((rgb_ab - rgb_b - rgb_a).abs().maxCoeff()), 1.e-3);
  };
  CheckLinear(spectra[0], spectra[1]);
  CheckLinear(spectra[0], spectra[2]);
  CheckLinear(spectra[1], spectra[2]);
}

TEST(Spectral, RGBConversionSelection)
{
  /* Back and forth transform using RGBToSpectralSelection and SpectralSelectionToRGB.
   * Only part of the spectrum is considered by these function, namely the wavelengths
   * given by the indices. Because of that, the complete reconstruction of the RGB
   * original requires summation over parts of the spectrum. It is basically an inner
   * product of the spectrum with the color matching function, except that on each iteration
   * is not one wavelengths but multiple added to the output. */

  using namespace Color;
  auto rgb = RGB(0.8_rgb, 0.2_rgb, 0.6_rgb);
  RGB converted_rgb = RGB::Zero();
  for (int lambda=0; lambda<Color::NBINS-2; lambda += static_size<Spectral3>())
  {
    Index3 idx{lambda, lambda+1, lambda+2};
    converted_rgb += SpectralSelectionToRGB(RGBToSpectralSelection(rgb, idx), idx);
  }
  EXPECT_NEAR(value(rgb[0]), value(converted_rgb[0]), 1.e-2);
  EXPECT_NEAR(value(rgb[1]), value(converted_rgb[1]), 1.e-2);
  EXPECT_NEAR(value(rgb[2]), value(converted_rgb[2]), 1.e-2);
}

class IntersectorTests : public testing::Test
{
  EmbreeAccelerator scene;
protected:
  SurfaceInteraction intersection;
  double distance;

  void Initialize(std::function<void(EmbreeAccelerator &)> scene_filler)
  {
    scene_filler(scene);
    scene.Build();
  }

  void Intersect(const Ray &ray, bool expect_hit = true)
  {
    RaySegment rs{ray, LargeNumber};
    bool bhit = scene.FirstIntersection(rs.ray, 0., rs.length, intersection);
    ASSERT_TRUE(bhit == expect_hit);
    distance = rs.length;
  }

  void CheckPosition(const Double3 &p) const
  {
    for (int i=0; i<3; ++i)
      EXPECT_NEAR(intersection.pos[i], p[i], 1.e-6);
  }

  void CheckNormal(const Double3 &n)
  {
    for (int i=0; i<3; ++i)
      EXPECT_NEAR(intersection.normal[i], n[i], 1.e-6);
  }
};

inline RaySegment MakeSegmentAt(const SurfaceInteraction &intersection, const Double3 &ray_dir)
{
  return RaySegment{
    {intersection.pos + AntiSelfIntersectionOffset(intersection, ray_dir), ray_dir},
    LargeNumber
  };
}

TEST_F(IntersectorTests, Sphere)
{
  Spheres s;
  this->Initialize([&](EmbreeAccelerator &scene) {
    s.Append({ 0., 0., 2. }, 2.);
    scene.InsertRefTo(s);
  });
  Intersect({{0., 0., -1.},{0., 0., 1.}});
  EXPECT_NEAR(distance, 1., 1.e-6);
  CheckPosition({0., 0., 0.});
  CheckNormal({0.,0.,-1.});
}

TEST_F(IntersectorTests, Triangle)
{
  /*     x
   *   / |
   *  /  |
   * x---x
   * Depiction of the triangle */
  Mesh m{ 0,0 };
  this->Initialize([&](EmbreeAccelerator &scene) {
    float q = 0.5;
    AppendSingleTriangle(m, {-q, -q, 0}, {q, -q, 0}, {q, q, 0}, {0,0,0});
    m.MakeFlatNormals();
    scene.InsertRefTo(m);
  });
  Intersect(Ray{{0.1, 0., -1.},{0., 0., 1.}});
  EXPECT_NEAR(distance, 1., 1.e-6);
  CheckPosition({0.1, 0., 0.});
  CheckNormal({0., 0., -1.});
}

TEST_F(IntersectorTests, TriangleEdgeCase)
{
  Mesh m{ 0,0 };
  this->Initialize([&](EmbreeAccelerator &scene) {
    float q = 0.5;
    AppendSingleTriangle(m, {-q, -q, 0}, {q, -q, 0}, {q, q, 0}, {0,0,0});
    AppendSingleTriangle(m, {-q, -q, 0}, {-q, q, 0}, {q, q, 0}, {0,0,0});
    m.MakeFlatNormals();
    scene.InsertRefTo(m);
  });
  Intersect(Ray{{0., 0., -1.},{0., 0., 1.}});
  EXPECT_NEAR(distance, 1., 1.e-6);
  CheckPosition({0., 0., 0.});
  CheckNormal({0., 0., -1.});
  Intersect(Ray{{1.e-7, 0., -1.},{0., 0., 1.}}); // 10^{-7} still works. It happens that log2(10^7) ~= 23, the number of significant bits in IEEE float.
  EXPECT_EQ(intersection.hitid.index, int{0});
  Intersect(Ray{{-1.e-7, 0., -1.},{0., 0., 1.}});
  EXPECT_EQ(intersection.hitid.index, int{1});
}

TEST_F(IntersectorTests, SelfIntersection)
{
  // Intersect with primitive far off the origin.
  // Go to intersection point and shoot another ray.
  // Is there self intersection?
  Mesh m{ 0,0 };
  this->Initialize([&](EmbreeAccelerator &scene) {
    float q = 0.5;
    const float dz = 10;
    const float z = 1.e5;
    AppendSingleTriangle(m, { -q, -q, z+dz }, { q, -q, z-dz }, { q, q, z-dz }, { 0,0,0 });
    AppendSingleTriangle(m, { -q, -q, z+dz }, { -q, q, z+dz }, { q, q, z-dz }, { 0,0,0 });
    m.MakeFlatNormals();
    scene.InsertRefTo(m);
  });

  Ray r{{0, 0, 1.e5 + 1.}, {0, 0, -1}};
  Intersect(r, true);
  r.org = intersection.pos;
  Intersect(r, false);
}

TEST(Embree, SphereIntersectionMadness)
{
  // Intersect sphere with ray. Start new ray from intersection point.
  // Is there self-intersection?
  Sampler sampler;
  Double3 sphere_org{0., 0., 2};
  double sphere_rad = 6300;
  Spheres geom;
  geom.Append(sphere_org.cast<float>(), (float)sphere_rad);
  EmbreeAccelerator world;
  world.InsertRefTo(geom);
  world.Build();

  const int N = 100;
  for (int num = 0; num < N; ++num)
  {
    // Sample random position outside the sphere as start point.
    // And position inside the sphere as end point.
    // Intersection is thus guaranteed.
    Double3 org =
      SampleTrafo::ToUniformSphere(sampler.UniformUnitSquare())
      * sphere_rad * 10. + sphere_org;
    Double3 target =
      SampleTrafo::ToUniformHemisphere(sampler.UniformUnitSquare())
      * sphere_rad * 0.99 + sphere_org;
    Double3 dir = target - org;
    Normalize(dir);

    // Shoot ray to the inside. Expect intersection at the
    // front side of the sphere.
    RaySegment rs{{org, dir}, LargeNumber};
    SurfaceInteraction intersect1;
    bool bhit = world.FirstIntersection(rs.ray, 0, rs.length, intersect1);
    ASSERT_TRUE(bhit);
    ASSERT_LE(Length(org - intersect1.pos), Length(org - sphere_org));

    // Put the origin at the intersection and shoot a ray to the outside
    // in a random direction. Expect no further hit.
    auto m  = OrthogonalSystemZAligned(intersect1.geometry_normal);
    auto new_dir = m * SampleTrafo::ToUniformHemisphere(sampler.UniformUnitSquare());
    rs = MakeSegmentAt(intersect1, new_dir);
    bhit = world.FirstIntersection(rs.ray, 0, rs.length, intersect1);
    ASSERT_FALSE(bhit);
    ASSERT_EQ(rs.length, LargeNumber);
  }
}

TEST(Embree, SphereIntersectionMadness2)
{
  // Bounce a particle around in the inside of the sphere.
  // It must not escape.
  Sampler sampler;
  Double3 sphere_org{0., 0., 2};
  double sphere_rad = 6300;
  Spheres geom;
  geom.Append(sphere_org.cast<float>(), (float)sphere_rad);
  EmbreeAccelerator world;
  world.InsertRefTo(geom);
  world.Build();

  RaySegment rs{{sphere_org, {1., 0., 0.}}, LargeNumber};
  HitId hit, last_hit;
  const int N = 100;
  for (int num = 0; num < N; ++num)
  {
    SurfaceInteraction intersect;
    bool bhit = world.FirstIntersection(rs.ray, 0, rs.length, intersect);
    ASSERT_TRUE(bhit);
    double rho = Length(intersect.pos-sphere_org);
    auto m  = OrthogonalSystemZAligned(intersect.normal);
    auto new_dir = m * SampleTrafo::ToUniformHemisphere(sampler.UniformUnitSquare());
    rs = MakeSegmentAt(intersect, new_dir);
    rho = Length(rs.ray.org-sphere_org);
    EXPECT_LE(rho, sphere_rad);
    last_hit = hit;
  }
}

TEST(Embree, SphereIntersectionMadness3)
{
  // Bounce a particle around within a shell between two spheres.
  // It must not escape.
  Sampler sampler;
  Spheres geom;
  double rad1 = 6300.;
  double rad2 = 6350.;
  geom.Append({0.f, 0.f, 0.f}, (float)rad1);
  geom.Append({0.f, 0.f, 0.f}, (float)rad2);
  EmbreeAccelerator world;
  world.InsertRefTo(geom);
  world.Build();

  Double3 start_point = {0.5*(rad1+rad2), 0., 0.};
  double max_dist_from_start = 0.;
  RaySegment rs{{start_point, {1., 0., 0.}}, LargeNumber};
  const int N = 100;
  for (int num = 0; num < N; ++num)
  {
    SurfaceInteraction intersect;
    bool bhit = world.FirstIntersection(rs.ray, 0., rs.length, intersect);
    ASSERT_TRUE(bhit);
    double rho = Length(intersect.pos);
    max_dist_from_start = std::max(max_dist_from_start, Length(intersect.pos-start_point));
    auto eps = rad2*std::numeric_limits<float>::epsilon();
    EXPECT_LE(rho-eps, rad2);
    EXPECT_GE(rho+eps, rad1);
    auto m  = OrthogonalSystemZAligned(intersect.normal);
    auto new_dir = m * SampleTrafo::ToUniformHemisphere(sampler.UniformUnitSquare());
    rs = MakeSegmentAt(intersect, new_dir);
  }
  EXPECT_GE(max_dist_from_start, rad1*1.e-3);
}

TEST(Embree, FindPlaceIfUnique)
{
  ToyVector<int> items{ 1, 3, 5, 7, 9 };
  auto run = [&](int i) {
    return EmbreeAcceleratorDetail::FindPlaceIfUnique(
      items.begin(),
      items.end(),
      i, std::less<int>(), std::equal_to<int>());
  };
  {
    auto[it, found] = run(10);
    EXPECT_FALSE(found);
    EXPECT_EQ(it, items.end());
  }
  {
    auto[it, found] = run(8);
    EXPECT_FALSE(found);
    EXPECT_EQ(it, items.end() - 1);
  }
  {
    auto[it, found] = run(9);
    EXPECT_TRUE(found);
    EXPECT_EQ(it, items.end() - 1);
  }
  {
    auto[it, found] = run(0);
    EXPECT_FALSE(found);
    EXPECT_EQ(it, items.begin());
  }
  {
    auto[it, found] = run(1);
    EXPECT_TRUE(found);
    EXPECT_EQ(it, items.begin());
  }
  {
    auto[it, found] = run(5);
    EXPECT_TRUE(found);
    EXPECT_EQ(it, items.begin() + 2);
  }
  {
    auto[it, found] = run(4);
    EXPECT_FALSE(found);
    EXPECT_EQ(it, items.begin() + 2);
  }
}

TEST(Embree, AllIntersections)
{
  Spheres geom;
  geom.Append({ 0.f, 0.f, 0.f }, 1.f);
  geom.Append({ 0.f, 0.f, 0.f }, 2.f);
  geom.Append({ 0.f, 0.f, 0.f }, 2.f); // Intentional duplicate
  EmbreeAccelerator world;
  world.InsertRefTo(geom);
  world.Build(true);
  Ray ray{ {0, 0, -3}, {0, 0, 1} };
  //ToyVector<double> distances;
  auto intersections = world.IntersectionsInOrder(ray, 0, 3);
  for (auto is : intersections)
  {
    fmt::print("d={}, geo={}, prim={}", is.t, is.geom, is.prim);
  }
}

//TEST(Scene, VolumeIntersection)
//{
//  const char* scenestr = R"""(
//medium medium 1 1
//shader none
//m testing/scenes/unitcube.dae
//)""";
//  Scene scene;
//  scene.ParseNFFString(scenestr);
//  scene.BuildAccelStructure();
//  const Ray r{ {0, 0, -2.}, {0, 0, 1} };
//  auto is = scene.FirstIntersectionWithVolumes(r, 0, LargeNumber);
//  ASSERT_TRUE(is);
//  EXPECT_NEAR(is->t, 1.5, 1.e-6);
//  EXPECT_TRUE(is->entering);
//  is = scene.FirstIntersectionWithVolumes(r, is->t, LargeNumber);
//  ASSERT_TRUE(is);
//  EXPECT_NEAR(is->t, 2.5, 1.e-6);
//  EXPECT_FALSE(is->entering);
//  is = scene.FirstIntersectionWithVolumes(r, is->t, LargeNumber);
//  ASSERT_FALSE(is);
//}

TEST(Display, Open)
{
  ImageDisplay display;
  Image img(128, 128);
  img.SetColor(255, 0, 0);
  img.DrawLine(0, 0, 128, 128, 5);
  img.DrawLine(0, 128, 128, 0, 5);
  display.show(img);
  std::this_thread::sleep_for(std::chrono::milliseconds(200));
  img.SetColor(255,255,255);
  img.DrawLine(64, 0, 64, 128);
  img.DrawLine(0, 64, 128, 64);
  display.show(img);
  std::this_thread::sleep_for(std::chrono::milliseconds(200));
}

class PerspectiveCameraTesting : public testing::Test
{
  Double3 pos{0, 0, 0},
          dir{0, 0, 1},
          up{0, 1, 0};
  double fov = 90;
  Sampler sampler;
  InfinitePlane imageplane;
  std::unique_ptr<PerspectiveCamera> cam;
  Scene scene;
public:
  PerspectiveCameraTesting() :
    imageplane({0, 0, 1}, {0, 0, 1})
  {
  }

  void init(int _xres, int _yres)
  {
    cam = std::make_unique<PerspectiveCamera>(pos, dir, up, fov, _xres, _yres);
  }

  void run(int _pixel_x, int _pixel_y)
  {
  /* Pixel bins:
    |         i=0         |           i=1           |        .....           |         x=xres-1       |
    At distance z=1:
    x=0                   x=i/xres                 x=2/xres           ....   x=(xres-1)/xres          x=1
  */
    Double3 possum{0,0,0}, possqr{0,0,0};
    Box box;
    constexpr int Nsamples = 100;
    for (int i = 0; i < Nsamples; ++i)
    {
      auto s = cam->TakeDirectionSampleFrom(
        cam->PixelToUnit({_pixel_x, _pixel_y}), pos, sampler,
        PathContext{SelectRgbPrimaryWavelengths()});
      Double3 barry;
      double length = 100.;
      bool is_hit = imageplane.Intersect({pos, s.coordinates}, 0., length, barry);
      ASSERT_TRUE(is_hit);
      Double3 endpos = pos + length * s.coordinates;
      possum += endpos;
      possqr += Product(endpos, endpos);
      box.Extend(endpos);
    }
    // int_x=-s-s x^2 / (2s) dx = 1/3 x^3 /2s | x=-s..s = 1/(6s) 2*(s^3) = 1/3 s^2. -> std = sqrt(1/3) s
    // s = 1 -> std = 0.577
    Double3 average = possum / Nsamples;
    Double3 stddev  = (possqr / Nsamples - Product(average, average)).array().sqrt().matrix();
    std::cout << "@Pixel: ix=" << _pixel_x << "," << _pixel_y << std::endl;
    std::cout << "Pixel: " << average << " +/- " << stddev << std::endl;
    Double3 exactpos {((_pixel_x + 0.5) * 2.0 / cam->xres - 1.0),
                      ((_pixel_y + 0.5) * 2.0 / cam->yres - 1.0),
                      1.0};
    std::cout << "Box: " << box.min << " to " << box.max << std::endl;
    Double3 radius { 2.0/cam->xres, 2.0/cam->yres, 0. };
    std::cout << "Exact: " << exactpos << " +/- " << radius << std::endl;
  }
};

TEST_F(PerspectiveCameraTesting, Sampling1)
{
  init(11,11);
  run(0,0);
  run(10,10);
  run(5,5);
}

class TextureLoadTest : public testing::Test
{
protected:
  void CompareTexture(const Texture &tex, int w, int h, const ToyVector<RGB> &expected)
  {
    ASSERT_EQ(tex.Width() , w);
    ASSERT_EQ(tex.Height() , h);
    for (int y=0; y<tex.Height(); ++y)
    {
    for (int x=0; x<tex.Width(); ++x)
    {
      RGB tex_col = tex.GetPixel(x, y);
      RGB expected_col = expected[y*tex.Width()+x];
      EXPECT_TRUE(((tex_col-expected_col).abs() < 1.e-3_rgb).all());
    }
    std::cerr << "\n";
    }
  }
};

TEST_F(TextureLoadTest, UCharSingleChn)
{
  ToyVector<RGB> expected {
    { 1._rgb, 1._rgb, 1._rgb }, { 0._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 0._rgb }, { 1._rgb, 1._rgb, 1._rgb },
    { 0._rgb, 0._rgb, 0._rgb }, { 0._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 0._rgb }, { 0._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 0._rgb }, { 0._rgb, 0._rgb, 0._rgb }
  };
  CompareTexture(Texture("testing/scenes/texloadtest1.png"), 2, 5, expected);
}

TEST_F(TextureLoadTest, Float3Chn)
{
  ToyVector<RGB> expected {
    { 10._rgb, 9._rgb, 8._rgb }  , { 0._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 0._rgb }   , { 1._rgb, 1._rgb, 1._rgb },
    { 0._rgb, 0._rgb, 0._rgb }   , { 0._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 0._rgb }   , { 0._rgb, 0._rgb, 0._rgb },
    { 0._rgb, 0._rgb, 0._rgb }   , { 0._rgb, 0._rgb, 0._rgb }
  };
  CompareTexture(Texture("testing/scenes/texloadtest2.exr"), 2, 5, expected);
}

namespace materials { namespace Atmosphere
{

TEST(SimpleAtmosphereTest, Altitude)
{
  Atmosphere::SphereGeometry geom{{0.,0.,1.}, 100.};
  Double3 pos{0., 110., 1.};
  double altitude = geom.ComputeAltitude(pos);
  EXPECT_NEAR(altitude, 10., 1.e-6);
}

TEST(SimpleAtmosphereTest, CollisionCoefficients)
{
  Atmosphere::ExponentialConstituentDistribution constituents{};
  double altitude = 10.;
  Spectral3 sigma_s, sigma_a;
  Index3 lambda_idx = Color::LambdaIdxClosestToRGBPrimaries();
  constituents.ComputeCollisionCoefficients(altitude, sigma_s, sigma_a, lambda_idx);
  double expected_sigma_s =
      std::exp(-altitude/8.)*0.0076 +
      std::exp(-altitude/1.2)*20.e-3;
  double expected_sigma_a =
      std::exp(-altitude/8.)*0.e-3 +
      std::exp(-altitude/1.2)*2.22e-3;
  EXPECT_NEAR(sigma_s[0], expected_sigma_s, expected_sigma_s*1.e-2);
  EXPECT_NEAR(sigma_a[0], expected_sigma_a, expected_sigma_a*1.e-2);
}

TEST(SimpleAtmosphereTest, LowestPoint)
{
  Atmosphere::SphereGeometry geom{{0.,0.,1.}, 100.};
  { // Looking down through the atmospheric layer.
    RaySegment seg{{{-1000., 0., 101.}, {1., 0., 0.}}, 2000.};
    Double3 p = geom.ComputeLowestPointAlong(seg);
    EXPECT_NEAR(p[0], 0., 1.e-8);
    EXPECT_NEAR(p[1], 0., 1.e-8);
    EXPECT_NEAR(p[2], 101., 1.e-8);
  }

  { // Same as above but limit the distance.
    RaySegment seg{{{-1000., 0., 101.}, {1., 0., 0.}}, 20.};
    Double3 p = geom.ComputeLowestPointAlong(seg);
    EXPECT_NEAR(p[0], -1000+20., 1.e-8);
    EXPECT_NEAR(p[1], 0., 1.e-8);
    EXPECT_NEAR(p[2], 101., 1.e-8);
  }

  { // Look up
    RaySegment seg{{{0., 0., 101.}, Normalized(Double3{1., 0., 10.})}, 5.};
    Double3 p = geom.ComputeLowestPointAlong(seg);
    EXPECT_NEAR(p[0], 0., 1.e-8);
    EXPECT_NEAR(p[1], 0., 1.e-8);
    EXPECT_NEAR(p[2], 101., 1.e-8);
  }
}

#ifdef HAVE_JSON
TEST(AtmosphereTest, LoadTabulatedData)
{
  const std::string filename = "testing/scenes/earth_atmosphere_collision_coefficients.json";
  std::ifstream is(filename.c_str(), std::ios::binary | std::ios::ate);
  std::string data;
  { // Following http://en.cppreference.com/w/cpp/io/basic_istream/read
    auto size = is.tellg();
    data.resize(size);
    is.seekg(0);
    is.read(&data[0], size);
  }
  rapidjson::Document d;
  d.Parse(data.c_str());
  ASSERT_TRUE(d.IsObject());
  const rapidjson::Value& val = d["H0"];
  ASSERT_TRUE(std::isfinite(val.GetDouble()));
  const auto& sigma_t = d["sigma_t"]; // Note: My version of rapidjson is age old. In up to date versions this should read blabla.GetArray().
  ASSERT_GT(sigma_t.Size(), 0ul);
  const auto& sigma_t_at_altitude0 = sigma_t[0];
  ASSERT_EQ(sigma_t_at_altitude0.Size(), Color::NBINS);
  const auto& value = sigma_t_at_altitude0[0];
  ASSERT_TRUE(std::isfinite(value.GetDouble()));
}
#endif

} } // materials::atmosphere

TEST(Misc, Sample)
{
  struct Testing {};
  Sample<double, double, Testing> smpl_pdf{1., 1., 42.};
  Sample<double, double, Testing> smpl_pmf{1., 1., 42.};
  Sample<double, double, Testing> zero_pmf{1., 1., 0.};
  Sample<double, double, Testing> zero_pdf{1., 1., 0.};
  SetPmfFlag(zero_pmf);
  SetPmfFlag(smpl_pmf);
  ASSERT_TRUE(IsFromPdf(smpl_pdf));
  ASSERT_TRUE(IsFromPdf(zero_pdf));
  ASSERT_FALSE(IsFromPdf(smpl_pmf));
  ASSERT_FALSE(IsFromPdf(zero_pmf));
  ASSERT_EQ(PdfValue(smpl_pdf),42.);
  ASSERT_EQ(PmfValue(smpl_pmf),42.);
}

//////////////////////////////////////////////
/// Bisection test
//////////////////////////////////////////////

TEST(Bisection, Search)
{
  std::vector<float> vals{
    1., 3., 9., 10.
  };
  EXPECT_EQ(BisectionSearch<float>(AsSpan(vals), 0.5), 0);
  EXPECT_EQ(BisectionSearch<float>(AsSpan(vals), 1.5), 1);
  EXPECT_EQ(BisectionSearch<float>(AsSpan(vals), 9.5), 3);
  EXPECT_EQ(BisectionSearch<float>(AsSpan(vals), 11), 4);
}

TEST(TowerSampling, ComputeCumSum)
{
  std::vector<float> vals{
    1,3,3,2,1
  };
  TowerSamplingComputeNormalizedCumSum(AsSpan(vals));
  EXPECT_NEAR(vals[0], 1./10., 1.e-4);
  EXPECT_NEAR(vals[1], 4./10., 1.e-3);
  EXPECT_NEAR(vals[2], 7./10., 1.e-3);
  EXPECT_NEAR(vals[3], 9./10., 1.e-3);
  EXPECT_NEAR(vals[4], 1., 1.e-3);
}

//////////////////////////////////////////////
/// UV projection
//////////////////////////////////////////////

TEST(Projections, UvToSpherical)
{
  Float2 uv{0.1f, 0.2f};
  Float2 angles = Projections::UvToSpherical(uv);
  Float2 uv_back = Projections::SphericalToUv(angles);
  EXPECT_NEAR(uv[0], uv_back[0], 1.e-4);
  EXPECT_NEAR(uv[1], uv_back[1], 1.e-4);
}

TEST(Projections, KartesianToSpherical2)
{
  const float phi = Pi*0.4;
  const float theta = Pi*0.2;
  Float2 angles{phi, theta};
  Float3 dir = Projections::SphericalToUnitKartesian(angles);
  Float2 angles_back = Projections::KartesianToSpherical(dir);
  EXPECT_NEAR(phi, angles_back[0], 1.e-4);
  EXPECT_NEAR(theta, angles_back[1], 1.e-4);
}

TEST(Projections, KartesianToSpherical1)
{
  Float3 dir;
  dir = Projections::SphericalToUnitKartesian(Float2{0.,0.});
  EXPECT_NEAR(dir[0], 0., 1.e-4);
  EXPECT_NEAR(dir[1], 0., 1.e-4);
  EXPECT_NEAR(dir[2], 1., 1.e-4);
  dir = Projections::SphericalToUnitKartesian(Float2{0.,Pi*0.5});
  EXPECT_NEAR(dir[0], 1., 1.e-4);
  EXPECT_NEAR(dir[1], 0., 1.e-4);
  EXPECT_NEAR(dir[2], 0., 1.e-4);
}

//////////////////////////////////////////////
/// Grid Indices
//////////////////////////////////////////////

TEST(GridIndices, RowMajor2d)
{
  int x = 3, y = 4, w = 5, h = 9;
  int offset = util::RowMajorOffset(x, y, w, h);
  std::tie(x, y) = util::RowMajorPixel(offset, w, h);
  EXPECT_EQ(x, 3);
  EXPECT_EQ(y, 4);
}

//////////////////////////////////////////////
/// Utilities
//////////////////////////////////////////////

TEST(Utils,StartsEndsWith)
{
    EXPECT_TRUE(util::startswith("foobar","foo"));
    EXPECT_FALSE(util::startswith("foobar","baz"));
    EXPECT_TRUE(util::endswith("foobar","bar"));
    EXPECT_FALSE(util::endswith("foobar","foo"));
}

TEST(Utils,TransformArray)
{
  std::array<int,2> xs = { 1, 5 };
  std::array<std::tuple<int>,2> y = util::TransformArray(xs, [](size_t x) { return std::make_tuple(int(x)*2); });
  EXPECT_EQ(std::get<0>(y[0]), 2);
  EXPECT_EQ(std::get<0>(y[1]), 2*5);
}

TEST(Utils,GenerateArray)
{
  std::array<std::tuple<int>,2> y = util::GenerateArray<2>([](size_t i) { return std::make_tuple(42+int(i)*2); });
  EXPECT_EQ(std::get<0>(y[0]), 42);
  EXPECT_EQ(std::get<0>(y[1]), 44);
}

///////////////////////////////////////////////
/// HashGrid
///////////////////////////////////////////////

TEST(HashGrid,HashGrid)
{
  // Generate points uniformly distributed on a sphere. Put them into the hash grid.
  // Generate query points, check if points in the hashgrid are correctly reported
  Sampler sampler;
  ToyVector<Double3> points;
  for (int i=0; i<10; ++i)
  {
    points.emplace_back(SampleTrafo::ToUniformSphere(sampler.UniformUnitSquare()));
  }
  HashGrid hashgrid(0.1, points);
  ToyVector<bool> reported_in_range(points.size(), false);
  for (int j=0; j<10; ++j)
  {
    Double3 p_query = SampleTrafo::ToUniformSphere(sampler.UniformUnitSquare());
    std::fill(reported_in_range.begin(), reported_in_range.end(), false);
    hashgrid.Query(p_query,[&](int i){
      reported_in_range[i] = (p_query - points[i]).norm() <= hashgrid.Radius();
    });
    for (int i=0; i<points.size(); ++i)
    {
      if (!reported_in_range[i])
      {
        auto distance = (p_query - points[i]).norm();
        EXPECT_GE(distance,hashgrid.Radius());
      }
    }
  }
}

///////////////////////////////////////////////
/// Photonintersector
///////////////////////////////////////////////

inline std::tuple<double, double> PointLineDistance(const Ray &r, const Double3 &p)
{
  Double3 d = p - r.org;
  double distance = Dot(d, r.dir);
  double radial_distance2 = LengthSqr(Cross(d, r.dir));
  return std::make_tuple(distance, radial_distance2);
}

TEST(Photonintersector, Photonintersector)
{
  // Like the hash grid case. But here we use ray intersections.
  const double radius = 0.2;
  Sampler sampler;
  ToyVector<Double3> points;
  for (int i=0; i<10; ++i)
  {
    points.emplace_back(SampleTrafo::ToUniformSphere(sampler.UniformUnitSquare()));
  }
  PhotonIntersector intersector(radius, points);
  ToyVector<bool> reported_in_range(points.size(), false);
  ToyVector<double> reported_distance(points.size());
  for (int j=0; j<100; ++j)
  {
    Ray ray {
      SampleTrafo::ToUniformDisc(sampler.UniformUnitSquare())*1.5,
      {0,0,1}};
    ray.org[2] = -2.;
    const double distance = 2.;

    std::fill(reported_in_range.begin(), reported_in_range.end(), false);
    constexpr int BUFFER_SIZE = 1024;
    int items[BUFFER_SIZE];
    float distances[BUFFER_SIZE];
    int n = intersector.Query(ray, distance, items, distances, BUFFER_SIZE);
    for (int i=0; i<n; ++i)
    {
      reported_in_range[items[i]] = true;
      reported_distance[items[i]] = distances[i];
    }
    for (int i=0; i<points.size(); ++i)
    {
      auto [d, r2] = PointLineDistance(ray, points[i]);
      bool in_range = d>=0. && d<=distance && r2 <= radius*radius;
      EXPECT_EQ(in_range, reported_in_range[i]);
      if (reported_in_range[i])
        EXPECT_NEAR(d, reported_distance[i], 1.e-3);
    }
  }
}

///////////////////////////////////////////////
////   Light Picker
///////////////////////////////////////////////
#if 0
TEST(LightPickerUcb, SelectionProbabilities)
{
  const char* scenestr = R"""(
{
larea arealight2 uniform 1 1 1 1
diffuse black  1 1 1 0.
m testing/scenes/unitrectangle.obj
}

l 0 0.75 0  1 1 1 1

lddome 0 1 0  0.02 0.02 0.5
)""";

  Scene scene;
  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();
  using Lights::MakeLightRef;
  using Lights::LightRef;
  namespace ROI = RadianceOrImportance;

  const int NUM_PICKERS = 4;
  const int NUM_LIGHTS = 4;
  const int NUM_ITERS = 2;
  const int NUM_INNER_ITERS = 4;

  std::array<UcbLightPicker, NUM_PICKERS> pickers{
    scene, scene, scene, scene
  };
  pickers[0].Init();
  for (int i=1; i<NUM_PICKERS; ++i)
    pickers[i].InitSharedWith(pickers[0]);
  pickers[0].UpdateAllShared();

  auto CheckDistributionProbabilities = [&](const UcbLightPicker &picker, Span<const double> desired_probs)
  {
    const double p_env = picker.PmfOfLight(MakeLightRef(scene, scene.GetTotalEnvLight()));
    const double p_area1 = picker.PmfOfLight(MakeLightRef(scene, scene.GetPrimitiveFromAreaLightIndex(0)));
    const double p_area2 = picker.PmfOfLight(MakeLightRef(scene, scene.GetPrimitiveFromAreaLightIndex(1)));
    const double p_point = picker.PmfOfLight(MakeLightRef(scene, scene.GetPointLight(0)));
    const double prob_array[4] = {
      p_env, p_area1, p_area2, p_point
    };
    for (int i = 0; i < NUM_LIGHTS; ++i)
      ASSERT_LE(std::abs(prob_array[i] - desired_probs[i]), 0.1);
  };

  const auto UNIFORM_PROBS =
    Eigen::Array<double, NUM_LIGHTS, 1>::Constant(1. / NUM_LIGHTS).eval();
  const double TARGET_PROBS[NUM_LIGHTS] = {
    1. / 10., 2. / 10., 3. / 10., 4. / 10.
  };

  for (const auto &picker : pickers)
    CheckDistributionProbabilities(picker, AsSpan(UNIFORM_PROBS));

  struct Callback
  {
    UcbLightPicker &picker;
    void operator()(const Lights::Env &, double prob, LightRef ref)
    {
      picker.ObserveLightContribution(ref, Spectral3{ 1. });
    }
    void operator()(const Lights::Point &, double prob, LightRef ref)
    {
      picker.ObserveLightContribution(ref, Spectral3{ 4. });
    }
    void operator()(const Lights::Area &light, double prob, LightRef ref)
    {
      const double val = light.Get().index == 0 ? 2. : 3.;
      picker.ObserveLightContribution(ref, Spectral3{ val });
    }
    void operator()(const Medium &, double prob, LightRef ref)
    {
      picker.ObserveLightContribution(ref, Spectral3::Zero());
    }
  };

  Sampler sampler;
  int picker_idx = 0;
  for (int iter = 0; iter < NUM_ITERS; ++iter)
  {
    for (int inner = 0; inner < NUM_INNER_ITERS; ++inner)
    {
      pickers[picker_idx].PickLight(sampler, Callback{ pickers[picker_idx] });
      picker_idx = (picker_idx + 1) % NUM_PICKERS;
    }
    pickers[0].UpdateAllShared();
  }

  for (const auto &picker : pickers)
    CheckDistributionProbabilities(picker, Span<const double>(TARGET_PROBS, NUM_LIGHTS));
}
#endif

//////////////////////////////////////////////
/// JSON
//////////////////////////////////////////////

#ifdef HAVE_JSON
TEST(JSON, EigenToJson)
{
  using namespace rapidjson_util;
  Double3 x{1,2,3};
  Float3 y{4,5,6};
  Index3 i{1,2,3};

  rj::Document doc;
  auto &alloc = doc.GetAllocator();
  doc.SetObject();
  doc.AddMember("Double3", ToJSON(x, alloc), alloc);
  doc.AddMember("Float3", ToJSON(y, alloc), alloc);
  doc.AddMember("Index3", ToJSON(i, alloc), alloc);

  const std::string s = ToString(doc);
  //std::cout << s << std::endl;

  const std::string expected = \
R"""({
    "Double3": [
        1.0,
        2.0,
        3.0
    ],
    "Float3": [
        4.0,
        5.0,
        6.0
    ],
    "Index3": [
        1,
        2,
        3
    ]
})""";
  EXPECT_EQ(s, expected);
}

TEST(JSON, ContainerToJSON)
{
  using namespace rapidjson_util;
  std::list<ToyVector<int>> c;
  c.push_back({1,2,3});
  c.push_back({4,5,6});

  rj::Document doc;
  auto &a = doc.GetAllocator();
  doc.SetObject();
  doc.AddMember("Container", ToJSON(c, a), a);

  const std::string s = ToString(doc);
  //std::cout << s << std::endl;

  const std::string expected = \
R"""({
    "Container": [
        [
            1,
            2,
            3
        ],
        [
            4,
            5,
            6
        ]
    ]
})""";

  EXPECT_EQ(expected, s);
}
#endif

//////////////////////////////////////////////
// pmr pointer bump allocator
//////////////////////////////////////////////

struct alignas(128) PmrAlignTester
{
  PmrAlignTester(int x_) : x(x_)
  {
    //std::cout << "PmrAlignTester(" << x_ << ")" << std::endl;
  }
  ~PmrAlignTester()
  {
    //std::cout << "~PmrAlignTester(" << x << ")" << std::endl;
  }
  int x;
};

TEST(MemoryArena, Alignment)
{
  util::MemoryArena arena{1024*4096};
  auto t1 = arena.MakeUnique<PmrAlignTester>(7);
  auto t2 = arena.MakeUnique<PmrAlignTester>(42);
  size_t misalign1 = (size_t)t1.get() % 128;
  size_t misalign2 = (size_t)t2.get() % 128;
  ASSERT_EQ(misalign1, 0);
  ASSERT_EQ(misalign2, 0);
}

TEST(MemoryArena, DefaultBufferSize)
{
  constexpr size_t itemsize = sizeof(PmrAlignTester);
  constexpr size_t n = 100000;
  constexpr size_t sz = itemsize * (n+1); // +1 because alignment, to not cause additional allocations other than the first.
  util::MemoryArena arena{sz};

  auto AllocationLoop = [&]() {
    auto last = arena.MakeUnique<PmrAlignTester>(0);
    for (size_t i = 1; i<n; ++i)
    {
      auto current = arena.MakeUnique<PmrAlignTester>(static_cast<int>(i));
      ASSERT_EQ(current.get() - last.get(), 1); // If it's really just bumping the pointer ...
      last = std::move(current);
    }
  };

  AllocationLoop();

  arena.Release();

  AllocationLoop();
}

//////////////////////////////////////////////
// nd array and lookup table
//////////////////////////////////////////////

TEST(NdArray, Lattice)
{
  LatticeIndexing<4, long> lattice;
  lattice = LatticeIndexing<4,long>({ 2l, 3l, 3l, 2l });
  const auto s = lattice.Size();
  ASSERT_EQ(s[0], 2l);
  ASSERT_EQ(s[1], 3l);
  ASSERT_EQ(s[2], 3l);
  ASSERT_EQ(s[3], 2l);

  ASSERT_EQ(lattice.LatticeToSite({1l, 0l, 0l, 0l}), 1l);
  ASSERT_EQ(lattice.LatticeToSite({1l, 2l, 2l, 1l}), lattice.NumSites()-1);

  const auto pt = lattice.SiteToLattice(lattice.LatticeToSite({0l,2l,1l,1l}));
  ASSERT_TRUE(
    (pt == Eigen::Array<long,4,1>({0l,2l,1l,1l})).all()
  );
}

TEST(NdArray, RuleOfFive)
{
  using A = SimpleNdArray<int, 3>;
  A a{{2,2,2}};
  a[{0,0,0}] = 42;
  a[{1,1,1}] = 666;

  Eigen::Array<int,3,1> i0{0,0,0}, i1{1,1,1};

  // Copy C'tor
  A b{a};
  ASSERT_EQ(b[i0], 42);
  ASSERT_EQ(b[i1], 666);

  // Move C'tor
  A c{std::move(a)};
  ASSERT_EQ(c.size(), 2*2*2);
  ASSERT_EQ(c[i0], 42);
  ASSERT_EQ(c[i1], 666);
  ASSERT_EQ(a.size(), 0);

  // Copy Assign
  a = c;
  ASSERT_EQ(a[i0], 42);
  ASSERT_EQ(a[i1], 666);

  // Move Assign
  // First clear c
  std::fill(c.begin(), c.end(), 0);
  ASSERT_EQ(c[i0], 0);
  ASSERT_EQ(c[i1], 0);
  c = std::move(a);
  ASSERT_EQ(c[i0], 42);
  ASSERT_EQ(c[i1], 666);
  ASSERT_EQ(a.size(), 2*2*2); // Still has the memory from c
}

//////////////////////////////////////////////
int main(int argc, char **argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <iostream>
#include <boost/shared_array.hpp>
#include <gazebo/ode/odemath.h>
#include <gazebo/ode/matrix.h>
#include <vector>
#include <limits>
#include "util.h"
#include "optimize.h"

using std::vector;
using boost::shared_array;
using std::cout;
using std::endl;

// misc defines
#define ALLOCA dALLOCA16
#define DEBUG

// LAPACK routine for solving a symmetric indefinite system
extern "C" {
int ssysv_(char* uplo, int* n, int* nrhs, dReal* A, int* lda, int* ipiv, dReal* xb, int* ldb, dReal* work, int* lwork, int* info, short uplo_len);
}

// routine for solving a symmetric indefinite system
void solveSymmetric(dReal* A, dReal* xb, int n)
{
  // form inputs to LAPACK
  char UPLO = 'U';
  int NRHS = 1;
  int LDA = n;
  int LDB = n;
  int LWORK = -1;
  dReal WORK_QUERY;
  int INFO;
  short UPLO_LEN = 1;

  // memory for pivots
  int* IPIV = new int[n];

  // first, determine workspace size
  ssysv_(&UPLO, &n, &NRHS, A, &LDA, IPIV, xb, &LDB, &WORK_QUERY, &LWORK, &INFO, UPLO_LEN);
  dIASSERT(INFO == 0);

  // setup workspace
  LWORK = (int) WORK_QUERY;
  dReal* WORK = new dReal[LWORK];

  // do the solve
  ssysv_(&UPLO, &n, &NRHS, A, &LDA, IPIV, xb, &LDB, WORK, &LWORK, &INFO, UPLO_LEN);
  dIASSERT(INFO == 0);

  delete [] IPIV;
  delete [] WORK;
}

// non-padded Cholesky factorization
int dNPFactorCholesky (dReal *A, int n)
{
// ODE's version of cholesky factorization seems to have a bug detecting when
// matrix is singular when n=2, substituting a check in here...
  if (n == 2)
  {
    if (dFabs(A[0]*A[3] - A[1]*A[2]) < dSqrt(std::numeric_limits<dReal>::epsilon()))
      return false;
  }

  int i,j,k,nskip;
  dReal sum,*a,*b,*aa,*bb,*cc,*recip;
  dAASSERT (n > 0 && A);
  nskip = n;
  recip = (dReal*) ALLOCA (n * sizeof(dReal));
  aa = A;
  for (i=0; i<n; i++) {
    bb = A;
    cc = A + i*nskip;
    for (j=0; j<i; j++) {
      sum = *cc;
      a = aa;
      b = bb;
      for (k=j; k; k--) sum -= (*(a++))*(*(b++));
      *cc = sum * recip[j];
      bb += nskip;
      cc++;
    }
    sum = *cc;
    a = aa;
    for (k=i; k; k--, a++) sum -= (*a)*(*a);
    if (sum <= REAL(0.0)) return 0;
    *cc = dSqrt(sum);
    recip[i] = dRecip (*cc);
    aa += nskip;
  }
  return 1;
}

// non-padded Cholesky solve
static void dNPSolveCholesky (const dReal *L, dReal *b, int n)
{
  int i,k,nskip;
  dReal sum,*y;
  dAASSERT (n > 0 && L && b);
  nskip = n;
  y = (dReal*) ALLOCA (n*sizeof(dReal));
  for (i=0; i<n; i++) {
    sum = 0;
    for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k];
    y[i] = (b[i]-sum)/L[i*nskip+i];
  }
  for (i=n-1; i >= 0; i--) {
    sum = 0;
    for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k];
    b[i] = (y[i]-sum)/L[i*nskip+i];
  }
}

// non-padded matrix multiplication
void dNPMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
{
  int i,j,k,qskip,rskip,rpad;
  if (p == 0 || q == 0 || r == 0)
    return;
  dAASSERT (A && B && C && p>0 && q>0 && r>0);
  qskip = q;
  rskip = r;
  rpad = rskip - r;
  dReal sum;
  const dReal *b,*c,*bb;
  bb = B;
  for (i=p; i; i--) {
    for (j=0 ; j<r; j++) {
      c = C + j;
      b = bb;
      sum = 0;
      for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c);
      *(A++) = sum;
    }
    A += rpad;
    bb += qskip;
  }
}

// non-padded matrix multiplication
void dNPMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
{
  int i,j,k,pskip,rskip;
  dReal sum;
  if (p == 0 || q == 0 || r == 0)
    return;
  dAASSERT (A && B && C && p>0 && q>0 && r>0);
  pskip = p;
  rskip = r;
  for (i=0; i<p; i++) {
    for (j=0; j<r; j++) {
      sum = 0;
      for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip];
      A[i*rskip+j] = sum;
    }
  }
}

void dNPMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
{
  int i,j,k,z,rpad,qskip;
  dReal sum;
  const dReal *bb,*cc;
  if (p == 0 || q == 0 || r == 0)
    return;
  dAASSERT (A && B && C && p>0 && q>0 && r>0);
  rpad = 0;
  qskip = q;
  bb = B;
  for (i=p; i; i--) {
    cc = C;
    for (j=r; j; j--) {
      z = 0;
      sum = 0;
      for (k=q; k; k--,z++) sum += bb[z] * cc[z];
      *(A++) = sum;
      cc += qskip;
    }
    A += rpad;
    bb += qskip;
  }
}

// prints a matrix
void printMatrix(dReal* A, int m, int n)
{
  for (int i=0; i< m*n; i++)
  {
    if (i % n == 0 && i > 0)
      std::cout << std::endl;
    std::cout << A[i] << " ";
  }
  std::cout << std::endl;
}

// copies real arrays
void dCopy(dReal* target, int tstride, dReal* source, int sstride, int n)
{
  for (int i=0; i< n; i++, target += tstride, source += sstride)
    *target = *source;
}

// sets a submatrix of a matrix
// A: the matrix into which B will be copied
// ac: the number of columns of A
// B: the source matrix
// br: the number of rows of B
// bc: the number of columns of b
// sr: the starting row of A to which to copy
// sc: the starting column of A to which to copy
void dSetSubMat0(dReal* A, int ac, dReal* B, int br, int bc, int sr, int sc)
{
  int siA = sr*ac + sc;          // starting index for A
  const int Arowskip = ac;
  const int Browskip = bc;
  int siB = 0;                   // starting index for B

  for (int i=0; i< br; i++, siA += Arowskip, siB += Browskip)
    memcpy(A+siA, B+siB, bc*sizeof(dReal));
}

// sets a submatrix of a matrix to a transposed matrix
void dSetSubMat1(dReal* A, int ac, dReal* B, int br, int bc, int sr, int sc)
{
  int siA = sr*ac + sc;
  const int Arowskip = ac;
  int siB = 0;

  for (int i=0; i< bc; i++, siA += Arowskip, siB++)
    dCopy(A+siA, 1, B+siB, bc, br);
}

// Data structure for solving the LCP w = Mx + q for x
struct LCPData
{
  dReal const* M;  // the n x n LCP matrix
  dReal const* q;  // the n-dimensional LCP vector
  dReal* work;     // an n-dimensional work vector

  LCPData(int n) { work = new dReal[n]; }
  ~LCPData() { delete [] work; }
};

// data for computing feasible starting point for convex optimization
struct FeasibilityData
{
  int m;          // number of constraints in the original problem
  int n;          // size of x in the original problem
  void* data;     // data in the original problem
  dReal* workn;   // work array of size n
  dReal* workm;   // work array of size m+1
  dReal* worknxn; // work array of size n x n

  // functions in the original problem
  void (*fx)(dReal*, int, dReal*, int, void*);
  void (*grad)(dReal*, int, int, dReal*, void*);
  bool (*hess)(dReal*, int, int, dReal*, void*);

  FeasibilityData(int _m, int _n)
  {
    this->m = _m;
    this->n = _n;
    workn = new dReal[_n];
    workm = new dReal[_m+1];
    worknxn = new dReal[_n*_n];
  }

  ~FeasibilityData() { delete [] workm; delete [] workn; delete [] worknxn; }
};

// checks to see whether we can terminate early
static bool make_feasible_tcheck(dReal* y, int /*n*/, void* data)
{
  // get the feasibility data
  FeasibilityData& fdata = *((FeasibilityData*) data);
  //dIASSERT(n == fdata.n+1);
  const int OLDN = fdata.n;
  const int OLDM = fdata.m;

  // remove v from y to yield x
  memcpy(fdata.workn, y, OLDN*sizeof(dReal));

  // call the original objective / constraint function
  (*fdata.fx)(fdata.workn, OLDN, fdata.workm, OLDM, fdata.data);

  // determine the maximum constraint value
  dReal v = *std::max_element(fdata.workm+1, fdata.workm+1+OLDM);
  return (v <= (dReal) 0.0);
}

// evaluation function for making an initial point feasible
static void make_feasible_fx(dReal* y, int /*n*/, dReal* f, int /*m*/, void* data)
{
  // get the feasibility data
  FeasibilityData& fdata = *((FeasibilityData*) data);
  //dIASSERT(n == fdata.n+1);
  const int OLDN = fdata.n;
  const int OLDM = fdata.m;

  // remove v from y to yield x
  memcpy(fdata.workn, y, OLDN*sizeof(dReal));

  // call the original objective / constraint function
  (*fdata.fx)(fdata.workn, OLDN, fdata.workm, OLDM, fdata.data);

  // get v
  dReal v = y[OLDN];
  f[0] = v;

  // constraints of f are fdata.workm[i] <= v
  for (int i=0; i< OLDM; i++)
    f[i+1] = fdata.workm[i+1] - v;
}

// gradient function for making an initial point feasible
static void make_feasible_grad(dReal* y, int n, int idx, dReal* g, void* data)
{
  // get the feasibility data
  FeasibilityData& fdata = *((FeasibilityData*) data);
  dIASSERT(n == fdata.n+1);
  const int OLDN = fdata.n;

  // new objective function is simple
  if (idx == 0)
  {
    dSetZero(g, n);
    g[OLDN] = (dReal) 1.0;
    return;
  }

  // still here?  need to call original gradient function

  // remove v from y to yield x
  memcpy(fdata.workn, y, OLDN*sizeof(dReal));

  // call the original gradient function
  (*fdata.grad)(fdata.workn, OLDN, idx, g, fdata.data);
  g[OLDN] = (dReal) -1.0;
}

// Hessian function for making an initial point feasible
static bool make_feasible_hess(dReal* y, int n, int idx, dReal* H, void* data)
{
  // get the feasibility data
  FeasibilityData& fdata = *((FeasibilityData*) data);
  dIASSERT(n == fdata.n+1);
  const int OLDN = fdata.n;

  // new objective function is simple
  if (idx == 0)
  {
    dSetZero(H, n*n);
    return false;
  }

  // still here?  need to call original Hessian function

  // remove v from y to yield x
  memcpy(fdata.workn, y, OLDN*sizeof(dReal));

  // call the original Hessian function
  if (!(*fdata.hess)(fdata.workn, OLDN, idx, fdata.worknxn, fdata.data))
    return false;

  // set the submatrix
  dSetSubMat0(H, n, fdata.worknxn, OLDN, OLDN, 0, 0);

  // set the last column and row of H to zeros
  dSetZero(H+n*OLDN, n);
  int Hidx = OLDN;
  for (int i=0; i< OLDN; i++, Hidx+= n)
    H[Hidx] = (dReal) 0.0;

  return true;
}

// Hessian function for solving convex LCPs
static bool lcp_ip_hess(dReal* /*x*/, int n, int idx, dReal* H, void* data)
{
  // get the LCP data
  const LCPData& lcpdata = *((LCPData*) data);

  // get M and q
  const dReal* M = lcpdata.M;
  //const dReal* q = lcpdata.q;

  // copy precomputed Hessian
  if (idx == 0)
  {
    memcpy(H, M, n*n*sizeof(dReal));
    return true;
  }
  else
  {
    dSetZero(H, n*n);
    return false;
  }
}

// gradient function for solving convex LCPs
static void lcp_ip_grad(dReal* x, int n, int idx, dReal* g, void* data)
{
  // get the LCP data
  const LCPData& lcpdata = *((LCPData*) data);

  // get M and q
  const dReal* M = lcpdata.M;
  const dReal* q = lcpdata.q;

  // objective function gradient: M*x + 1/2q
  if (idx == 0)
  {
    dNPMultiply0(g, M, x, n, n, 1);
    for (int i=0; i< n; i++)
      g[i] += q[i]*0.5;
  }
  // constraint x >= 0
  else if (idx <= n)
  {
    dSetZero(g, n);
    g[idx-1] = (dReal) -1.0;
  }
  // constraint w = Mx + q >= 0
  else
  {
    // copy and negate row idx - n - 1 of M
    memcpy(g, M+n*(idx-n-1), n*sizeof(dReal));
    for (int i=0; i< n; i++)
      g[i] = -g[i];
  }
}

// objective / constraint evaluation function for solving convex LCPs
static void lcp_ip_fx(dReal* x, int n, dReal* f, int /*m*/, void* data)
{
  const dReal S_BUFFER = std::numeric_limits<dReal>::epsilon();

  // get the LCP data
  const LCPData& lcpdata = *((LCPData*) data);

  // get M and q
  const dReal* M = lcpdata.M;
  const dReal* q = lcpdata.q;

  // get the work variable
  dReal* work = lcpdata.work;

  // evaluate the objective function
  dNPMultiply0(work, M, x, n, n, 1);
  for (int i=0; i< n; i++)
    work[i] += q[i];
  f[0] = dDot(x, work, n) * (dReal) 0.5;

  // evaluate the x >= 0 constraint functions
  int fidx = 1;
  for (int i=0; i< n; i++)
    if (!_dequal(x[i], (dReal) 0.0))
      f[fidx++] = -x[i];
    else
      f[fidx++] = -S_BUFFER;

  // evaluate the w >= 0 constraint functions
  for (int i=0; i< n; i++)
    if (!_dequal(work[i], (dReal) 0.0))
      f[fidx++] = -work[i];
    else
      f[fidx++] = -S_BUFFER;
}

// checks whether the inequality constraints are feasible
static bool feasible(dReal* f, int m)
{
  const dReal S_BUFFER = dSqrt(std::numeric_limits<dReal>::epsilon());

  for (int i=0; i< m; i++)
    if (!(f[i] < -S_BUFFER))
      return false;

  return true;
}

// makes a point feasible for solving a convex optimization problem
bool dMakeFeasibleConvex(CvxOptParams& cparams, dReal eps_feas, dReal* x, int n)
{
  // setup the feasibility data
  FeasibilityData fdata(cparams.m, n);
  fdata.fx = cparams.fx;
  fdata.grad = cparams.grad;
  fdata.hess = cparams.hess;
  fdata.data = cparams.data;

  // evaluate f at current x
  shared_array<dReal> f(new dReal[cparams.m+1]);
  (*cparams.fx)(x, n, f.get(), cparams.m, cparams.data);

  // verify that x is not already feasible (setup v simultaneously)
  dReal v = *std::max_element(f.get()+1, f.get()+cparams.m+1);
  if (v < (dReal) 0.0)
    return true;

  // setup y
  shared_array<dReal> y(new dReal[n+1]);
  memcpy(y.get(), x, n*sizeof(dReal));
  y[n] = v + (dReal) 1.0;

  // make new matrix A to account for increased size of y
  shared_array<dReal> Anew(new dReal[(n+1)*cparams.nu_len]);
  dSetSubMat0(Anew.get(), n+1, cparams.A, cparams.nu_len, n, 0, 0);
  for (int i=0; i< cparams.nu_len; i++)
    Anew[i*(n+1)+n] = (dReal) 0.0;

  // setup convex optimization parameters
  CvxOptParams cp;
  cp.m = cparams.m;
  cp.nu_len = cparams.nu_len;
  cp.eps = cparams.eps;
  cp.tcheck = &make_feasible_tcheck;
  cp.fx = &make_feasible_fx;
  cp.grad = &make_feasible_grad;
  cp.hess = &make_feasible_hess;
  cp.data = &fdata;
  cp.A = Anew.get();
  cp.b = cparams.b;
  cp.max_iterations = std::max(1000 + (int) log(n), cparams.max_iterations);

  // do convex optimization with primal-dual method
  dOptimizeConvexPrimalDual(cp, eps_feas, y.get(), n+1);

  // get x out
  memcpy(x, y.get(), n*sizeof(dReal));

  // determine s
  (*cparams.fx)(x, n, f.get(), cparams.m, cparams.data);
  v = *std::max_element(f.get()+1, f.get()+cparams.m+1);
  return v < eps_feas;
}

// interior point method for solving convex LCPs
bool dSolveLCPIP(dReal* M, dReal* q, dReal* x, int n, dReal eps_feas)
{
  // create LCP data
  LCPData lcpd(n);
  lcpd.M = M;
  lcpd.q = q;

  // setup convex optimization parameters
  CvxOptParams cparams;
  cparams.m = n*2;
  cparams.fx = &lcp_ip_fx;
  cparams.grad = &lcp_ip_grad;
  cparams.hess = &lcp_ip_hess;
  cparams.eps = eps_feas;
  cparams.data = &lcpd;
  cparams.max_iterations = std::numeric_limits<int>::max();

  // first, make feasible
  if (!dMakeFeasibleConvex(cparams, eps_feas, x, n))
    return false;

  // now optimize
  if (!dOptimizeConvexPrimalDual(cparams, eps_feas, x, n))
    return false;

  // check that x >= 0
  for (int i=0; i< n; i++)
    if (x[i] < -eps_feas)
      return false;

  // check that w >= 0
  dNPMultiply0(lcpd.work, M, x, n, n, 1);
  for (int i=0; i< n; i++)
    if (lcpd.work[i] < -eps_feas)
      return false;

  // check complementarity condition
  return dDot(lcpd.work, x, n) < eps_feas;
}

static void setupM(dReal* M, dReal* H, int n, dReal* A, int nu_len)
{
  // first zero the matrix
  dSetZero(M, (n+nu_len)*(n+nu_len));

  // copy H to M, row by row
  dSetSubMat0(M, n+nu_len, H, n, n, 0, 0);
  dSetSubMat0(M, n+nu_len, A, nu_len, n, n, 0);
  dSetSubMat1(M, n+nu_len, A, nu_len, n, 0, n);
}

// conditions (makes positive-definite) the Hessian matrix
static void conditionHessian(dReal* M, dReal* tmp, int n)
{
  // Hessian _must_ be positive-definite for the optimization to work
  const dReal BETA = 1e-3;
  dReal tau = M[0];
  for (unsigned i=1; i< (unsigned)n; i++)
    tau = std::min(tau, M[i*n+i]);
  tau = (tau > (dReal) 0.0) ? BETA : -tau + BETA;

  // copy M to tmp
  memcpy(tmp, M, n*n*sizeof(dReal));

  if (!dNPFactorCholesky(tmp, n))
    while (true)
    {
      // copy M to tmp
      memcpy(tmp, M, n*n*sizeof(dReal));

      // update tmp += I*tau
      for (int i=0; i< n; i++)
        tmp[i*n+i] += tau;

      // try Cholesky factorization
      if (dNPFactorCholesky(tmp, n))
        break;

      // update tau
      tau = std::max(tau*(dReal) 2.0, BETA);
    }

  // augment M
  for (int i=0; i< n; i++)
    M[i*n+i] += tau;
}

bool dOptimizeConvexPrimalDual(CvxOptParams& cparams, dReal eps_feas, dReal* x, int n)
{
  const int m = cparams.m;
  const int nu_len = cparams.nu_len;
  dReal rdual_nrm, rpri_nrm, f0_best;
  const dReal NEAR_ZERO = dSqrt(std::numeric_limits<dReal>::epsilon());

  // see whether we can quit immediately
  if (cparams.tcheck && (*cparams.tcheck)(x, n, cparams.data))
    return true;

  // setup storage for best found
  shared_array<dReal> x_best(new dReal[n]);

  // setup vector of ones
  shared_array<dReal> ones_m(new dReal[m]);
  dSetValue(ones_m.get(), m, (dReal) 1.0);

  // setup mu*m
  const dReal mum = cparams.mu*m;

  // setup nu
  shared_array<dReal> nu(new dReal[nu_len]);
  dSetZero(nu.get(),nu_len);

  // determine f(x)
  shared_array<dReal> f(new dReal[m+1]);
  (*cparams.fx)(x, n, f.get(), m, cparams.data);
  f0_best = f[0];
  memcpy(x_best.get(), x, sizeof(dReal)*n);

  // verify that f is non-positive for constraint functions
  shared_array<dReal> fc(new dReal[m]);
  memcpy(fc.get(), f.get()+1, sizeof(dReal)*m);
  //dReal maxfc = *std::max_element(fc.get(), fc.get()+m);
  dUASSERT(maxfc <= 0.0, "initial point infeasible");

  // setup lambda: must be greater than zero
  shared_array<dReal> lambda(new dReal[m]);
  for (int i=0; i< m; i++)
    lambda[i] = (fc[i] < (dReal) 0.0) ? (dReal) -1.0/fc[i] : dRecip(NEAR_ZERO);

  // setup eta
  dReal eta = -dDot(fc.get(), lambda.get(), m);

  // init t
  dReal t = mum/eta;

  // init y and r
  shared_array<dReal> y(new dReal[n+m+nu_len]);
  shared_array<dReal> r(new dReal[n+m+nu_len]);

  // init vectors and matrices so we don't have to keep reallocating memory
  shared_array<dReal> x_plus(new dReal[n]);
  shared_array<dReal> dx(new dReal[n]);
  shared_array<dReal> lambda_plus(new dReal[m]);
  shared_array<dReal> dlambda(new dReal[m]);
  shared_array<dReal> nu_plus(new dReal[nu_len]);
  shared_array<dReal> dnu(new dReal[nu_len]);
  shared_array<dReal> Df(new dReal[m*n]);
  shared_array<dReal> inv_t(new dReal[m]);
  shared_array<dReal> l_fc(new dReal[m]);
  shared_array<dReal> tmp1(new dReal[n]);
  vector<shared_array<dReal> > g(m+1);
  for (int i=0; i< m+1; i++)
    g[i] = shared_array<dReal>(new dReal[n]);
  shared_array<dReal> H(new dReal[n*n]);
  shared_array<dReal> tmp2(new dReal[(n+nu_len)*(n+nu_len)]);
  shared_array<dReal> M(new dReal[(n+nu_len)*(n+nu_len)]);
  shared_array<dReal> rdual(new dReal[n]);
  shared_array<dReal> rcent(new dReal[m]);
  shared_array<dReal> rpri(new dReal[nu_len]);
  shared_array<dReal> rhs(new dReal[n+nu_len]);
  shared_array<dReal> tmp3(new dReal[m*m]);
  shared_array<dReal> tmp4(new dReal[m]);

  for (cparams.iterations = 0; cparams.iterations < cparams.max_iterations; cparams.iterations++)
  {
    // zero temporaries
    dSetZero(tmp1.get(), n);
    dSetZero(tmp2.get(), n*n);
    dSetZero(tmp3.get(), m*m);
    dSetZero(tmp4.get(), m);

    // reset t
    t = mum/eta;

    #ifdef DEBUG
    cout << "-------------------------------------------------------" << endl;
    cout << "iteration: " << cparams.iterations << endl;
    cout << " t: "  << t << endl;
    cout << " x: ";
    printMatrix(x, 1, n);
    cout << " lambda: ";
    printMatrix(lambda.get(), 1, m);
    #endif

    // compute the residual
    for (int i=0; i< m+1; i++) (*cparams.grad)(x, n, i, g[i].get(), cparams.data);
    (*cparams.fx)(x, n, f.get(), m, cparams.data);
    memcpy(fc.get(), f.get()+1, sizeof(dReal)*m);
    for (int i=0, k=0; i< m; i++)
      for (int j=0; j< n; j++)
        Df[k++] = g[i+1][j];
    dSetZero(r.get(), n+m+nu_len);
    dNPMultiply1(r.get(), Df.get(), lambda.get(), n, m, 1);
    dNPMultiply1(tmp1.get(), cparams.A, nu.get(), n, nu_len, 1);
    for (int i=0; i< n; i++) r[i] += tmp1[i] + g[0][i];
    dSetValue(inv_t.get(), m, (dReal) 1.0);
    for (int i=0; i< m; i++) inv_t[i] /= t;
    memcpy(l_fc.get(), fc.get(), m*sizeof(dReal));
    for (int i=0; i< m; i++)  l_fc[i] *= -lambda[i];
    memcpy(r.get()+n, l_fc.get(), m*sizeof(dReal));
    for (int i=0; i< m; i++)  r[n+i] -= inv_t[i];
    dNPMultiply0(r.get()+m+n, cparams.A, x, nu_len, n, 1);
    for (int i=0; i< nu_len; i++)  r[n+m+i] -= cparams.b[i];

    #ifdef DEBUG
    cout << "Df: ";
    printMatrix(Df.get(), m, n);
    cout << " f0: " << f[0] << endl;
    cout << " f: ";
    printMatrix(fc.get(), 1, m);
    cout << " rdual: ";
    printMatrix(r.get(), 1, n);
    cout << " rcent: ";
    printMatrix(r.get()+n, 1, m);
    #endif

    // solve KKT equations for dx, dlambda, dnu
    if (!(*cparams.hess)(x, n, 0, H.get(), cparams.data))
      dSetZero(H.get(), n*n);
    for (int i=0; i< m; i++)  { dIASSERT(fc[i] < 0.0); fc[i] = dRecip(fc[i]); }
    for (int i=0; i< m; i++)
    {
      if ((*cparams.hess)(x, n, i+1, tmp2.get(), cparams.data))
        for (int j=0; j< n*n; j++)  H[j] += tmp2[j] * lambda[i];
      dNPMultiply2(tmp2.get(), g[i+1].get(), g[i+1].get(), n, 1, n);
      for (int j=0; j< n*n; j++)  tmp2[j] *= lambda[i]*fc[i];
      for (int j=0; j< n*n; j++)  H[j] -= tmp2[j];
    }
    #ifdef DEBUG
    cout << "Hessian matrix [unconditioned]: " << std::endl;
    printMatrix(H.get(), n, n);
    #endif
    conditionHessian(H.get(), tmp2.get(), n);
    setupM(M.get(), H.get(), n, cparams.A, nu_len);
    memcpy(rdual.get(), r.get(), n*sizeof(dReal));
    memcpy(rcent.get(), r.get()+n, m*sizeof(dReal));
    memcpy(rpri.get(), r.get()+n+m, nu_len*sizeof(dReal));
    dSetZero(tmp3.get(), m*m);
    for (int i=0; i< m; i++) tmp3[m*i+i] = fc[i];
    dNPMultiply0(tmp4.get(), tmp3.get(), rcent.get(), m, m, 1);
    dNPMultiply1(tmp1.get(), Df.get(), tmp4.get(), n, m, 1);
    for (int i=0; i< n; i++) rhs[i] = -(rdual[i] + tmp1[i]);
    for (int i=0; i< nu_len; i++) rhs[i+n] = -rpri[i];
    #ifdef DEBUG
    cout << "M (KKT matrix) [conditioned]: " << std::endl;
    printMatrix(M.get(), n+nu_len, n+nu_len);
    cout << "rhs: ";
    printMatrix(rhs.get(), 1, n+nu_len);
    #endif
    if (nu_len > 0)
    {
      solveSymmetric(M.get(), rhs.get(), n+nu_len);
    }
    else
    {
      dNPFactorCholesky(M.get(), n);
      dNPSolveCholesky(M.get(), rhs.get(), n);
    }
for (int i=0; i< n+nu_len; i++)
dIASSERT(!isnan(rhs[i]));
    memcpy(dx.get(), rhs.get(), n*sizeof(dReal));
    memcpy(dnu.get(), rhs.get()+n, nu_len*sizeof(dReal));
    dSetZero(tmp3.get(), m*m);
    for (int i=0; i< m; i++) tmp3[i*m+i] = fc[i]*lambda[i];
    dNPMultiply0(tmp4.get(), Df.get(), dx.get(), m, n, 1);
    dNPMultiply0(dlambda.get(), tmp3.get(), tmp4.get(), m, m, 1);
    for (int i=0; i< m; i++)
      dlambda[i] = -dlambda[i] + fc[i]*rcent[i];
    #ifdef DEBUG
    cout << " dx: ";
    printMatrix(dx.get(), 1, n);
    cout << " dlambda: ";
    printMatrix(dlambda.get(), 1, m);
    #endif

    // compute primal-dual search direction
    const dReal dir_norm = dSqrt(dDot(rhs.get(), rhs.get(), n+nu_len));

    // prepare to do backtracking line search
    dReal smax = (dReal) 1.0;
    for (int i=0; i< m; i++)
      if (dlambda[i] < 0)
      {
        smax = std::min(smax, -lambda[i]/dlambda[i]);
        if (smax < std::numeric_limits<dReal>::epsilon())
          lambda[i] += NEAR_ZERO;
      }
    dReal s = (dReal) 0.99 * smax;
    #ifdef DEBUG
    cout << " maximum s: " << s << endl;
    #endif
    for (int i=0; i< n; i++) x_plus[i] = x[i] + dx[i]*s;
    for (int i=0; i< m; i++) lambda_plus[i] = lambda[i] + dlambda[i]*s;
    for (int i=0; i< nu_len; i++) nu_plus[i] = nu[i] + dnu[i]*s;

    // satisfy inequality constraints
    (*cparams.fx)(x_plus.get(), n, f.get(), m, cparams.data);
    memcpy(fc.get(), f.get()+1, sizeof(dReal)*m);
    #ifdef DEBUG
    cout << " preparing to attempt to satisfy inequality constraints..." << endl;
    #endif
    while (!feasible(fc.get(), m))
    {
      #ifdef DEBUG
      cout << "    s: " << s << "  fc: ";
      printMatrix(fc.get(), 1, m);
      #endif
      s *= cparams.beta;
      if (s*dir_norm < std::numeric_limits<dReal>::epsilon())
      {
        #ifdef DEBUG
        cout << "*** s too small: exiting ***" << endl;
        #endif
        memcpy(x, x_best.get(), sizeof(dReal)*n);
        return false;
      }

      // update f
      for (int i=0; i< n; i++) x_plus[i] = x[i] + dx[i]*s;
      (*cparams.fx)(x_plus.get(), n, f.get(), m, cparams.data);
      memcpy(fc.get(), f.get()+1, sizeof(dReal)*m);
    }

    // update lambda and nu
    for (int i=0; i< m; i++) lambda_plus[i] = lambda[i] + dlambda[i]*s;
    for (int i=0; i< nu_len; i++) nu_plus[i] = nu[i] + dnu[i]*s;

    // re-evaluate the objective function
    (*cparams.fx)(x_plus.get(), n, f.get(), m, cparams.data);
    if (f[0] < f0_best)
    {
      f0_best = f[0];
      memcpy(x_best.get(), x, sizeof(dReal)*n);
    }

    // determine norm of r target
    const dReal rnorm = dSqrt(dDot(r.get(), r.get(), n+m+nu_len));
    #ifdef DEBUG
    cout << " current (old) residual: ";
    printMatrix(r.get(), 1, n+m+nu_len);
    #endif

    // compute new residual
    dSetZero(r.get(), n+m+nu_len);
    dSetZero(tmp1.get(), n);
    for (int i=0; i< m+1; i++) (*cparams.grad)(x_plus.get(), n, i, g[i].get(), cparams.data);
    (*cparams.fx)(x_plus.get(), n, f.get(), m, cparams.data);
    memcpy(fc.get(), f.get()+1, sizeof(dReal)*m);
    for (int i=0, k=0; i< m; i++)
      for (int j=0; j< n; j++)
        Df[k++] = g[i+1][j];
    dNPMultiply1(r.get(), Df.get(), lambda_plus.get(), n, m, 1);
    dNPMultiply1(tmp1.get(), cparams.A, nu_plus.get(), n, nu_len, 1);
    for (int i=0; i< n; i++) r[i] += tmp1[i] + g[0][i];
    dSetValue(inv_t.get(), m, (dReal) 1.0);
    for (int i=0; i< m; i++) inv_t[i] /= t;
    memcpy(l_fc.get(), fc.get(), m*sizeof(dReal));
    for (int i=0; i< m; i++)  l_fc[i] *= -lambda_plus[i];
    memcpy(r.get()+n, l_fc.get(), m*sizeof(dReal));
    for (int i=0; i< m; i++)  r[n+i] -= inv_t[i];
    dNPMultiply0(r.get()+m+n, cparams.A, x_plus.get(), nu_len, n, 1);
    for (int i=0; i< nu_len; i++)  r[n+m+i] -= cparams.b[i];

    // do backtracking line search
    #ifdef DEBUG
    cout << " starting BLS..." << endl;
    #endif
    while (dSqrt(dDot(r.get(), r.get(), n+m+nu_len)) > (1-cparams.alpha*s)*rnorm)
    {
      #ifdef DEBUG
      cout << "  s: " << s << endl;
      #endif
      s *= cparams.beta;

      // verify that s is not too small
      if (s*dir_norm < std::numeric_limits<dReal>::epsilon())
      {
        memcpy(x, x_best.get(), sizeof(dReal)*n);
        #ifdef DEBUG
        cout << "  *** s is too small: exiting ***" << endl;
        #endif
        return false;
      }

      // update x+, lambda+, nu+
      for (int i=0; i< n; i++) x_plus[i] = x[i] + dx[i]*s;
      for (int i=0; i< m; i++) lambda_plus[i] = lambda[i] + dlambda[i]*s;
      for (int i=0; i< nu_len; i++) nu_plus[i] = nu[i] + dnu[i]*s;

      // re-valuate the objective function
      (*cparams.fx)(x_plus.get(), n, f.get(), m, cparams.data);
      if (f[0] < f0_best)
      {
        f0_best = f[0];
        memcpy(x_best.get(), x, sizeof(dReal)*n);
      }

      // recalculate the residual
      dSetZero(r.get(), n+m+nu_len);
      dSetZero(tmp1.get(), n);
      for (int i=0; i< m+1; i++) (*cparams.grad)(x_plus.get(), n, i, g[i].get(), cparams.data);
      (*cparams.fx)(x_plus.get(), n, f.get(), m, cparams.data);
      memcpy(fc.get(), f.get()+1, sizeof(dReal)*m);
      for (int i=0, k=0; i< m; i++)
        for (int j=0; j< n; j++)
          Df[k++] = g[i+1][j];
      dNPMultiply1(r.get(), Df.get(), lambda_plus.get(), n, m, 1);
      dNPMultiply1(tmp1.get(), cparams.A, nu_plus.get(), n, nu_len, 1);
      for (int i=0; i< n; i++) r[i] += tmp1[i] + g[0][i];
      dSetValue(inv_t.get(), m, (dReal) 1.0);
      for (int i=0; i< m; i++) inv_t[i] /= t;
      memcpy(l_fc.get(), fc.get(), m*sizeof(dReal));
      for (int i=0; i< m; i++)  l_fc[i] *= -lambda_plus[i];
      memcpy(r.get()+n, l_fc.get(), m*sizeof(dReal));
      for (int i=0; i< m; i++)  r[n+i] -= inv_t[i];
      dNPMultiply0(r.get()+m+n, cparams.A, x_plus.get(), nu_len, n, 1);
      for (int i=0; i< nu_len; i++)  r[n+m+i] -= cparams.b[i];
      #ifdef DEBUG
      cout << "   determined residual: ";
      printMatrix(r.get(), 1, n+m+nu_len);
      #endif
    }
    #ifdef DEBUG
    cout << "  new r: ";
    printMatrix(r.get(), 1, n+m+nu_len);
    #endif

    // get x, lambda, and nu
    memcpy(x, x_plus.get(), n*sizeof(dReal));
    std::swap(lambda, lambda_plus);
    std::swap(nu, nu_plus);
    #ifdef DEBUG
    printMatrix(x, 1, n);
    #endif

    // see whether we can quit
    if (cparams.tcheck && (*cparams.tcheck)(x, n, cparams.data))
    {
      #ifdef DEBUG
      cout << " *** termination check indicates we can quit ***" << endl;
      #endif
      return true;
    }

    // redetermine eta
    eta = -dDot(fc.get(), lambda.get(), m);

    // recompute primary and dual norms
    rdual_nrm = dSqrt(dDot(r.get(), r.get(), n));
    rpri_nrm = dSqrt(dDot(r.get()+n+m, r.get()+n+m, nu_len));

    // check for convergence
    if (rpri_nrm <= eps_feas && rdual_nrm <= eps_feas && eta < cparams.eps)
    {
      #ifdef DEBUG
      cout << "solution is within tolerances... exiting" << endl;
      #endif
      memcpy(x, x_best.get(), n*sizeof(dReal));
      return true;
    }
  }

  #ifdef DEBUG
  cout << " *** maximum number of iterations exceeded... exiting" << endl;
  #endif

  // if we're here, the max number of iterations has been exceeded
  memcpy(x, x_best.get(), n*sizeof(dReal));
  return false;
}