#include "std_library.hpp"

#include <util/llvm_type.hpp>
#include <util/misc.hpp>
#include <algorithm>
#include <llpm/exceptions.hpp>
#include <boost/foreach.hpp>
#include <llvm/IR/DerivedTypes.h>

using namespace std;

namespace llpm {
using namespace llvm;

void StdLibStops(BaseLibraryStopCondition& sc) {
    sc.addClass<IntAddition>();
    sc.addClass<IntSubtraction>();
    sc.addClass<ConstShift>();
    sc.addClass<Shift>();
    sc.addClass<IntMultiply>();
    sc.addClass<IntDivide>();
    sc.addClass<IntRemainder>();
    sc.addClass<Bitwise>();
    sc.addClass<IntCompare>();
    sc.addClass<IntTruncate>();
    sc.addClass<IntExtend>();

    sc.addClass<Constant>();
    sc.addClass<Once>();
    sc.addClass<Never>();
    sc.addClass<NullSink>();
    sc.addClass<BooleanLogic>();
    sc.addClass<CommunicationIntrinsic>();
}

llvm::Type* IntAddition::InType(std::vector<llvm::Type*> tVec) {
    if (tVec.size() == 0)
        throw InvalidArgument("Type vector must not be empty");
    for(auto&& t: tVec) {
        if (!t->isIntegerTy())
            throw InvalidArgument("All inputs to Int* blocks must be ints!");
    }
    return StructType::get(tVec[0]->getContext(), tVec);
}

llvm::Type* IntAddition::OutType(std::vector<llvm::Type*> tVec) {
    vector<unsigned> widths;
    for(auto&& t: tVec) {
        if (!t->isIntegerTy())
            throw InvalidArgument("All inputs to IntAddition must be ints!");
        unsigned tw = t->getScalarSizeInBits();
        widths.push_back(tw);
    }

    sort(widths.begin(), widths.end());
    unsigned width = 0;
    for(auto&& tw: widths) {
        if (width == 0)
            width = tw;
        else if (tw > width)
            width = tw + 1;
        else
            width = width + 1;
    }
    return Type::getIntNTy(tVec[0]->getContext(), width);
}

IntAddition::IntAddition(std::vector<llvm::Type*> t) :
    Function(InType(t), OutType(t))
{
}

IntSubtraction::IntSubtraction(llvm::Type* a, llvm::Type* b) :
    Function(IntAddition::InType({a, b}), IntAddition::OutType({a, b}))
{
}

llvm::Type* ConstShift::InType(llvm::Type* a, int, Style) {
    if (!a->isIntegerTy())
        throw InvalidArgument("All inputs to IntAddition must be ints!");
    return a;
}

llvm::Type* ConstShift::OutType(llvm::Type* a, int shift, Style style) {
    if (!a->isIntegerTy())
        throw InvalidArgument("All inputs to IntAddition must be ints!");

    bool trunc = style == LogicalTruncating;
    unsigned ashift = (shift > 0) ? shift : -1 * shift;
    if (a->getScalarSizeInBits() < ashift)
        throw InvalidArgument("Cannot shift more than the width of the type!");

    if ((trunc && shift > 0) || (!trunc && shift < 0) || shift == 0)
        return a;
    else
        return Type::getIntNTy(a->getContext(), a->getScalarSizeInBits() + shift);
}

ConstShift::ConstShift(llvm::Type* a, int shift, Style style)  :
    Function(InType(a, shift, style), OutType(a, shift, style)),
    _shift(shift),
    _style(style)
{
}

llvm::Type* Shift::InType(llvm::Type* a, Direction, Style) {
    return llvm::StructType::get(a->getContext(), vector<llvm::Type*>(
        {a, llvm::Type::getIntNTy(a->getContext(), idxwidth(bitwidth(a)))}));
}

llvm::Type* Shift::OutType(llvm::Type* a, Direction, Style) {
    if (!a->isIntegerTy())
        throw InvalidArgument("All inputs to IntAddition must be ints!");
    return a;
}

Shift::Shift(llvm::Type* a, Direction dir, Style style) :
    Function(InType(a, dir, style), OutType(a, dir, style)),
    _dir(dir),
    _style(style)
{
}

llvm::Type* IntTruncate::InType(unsigned, llvm::Type* t) {
    if (!t->isIntegerTy())
        throw InvalidArgument("Input to IntTruncate must be int!");
    return t;
}

llvm::Type* IntTruncate::OutType(unsigned N, llvm::Type* t) {
    if (!t->isIntegerTy())
        throw InvalidArgument("Input to IntTruncate must be int!");
    if (N > t->getScalarSizeInBits())
        throw InvalidArgument("Cannot truncate by more bits than input has!");

    return llvm::Type::getIntNTy(t->getContext(), t->getScalarSizeInBits() - N);
}

IntTruncate::IntTruncate(unsigned N, llvm::Type* t) :
    Function(InType(N, t), OutType(N, t))
{ }

IntTruncate::IntTruncate(llvm::Type* a, llvm::Type* b) :
    Function(a, b)
{
    if (!a->isIntegerTy() || !b->isIntegerTy())
        throw InvalidArgument("Input and outputs to IntTruncate must be ints!");
    if (a->getScalarSizeInBits() < b->getScalarSizeInBits())
        throw InvalidArgument("Input to IntTrunc must be wider than output!");
}

llvm::Type* IntExtend::InType(unsigned, llvm::Type* t) {
    if (!t->isIntegerTy())
        throw InvalidArgument("Input to IntExtend must be int!");
    return t;
}

llvm::Type* IntExtend::OutType(unsigned N, llvm::Type* t) {
    if (!t->isIntegerTy())
        throw InvalidArgument("Input to IntExtend must be int!");

    return llvm::Type::getIntNTy(t->getContext(), t->getScalarSizeInBits() + N);
}

IntExtend::IntExtend(unsigned N, bool signExtend, llvm::Type* t) :
    Function(InType(N, t), OutType(N, t)),
    _signExtend(signExtend)
{ }

llvm::Type* IntMultiply::InType(std::vector<llvm::Type*> t) {
    return IntAddition::InType(t);
}

llvm::Type* IntMultiply::OutType(std::vector<llvm::Type*> tVec) {
    if (tVec.size() == 0)
        throw InvalidArgument("There must be at least one input to IntMultiply");
    unsigned width = 0;
    for(auto&& t: tVec) {
        if (!t->isIntegerTy())
            throw InvalidArgument("All inputs to IntMultiply must be ints!");
        width += t->getScalarSizeInBits();
    }
    return Type::getIntNTy(tVec[0]->getContext(), width);
}

IntMultiply::IntMultiply(std::vector<llvm::Type*> t) :
    Function(InType(t), OutType(t))
{
}

llvm::Type* IntDivide::InType(llvm::Type* a, llvm::Type* b, bool) {
    return IntAddition::InType({a, b});
}

llvm::Type* IntDivide::OutType(llvm::Type* a, llvm::Type* b, bool) {
    if (!a->isIntegerTy() || !b->isIntegerTy())
        throw InvalidArgument("All inputs to IntDivide must be ints!");
    return a;
}

IntDivide::IntDivide(llvm::Type* a, llvm::Type* b, bool isSigned) :
    Function(InType(a, b, isSigned), OutType(a, b, isSigned)),
    _isSigned(isSigned)
{
}

llvm::Type* IntRemainder::InType(llvm::Type* a, llvm::Type* b, bool) {
    return IntAddition::InType({a, b});
}

llvm::Type* IntRemainder::OutType(llvm::Type* a, llvm::Type* b, bool) {
    if (!a->isIntegerTy() || !b->isIntegerTy())
        throw InvalidArgument("All inputs to IntDivide must be ints!");
    return b;
}

IntRemainder::IntRemainder(llvm::Type* a, llvm::Type* b, bool isSigned) :
    Function(InType(a, b, isSigned), OutType(a, b, isSigned)),
    _isSigned(isSigned)
{
}

llvm::Type* Bitwise::InType(unsigned N, llvm::Type* t) {
    if (N == 0)
        throw InvalidArgument("Bitwise must have at least one input!");
    if (!t->isIntegerTy())
        throw InvalidArgument("All inputs to Bitwise must be ints!");
    vector<llvm::Type*> sig;
    for (unsigned i=0; i<N; i++)
        sig.push_back(t);
    return StructType::get(t->getContext(), sig);
}

llvm::Type* Bitwise::OutType(unsigned N, llvm::Type* t) {
    if (N == 0)
        throw InvalidArgument("Bitwise must have at least one input!");
    if (!t->isIntegerTy())
        throw InvalidArgument("All inputs to Bitwise must be ints!");
    return t;
}

Bitwise::Bitwise(unsigned N, llvm::Type* t, Op op) :
    Function(InType(N, t), OutType(N, t)),
    _op(op)
{
}

llvm::Type* IntCompare::InType(llvm::Type* a, llvm::Type* b,
                               Cmp, bool)
{
    return IntAddition::InType({a, b});
}

llvm::Type* IntCompare::OutType(llvm::Type* a, llvm::Type*, Cmp, bool)
{
    return Type::getInt1Ty(a->getContext());
}
IntCompare::IntCompare(llvm::Type* a, llvm::Type* b,
                       Cmp op, bool isSigned) :
    Function(InType(a, b, op, isSigned), OutType(a, b, op, isSigned)),
    _op(op),
    _isSigned(isSigned)
{
}

} // namespace llpm


#include <android/sensor.h>
#include <sys/sysinfo.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <vector>
#include <string>
#include <strings.h>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <time.h>
#include <sys/socket.h>
#include <arpa/inet.h>

#include <Eigen>

#include <ros/ros.h>
#include <sandshark_common/main.h>
#include <sandshark_common/task_base.h>
#include <sandshark_common/driver_base.h>

#include <sandshark_msgs/OrientationEuler.h>
#include <sandshark_msgs/OrientationQuaternion.h>
#include <sandshark_msgs/OrientationDebug.h>
#include <sandshark_msgs/MotionRaw.h>
#include <sandshark_msgs/MotionCorrected.h>
#include <sandshark_msgs/AccelCalib.h>
#include <sandshark_msgs/MagCalib.h>

#define LOOPER_ID_USER 3

using namespace std;
using namespace Eigen;
namespace bluefin {
namespace sandshark {

class MotionDriver: public DriverBase {
  private:
    ALooper* _looper;
    ASensorManager* _sensorManager;
    vector<const ASensor*> _sensorList;
    vector<ASensorEventQueue*> _queueList;
    vector<ASensorEvent> _eventList;

    ros::Publisher _orientPub;
    sandshark_msgs::OrientationEuler _orientMsg;

    ros::Publisher _orientDebugPub;
    sandshark_msgs::OrientationDebug _orientDebugMsg;

    ros::Publisher _orientQuaternionPub;
    sandshark_msgs::OrientationQuaternion _orientQuaternionMsg;

    ros::Publisher _rawPub;
    sandshark_msgs::MotionRaw _rawMsg;

    ros::Publisher _correctedPub;
    sandshark_msgs::MotionCorrected _correctedMsg;

    ros::ServiceServer _accelService;
    ros::ServiceServer _magService;

    struct timespec _start;
    bool _firstRun;
    bool _needSleep;
    Vector3d _correctedAccel;
    Vector3d _gravityAccel;
    Vector3d _linearAccel;

    Vector3d _accelOffset;
    Vector3d _accelScaling;

    Vector3d _correctedMag;

    Vector3d _magOffset;
    Vector3d _magScaling;
    Matrix3d _magAdjust;

    Quaternion<double> _attitudeEstimate;

    Quaternion<double> _rCQuaternion;

    double _alpha;
    double _beta;
    double _zeta;

    double _gyroBiasX;
    double _gyroBiasY;
    double _gyroBiasZ;

    double _firstBetaDuration;
    double _firstBeta;
    double _start_time;

    bool _imuFilterEnabled;
    bool _imuFilterAfterBeta;
    bool _imuFilterWithBadMag;
    double _minGoodMag;
    double _maxGoodMag;

    bool _sendUDP;
    std::string _sendIPAddress;
    int _sendIPPort;
    int _sendSocket;
    struct sockaddr_in _sendAddrStruct;
    bool _sendAndroidQuaternion;

    void convertAndPublishEuler();

    bool ahrsEnabled(double magMagnitude);
    void publishEulerAngles();
    void publishDebug();

    void reset(bool writeParams);

    void cleanup();
  protected:
    void startupInitCallback();
    bool doInitialize();
    bool doRun();
  public:
    MotionDriver() :
        DriverBase("MotionDriver", "motion") {
    }

    bool handleAccelCalibUpdate(sandshark_msgs::AccelCalib::Request &req, sandshark_msgs::AccelCalib::Response &res);
    bool handleMagCalibUpdate(sandshark_msgs::MagCalib::Request &req, sandshark_msgs::MagCalib::Response &res);

    void handleSleep();
    void handleShutdown() {
      cleanup();
    }
};

void MotionDriver::startupInitCallback() {
  ROS_INFO("Task init! - creating orient node");
  fflush( stdout);

  _orientPub = _publicNode->advertise<sandshark_msgs::OrientationEuler>("orientation_euler", 1);
  _orientDebugPub = _publicNode->advertise<sandshark_msgs::OrientationDebug>("orientation_debug", 1);
  _orientQuaternionPub = _publicNode->advertise<sandshark_msgs::OrientationQuaternion>("orientation_quaternion", 1);
  _rawPub = _publicNode->advertise<sandshark_msgs::MotionRaw>("raw_sensors", 1);
  _correctedPub = _publicNode->advertise<sandshark_msgs::MotionCorrected>("corrected_sensors", 1);

  _accelService = _publicNode->advertiseService("calibrateAccel", &MotionDriver::handleAccelCalibUpdate, this);
  _magService = _publicNode->advertiseService("calibrateMag", &MotionDriver::handleMagCalibUpdate, this);

  _sensorManager = ASensorManager_getInstance();
  _looper = ALooper_forThread();
  if (_looper == NULL) {
    _looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
    ROS_INFO("Looper was NULL called prepare!");
  }
  ROS_INFO("Task init! Param stuff looper is %p", _looper);
  fflush( stdout);

  string key;
  vector<double> values;
  char comma;
  double value;

  _accelOffset << 0.0, 0.0, 0.0;
  values.clear();
  if (ros::param::search("accel_offset", key)) {
    string accel_offset;
    ros::param::get(key, accel_offset);

    istringstream iss(accel_offset);
    while (iss >> value) {
      values.push_back(value);
      iss >> comma;
    }

    if (values.size() == 3) {
      for (unsigned int i = 0; i < values.size(); ++i) {
        _accelOffset[i] = values[i];
      }
    }
  }
  cout << "Accel Offset: " << _accelOffset << std::endl;

  ROS_INFO("Task init! Param aOffset");
  fflush( stdout);

  _accelScaling << 9.8, 9.8, 9.8;
  values.clear();
  if (ros::param::search("accel_scaling", key)) {
    string accel_scaling;
    ros::param::get(key, accel_scaling);

    istringstream iss(accel_scaling);
    while (iss >> value) {
      values.push_back(value);
      iss >> comma;
    }

    if (values.size() == 3) {
      for (unsigned int i = 0; i < values.size(); ++i) {
        _accelScaling[i] = values[i];
      }
    }
  }

  cout << "Accel Scaling " << _accelScaling << std::endl;

  _magOffset << 0.0, 0.0, 0.0;
  values.clear();
  if (ros::param::search("mag_offset", key)) {
    string mag_offset;
    ros::param::get(key, mag_offset);

    istringstream iss(mag_offset);
    while (iss >> value) {
      values.push_back(value);
      iss >> comma;
    }

    if (values.size() == 3) {
      for (unsigned int i = 0; i < values.size(); ++i) {
        _magOffset[i] = (values[i]);
      }
    }
  }
  cout << "Mag Offset: " << _magOffset << std::endl;

  fflush( stdout);
  _magAdjust = Matrix3d::Identity();
  values.clear();
  if (ros::param::search("mag_adjust", key)) {
    string mag_adjust;
    ros::param::get(key, mag_adjust);

    istringstream iss(mag_adjust);
    while (iss >> value) {
      values.push_back(value);
      iss >> comma;
    }

    if (values.size() == 9) {
      for (unsigned int i = 0; i < values.size(); ++i) {
        int row = i / 3;
        int col = i % 3;
        ROS_INFO("i %d row %d col %d val %f", i, row, col, values[i]);
        fflush( stdout);
        _magAdjust(row, col) = values[i];
      }
    }
  }
  cout << "Mag Adjust: " << _magAdjust << std::endl;

  _magScaling << 1.0, 1.0, 1.0;
  values.clear();
  if (ros::param::search("mag_scaling", key)) {
    string mag_scaling;
    ros::param::get(key, mag_scaling);

    istringstream iss(mag_scaling);
    while (iss >> value) {
      values.push_back(value);
      iss >> comma;
    }

    if (values.size() == 3) {
      for (unsigned int i = 0; i < values.size(); ++i) {
        _magScaling[i] = values[i];
      }
    }
  }

  cout << "Mag Scaling: " << _magScaling << std::endl;

  ROS_INFO("Task init! Private Param stuff");
  fflush( stdout);
  _privateNode->param("alpha", _alpha, double(0.8));
  _privateNode->param("beta", _beta, double(0.2));
  _privateNode->param("zeta", _zeta, double(0.05));

  _privateNode->param("firstBetaDuration", _firstBetaDuration, double(10.0));
  _privateNode->param("firstBeta", _firstBeta, double(2.5));

  _privateNode->param("sendUDP", _sendUDP, false);
  _privateNode->param("sendIPAddress", _sendIPAddress, std::string("127.0.0.1"));
  _privateNode->param("sendIPPort", _sendIPPort, int(5555));
  ROS_INFO("SendIP %s : %d", _sendIPAddress.c_str(), _sendIPPort);
  _privateNode->param("sendAndroidQuaternion", _sendAndroidQuaternion, false);

  _privateNode->param("imuFilterEnabled", _imuFilterEnabled, false);
  _privateNode->param("imuFilterAfterBeta", _imuFilterAfterBeta, false);
  _privateNode->param("imuFilterWithBadMag", _imuFilterWithBadMag, false);
  _privateNode->param("minGoodMag", _minGoodMag, double(25)); //Units ?!?!?
  _privateNode->param("maxGoodMag", _maxGoodMag, double(65));

  _sendSocket = -1;
  if (_sendUDP) {
    _sendSocket = socket( AF_INET, SOCK_DGRAM, 0);
    if (_sendSocket < 0) {
      fprintf( stderr, "Socket is bad!\n");
      exit(-1);
    }

    struct sockaddr_in myaddr;
    memset((char *) &myaddr, 0, sizeof(myaddr));
    myaddr.sin_family = AF_INET;
    myaddr.sin_addr.s_addr = htonl( INADDR_ANY);
    myaddr.sin_port = htons(0);
    if (::bind(_sendSocket, (struct sockaddr *) &myaddr, sizeof(myaddr)) < 0) {
      fprintf( stderr, "bind failed! %d:%s\n", errno, strerror( errno));
      exit(-1);
    }

    memset(&_sendAddrStruct, 0, sizeof(_sendAddrStruct));
    _sendAddrStruct.sin_addr.s_addr = inet_addr(_sendIPAddress.c_str());
    _sendAddrStruct.sin_family = AF_INET;
    _sendAddrStruct.sin_port = htons(_sendIPPort);

  }
  ROS_INFO("Param alpha %f beta %f zeta %f", _alpha, _beta, _zeta);
  ROS_INFO("Task start! Param stuff complete");
  fflush( stdout);
}

bool MotionDriver::handleAccelCalibUpdate(sandshark_msgs::AccelCalib::Request &req,
    sandshark_msgs::AccelCalib::Response &res) {
  std::ostringstream strs;
  if ((req.accelOffset.size() < 3) || (req.accelScaling.size() < 3)) {
    res.accepted = false;
    return true;
  }

  _accelOffset << req.accelOffset[0], req.accelOffset[1], req.accelOffset[2];
  strs << _accelOffset[0] << "," << _accelOffset[1] << "," << _accelOffset[2];
  _privateNode->setParam("accel_offset", strs.str());

  strs.str("");
  strs.clear();

  _accelScaling << req.accelScaling[0], req.accelScaling[1], req.accelScaling[2];
  strs << _accelScaling[0] << "," << _accelScaling[1] << "," << _accelScaling[2];
  _privateNode->setParam("accel_scaling", strs.str());

  cout << "Service UPDATE Accel Offset: " << _accelOffset << std::endl;
  cout << "Service UPDATE Accel Scaling: " << _accelScaling << std::endl;
  res.accepted = true;
  reset(true);
  return true;
}

bool MotionDriver::handleMagCalibUpdate(sandshark_msgs::MagCalib::Request &req,
    sandshark_msgs::MagCalib::Response &res) {
  std::ostringstream strs;
  if ((req.magOffset.size() < 3) || (req.magScaling.size() < 3) || (req.magAdjust.size() < 9)) {
    res.accepted = false;
    return true;
  }

  _magOffset << req.magOffset[0], req.magOffset[1], req.magOffset[2];
  strs << _magOffset[0] << "," << _magOffset[1] << "," << _magOffset[2];
  _privateNode->setParam("mag_offset", strs.str());

  strs.str("");
  strs.clear();

  _magScaling << req.magScaling[0], req.magScaling[1], req.magScaling[2];
  strs << _magScaling[0] << "," << _magScaling[1] << "," << _magScaling[2];
  _privateNode->setParam("mag_scaling", strs.str());

  strs.str("");
  strs.clear();

  _magAdjust << req.magAdjust[0], req.magAdjust[1], req.magAdjust[2], req.magAdjust[3], req.magAdjust[4], req.magAdjust[5], req.magAdjust[6], req.magAdjust[7], req.magAdjust[8];
  strs << _magAdjust(0, 0) << "," << _magAdjust(0, 1) << "," << _magAdjust(0, 2) << "," << _magAdjust(1, 0) << ","
      << _magAdjust(1, 1) << "," << _magAdjust(1, 2) << "," << _magAdjust(2, 0) << "," << _magAdjust(2, 1) << ","
      << _magAdjust(2, 2);
  _privateNode->setParam("mag_adjust", strs.str());

  cout << "Service UPDATE Mag Offset: " << _magOffset << std::endl;
  cout << "Service UPDATE Mag Scaling: " << _magScaling << std::endl;
  cout << "Service UPDATE Mag Adjust: " << _magAdjust << std::endl;

  res.accepted = true;
  reset(true);
  return true;
}

bool MotionDriver::doInitialize() {
  cleanup();

  _needSleep = false;
  ASensorList senList;
  int numAllSensors = ASensorManager_getSensorList(_sensorManager, &senList);

  ROS_INFO("Task init! Motion numsen %d", numAllSensors);
  fflush( stdout);
  int rc;
  for (int i = 0; i < numAllSensors; ++i) {
    const ASensor *sensor = senList[i];
    ROS_INFO("Sensor Name %s Vendor %s Res %f Type %d", ASensor_getName(sensor), ASensor_getVendor(sensor),
        ASensor_getResolution(sensor), ASensor_getType(sensor));
    if ((std::string(ASensor_getName(sensor)) == "MPL magnetic field")
        || (std::string(ASensor_getName(sensor)) == "MPL accel") || (std::string(ASensor_getName(sensor)) == "MPL Gyro")
        || (_sendAndroidQuaternion && (std::string(ASensor_getName(sensor)) == "MPL rotation vector"))) {
      ASensorEventQueue* queue = ASensorManager_createEventQueue(_sensorManager, _looper, LOOPER_ID_USER, NULL, NULL);
      if (!queue) {
        ROS_INFO("error creating sensor event queue");
        return false;
      }
      rc = ASensorEventQueue_enableSensor(queue, sensor);
      if (rc < 0) {
        ROS_INFO("ASensorEventQueue_enableSensor error: %d", rc);
        return false;
      }
      rc = ASensorEventQueue_setEventRate(queue, sensor, ASensor_getMinDelay(sensor));
      if (rc < 0) {
        ROS_INFO("ASensorEventQueue_setEventRate error: %d", rc);
        return false;
      }

      fprintf( stderr, "Pushing back Sensor Name %s Vendor %s Res %f\n", ASensor_getName(sensor),
          ASensor_getVendor(sensor), ASensor_getResolution(sensor));
      _sensorList.push_back(sensor);
      _queueList.push_back(queue);
      _eventList.push_back(ASensorEvent());
    }
  }

  ROS_INFO("Task init! Motion sen complete %d", (int) _queueList.size());
  fflush( stdout);

  reset(false);

  ROS_INFO("Task init! Motion init complete event %d", (int) _eventList.size());
  fflush( stdout);

  return true;
}

void MotionDriver::reset(bool writeParams) {
  _firstRun = true;
  clock_gettime( CLOCK_REALTIME, &_start);
  _start_time = ros::WallTime::now().toSec();

  _gyroBiasX = 0.0;
  _gyroBiasY = 0.0;
  _gyroBiasZ = 0.0;
  _attitudeEstimate.w() = 1.0;
  _attitudeEstimate.x() = 0.0;
  _attitudeEstimate.y() = 0.0;
  _attitudeEstimate.z() = 0.0;

  if (writeParams) {
    ROS_INFO("Writing parameters...");
    //System calls are so dirty, there has to be a better way to do this
    system("rosparam dump /data/app/bluefin/opt/sandshark/share/sandshark_apps/config/motion.yaml /motion");
    ROS_INFO("Done");
  }
}

void MotionDriver::cleanup() {
  ROS_INFO("Task init! Motion cleanup");
  fflush( stdout);
  for (unsigned int i = 0; i < _sensorList.size(); i++) {
    ASensorEventQueue_disableSensor(_queueList[i], _sensorList[i]);
    ASensorManager_destroyEventQueue(_sensorManager, _queueList[i]);
  }

  _sensorList.clear();
  _queueList.clear();
  ROS_INFO("Task init! Cleanup done");
  fflush( stdout);
}

bool MotionDriver::ahrsEnabled(double magMagnitude) {
  //replace AHRS with IMU filter
  if (_imuFilterEnabled) {
    return false;
  }

  //Switch to IMU filter after first Beta
  if (_imuFilterAfterBeta) {
    double curTime = ros::WallTime::now().toSec();
    if ((curTime - _start_time) > _firstBetaDuration) {
      return false;
    }
  }

  if (_imuFilterWithBadMag) {
    if ((magMagnitude < _minGoodMag) || (magMagnitude > _maxGoodMag)) {
      return false;
    }
  }

  return true;
}

bool MotionDriver::doRun() {
  static struct timespec current;
  static double magX, magY, magZ, accelX, accelY, accelZ, gyroX, gyroY, gyroZ;
  //A failure state of the magnetometer is that it will lock up and return
  //the same value forever, so we check for that and fail if it does
  static double lastMagX = 0, lastMagY = 0, lastMagZ = 0;
  static int magLockedCount = 0;
  static double o_x, o_y, o_z;
  static int magStat = 0, accelStat = 0, gyroStat = 0, count = 0;
  static Vector3d accelTemp, magTemp, accelNormalized, magNormalized;
  static bool haveMag = false, haveAccel = false, haveGyro = false, haveRotation = false;
  static Matrix<double, 3, 3> rotation;
  static Quaternion<double> attitudeConj;
  static Quaternion<double> magQuaternion;
  static Quaternion<double> gyroQuaternion, gyroError;
  static Quaternion<double> hQuaternion;
  static Quaternion<double> bQuaternion;
  static Matrix<double, 4, 1> stepVector;
  static Quaternion<double> stepQuaternion;
  static Matrix<double, 6, 4> J;
  static Matrix<double, 6, 1> F;
  static Matrix<double, 3, 4> imuJ;
  static Matrix<double, 3, 1> imuF;
  static double prev_elapsed = 0.0;
  static Vector4d rateOfChange;
  static char sendBuf[1400];
  static int noEventsCount = 0;

  clock_gettime( CLOCK_REALTIME, &current);
  //Assume start nsec is zero for simplicity
  double elapsed = (current.tv_sec - _start.tv_sec) + ((double) current.tv_nsec) / 1000000000;
  if (ASensorEventQueue_hasEvents(_queueList[0]) > 0) {  // if the master (first) sensor has new data
    for (unsigned int i = 0; i < _queueList.size(); ++i) {
      int ret = 0;
      int eventCount = 0;
      while ((ret = ASensorEventQueue_getEvents(_queueList[i], &_eventList[i], 1)) > 0) { //get all pending events
        //WARNING: do NOT put debug messages in here. The latency
        //will cause the motion driver to fail to get message
        //fast enough and will get stuck in an infinite loop

        clock_gettime( CLOCK_REALTIME, &current);
        double tempElapsed = (current.tv_sec - _start.tv_sec) + ((double) current.tv_nsec) / 1000000000.0;
        double lastEvent = (tempElapsed - elapsed);
        if (lastEvent > 2.0) { //No Sensor Data for 2 secs
          ROS_WARN("Getting current events for more than two seconds, driver failing!");
          return false;
        }

        eventCount += ret;
      }

      //make sure we got a new value
      if (ret < 0) {
        ROS_WARN("Error from android sensor manager (%d)", ret);
        setErrorMessage("Error from android sensor manager");
        noEventsCount = 0;
        //TODO: should we actually be failing here?
        return false;
      } else if (eventCount == 0) {
        noEventsCount++;
        if (noEventsCount > 5) {
          ROS_WARN("No events received from sensor manager!");
          setErrorMessage("Error from android sensor manager");
          noEventsCount = 0;
          //TODO: should we actually be failing here?
          return false;
        }
      } else {
        noEventsCount = 0;
        struct sysinfo info;
        int64_t uptime = 0;
        if (sysinfo(&info) == 0) {
          uptime = info.uptime;
        }

        float *d = _eventList[i].data;
        switch (_eventList[i].type) {
        case ASENSOR_TYPE_ACCELEROMETER:
          if ((_eventList[i].timestamp / 1000000000) - uptime > 2) {
            ROS_WARN("Processing old accelerometer data from sensor!!");
          }
          accelX = d[0];
          accelY = d[1];
          accelZ = d[2];
          accelStat = _eventList[i].acceleration.status;
          accelTemp(0) = accelX;
          accelTemp(1) = accelY;
          accelTemp(2) = accelZ;
          if (_firstRun) {
            _gravityAccel = accelTemp;
          }
          haveAccel = true;
          accelTemp -= _accelOffset;
          _correctedAccel = accelTemp;
          accelTemp(0) = _correctedAccel(0) / _accelScaling(0);
          accelTemp(1) = _correctedAccel(1) / _accelScaling(1);
          accelTemp(2) = _correctedAccel(2) / _accelScaling(2);
          _correctedAccel = accelTemp * 9.81;
          break;
        case ASENSOR_TYPE_MAGNETIC_FIELD:
          if ((_eventList[i].timestamp / 1000000000) - uptime > 2) {
            ROS_WARN("Processing old magnetometer data from sensor!!");
          }
          magX = d[0];
          magY = d[1];
          magZ = d[2];
          if (magX == lastMagX && magY == lastMagY && magZ == lastMagZ) {
            magLockedCount++;
            if (magLockedCount > 10) {
              //Something bad is happening! mag has locked up
              //Restart so we can get it unlocked
              ROS_WARN("Magnetometer has locked up!");
              setErrorMessage("Magnetometer has locked up!");
              return false;
            }
          } else {
            magLockedCount = 0;
          }
          lastMagX = magX;
          lastMagY = magY;
          lastMagZ = magZ;

          magStat = _eventList[i].magnetic.status;
          haveMag = true;
          magTemp(0) = magX;
          magTemp(1) = magY;
          magTemp(2) = magZ;
          magTemp -= _magOffset;
          //_correctedMag = magTemp;
          _correctedMag = magTemp.transpose() * _magAdjust;
          magTemp(0) = _correctedMag(0) / _magScaling(0);
          magTemp(1) = _correctedMag(1) / _magScaling(1);
          magTemp(2) = _correctedMag(2) / _magScaling(2);
          _correctedMag = magTemp.transpose() * _magAdjust.transpose();
          break;
        case ASENSOR_TYPE_GYROSCOPE:
          if ((_eventList[i].timestamp / 1000000000) - uptime > 2) {
            ROS_WARN("Processing old gyroscope data from sensor!!");
          }
          gyroX = d[0];
          gyroY = d[1];
          gyroZ = d[2];
          gyroStat = _eventList[i].vector.status;
          haveGyro = true;
          break;
        case 11:
          o_x = d[0];
          o_y = d[1];
          o_z = d[2];
          //                    o_w = d[3];
          haveRotation = true;
          break;
        }
      }
    }

    if (haveMag && haveAccel && haveGyro) {

      _rawMsg.accel_x = accelX;
      _rawMsg.accel_y = accelY;
      _rawMsg.accel_z = accelZ;
      _rawMsg.accel_status = accelStat;

      _rawMsg.mag_x = magX;
      _rawMsg.mag_y = magY;
      _rawMsg.mag_z = magZ;
      _rawMsg.mag_status = magStat;

      _rawMsg.gyro_x = gyroX;
      _rawMsg.gyro_y = gyroY;
      _rawMsg.gyro_z = gyroZ;
      _rawMsg.gyro_status = gyroStat;

      _rawPub.publish(_rawMsg);

      _gravityAccel = _alpha * _gravityAccel + (1 - _alpha) * _correctedAccel;
      _linearAccel = _correctedAccel - _gravityAccel;

      _correctedMsg.timestamp = current.tv_sec + ((double) current.tv_nsec) / 1000000000.0;
      _correctedMsg.accel_x = _correctedAccel(0);
      _correctedMsg.accel_y = _correctedAccel(1);
      _correctedMsg.accel_z = _correctedAccel(2);
      _correctedMsg.linear_x = _linearAccel(0);
      _correctedMsg.linear_y = _linearAccel(1);
      _correctedMsg.linear_z = _linearAccel(2);
      _correctedMsg.gravity_x = _gravityAccel(0);
      _correctedMsg.gravity_y = _gravityAccel(1);
      _correctedMsg.gravity_z = _gravityAccel(2);
      _correctedMsg.accel_status = accelStat;

      _correctedMsg.mag_x = _correctedMag(0);
      _correctedMsg.mag_y = _correctedMag(1);
      _correctedMsg.mag_z = _correctedMag(2);
      _correctedMsg.mag_status = magStat;

      _correctedMsg.gyro_x = gyroX;
      _correctedMsg.gyro_y = gyroY;
      _correctedMsg.gyro_z = gyroZ;
      _correctedMsg.gyro_status = gyroStat;

      _correctedPub.publish(_correctedMsg);

      accelNormalized = _gravityAccel / _gravityAccel.norm();
      magNormalized = _correctedMag / _correctedMag.norm();

      if (ahrsEnabled(_correctedMag.norm())) {
        magQuaternion.w() = 0.0;
        magQuaternion.x() = magNormalized.x();
        magQuaternion.y() = magNormalized.y();
        magQuaternion.z() = magNormalized.z();

        hQuaternion = _attitudeEstimate * (magQuaternion * _attitudeEstimate.conjugate());
        bQuaternion.w() = 0.0;
        bQuaternion.x() = sqrt(hQuaternion.x() * hQuaternion.x() + hQuaternion.y() * hQuaternion.y());
        bQuaternion.y() = 0.0;
        bQuaternion.z() = hQuaternion.z();

        F[0] = 2.0 * (_attitudeEstimate.x() * _attitudeEstimate.z() - _attitudeEstimate.w() * _attitudeEstimate.y())
            - accelNormalized.x();
        F[1] = 2.0 * (_attitudeEstimate.w() * _attitudeEstimate.x() + _attitudeEstimate.y() * _attitudeEstimate.z())
            - accelNormalized.y();
        F[2] = 2.0
            * (0.5 - _attitudeEstimate.x() * _attitudeEstimate.x() - _attitudeEstimate.y() * _attitudeEstimate.y())
            - accelNormalized.z();
        F[3] = 2.0 * bQuaternion.x()
            * (0.5 - _attitudeEstimate.y() * _attitudeEstimate.y() - _attitudeEstimate.z() * _attitudeEstimate.z())
            + 2.0 * bQuaternion.z()
                * (_attitudeEstimate.x() * _attitudeEstimate.z() - _attitudeEstimate.w() * _attitudeEstimate.y())
            - magNormalized.x();
        F[4] = 2.0 * bQuaternion.x()
            * (_attitudeEstimate.x() * _attitudeEstimate.y() - _attitudeEstimate.w() * _attitudeEstimate.z())
            + 2.0 * bQuaternion.z()
                * (_attitudeEstimate.w() * _attitudeEstimate.x() + _attitudeEstimate.y() * _attitudeEstimate.z())
            - magNormalized.y();
        F[5] = 2.0 * bQuaternion.x()
            * (_attitudeEstimate.w() * _attitudeEstimate.y() + _attitudeEstimate.x() * _attitudeEstimate.z())
            + 2.0 * bQuaternion.z()
                * (0.5 - _attitudeEstimate.x() * _attitudeEstimate.x() - _attitudeEstimate.y() * _attitudeEstimate.y())
            - magNormalized.z();

        J(0, 0) = -2.0 * _attitudeEstimate.y();
        J(0, 1) = 2.0 * _attitudeEstimate.z();
        J(0, 2) = -2.0 * _attitudeEstimate.w();
        J(0, 3) = 2.0 * _attitudeEstimate.x();
        J(1, 0) = 2.0 * _attitudeEstimate.x();
        J(1, 1) = 2.0 * _attitudeEstimate.w();
        J(1, 2) = 2.0 * _attitudeEstimate.z();
        J(1, 3) = 2.0 * _attitudeEstimate.y();
        J(2, 0) = 0.0;
        J(2, 1) = -4.0 * _attitudeEstimate.x();
        J(2, 2) = -4.0 * _attitudeEstimate.y();
        J(2, 3) = 0.0;
        J(3, 0) = -2.0 * bQuaternion.z() * _attitudeEstimate.y();
        J(3, 1) = 2.0 * bQuaternion.z() * _attitudeEstimate.z();
        J(3, 2) = -4.0 * bQuaternion.x() * _attitudeEstimate.y() - 2.0 * bQuaternion.z() * _attitudeEstimate.w();
        J(3, 3) = -4.0 * bQuaternion.x() * _attitudeEstimate.z() + 2.0 * bQuaternion.z() * _attitudeEstimate.x();
        J(4, 0) = -2.0 * bQuaternion.x() * _attitudeEstimate.z() + 2.0 * bQuaternion.z() * _attitudeEstimate.x();
        J(4, 1) = 2.0 * bQuaternion.x() * _attitudeEstimate.y() + 2.0 * bQuaternion.z() * _attitudeEstimate.w();
        J(4, 2) = 2.0 * bQuaternion.x() * _attitudeEstimate.x() + 2.0 * bQuaternion.z() * _attitudeEstimate.z();
        J(4, 3) = -2.0 * bQuaternion.x() * _attitudeEstimate.w() + 2.0 * bQuaternion.z() * _attitudeEstimate.y();
        J(5, 0) = 2.0 * bQuaternion.x() * _attitudeEstimate.y();
        J(5, 1) = 2.0 * bQuaternion.x() * _attitudeEstimate.z() - 4.0 * bQuaternion.z() * _attitudeEstimate.x();
        J(5, 2) = 2.0 * bQuaternion.x() * _attitudeEstimate.w() - 4.0 * bQuaternion.z() * _attitudeEstimate.y();
        J(5, 3) = 2.0 * bQuaternion.x() * _attitudeEstimate.x();

        stepVector = (J.transpose() * F);
        stepVector.normalize();
      } else {
        imuF[0] = 2.0 * (_attitudeEstimate.x() * _attitudeEstimate.z() - _attitudeEstimate.w() * _attitudeEstimate.y())
            - accelNormalized.x();
        imuF[1] = 2.0 * (_attitudeEstimate.w() * _attitudeEstimate.x() + _attitudeEstimate.y() * _attitudeEstimate.z())
            - accelNormalized.y();
        imuF[2] = 2.0
            * (0.5 - _attitudeEstimate.x() * _attitudeEstimate.x() - _attitudeEstimate.y() * _attitudeEstimate.y())
            - accelNormalized.z();

        imuJ(0, 0) = -2.0 * _attitudeEstimate.y();
        imuJ(0, 1) = 2.0 * _attitudeEstimate.z();
        imuJ(0, 2) = -2.0 * _attitudeEstimate.w();
        imuJ(0, 3) = 2.0 * _attitudeEstimate.x();
        imuJ(1, 0) = 2.0 * _attitudeEstimate.x();
        imuJ(1, 1) = 2.0 * _attitudeEstimate.w();
        imuJ(1, 2) = 2.0 * _attitudeEstimate.z();
        imuJ(1, 3) = 2.0 * _attitudeEstimate.y();
        imuJ(2, 0) = 0.0;
        imuJ(2, 1) = -4.0 * _attitudeEstimate.x();
        imuJ(2, 2) = -4.0 * _attitudeEstimate.y();
        imuJ(2, 3) = 0.0;

        stepVector = (imuJ.transpose() * imuF);
        stepVector.normalize();
      }
      stepQuaternion.w() = stepVector[0];
      stepQuaternion.x() = stepVector[1];
      stepQuaternion.y() = stepVector[2];
      stepQuaternion.z() = stepVector[3];
      gyroError = (_attitudeEstimate.conjugate() * stepQuaternion);

      double timestep = (elapsed - prev_elapsed);
      if (timestep < 0.0 || timestep > 2.0) {
        ROS_INFO("\ntime %f\n", timestep);
      }
      double curZeta = _zeta;
      double curTime = ros::WallTime::now().toSec();
      if ((curTime - _start_time) < _firstBetaDuration) {
        curZeta = 0.0;
      }

      _gyroBiasX += 2.0 * gyroError.x() * timestep * curZeta;
      _gyroBiasY += 2.0 * gyroError.y() * timestep * curZeta;
      _gyroBiasZ += 2.0 * gyroError.z() * timestep * curZeta;
      gyroQuaternion.w() = 0.0;
      gyroQuaternion.x() = gyroX - _gyroBiasX;
      gyroQuaternion.y() = gyroY - _gyroBiasY;
      gyroQuaternion.z() = gyroZ - _gyroBiasZ;

      _rCQuaternion = (_attitudeEstimate * gyroQuaternion);
      rateOfChange[0] = _rCQuaternion.w();
      rateOfChange[1] = _rCQuaternion.x();
      rateOfChange[2] = _rCQuaternion.y();
      rateOfChange[3] = _rCQuaternion.z();

      rateOfChange *= 0.5;
      if ((curTime - _start_time) < _firstBetaDuration) {
        rateOfChange -= _firstBeta * stepVector.transpose();
      } else {
        rateOfChange -= _beta * stepVector.transpose();
      }

      _attitudeEstimate.w() += rateOfChange[0] * timestep;
      _attitudeEstimate.x() += rateOfChange[1] * timestep;
      _attitudeEstimate.y() += rateOfChange[2] * timestep;
      _attitudeEstimate.z() += rateOfChange[3] * timestep;
      _attitudeEstimate.normalize();

      _orientQuaternionMsg.w = _attitudeEstimate.w();
      _orientQuaternionMsg.x = _attitudeEstimate.x();
      _orientQuaternionMsg.y = _attitudeEstimate.y();
      _orientQuaternionMsg.z = _attitudeEstimate.z();

      publishEulerAngles();
      publishDebug();
      _orientQuaternionPub.publish(_orientQuaternionMsg);

      if (_sendUDP && (count % 10 == 0)) {
        if (_sendAndroidQuaternion) {
          if (haveRotation) {
            snprintf(&sendBuf[0], sizeof(sendBuf), "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
                ros::WallTime::now().toSec(), accelX, accelY, accelZ, gyroX, gyroY, gyroZ, magX, magY, magZ,
                _attitudeEstimate.w(), _attitudeEstimate.x(), _attitudeEstimate.y(), _attitudeEstimate.z(),
                _orientMsg.yaw, _orientMsg.roll, _orientMsg.pitch,
                sqrt(1.0 - ((o_x * o_x) + (o_y * o_y) + (o_z * o_z))), o_x, o_y, o_z);

          }
        } else {
          snprintf(&sendBuf[0], sizeof(sendBuf), "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
              ros::WallTime::now().toSec(),
              //correctedAccel(0), correctedAccel(1), correctedAccel(2),
              accelX, accelY, accelZ, gyroX, gyroY, gyroZ,
              //correctedMag(0), correctedMag(1), correctedMag(2),
              magX, magY, magZ, _attitudeEstimate.w(), _attitudeEstimate.x(), _attitudeEstimate.y(),
              _attitudeEstimate.z(),
              //SEq_1, SEq_2, SEq_3, SEq_4,
              _orientMsg.yaw, _orientMsg.roll, _orientMsg.pitch);
        }
        sendBuf[sizeof(sendBuf) - 1] = '\0';
        int ret = sendto(_sendSocket, &sendBuf[0], strlen(sendBuf), 0, (struct sockaddr *) &_sendAddrStruct,
            sizeof(struct sockaddr));
        if (ret < 0) {
          fprintf( stderr, "SendTo failed ret %d - %d:%s\n", ret, errno, strerror( errno));
        }
      }
      prev_elapsed = elapsed;
      count++;
      haveMag = false;
      haveAccel = false;
      haveGyro = false;
    }
  } else {
    double lastEvent = (elapsed - prev_elapsed);
    if (lastEvent > 2.0) { //No Sensor Data for 2 secs

      elapsed = 0.0;
      prev_elapsed = 0.0;
      return false;
    }
    _needSleep = true;
  }

  _firstRun = false;
  return true;
}

void MotionDriver::handleSleep() {
  if (_needSleep) {
    usleep(10000);
  }
  _needSleep = false;
}

void MotionDriver::publishDebug() {
  //publish gyro bias and the rCQuaternion

  _orientDebugMsg.gyro_bias_x = _gyroBiasX;
  _orientDebugMsg.gyro_bias_y = _gyroBiasY;
  _orientDebugMsg.gyro_bias_z = _gyroBiasZ;

  _orientDebugMsg.rc_x = _rCQuaternion.x();
  _orientDebugMsg.rc_y = _rCQuaternion.y();
  _orientDebugMsg.rc_z = _rCQuaternion.z();
  _orientDebugMsg.rc_w = _rCQuaternion.w();

  _orientDebugPub.publish(_orientDebugMsg);
}

void MotionDriver::publishEulerAngles() {

  double froll = 0.0;
  double fpitch = 0.0;
  double fazimuth = 0.0;

  Matrix3d qrotation = _attitudeEstimate.matrix();
  //Rotate into vehicle orientation
  Matrix3d rotation = qrotation * AngleAxisd(0.5 * M_PI, Vector3d::UnitY());

  //rotation matrix to euler, pitch and azimuth are inverted
  froll = atan2(rotation(2, 1), rotation(2, 2));
  fpitch = asin(rotation(2, 0));
  fazimuth = atan2(rotation(1, 0), rotation(0, 0));

  froll *= 180.0 / M_PI;
  fpitch *= 180.0 / M_PI;
  fazimuth *= 180.0 / M_PI;
  if (fazimuth < 0.0) {
    fazimuth += 360.0;
  }

  if (fazimuth > 360.0) {
    fazimuth -= 360.0;
  }

  _orientMsg.roll = froll;
  _orientMsg.yaw = fazimuth;
  _orientMsg.pitch = fpitch;

  _orientPub.publish(_orientMsg);

}

}
}

int main(int argc, char *argv[]) {
  bluefin::sandshark::MotionDriver md;
  return bluefin::sandshark::app_main((bluefin::sandshark::TaskBase&) md, argc, argv);
}


#include <numpy_eigen/boost_python_headers.hpp>

#include <aslam/backend/BSplineMotionError.hpp>
#include <aslam/splines/BSplinePoseDesignVariable.hpp>
#include <aslam/splines/BSplineRSPoseDesignVariable.hpp>
#include <aslam/splines/EuclideanBSplineDesignVariable.hpp>

#include <boost/shared_ptr.hpp>

using namespace boost::python;
using namespace aslam::backend;

void exportBSplineMotionError() {
    class_<BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable>,
           boost::shared_ptr<BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable> >, bases<ErrorTerm> >(
        "BSplineEuclideanMotionError", init<aslam::splines::EuclideanBSplineDesignVariable*, Eigen::MatrixXd>(
                                           "BSplineEuclideanMotionError(EuclideanBSplineDesignVariable, W)"))
        .def(init<aslam::splines::EuclideanBSplineDesignVariable*, Eigen::MatrixXd, unsigned int>(
            "BSplineGenericMotionError(EuclideanBSplineDesignVariable, W, errorTermOrder)"))
        .def("rhs", &BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable>::rhs);

    class_<BSplineMotionError<aslam::splines::BSplinePoseDesignVariable>,
           boost::shared_ptr<BSplineMotionError<aslam::splines::BSplinePoseDesignVariable> >, bases<ErrorTerm> >(
        "BSplineMotionError", init<aslam::splines::BSplinePoseDesignVariable*, Eigen::MatrixXd>(
                                  "BSplineMotionError(BSplinePoseDesignVariable, W)"))
        .def(init<aslam::splines::BSplinePoseDesignVariable*, Eigen::MatrixXd, unsigned int>(
            "BSplineRSMotionError(BSplinePoseDesignVariable, W, errorTermOrder)"))
        .def("rhs", &BSplineMotionError<aslam::splines::BSplinePoseDesignVariable>::rhs);

    class_<BSplineMotionError<aslam::splines::BSplineRSPoseDesignVariable>,
           boost::shared_ptr<BSplineMotionError<aslam::splines::BSplineRSPoseDesignVariable> >, bases<ErrorTerm> >(
        "BSplineRSMotionError", init<aslam::splines::BSplineRSPoseDesignVariable*, Eigen::MatrixXd>(
                                    "BSplineRSMotionError(BSplinePoseDesignVariable, W)"))
        .def(init<aslam::splines::BSplineRSPoseDesignVariable*, Eigen::MatrixXd, unsigned int>(
            "BSplineRSMotionError(BSplinePoseDesignVariable, W, errorTermOrder)"))
        .def("rhs", &BSplineMotionError<aslam::splines::BSplineRSPoseDesignVariable>::rhs);
}


#include "solver.h"

#include <boost/functional/hash.hpp>
#include <boost/format.hpp>
#include "../parallel.h"
#include "../std.h"
#include "../time.h"
#include "../wavefunction/wavefunction.h"
#include "davidson.h"

Det Solver::generate_hf_det() {
  Det det;
  for (size_t i = 0; i < n_up; i++) det.up.set_orb(i, true);
  for (size_t i = 0; i < n_dn; i++) det.dn.set_orb(i, true);
  return det;
}

void Solver::variation(const double eps_var) {
  const double THRESHOLD = 1.0e-6;

  // Setup HF or existing wf as initial wf and evaluate energy.
  if (wf.size() == 0) {
    const Det& det_hf = generate_hf_det();
    wf.append_term(det_hf, 1.0);
    energy_hf = energy_var = hamiltonian(det_hf, det_hf);
    if (Parallel::is_master()) printf("HF energy: %#.15g Ha\n", energy_hf);
  }

  std::unordered_set<OrbitalsPair, boost::hash<OrbitalsPair>> var_dets_set;
  std::list<Det> new_dets;
  for (const auto& term : wf.get_terms()) var_dets_set.insert(term.det.encode());
  double energy_var_new = 0.0;  // Ensures the first iteration will run.
  size_t n_iter = 0;
  converged = false;
  while (!converged) {
    Time::start("Variation: " + std::to_string(n_iter));

    for (const auto& term : wf.get_terms()) {
      const auto& connected_dets = find_connected_dets(term.det, eps_var / fabs(term.coef));
      for (const auto& new_det : connected_dets) {
        const auto new_det_code = new_det.encode();
        if (var_dets_set.count(new_det_code) == 0) {
          var_dets_set.insert(new_det_code);
          new_dets.push_back(new_det);
        }
      }
    }

    if (Parallel::is_master()) {
      printf("New / total dets: %'zu / %'zu\n", new_dets.size(), var_dets_set.size());
    }
    Time::checkpoint("found new dets");

    for (const auto& new_det : new_dets) {
      wf.append_term(new_det, 0.0);
    }

    energy_var_new = diagonalize(new_dets.size() > 0);
    if (fabs(energy_var - energy_var_new) < THRESHOLD) converged = true;
    energy_var = energy_var_new;
    if (Parallel::is_master()) {
      printf("Variation energy: %#.15g Ha\n", energy_var);
      printf("Correlation energy (variation): %#.15g Ha\n", energy_var - energy_hf);
    }

    new_dets.clear();

    Time::end();
    n_iter++;
  }

  if (Parallel::is_master()) {
    printf("Final variation energy: %#.15g Ha\n", energy_var);
    printf("Correlation energy (variation): %#.15g Ha\n", energy_var - energy_hf);
  }
}

double Solver::diagonalize(const bool has_new_dets) {
  std::vector<double> diagonal;
  std::vector<double> initial_vector;
  const size_t max_iterations = has_new_dets ? 5 : 10;
  diagonal.reserve(wf.size());
  initial_vector.reserve(wf.size());
  for (const auto& term : wf.get_terms()) {
    const auto& det = term.det;
    diagonal.push_back(hamiltonian(det, det));
    initial_vector.push_back(term.coef);
  }

  std::function<double(Det, Det)> hamiltonian_func =
      std::bind(&Solver::hamiltonian, this, std::placeholders::_1, std::placeholders::_2);
  HelperStrings helper_strings(hamiltonian_func);
  helper_strings.setup(wf.get_dets());
  Time::checkpoint("helper strings generated");
  std::function<std::vector<double>(std::vector<double>)> apply_hamiltonian_func =
      std::bind(&Solver::apply_hamiltonian, this, std::placeholders::_1, helper_strings);

  Davidson davidson(diagonal, apply_hamiltonian_func, wf.size());
  if (Parallel::is_master()) davidson.set_verbose(true);
  const size_t n_iter = davidson.diagonalize(initial_vector, max_iterations);
  if (!has_new_dets && n_iter < max_iterations) converged = true;

  const double energy_var = davidson.get_lowest_eigenvalue();
  const auto& coefs_new = davidson.get_lowest_eigenvector();

  wf.set_coefs(coefs_new);
  wf.sort_by_coefs();

  return energy_var;
}

#pragma omp declare reduction(      \
    vec_double_plus : std::vector < \
    double > : std::transform(      \
                 omp_out            \
                     .begin(), omp_out.end(), omp_in.begin(), omp_out.begin(), std::plus < double > ())) initializer(omp_priv = omp_orig)

std::vector<double> Solver::apply_hamiltonian(
    const std::vector<double>& vec, HelperStrings& helper_strings) {
  const std::size_t n_dets = vec.size();
  const size_t proc_id = Parallel::get_id();
  const size_t n_procs = Parallel::get_n();
  std::vector<double> res(n_dets, 0.0);

  const auto& dets = wf.get_dets();

#pragma omp parallel for reduction(vec_double_plus : res) schedule(dynamic, 10)
  for (size_t i = proc_id; i < n_dets; i += n_procs) {
    const auto& connections = helper_strings.find_connections(i);
    for (const auto connection : connections) {
      const size_t j = connection.first;
      const double H_ij = connection.second;
      res[i] += H_ij * vec[j];
      if (i != j) {
        res[j] += H_ij * vec[i];
      }
    }
  }

  Parallel::reduce_to_sum_vector(res);
  Time::checkpoint("hamiltonian applied");

  return res;
}

void Solver::save_variation_result(const std::string& filename) {
  if (Parallel::is_master()) {
    std::ofstream var_file;
    var_file.open(filename);
    var_file << boost::format("%.17g %.17g\n") % energy_hf % energy_var;
    var_file << boost::format("%d %d %d\n") % n_up % n_dn % wf.size();
    for (const auto& term : wf.get_terms()) {
      var_file << boost::format("%.17g\n") % term.coef;
      var_file << term.det.up << std::endl << term.det.dn << std::endl;
    }
    var_file.close();
    printf("Variation result saved to: %s\n", filename.c_str());
  }
}

bool Solver::load_variation_result(const std::string& filename) {
  std::ifstream var_file;
  size_t n_dets;
  Orbital orb_id;
  double coef;
  var_file.open(filename);
  if (!var_file.is_open()) return false;  // Does not exist.
  var_file >> energy_hf >> energy_var;
  var_file >> n_up >> n_dn >> n_dets;
  wf.clear();
  for (std::size_t i = 0; i < n_dets; i++) {
    var_file >> coef;
    Det det;
    for (std::size_t j = 0; j < n_up; j++) {
      var_file >> orb_id;
      det.up.set_orb(orb_id, true);
    }
    for (std::size_t j = 0; j < n_dn; j++) {
      var_file >> orb_id;
      det.dn.set_orb(orb_id, true);
    }
    wf.append_term(det, coef);
  }
  var_file.close();
  if (Parallel::is_master()) {
    printf("Loaded %'zu dets from: %s\n", n_dets, filename.c_str());
    printf("HF energy: %#.15g Ha\n", energy_hf);
    printf("Variation energy: %#.15g Ha\n", energy_var);
    printf("Correlation energy (variation): %#.15g Ha\n", energy_var - energy_hf);
  }
  return true;
}


// #include "nabo/nabo.h"
#include "helpers.hpp"
#include <pcl/io/ply_io.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <omp.h>
#include <iostream>
#include <gdcpp.h>
#include <fstream>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

// using namespace Nabo;
using namespace Eigen;
using namespace std;
using namespace TransformationUtilities;
using namespace InputUtilities;
using namespace InterfaceUtilities;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

std::ofstream outfile;
std::ofstream errorfile;

class Optimizer
{
    private:
        string config_file;
        vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_downsampled;
        vector<MatrixXd> inverse_kinematics;
        std::vector<double> transformation_initial;
        std::vector<double> flange_transformation_initial;
        vector<double> getTransVector(boost::property_tree::ptree &pt,string s);

    public:
        std::vector<double> transformation;
        std::vector<double> flange_transformation;
        Optimizer(string filename);
        Optimizer(){};
        void getInputs();
        pcl::PointCloud<pcl::PointXYZ>::Ptr combined_cloud;
};

Optimizer::Optimizer(string filename)
{
    config_file = filename;
    cloud.reset (new PointCloudT);
    transformation = std::vector<double>(6,0);
    flange_transformation = std::vector<double>(6,0);
    transformation_initial = std::vector<double>(6,0);
    flange_transformation_initial = std::vector<double>(6,0);
    combined_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
}

vector<double> Optimizer::getTransVector(boost::property_tree::ptree &pt,string s)
{
    string angle_metric = pt.get<std::string>(s+".angle","radian");
    string camera_approx_trans_metric = pt.get<std::string>(s+".metric","m");
    double cam_approx_scale = 1.0;
    if(camera_approx_trans_metric=="cm")
        cam_approx_scale = 100.0;
    else if(camera_approx_trans_metric=="mm")
        cam_approx_scale=1000.0;
    string approx_transformation = pt.get<std::string>(s+".value","0,0,0,0,0,0");
    cout<<approx_transformation<<endl;
    vector<string> coords_str;
    boost::split(coords_str, approx_transformation , boost::is_any_of(","));
    vector<double> coords;
    for(int i=0;i<coords_str.size();i++)
        if(i<3)
            coords.push_back(stof(coords_str[i])/cam_approx_scale);
        else
        {
            double value = stof(coords_str[i]);
            if(angle_metric=="degree")
                value=degreeToRadian(value);
            coords.push_back(value);
        }
    for(auto x:coords)
        cout<<x<<" ";
    cout<<endl;
    return coords;
}

Eigen::MatrixXd getPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{

    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);
    Eigen::MatrixXd parameters(1,4);
    parameters<<coefficients->values[0],coefficients->values[1],coefficients->values[2],coefficients->values[3];
    return parameters;
}

Eigen::Vector4f fitPlane(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  Eigen::MatrixXd lhs (cloud->size(), 3);
  Eigen::VectorXd rhs (cloud->size());
  for (size_t i = 0; i < cloud->size(); ++i)
  {
    const auto& pt = cloud->points[i];
    lhs(i, 0) = pt.x;
    lhs(i, 1) = pt.y;
    lhs(i, 2) = 1.0;

    rhs(i) = -1.0 * pt.z;
  }
  Eigen::Vector3d params = lhs.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(rhs);
  Eigen::Vector3d normal (params(0), params(1), 1.0);
  auto length = normal.norm();
  normal /= length;
  params(2) /= length;
  return {normal(0), normal(1), normal(2), params(2)};
}

double pointToPlaneDistance(vector<double> plane, vector<double> pt)
{
    return fabs(plane[0]*pt[0]+plane[1]*pt[1]+plane[2]*pt[2]+plane[3])/(sqrt(pow(plane[0],2)+pow(plane[1],2)+pow(plane[2],2)));
}

void Optimizer::getInputs()
{
    ifstream file;
    std::cout<<"Config File: "<<config_file<<endl;
    file.open(config_file);
    using boost::property_tree::ptree;
    ptree pt;
    read_xml(file, pt);
    string camera_metric = pt.get<std::string>("data.camera.metric","m");
    cout<<"------------- "<<camera_metric<<endl;
    for (const auto &cloud : pt.get_child("data.camera.clouds"))
    {
        string filename = cloud.second.data();
        ifstream file(filename);
        string line;
        for(int c=0;c<14&&getline(file,line);c++);
        PointCloudT::Ptr pointcloud(new PointCloudT);
        while(getline(file,line))
        {
            vector<string> values_from_file;
            boost::split(values_from_file, line, boost::is_any_of(" "));
            pcl::PointXYZRGB pt;
            pt.x = stof(values_from_file[0]);
            pt.y = stof(values_from_file[1]);
            pt.z = stof(values_from_file[2]);
            pt.r = stof(values_from_file[3]);
            pt.g = stof(values_from_file[4]);
            pt.b = stof(values_from_file[5]);
            pointcloud->points.push_back(pt);
        }
        PointCloudT::Ptr temp_cloud(new PointCloudT);
        for(int i=0;i<pointcloud->points.size();i++)
        {
            auto pt = pointcloud->points[i];
            if(pt.x==0.0&&pt.y==0.0&&pt.z==0.0)
                continue;
            temp_cloud->points.push_back(pt);
        }
        clouds.push_back(temp_cloud);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bw_temp(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*temp_cloud,*cloud_bw_temp);
#if 0
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud (cloud_bw_temp);
        constexpr double leaf = 0.0225f;
        // constexpr double leaf = 0.007f;
        sor.setLeafSize (leaf, leaf, leaf);
        sor.filter (*cloud_filtered);
#else
        double distance_threshold = pt.get<double>("data.camera.threshold",0.000001);
        auto plane = fitPlane(cloud_bw_temp);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZRGB>);
        static int counter = 0;
        counter++;
        double average = 0.0;
        double maxx = -1e9;
        for(auto pt:cloud_bw_temp->points)
        {
            auto distance_to_plane = pointToPlaneDistance({plane(0),plane(1),plane(2),plane(3)},{pt.x,pt.y,pt.z});
            pcl::PointXYZRGB pt_temp;
            pt_temp.x = pt.x;
            pt_temp.y = pt.y;
            pt_temp.z = pt.z;
            if(distance_to_plane*1000.0<1.0)
                pt_temp.g=255;
            else if(distance_to_plane*1000.0<2.5)
                pt_temp.b=255;
            else if(distance_to_plane*1000.0<3.5)
            {
                pt_temp.b=100;
                pt_temp.r=100;
                pt_temp.g=100;
            }
            else
                pt_temp.r=255;
            cloud_temp->points.push_back(pt_temp);
            if(distance_to_plane<distance_threshold)
            {
                cloud_filtered->points.push_back(pt);
            }
            if(distance_to_plane>maxx)
                maxx=distance_to_plane;
            average+=distance_to_plane;
        }
        cout<<counter<<","<<int((average/cloud_bw_temp->points.size())*100000.0)/100.0<<","<<int(maxx*100000.0)/100.0<<endl;
        pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
        viewer->setBackgroundColor (1.0,1.0,1.0);
        viewer->initCameraParameters ();
        viewer->addPointCloud<pcl::PointXYZRGB>(cloud_temp);
        pcl::ModelCoefficients::Ptr plane_1 (new pcl::ModelCoefficients);
        plane_1->values.resize (4);
        plane_1->values[0] = plane(0);
        plane_1->values[1] = plane(1);
        plane_1->values[2] = plane(2);
        plane_1->values[3] = plane(3);
        // viewer->addPlane (*plane_1, "plane_1", 0);
        while(!viewer->wasStopped())
            viewer->spinOnce(100);
#endif
        cloud_downsampled.push_back(cloud_filtered);
        // PointCloudT::Ptr pointcloud_output(new PointCloudT);
        // cloud_outputs.push_back(pointcloud_output);
    }
    string ik_filename  = pt.get<std::string>("data.camera.transformations.inverse_kinematics");
    cout<<ik_filename<<endl;
    inverse_kinematics = readTransformations(ik_filename,true);
    cout<<"Transformations Read"<<endl;
    auto transformation_initial_flange= getTransVector(pt,"data.camera.transformations.approximate_transformation");
    for(int i=0;i<transformation_initial_flange.size();i++)
    {
        flange_transformation[i]=transformation_initial_flange[i];
        flange_transformation_initial[i]=transformation_initial_flange[i];
    }

    for(int j=0;j<clouds.size();j++)
    {
        Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
        Eigen::MatrixXd transformation = inverse_kinematics[j]*cam_T_flange;
        Eigen::Affine3d trans;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                trans(a,b) = transformation(a,b);
        for(int i=0;i<clouds[j]->points.size();i++)
        {
            auto point = clouds[j]->points[i];
#if 1
            float src[3];
            float out[3];
            src[0] = point.x;
            src[1] = point.y;
            src[2] = point.z;
            apply_transformation_optimized(src,out,trans);
            pcl::PointXYZ pt;
            pt.x  = out[0];
            pt.y =  out[1];
            pt.z =  out[2];
            combined_cloud->points.push_back(pt);
#else
            pts(0,0)=point.x;
            pts(0,1)=point.y;
            pts(0,2)=point.z;
            pts=apply_transformation(pts,transformation);
            pts=apply_transformation(pts,world_T_object);
            Ns[j](0,i) = pts(0,0);
            Ns[j](1,i) = pts(0,1);
            Ns[j](2,i) = pts(0,2);
#endif
        }
    }
}

string k_Filename;
Optimizer opti;

int main(int argc, char** argv)
{
    if(argc<2)
    {
        std::cout<<"Usage: optimizer_test <config file>"<<endl;
        exit(-1);
    }
    outfile.open("new_experiments.txt", std::ios_base::app); // append instead of overwrite
    k_Filename = argv[1];
    errorfile.open("new_experiments_errors.txt", std::ios_base::app); // append instead of overwrite
    k_Filename = argv[1];
    outfile<<"Results: "<<k_Filename<<endl;
    errorfile<<"Results: "<<k_Filename<<endl;
    opti = Optimizer(k_Filename);
    opti.getInputs();
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2018 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_BOUNDARY_CHECKER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_BOUNDARY_CHECKER_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/sub_range.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

#include <boost/geometry/policies/compare.hpp>

#include <boost/geometry/util/has_nan_coordinate.hpp>
#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace relate {

enum boundary_query { boundary_front, boundary_back, boundary_any };

template <typename Geometry,
          typename WithinStrategy, // Point/Point equals (within) strategy
          typename Tag = typename geometry::tag<Geometry>::type>
class boundary_checker {};

template <typename Geometry, typename WithinStrategy>
class boundary_checker<Geometry, WithinStrategy, linestring_tag>
{
    typedef typename point_type<Geometry>::type point_type;

public:
    typedef WithinStrategy equals_strategy_type;

    boundary_checker(Geometry const& g)
        : has_boundary( boost::size(g) >= 2
                     && !detail::equals::equals_point_point(range::front(g),
                                                            range::back(g),
                                                            equals_strategy_type()) )
#ifdef BOOST_GEOMETRY_DEBUG_RELATE_BOUNDARY_CHECKER
        , geometry(g)
#endif
    {}

    template <boundary_query BoundaryQuery>
    bool is_endpoint_boundary(point_type const& pt) const
    {
        boost::ignore_unused(pt);
#ifdef BOOST_GEOMETRY_DEBUG_RELATE_BOUNDARY_CHECKER
        // may give false positives for INT
        BOOST_GEOMETRY_ASSERT( (BoundaryQuery == boundary_front || BoundaryQuery == boundary_any)
                   && detail::equals::equals_point_point(pt, range::front(geometry), WithinStrategy())
                   || (BoundaryQuery == boundary_back || BoundaryQuery == boundary_any)
                   && detail::equals::equals_point_point(pt, range::back(geometry), WithinStrategy()) );
#endif
        return has_boundary;
    }

private:
    bool has_boundary;
#ifdef BOOST_GEOMETRY_DEBUG_RELATE_BOUNDARY_CHECKER
    Geometry const& geometry;
#endif
};

template <typename Geometry, typename WithinStrategy>
class boundary_checker<Geometry, WithinStrategy, multi_linestring_tag>
{
    typedef typename point_type<Geometry>::type point_type;

public:
    typedef WithinStrategy equals_strategy_type;

    boundary_checker(Geometry const& g)
        : is_filled(false), geometry(g)
    {}

    // First call O(NlogN)
    // Each next call O(logN)
    template <boundary_query BoundaryQuery>
    bool is_endpoint_boundary(point_type const& pt) const
    {
        typedef typename boost::range_size<Geometry>::type size_type;
        size_type multi_count = boost::size(geometry);

        if ( multi_count < 1 )
            return false;

        if ( ! is_filled )
        {
            //boundary_points.clear();
            boundary_points.reserve(multi_count * 2);

            typedef typename boost::range_iterator<Geometry const>::type multi_iterator;
            for ( multi_iterator it = boost::begin(geometry) ;
                  it != boost::end(geometry) ; ++ it )
            {
                typename boost::range_reference<Geometry const>::type
                    ls = *it;

                // empty or point - no boundary
                if (boost::size(ls) < 2)
                {
                    continue;
                }

                typedef typename boost::range_reference
                    <
                        typename boost::range_value<Geometry const>::type const
                    >::type point_reference;

                point_reference front_pt = range::front(ls);
                point_reference back_pt = range::back(ls);

                // linear ring or point - no boundary
                if (! equals::equals_point_point(front_pt, back_pt, equals_strategy_type()))
                {
                    // do not add points containing NaN coordinates
                    // because they cannot be reasonably compared, e.g. with MSVC
                    // an assertion failure is reported in std::equal_range()
                    if (! geometry::has_nan_coordinate(front_pt))
                    {
                        boundary_points.push_back(front_pt);
                    }
                    if (! geometry::has_nan_coordinate(back_pt))
                    {
                        boundary_points.push_back(back_pt);
                    }
                }
            }

            std::sort(boundary_points.begin(),
                      boundary_points.end(),
                      geometry::less<point_type>());

            is_filled = true;
        }

        std::size_t equal_points_count
            = boost::size(
                std::equal_range(boundary_points.begin(),
                                 boundary_points.end(),
                                 pt,
                                 geometry::less<point_type>())
            );

        return equal_points_count % 2 != 0;// && equal_points_count > 0; // the number is odd and > 0
    }

private:
    mutable bool is_filled;
    // TODO: store references/pointers instead of points?
    mutable std::vector<point_type> boundary_points;

    Geometry const& geometry;
};

}} // namespace detail::relate
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_BOUNDARY_CHECKER_HPP