#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 "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;
}


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

#include "bempp/common/config_trilinos.hpp"

#ifdef WITH_TRILINOS

#include "default_iterative_solver.hpp"

#include "belos_solver_wrapper.hpp"
#include "solution.hpp"
#include "blocked_solution.hpp"
#include "../assembly/abstract_boundary_operator.hpp"
#include "../assembly/abstract_boundary_operator_pseudoinverse.hpp"
#include "../assembly/blocked_boundary_operator.hpp"
#include "../assembly/boundary_operator.hpp"
#include "../assembly/context.hpp"
#include "../assembly/discrete_boundary_operator.hpp"
#include "../assembly/discrete_boundary_operator_composition.hpp"
#include "../assembly/identity_operator.hpp"
#include "../assembly/vector.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../space/space.hpp"

#include <Teuchos_RCPBoostSharedPtrConversions.hpp>
#include <Thyra_DefaultSpmdVectorSpace.hpp>

#include <boost/make_shared.hpp>
#include <boost/variant.hpp>

#include <tbb/task_scheduler_init.h>

namespace Bempp {

template <typename ValueType>
Teuchos::RCP<Thyra::DefaultSpmdVector<ValueType>>
wrapInTrilinosVector(arma::Col<ValueType> &col) {
  size_t size = col.n_rows;
  Teuchos::ArrayRCP<ValueType> trilinosArray = Teuchos::arcp(
      col.memptr(), 0 /* lowerOffset */, size, false /* doesn't own memory */);
  typedef Thyra::DefaultSpmdVector<ValueType> TrilinosVector;
  return Teuchos::RCP<TrilinosVector>(
      new TrilinosVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                         trilinosArray, 1 /* stride */));
}

/** \cond HIDDEN_INTERNAL */

template <typename BasisFunctionType, typename ResultType>
struct DefaultIterativeSolver<BasisFunctionType, ResultType>::Impl {

  // Constructor for non-blocked operators
  Impl(const BoundaryOperator<BasisFunctionType, ResultType> &op_,
       ConvergenceTestMode::Mode mode_)
      : op(op_), mode(mode_) {
    typedef BoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;
    typedef Solver<BasisFunctionType, ResultType> Solver_;
    const BoundaryOp &boundaryOp = boost::get<BoundaryOp>(op);
    if (!boundaryOp.isInitialized())
      throw std::invalid_argument("DefaultIterativeSolver::Impl::Impl(): "
                                  "boundary operator must be initialized");

    if (mode == ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE) {
      if (boundaryOp.domain()->globalDofCount() !=
          boundaryOp.dualToRange()->globalDofCount())
        throw std::invalid_argument("DefaultIterativeSolver::Impl::Impl(): "
                                    "non-square system provided");

      solverWrapper.reset(new BelosSolverWrapper<ResultType>(
          Teuchos::rcp<const Thyra::LinearOpBase<ResultType>>(
              boundaryOp.weakForm())));
    } else if (mode == ConvergenceTestMode::TEST_CONVERGENCE_IN_RANGE) {
      if (boundaryOp.domain()->globalDofCount() !=
          boundaryOp.range()->globalDofCount())
        throw std::invalid_argument("DefaultIterativeSolver::Impl::Impl(): "
                                    "non-square system provided");

      BoundaryOp id =
          identityOperator(boundaryOp.context(), boundaryOp.range(),
                           boundaryOp.range(), boundaryOp.dualToRange());
      pinvId = pseudoinverse(id, boundaryOp.dualToRange());
      // dualToRange could be anything here.
      shared_ptr<DiscreteBoundaryOperator<ResultType>> totalBoundaryOp =
          boost::make_shared<DiscreteBoundaryOperatorComposition<ResultType>>(
              boost::get<BoundaryOp>(pinvId).weakForm(), boundaryOp.weakForm());
      solverWrapper.reset(new BelosSolverWrapper<ResultType>(
          Teuchos::rcp<const Thyra::LinearOpBase<ResultType>>(
              totalBoundaryOp)));
    } else
      throw std::invalid_argument(
          "DefaultIterativeSolver::DefaultIterativeSolver(): "
          "invalid convergence test mode");
  }

  // Constructor for blocked operators
  Impl(const BlockedBoundaryOperator<BasisFunctionType, ResultType> &op_,
       ConvergenceTestMode::Mode mode_)
      : op(op_), mode(mode_) {
    typedef BlockedBoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;
    typedef Solver<BasisFunctionType, ResultType> Solver_;
    const BoundaryOp &boundaryOp = boost::get<BoundaryOp>(op);

    if (mode == ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE) {
      if (boundaryOp.totalGlobalDofCountInDomains() !=
          boundaryOp.totalGlobalDofCountInDualsToRanges())
        throw std::invalid_argument("DefaultIterativeSolver::Impl::Impl(): "
                                    "non-square system provided");
      solverWrapper.reset(new BelosSolverWrapper<ResultType>(
          Teuchos::rcp<const Thyra::LinearOpBase<ResultType>>(
              boundaryOp.weakForm())));
    } else if (mode == ConvergenceTestMode::TEST_CONVERGENCE_IN_RANGE) {
      if (boundaryOp.totalGlobalDofCountInDomains() !=
          boundaryOp.totalGlobalDofCountInRanges())
        throw std::invalid_argument("DefaultIterativeSolver::Impl::Impl(): "
                                    "non-square system provided");

      // Construct a block-diagonal operator composed of pseudoinverses
      // for appropriate spaces
      BlockedOperatorStructure<BasisFunctionType, ResultType> pinvIdStructure;
      size_t rowCount = boundaryOp.rowCount();
      size_t columnCount = boundaryOp.columnCount();
      for (size_t row = 0; row < rowCount; ++row) {
        // Find the first non-zero block in row #row and retrieve its context
        shared_ptr<const Context<BasisFunctionType, ResultType>> context;
        for (int col = 0; col < columnCount; ++col)
          if (boundaryOp.block(row, col).context()) {
            context = boundaryOp.block(row, col).context();
            break;
          }
        assert(context);

        BoundaryOperator<BasisFunctionType, ResultType> id = identityOperator(
            context, boundaryOp.range(row), boundaryOp.range(row),
            boundaryOp.dualToRange(row));
        pinvIdStructure.setBlock(row, row, pseudoinverse(id));
      }
      pinvId = BlockedBoundaryOperator<BasisFunctionType, ResultType>(
          pinvIdStructure);

      shared_ptr<DiscreteBoundaryOperator<ResultType>> totalBoundaryOp =
          boost::make_shared<DiscreteBoundaryOperatorComposition<ResultType>>(
              boost::get<BoundaryOp>(pinvId).weakForm(), boundaryOp.weakForm());
      solverWrapper.reset(new BelosSolverWrapper<ResultType>(
          Teuchos::rcp<const Thyra::LinearOpBase<ResultType>>(
              totalBoundaryOp)));
    } else
      throw std::invalid_argument(
          "DefaultIterativeSolver::DefaultIterativeSolver(): "
          "invalid convergence test mode");
  }

  boost::variant<BoundaryOperator<BasisFunctionType, ResultType>,
                 BlockedBoundaryOperator<BasisFunctionType, ResultType>> op;
  ConvergenceTestMode::Mode mode;
  boost::scoped_ptr<BelosSolverWrapper<ResultType>> solverWrapper;
  boost::variant<BoundaryOperator<BasisFunctionType, ResultType>,
                 BlockedBoundaryOperator<BasisFunctionType, ResultType>> pinvId;
};

/** \endcond */

template <typename BasisFunctionType, typename ResultType>
DefaultIterativeSolver<BasisFunctionType, ResultType>::DefaultIterativeSolver(
    const BoundaryOperator<BasisFunctionType, ResultType> &boundaryOp,
    ConvergenceTestMode::Mode mode)
    : m_impl(new Impl(boundaryOp, mode)) {}

template <typename BasisFunctionType, typename ResultType>
DefaultIterativeSolver<BasisFunctionType, ResultType>::DefaultIterativeSolver(
    const BlockedBoundaryOperator<BasisFunctionType, ResultType> &boundaryOp,
    ConvergenceTestMode::Mode mode)
    : m_impl(new Impl(boundaryOp, mode)) {}

template <typename BasisFunctionType, typename ResultType>
DefaultIterativeSolver<BasisFunctionType,
                       ResultType>::~DefaultIterativeSolver() {}

template <typename BasisFunctionType, typename ResultType>
void DefaultIterativeSolver<BasisFunctionType, ResultType>::setPreconditioner(
    const Preconditioner<ResultType> &preconditioner) {
  m_impl->solverWrapper->setPreconditioner(preconditioner.get());
}

template <typename BasisFunctionType, typename ResultType>
void DefaultIterativeSolver<BasisFunctionType, ResultType>::initializeSolver(
    const Teuchos::RCP<Teuchos::ParameterList> &paramList) {
  m_impl->solverWrapper->initializeSolver(paramList);
}

template <typename BasisFunctionType, typename ResultType>
void DefaultIterativeSolver<BasisFunctionType, ResultType>::initializeSolver(
    const Teuchos::RCP<Teuchos::ParameterList> &paramList,
    const Preconditioner<ResultType> &preconditioner) {
  m_impl->solverWrapper->setPreconditioner(preconditioner.get());
  m_impl->solverWrapper->initializeSolver(paramList);
}

template <typename BasisFunctionType, typename ResultType>
Solution<BasisFunctionType, ResultType>
DefaultIterativeSolver<BasisFunctionType, ResultType>::solveImplNonblocked(
    const GridFunction<BasisFunctionType, ResultType> &rhs) const {
  typedef BoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;
  typedef typename ScalarTraits<ResultType>::RealType MagnitudeType;
  typedef Thyra::MultiVectorBase<ResultType> TrilinosVector;

  const BoundaryOp *boundaryOp = boost::get<BoundaryOp>(&m_impl->op);
  if (!boundaryOp)
    throw std::logic_error(
        "DefaultIterativeSolver::solve(): for solvers constructed "
        "from a BlockedBoundaryOperator the other solve() overload "
        "must be used");
  Solver<BasisFunctionType, ResultType>::checkConsistency(*boundaryOp, rhs,
                                                          m_impl->mode);

  // Construct rhs vector
  Vector<ResultType> projectionsVector(
      rhs.projections(boundaryOp->dualToRange()));
  Teuchos::RCP<TrilinosVector> rhsVector;
  if (m_impl->mode == ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE)
    rhsVector = Teuchos::rcpFromRef(projectionsVector);
  else {
    const size_t size = boundaryOp->range()->globalDofCount();
    rhsVector.reset(new Vector<ResultType>(size));
    boost::get<BoundaryOp>(m_impl->pinvId).weakForm()->apply(
        Thyra::NOTRANS, projectionsVector, rhsVector.ptr(), 1., 0.);
  }

  // Construct solution vector
  arma::Col<ResultType> armaSolution(rhsVector->range()->dim());
  armaSolution.fill(static_cast<ResultType>(0.));
  Teuchos::RCP<TrilinosVector> solutionVector =
      wrapInTrilinosVector(armaSolution);

  // Get number of threads
  Fiber::ParallelizationOptions parallelOptions =
      boundaryOp->context()->assemblyOptions().parallelizationOptions();
  int maxThreadCount = 1;
  if (!parallelOptions.isOpenClEnabled()) {
    if (parallelOptions.maxThreadCount() == ParallelizationOptions::AUTO)
      maxThreadCount = tbb::task_scheduler_init::automatic;
    else
      maxThreadCount = parallelOptions.maxThreadCount();
  }

  // Solve
  Thyra::SolveStatus<MagnitudeType> status;
  {
    // Initialize TBB threads here (to prevent their construction and
    // destruction on every matrix-vector multiplication)
    tbb::task_scheduler_init scheduler(maxThreadCount);
    status = m_impl->solverWrapper->solve(Thyra::NOTRANS, *rhsVector,
                                          solutionVector.ptr());
  }

  // Construct grid function and return
  return Solution<BasisFunctionType, ResultType>(
      GridFunction<BasisFunctionType, ResultType>(
          boundaryOp->context(), boundaryOp->domain(), armaSolution),
      status);
}

template <typename BasisFunctionType, typename ResultType>
BlockedSolution<BasisFunctionType, ResultType>
DefaultIterativeSolver<BasisFunctionType, ResultType>::solveImplBlocked(
    const std::vector<GridFunction<BasisFunctionType, ResultType>> &rhs) const {
  typedef BlockedBoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;
  typedef typename ScalarTraits<ResultType>::RealType MagnitudeType;
  typedef Thyra::MultiVectorBase<ResultType> TrilinosVector;

  const BoundaryOp *boundaryOp = boost::get<BoundaryOp>(&m_impl->op);
  if (!boundaryOp)
    throw std::logic_error(
        "DefaultIterativeSolver::solve(): for solvers constructed "
        "from a (non-blocked) BoundaryOperator the other solve() overload "
        "must be used");
  std::vector<GridFunction<BasisFunctionType, ResultType>> canonicalRhs =
      Solver<BasisFunctionType, ResultType>::canonicalizeBlockedRhs(
          *boundaryOp, rhs, m_impl->mode);
  // Shouldn't be needed, but better safe than sorry...
  Solver<BasisFunctionType, ResultType>::checkConsistency(
      *boundaryOp, canonicalRhs, m_impl->mode);

  // Currently we only support convergence testing in space dual to range.

  // Construct the right-hand-side vector
  arma::Col<ResultType> armaProjections(
      boundaryOp->totalGlobalDofCountInDualsToRanges());
  for (size_t i = 0, start = 0; i < canonicalRhs.size(); ++i) {
    const arma::Col<ResultType> &chunkProjections =
        canonicalRhs[i].projections(boundaryOp->dualToRange(i));
    size_t chunkSize = chunkProjections.n_rows;
    armaProjections.rows(start, start + chunkSize - 1) = chunkProjections;
    start += chunkSize;
  }

  Vector<ResultType> projectionsVector(armaProjections);
  Teuchos::RCP<TrilinosVector> rhsVector;
  if (m_impl->mode == ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE)
    rhsVector = Teuchos::rcpFromRef(projectionsVector);
  else {
    const size_t rhsSize = boundaryOp->totalGlobalDofCountInRanges();
    rhsVector.reset(new Vector<ResultType>(rhsSize));
    boost::get<BoundaryOp>(m_impl->pinvId).weakForm()->apply(
        Thyra::NOTRANS, projectionsVector, rhsVector.ptr(), 1., 0.);
  }

  // Initialize the solution vector
  size_t solutionSize = 0;
  for (size_t i = 0; i < canonicalRhs.size(); ++i)
    solutionSize += boundaryOp->domain(i)->globalDofCount();
  arma::Col<ResultType> armaSolution(solutionSize);
  armaSolution.fill(static_cast<ResultType>(0.));
  Teuchos::RCP<TrilinosVector> solutionVector =
      wrapInTrilinosVector(armaSolution);

  // Get context of the first non-empty operator
  size_t rowCount = boundaryOp->rowCount();
  shared_ptr<const Context<BasisFunctionType, ResultType>> context;
  for (size_t row = 0; row < rowCount; ++row)
    if (boundaryOp->block(row, 0).context()) {
      context = boundaryOp->block(row, 0).context();
      break;
    }
  assert(context);

  // Get number of threads
  Fiber::ParallelizationOptions parallelOptions =
      context->assemblyOptions().parallelizationOptions();
  int maxThreadCount = 1;
  if (!parallelOptions.isOpenClEnabled()) {
    if (parallelOptions.maxThreadCount() == ParallelizationOptions::AUTO)
      maxThreadCount = tbb::task_scheduler_init::automatic;
    else
      maxThreadCount = parallelOptions.maxThreadCount();
  }

  // Solve
  Thyra::SolveStatus<MagnitudeType> status;
  {
    // Initialize TBB threads here (to prevent their construction and
    // destruction on every matrix-vector multiplication)
    tbb::task_scheduler_init scheduler(maxThreadCount);
    status = m_impl->solverWrapper->solve(Thyra::NOTRANS, *rhsVector,
                                          solutionVector.ptr());
  }

  // Convert chunks of the solution vector into grid functions
  std::vector<GridFunction<BasisFunctionType, ResultType>> solutionFunctions;
  Solver<BasisFunctionType, ResultType>::constructBlockedGridFunction(
      armaSolution, *boundaryOp, solutionFunctions);

  // Return solution
  return BlockedSolution<BasisFunctionType, ResultType>(solutionFunctions,
                                                        status);
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(DefaultIterativeSolver);

} // namespace Bempp

#endif // WITH_TRILINOS


/* decision_tree.cc                                                -*- C++ -*-
   Jeremy Barnes, 22 March 2004
   Copyright (c) 2004 Jeremy Barnes.  All rights reserved.
   $Source$

   Implementation of the decision tree.
*/

#include "decision_tree.h"
#include "classifier_persist_impl.h"
#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <functional>
#include "jml/utils/vector_utils.h"
#include "config_impl.h"
#include "jml/utils/exc_assert.h"
#include "jml/utils/smart_ptr_utils.h"

using namespace std;
using namespace DB;

namespace ML {

/*****************************************************************************/
/* DECISION_TREE                                                             */
/*****************************************************************************/

Decision_Tree::Decision_Tree()
    : encoding(OE_PROB), optimized_(false)
{
}

Decision_Tree::
Decision_Tree(DB::Store_Reader & store,
              const std::shared_ptr<const Feature_Space> & fs)
    : optimized_(false)
{
    throw Exception("Decision_Tree constructor(reconst): not implemented");
}

Decision_Tree::
Decision_Tree(std::shared_ptr<const Feature_Space> feature_space,
              const Feature & predicted)
    : Classifier_Impl(feature_space, predicted),
      encoding(OE_PROB),
      optimized_(false)
{
}

Decision_Tree::
~Decision_Tree()
{
}

void
Decision_Tree::
swap(Decision_Tree & other)
{
    Classifier_Impl::swap(other);
    std::swap(tree, other.tree);
    std::swap(encoding, other.encoding);
    std::swap(optimized_, other.optimized_);
}

namespace {

struct StandardGetFeatures {
    StandardGetFeatures(const Feature_Set & features)
        : features(features)
    {
    }

    const Feature_Set & features;

    Split::Weights operator () (const Split & split) const
    {
        return split.apply(features);
    }
};

struct OptimizedGetFeatures {
    OptimizedGetFeatures(const float * features)
        : features(features)
    {
    }

    const float * features;

    JML_ALWAYS_INLINE Split::Weights operator () (const Split & split) const
    {
        return split.apply(features);
    }

};

struct AccumResults {
    explicit AccumResults(double * accum, int nl, double weight)
        : accum(accum), nl(nl), weight(weight)
    {
    }

    double * accum;
    int nl;
    double weight;

    JML_ALWAYS_INLINE
    void operator () (const Label_Dist & dist, float weight1)
    {
        if (JML_LIKELY(nl == 2)) {
            double factor = weight1 * weight;
            accum[0] += dist[0] * factor;
            accum[1] += dist[1] * factor;
            return;
        }

        for (unsigned i = 0;  i < nl;  ++i)
            accum[i] += dist[i] * weight1 * weight;
    }
};

struct DistResults {
    explicit DistResults(double * accum, int nl)
        : accum(accum), nl(nl)
    {
        std::fill(accum, accum + nl, 0.0);
    }

    double * accum;
    int nl;

    JML_ALWAYS_INLINE
    void operator () (const Label_Dist & dist, float weight)
    {
        for (unsigned i = 0;  i < nl;  ++i)
            accum[i] += dist[i] * weight;
    }

    operator Label_Dist () const { return Label_Dist(accum, accum + nl); }
};

struct LabelResults {
    explicit LabelResults(int label)
        : label(label), result(0.0)
    {
    }

    JML_ALWAYS_INLINE
    void operator () (const Label_Dist & dist, float weight)
    {
        result += weight * dist[label];
    }

    int label;
    double result;

    operator double () const { return result; }
};

} // file scope

float
Decision_Tree::
predict(int label, const Feature_Set & features,
        PredictionContext * context) const
{
    StandardGetFeatures get_features(features);
    LabelResults results(label);

    predict_recursive_impl(get_features, results, tree.root);
    return results;
}

Label_Dist
Decision_Tree::
predict(const Feature_Set & features,
        PredictionContext * context) const
{
    StandardGetFeatures get_features(features);
    int nl = label_count();
    double accum[nl];
    DistResults results(accum, nl);

    predict_recursive_impl(get_features, results, tree.root);
    return results;
}

bool
Decision_Tree::
optimization_supported() const
{
    return true;
}

bool
Decision_Tree::
predict_is_optimized() const
{
    return optimized_;
}

bool
Decision_Tree::
optimize_impl(Optimization_Info & info)
{
    optimize_recursive(info, tree.root);
    optimized_ = true;
    return true;
}

void
Decision_Tree::
optimize_recursive(Optimization_Info & info,
                   const Tree::Ptr & ptr)
{
    if (!ptr) return;
    if (!ptr.node()) return;

    Tree::Node & node = *ptr.node();

    node.split.optimize(info);
    optimize_recursive(info, node.child_true);
    optimize_recursive(info, node.child_false);
    optimize_recursive(info, node.child_missing);
}

Label_Dist
Decision_Tree::
optimized_predict_impl(const float * features,
                       const Optimization_Info & info,
                       PredictionContext * context) const
{
    OptimizedGetFeatures get_features(features);

    int nl = label_count();
    double accum[nl];
    DistResults results(accum, nl);

    predict_recursive_impl(get_features, results, tree.root);
    return results;
}

void
Decision_Tree::
optimized_predict_impl(const float * features,
                       const Optimization_Info & info,
                       double * accum,
                       double weight,
                       PredictionContext * context) const
{
    OptimizedGetFeatures get_features(features);
    AccumResults results(accum, label_count(), weight);

    predict_recursive_impl(get_features, results, tree.root);
}

float
Decision_Tree::
optimized_predict_impl(int label,
                       const float * features,
                       const Optimization_Info & info,
                       PredictionContext * context) const
{
    OptimizedGetFeatures get_features(features);
    LabelResults results(label);

    predict_recursive_impl(get_features, results, tree.root);
    return results;
}

template<class GetFeatures, class Results>
void
Decision_Tree::
predict_recursive_impl(const GetFeatures & get_features,
                       Results & results,
                       const Tree::Ptr & ptr,
                       double weight) const
{
    if (!ptr) return;

    if (!ptr.node()) {
        results(ptr.leaf()->pred, weight);
        return;
    }

    const Tree::Node & node = *ptr.node();

    Split::Weights weights = get_features(node.split);

    /* Go down all of the edges that we need to for this example. */
    if (weights[true] > 0.0)
        predict_recursive_impl(get_features, results, node.child_true,
                               weights[true]);
    if (weights[false] > 0.0)
        predict_recursive_impl(get_features, results, node.child_false,
                               weights[false]);
    if (weights[MISSING] > 0.0)
        predict_recursive_impl(get_features, results, node.child_missing,
                               weights[MISSING]);
}

std::string
printLabels(const distribution<float> & dist)
{
    string result = "";
    for (unsigned i = 0;  i < dist.size();  ++i)
        if (dist[i] != 0.0) result += format(" %d/%.3f", i, dist[i]);
    return result;
}

std::string
printLabels(const Tree::Ptr & ptr)
{
    return printLabels(ptr.pred());
}

std::string
Decision_Tree::
print() const
{
    string result = "Decision Tree: ";
    float total_weight = tree.root.examples();
    if (tree.root.node()) total_weight = tree.root.examples();
    result = format("Decision Tree: (weight = %.2f, cov = %.2f%%) ",
                    total_weight, 100.0);
    result += printLabels(tree.root) + "\n";
    result += print_recursive(0, tree.root, total_weight);
    return result;
}

std::string
Decision_Tree::
summary() const
{
    if (!tree.root)
        return "NULL";

    float total_weight = 0.0;
    if (tree.root.node()) total_weight = tree.root.node()->examples;

    if (tree.root.node()) {
        Tree::Node & n = *tree.root.node();
        float cov = n.examples / total_weight;
        float z_adj = n.z / cov;
        return "Root: " + n.split.print(*feature_space())
            + format(" (z = %.4f)", z_adj);
    }
    else {
        string result = "leaf: ";
        Tree::Leaf & l = *tree.root.leaf();
        const distribution<float> & dist = l.pred;
        for (unsigned i = 0;  i < dist.size();  ++i)
            if (dist[i] != 0.0) result += format(" %d/%.3f", i, dist[i]);
        return result;
    }
}

string
Decision_Tree::
print_recursive(int level, const Tree::Ptr & ptr,
                float total_weight) const
{
    string spaces(level * 4, ' ');
    if (ptr.node()) {
        Tree::Node & n = *ptr.node();
        string result;
        float z_cov = n.examples / total_weight;
        float z_adj = n.z / z_cov;

        if (n.child_false && n.child_false.examples() > 0) {
            float cov = n.child_false.examples() / total_weight;
            result += spaces
                + format(" %s (z = %.4f, weight = %.2f, cov = %.2f%%) ",
                         n.split.print(*feature_space(), false).c_str(),
                         z_adj, n.child_false.examples(), cov * 100.0);
            result += printLabels(n.child_false) + "\n";
            result += print_recursive(level + 1, n.child_false, total_weight);
        }

        if (n.child_true && n.child_true.examples() > 0) {
            float cov = n.child_true.examples() / total_weight;
            result += spaces
                + format(" %s (z = %.4f, weight = %.2f, cov = %.2f%%) ",
                         n.split.print(*feature_space(), true).c_str(),
                         z_adj, n.child_true.examples(), cov * 100.0);
            result += printLabels(n.child_true) + "\n";
            result += print_recursive(level + 1, n.child_true, total_weight);
        }

        if (n.child_missing && n.child_missing.examples() > 0) {
            float cov = n.child_missing.examples() / total_weight;
            result += spaces
                + format(" %s (z = %.4f, weight = %.2f, cov = %.2f%%) ",
                         n.split.print(*feature_space(), MISSING).c_str(),
                         z_adj, n.child_missing.examples(), cov * 100.0);
            result += printLabels(n.child_missing) + "\n";
            result += print_recursive(level + 1, n.child_missing, total_weight);
        }
        return result;
    }
    else if (ptr.leaf()) {
        return "";
        string result = spaces + "leaf ";
        Tree::Leaf & l = *ptr.leaf();
        const distribution<float> & dist = l.pred;
        float cov = l.examples / total_weight;
        result += format(" (weight = %.2f, cov = %.2f%%) ",
                         l.examples, cov * 100.0);
        result += printLabels(dist);
        result += "\n";
        return result;
    }
    else return spaces + "NULL";
}

Explanation
Decision_Tree::
explain(const Feature_Set & feature_set,
        int label,
        double weight,
        PredictionContext * context) const
{
    Explanation result(feature_space(), weight);

    explain_recursive(result, feature_set, label, weight, tree.root, 0);

    return result;
}

void
Decision_Tree::
explain_recursive(Explanation & explanation,
                  const Feature_Set & feature_set,
                  int label,
                  double weight,
                  const Tree::Ptr & ptr,
                  const Tree::Node * parent) const
{
    StandardGetFeatures get_features(feature_set);
    int nl = label_count();

    if (label < 0 || label >= nl)
        throw Exception("Decision_Tree::explain(): no label");

    if (!ptr) return;

    if (!ptr.node()) {
        // It's a leaf; we give the difference to the parent's feature
        if (parent) {
            explanation.feature_weights[parent->split.feature()]
                += weight
                * (ptr.leaf()->pred.at(label) - parent->pred.at(label));
        }
        else {
            // No parent, therefore it's all bias
            explanation.bias += weight * (ptr.leaf()->pred.at(label));
        }

        return;
    }

    const Tree::Node & node = *ptr.node();

    Split::Weights weights = get_features(node.split);

    // Accumulate the weight for this split
    if (!parent)
        explanation.bias += weight * node.pred.at(label);
    else
        explanation.feature_weights[parent->split.feature()]
            += weight
            * (node.pred.at(label) - parent->pred.at(label));

    /* Go down all of the edges that we need to for this example. */
    if (weights[true] > 0.0)
        explain_recursive(explanation, feature_set, label,
                          weight * weights[true],
                          node.child_true, &node);

    if (weights[false] > 0.0)
        explain_recursive(explanation, feature_set, label,
                          weight * weights[false],
                          node.child_false, &node);

    if (weights[MISSING] > 0.0)
        explain_recursive(explanation, feature_set, label,
                          weight * weights[MISSING],
                          node.child_missing, &node);
}

Disjunction<Tree::Leaf>
Decision_Tree::
to_rules() const
{
    Disjunction<Tree::Leaf> result;
    result.feature_space = feature_space();
    std::vector<std::shared_ptr<Predicate> > path;

    to_rules_recursive(result, path, tree.root);

    ExcAssert(path.empty());

    return result;
}

void
Decision_Tree::
to_rules_recursive(Disjunction<Tree::Leaf> & result,
                   std::vector<std::shared_ptr<Predicate> > & path,
                   const Tree::Ptr & ptr) const
{
    if (ptr.examples() == 0.0) return;

    if (ptr.node()) {
        const Tree::Node & node = *ptr.node();

        {
            path.push_back(make_sp(new Predicate(node.split, false)));
            to_rules_recursive(result, path, node.child_false);
            path.pop_back();
        }

        {
            path.push_back(make_sp(new Predicate(node.split, true)));
            to_rules_recursive(result, path, node.child_true);
            path.pop_back();
        }

        {
            path.push_back(make_sp(new Predicate(node.split, MISSING)));
            to_rules_recursive(result, path, node.child_missing);
            path.pop_back();
        }
    }
    else if (ptr.leaf()) {
        std::shared_ptr<Conjunction<Tree::Leaf> > c
            (new Conjunction<Tree::Leaf>());
        c->predicates = path;
        c->outcome = *ptr.leaf();
        result.predicates.push_back(c);
    }
}

namespace {

void all_features_recursive(const Tree::Ptr & ptr,
                            vector<Feature> & result)
{
    if (ptr.node()) {
        Tree::Node & n = *ptr.node();
        result.push_back(n.split.feature());

        all_features_recursive(n.child_true,    result);
        all_features_recursive(n.child_false,   result);
        all_features_recursive(n.child_missing, result);
    }
}

} // file scope

std::vector<ML::Feature>
Decision_Tree::
all_features() const
{
    std::vector<ML::Feature> result;
    all_features_recursive(tree.root, result);
    make_vector_set(result);
    return result;
}

Output_Encoding
Decision_Tree::
output_encoding() const
{
    return encoding;
}

void
Decision_Tree::
serialize(DB::Store_Writer & store) const
{
    store << string("DECISION_TREE");
    store << compact_size_t(3);  // version
    store << compact_size_t(label_count());
    feature_space_->serialize(store, predicted_);
    tree.serialize(store, *feature_space());
    store << encoding;
    store << compact_size_t(12345);  // end marker
}

void
Decision_Tree::
reconstitute(DB::Store_Reader & store,
             const std::shared_ptr<const Feature_Space> & feature_space)
{
    string id;
    store >> id;

    if (id != "DECISION_TREE")
        throw Exception("Decision_Tree::reconstitute: read bad ID '"
                        + id + "'");

    compact_size_t version(store);

    switch (version) {
    case 1: {
        compact_size_t label_count(store);
        Classifier_Impl::init(feature_space, MISSING_FEATURE, label_count);
        tree.reconstitute(store, *feature_space);
        break;
    }
    case 2:
    case 3: {
        compact_size_t label_count(store);
        feature_space->reconstitute(store, predicted_);
        Classifier_Impl::init(feature_space, predicted_);
        tree.reconstitute(store, *feature_space);
        if (version >= 3)
            store >> encoding;
        else encoding = OE_PROB;
        break;
    }
    default:
        throw Exception("Decision tree: Attempt to reconstitute tree of "
                        "unknown version " + ostream_format(version.size_));
    }

    compact_size_t marker(store);
    if (marker != 12345)
        throw Exception("Decision_Tree::reconstitute: read bad marker at end");

    optimized_ = false;
}

std::string
Decision_Tree::
class_id() const
{
    return "DECISION_TREE";
}

Decision_Tree *
Decision_Tree::
make_copy() const
{
    return new Decision_Tree(*this);
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Impl, Decision_Tree> REGISTER("DECISION_TREE");

} // file scope

} // namespace ML


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_FOUC_S_HPP
#define BOOST_GEOMETRY_PROJECTIONS_FOUC_S_HPP

#include <boost/geometry/srs/projections/impl/aasincos.hpp>
#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace fouc_s
    {

            static const int max_iter = 10;
            static const double loop_tol = 1e-7;

            template <typename T>
            struct par_fouc_s
            {
                T n, n1;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_fouc_s_spheroid
                : public base_t_fi<base_fouc_s_spheroid<T, Parameters>, T, Parameters>
            {
                par_fouc_s<T> m_proj_parm;

                inline base_fouc_s_spheroid(const Parameters& par)
                    : base_t_fi<base_fouc_s_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T t;

                    t = cos(lp_lat);
                    xy_x = lp_lon * t / (this->m_proj_parm.n + this->m_proj_parm.n1 * t);
                    xy_y = this->m_proj_parm.n * lp_lat + this->m_proj_parm.n1 * sin(lp_lat);
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static T const half_pi = detail::half_pi<T>();

                    T V;
                    int i;

                    if (this->m_proj_parm.n != 0.0) {
                        lp_lat = xy_y;
                        for (i = max_iter; i ; --i) {
                            lp_lat -= V = (this->m_proj_parm.n * lp_lat + this->m_proj_parm.n1 * sin(lp_lat) - xy_y ) /
                                (this->m_proj_parm.n + this->m_proj_parm.n1 * cos(lp_lat));
                            if (fabs(V) < loop_tol)
                                break;
                        }
                        if (!i)
                            lp_lat = xy_y < 0. ? -half_pi : half_pi;
                    } else
                        lp_lat = aasin(xy_y);
                    V = cos(lp_lat);
                    lp_lon = xy_x * (this->m_proj_parm.n + this->m_proj_parm.n1 * V) / V;
                }

                static inline std::string get_name()
                {
                    return "fouc_s_spheroid";
                }

            };

            // Foucaut Sinusoidal
            template <typename Params, typename Parameters, typename T>
            inline void setup_fouc_s(Params const& params, Parameters& par, par_fouc_s<T>& proj_parm)
            {
                proj_parm.n = pj_get_param_f<T, srs::spar::n>(params, "n", srs::dpar::n);
                if ((proj_parm.n < 0.) || (proj_parm.n > 1.))
                    BOOST_THROW_EXCEPTION( projection_exception(error_n_out_of_range) );

                proj_parm.n1 = 1. - proj_parm.n;
                par.es = 0;
            }

    }} // namespace detail::fouc_s
    #endif // doxygen

    /*!
        \brief Foucaut Sinusoidal projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Projection parameters
         - n (real)
        \par Example
        \image html ex_fouc_s.gif
    */
    template <typename T, typename Parameters>
    struct fouc_s_spheroid : public detail::fouc_s::base_fouc_s_spheroid<T, Parameters>
    {
        template <typename Params>
        inline fouc_s_spheroid(Params const& params, Parameters const& par)
            : detail::fouc_s::base_fouc_s_spheroid<T, Parameters>(par)
        {
            detail::fouc_s::setup_fouc_s(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_fouc_s, fouc_s_spheroid, fouc_s_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(fouc_s_entry, fouc_s_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(fouc_s_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(fouc_s, fouc_s_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_FOUC_S_HPP


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

namespace graphene { namespace chain {
      typedef boost::multiprecision::uint128_t uint128_t;
      typedef boost::multiprecision::int128_t  int128_t;

      bool operator == ( const price& a, const price& b )
      {
         if( std::tie( a.base.asset_id, a.quote.asset_id ) != std::tie( b.base.asset_id, b.quote.asset_id ) )
            return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult == bmult;
      }

      bool operator < ( const price& a, const price& b )
      {
         if( a.base.asset_id < b.base.asset_id ) return true;
         if( a.base.asset_id > b.base.asset_id ) return false;
         if( a.quote.asset_id < b.quote.asset_id ) return true;
         if( a.quote.asset_id > b.quote.asset_id ) return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult < bmult;
      }

      asset operator * ( const asset& a, const price& b )
      {
         if( a.asset_id == b.base.asset_id )
         {
            FC_ASSERT( b.base.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.quote.amount.value)/b.base.amount.value;
            FC_ASSERT( result <= GRAPHENE_MAX_SHARE_SUPPLY );
            return asset( result.convert_to<int64_t>(), b.quote.asset_id );
         }
         else if( a.asset_id == b.quote.asset_id )
         {
            FC_ASSERT( b.quote.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.base.amount.value)/b.quote.amount.value;
            FC_ASSERT( result <= GRAPHENE_MAX_SHARE_SUPPLY );
            return asset( result.convert_to<int64_t>(), b.base.asset_id );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset * price", ("asset",a)("price",b) );
      }

      asset asset::multiply_and_round_up( const price& b )const
      {
         const asset& a = *this;
         if( a.asset_id == b.base.asset_id )
         {
            FC_ASSERT( b.base.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.quote.amount.value + b.base.amount.value - 1)/b.base.amount.value;
            FC_ASSERT( result <= GRAPHENE_MAX_SHARE_SUPPLY );
            return asset( result.convert_to<int64_t>(), b.quote.asset_id );
         }
         else if( a.asset_id == b.quote.asset_id )
         {
            FC_ASSERT( b.quote.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.base.amount.value + b.quote.amount.value - 1)/b.quote.amount.value;
            FC_ASSERT( result <= GRAPHENE_MAX_SHARE_SUPPLY );
            return asset( result.convert_to<int64_t>(), b.base.asset_id );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset::multiply_and_round_up(price)", ("asset",a)("price",b) );
      }

      price operator / ( const asset& base, const asset& quote )
      { try {
         FC_ASSERT( base.asset_id != quote.asset_id );
         return price{base,quote};
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      price price::max( asset_id_type base, asset_id_type quote ) { return asset( share_type(GRAPHENE_MAX_SHARE_SUPPLY), base ) / asset( share_type(1), quote); }
      price price::min( asset_id_type base, asset_id_type quote ) { return asset( 1, base ) / asset( GRAPHENE_MAX_SHARE_SUPPLY, quote); }

      price operator *  ( const price& p, const ratio_type& r )
      { try {
         p.validate();

         FC_ASSERT( r.numerator() > 0 && r.denominator() > 0 );

         if( r.numerator() == r.denominator() ) return p;

         boost::rational<int128_t> p128( p.base.amount.value, p.quote.amount.value );
         boost::rational<int128_t> r128( r.numerator(), r.denominator() );
         auto cp = p128 * r128;
         auto ocp = cp;

         bool shrinked = false;
         bool using_max = false;
         static const int128_t max( GRAPHENE_MAX_SHARE_SUPPLY );
         while( cp.numerator() > max || cp.denominator() > max )
         {
            if( cp.numerator() == 1 )
            {
               cp = boost::rational<int128_t>( 1, max );
               using_max = true;
               break;
            }
            else if( cp.denominator() == 1 )
            {
               cp = boost::rational<int128_t>( max, 1 );
               using_max = true;
               break;
            }
            else
            {
               cp = boost::rational<int128_t>( cp.numerator() >> 1, cp.denominator() >> 1 );
               shrinked = true;
            }
         }
         if( shrinked ) // maybe not accurate enough due to rounding, do additional checks here
         {
            int128_t num = ocp.numerator();
            int128_t den = ocp.denominator();
            if( num > den )
            {
               num /= den;
               if( num > max )
                  num = max;
               den = 1;
            }
            else
            {
               den /= num;
               if( den > max )
                  den = max;
               num = 1;
            }
            boost::rational<int128_t> ncp( num, den );
            if( num == max || den == max ) // it's on the edge, we know it's accurate enough
               cp = ncp;
            else
            {
               // from the accurate ocp, now we have ncp and cp. use the one which is closer to ocp.
               // TODO improve performance
               auto diff1 = abs( ncp - ocp );
               auto diff2 = abs( cp - ocp );
               if( diff1 < diff2 ) cp = ncp;
            }
         }

         price np = asset( cp.numerator().convert_to<int64_t>(), p.base.asset_id )
                  / asset( cp.denominator().convert_to<int64_t>(), p.quote.asset_id );

         if( shrinked || using_max )
         {
            if( ( r.numerator() > r.denominator() && np < p )
                  || ( r.numerator() < r.denominator() && np > p ) )
               // even with an accurate result, if p is out of valid range, return it
               np = p;
         }

         np.validate();
         return np;
      } FC_CAPTURE_AND_RETHROW( (p)(r.numerator())(r.denominator()) ) }

      price operator /  ( const price& p, const ratio_type& r )
      { try {
         return p * ratio_type( r.denominator(), r.numerator() );
      } FC_CAPTURE_AND_RETHROW( (p)(r.numerator())(r.denominator()) ) }

      /**
       *  The black swan price is defined as debt/collateral, we want to perform a margin call
       *  before debt == collateral.   Given a debt/collateral ratio of 1 USD / CORE and
       *  a maintenance collateral requirement of 2x we can define the call price to be
       *  2 USD / CORE.
       *
       *  This method divides the collateral by the maintenance collateral ratio to derive
       *  a call price for the given black swan ratio.
       *
       *  There exists some cases where the debt and collateral values are so small that
       *  dividing by the collateral ratio will result in a 0 price or really poor
       *  rounding errors.   No matter what the collateral part of the price ratio can
       *  never go to 0 and the debt can never go more than GRAPHENE_MAX_SHARE_SUPPLY
       *
       *  CR * DEBT/COLLAT or DEBT/(COLLAT/CR)
       *
       *  Note: this function is only used before core-1270 hard fork.
       */
      price price::call_price( const asset& debt, const asset& collateral, uint16_t collateral_ratio)
      { try {
         boost::rational<int128_t> swan(debt.amount.value,collateral.amount.value);
         boost::rational<int128_t> ratio( collateral_ratio, GRAPHENE_COLLATERAL_RATIO_DENOM );
         auto cp = swan * ratio;

         while( cp.numerator() > GRAPHENE_MAX_SHARE_SUPPLY || cp.denominator() > GRAPHENE_MAX_SHARE_SUPPLY )
            cp = boost::rational<int128_t>( (cp.numerator() >> 1)+1, (cp.denominator() >> 1)+1 );

         return  (  asset( cp.denominator().convert_to<int64_t>(), collateral.asset_id )
                  / asset( cp.numerator().convert_to<int64_t>(), debt.asset_id ) );
      } FC_CAPTURE_AND_RETHROW( (debt)(collateral)(collateral_ratio) ) }

      bool price::is_null() const
      {
         // Effectively same as "return *this == price();" but perhaps faster
         return ( base.asset_id == asset_id_type() && quote.asset_id == asset_id_type() );
      }

      void price::validate() const
      { try {
         FC_ASSERT( base.amount > share_type(0) );
         FC_ASSERT( quote.amount > share_type(0) );
         FC_ASSERT( base.asset_id != quote.asset_id );
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      void price_feed::validate() const
      { try {
         if( !settlement_price.is_null() )
            settlement_price.validate();
         FC_ASSERT( maximum_short_squeeze_ratio >= GRAPHENE_MIN_COLLATERAL_RATIO );
         FC_ASSERT( maximum_short_squeeze_ratio <= GRAPHENE_MAX_COLLATERAL_RATIO );
         FC_ASSERT( maintenance_collateral_ratio >= GRAPHENE_MIN_COLLATERAL_RATIO );
         FC_ASSERT( maintenance_collateral_ratio <= GRAPHENE_MAX_COLLATERAL_RATIO );
         // Note: there was code here calling `max_short_squeeze_price();` before core-1270 hard fork,
         //       in order to make sure that it doesn't overflow,
         //       but the code doesn't actually check overflow, and it won't overflow, so the code is removed.

         // Note: not checking `maintenance_collateral_ratio >= maximum_short_squeeze_ratio` since launch
      } FC_CAPTURE_AND_RETHROW( (*this) ) }

      bool price_feed::is_for( asset_id_type asset_id ) const
      {
         try
         {
            if( !settlement_price.is_null() )
               return (settlement_price.base.asset_id == asset_id);
            if( !core_exchange_rate.is_null() )
               return (core_exchange_rate.base.asset_id == asset_id);
            // (null, null) is valid for any feed
            return true;
         }
         FC_CAPTURE_AND_RETHROW( (*this) )
      }

      // This function is kept here due to potential different behavior in edge cases.
      // TODO check after core-1270 hard fork to see if we can safely remove it
      price price_feed::max_short_squeeze_price_before_hf_1270()const
      {
         // settlement price is in debt/collateral
         boost::rational<int128_t> sp( settlement_price.base.amount.value, settlement_price.quote.amount.value );
         boost::rational<int128_t> ratio( GRAPHENE_COLLATERAL_RATIO_DENOM, maximum_short_squeeze_ratio );
         auto cp = sp * ratio;

         while( cp.numerator() > GRAPHENE_MAX_SHARE_SUPPLY || cp.denominator() > GRAPHENE_MAX_SHARE_SUPPLY )
            cp = boost::rational<int128_t>( (cp.numerator() >> 1)+(cp.numerator()&1),
                                            (cp.denominator() >> 1)+(cp.denominator()&1) );

         return (  asset( cp.numerator().convert_to<int64_t>(), settlement_price.base.asset_id )
                 / asset( cp.denominator().convert_to<int64_t>(), settlement_price.quote.asset_id ) );
      }

      price price_feed::max_short_squeeze_price()const
      {
         // settlement price is in debt/collateral
         return settlement_price * ratio_type( GRAPHENE_COLLATERAL_RATIO_DENOM, maximum_short_squeeze_ratio );
      }

      price price_feed::maintenance_collateralization()const
      {
         if( settlement_price.is_null() )
            return price();
         return ~settlement_price * ratio_type( maintenance_collateral_ratio, GRAPHENE_COLLATERAL_RATIO_DENOM );
      }

// compile-time table of powers of 10 using template metaprogramming

template< int N >
struct p10
{
   static const int64_t v = 10 * p10<N-1>::v;
};

template<>
struct p10<0>
{
   static const int64_t v = 1;
};

const int64_t scaled_precision_lut[19] =
{
   p10<  0 >::v, p10<  1 >::v, p10<  2 >::v, p10<  3 >::v,
   p10<  4 >::v, p10<  5 >::v, p10<  6 >::v, p10<  7 >::v,
   p10<  8 >::v, p10<  9 >::v, p10< 10 >::v, p10< 11 >::v,
   p10< 12 >::v, p10< 13 >::v, p10< 14 >::v, p10< 15 >::v,
   p10< 16 >::v, p10< 17 >::v, p10< 18 >::v
};

} } // graphene::chain