/* written by sleutenegger 18th Dec 2014 */

#include <iostream>
#include <iomanip>

#include <boost/thread/thread.hpp>
#include <boost/timer/timer.hpp>

#include "BrickPi.h"

#include "brickpi/Interface.hpp"

namespace brickpi {

void checkValidPort(unsigned char port)
{
  if (port > 3) {
    throw std::runtime_error("invalid port requested.");
  }
}

const double Interface::TICKS_PER_RAD = 720.0 / (2.0 * M_PI);
const double Interface::CONTROL_INTERVAL_SECONDS = 0.01;  // 100 Hz
const int Interface::ULTRASONIC_INTERVAL_MILLISECONDS = 15;

Interface::Interface()
{
  for (size_t i = 0; i < 4; ++i) {
    // set all references to zero, manual control
    pwmReferences_[i].store(0.0);  ///< these may be set by the user
    rotationSpeedReferences_[i].store(0.0);  ///< these may be set by the user
    angleReferences_[i].store(0.0);  ///< these may be set by the user
    motorAngles_[i] = (0.0);  ///< these may be queried by the user
    filteredMaxRotationAccelerations_[i].store(0.0);  ///< these are constantly calculated by respecting limits
    filteredRotationSpeedReferences_[i].store(0.0);  ///< these are constantly calculated by respecting limits
    filteredAngleReferences_[i].store(0.0);  ///< these are constantly calculated by respecting limits
    inDeceleration_[i] = false;
    controlModes_[i].store(Disabled);
    sensorTypes_[i] = SENSOR_NONE;
    referenceReached_[i] = false;
  }
  loggingEnabled = false;
  timer.start();

  ultrasonicEnabled.store(false);

  shutdown_.store(false);  // no shutdown

  lastTime_ = 0;

  // reset the clock
  ClearTick();
}

Interface::~Interface()
{
  // check if control loop is already running
  bool success = true;
  if (!controlAndSensingThread_) {
    for (size_t i = 0; i < 4; ++i) {
      if (controlModes_[i].load() == Disabled) {
        continue;  // we will ignore the disable ones
      }
      success &= setMotorPwm(i, 0);
    }
  } else {
    terminate();
  }
  std::cout.flush();
}

/// this starts a separate thread that continuously polls the activated sensors
/// as well as controls the motors that were started:
bool Interface::initialize()
{

  // make sure this is only called once.
  if (controlAndSensingThread_) {
    std::cout << "initialized called before, ignoring." << std::endl;
    return false;
  }

  int result = BrickPiSetup();
  printf("BrickPiSetup: %d\n", result);
  if (result != 0) {
    return false;
  }

  // TODO sleutene: figure out what these two really do...
  BrickPi.Address[0] = 1;
  BrickPi.Address[1] = 2;

  // start the sensing and control loop
  controlAndSensingThread_.reset(
      new boost::thread(&Interface::controlAndSensingLoop, this));

  return true;
}

/// softly stop all motors and sensors, stop the polling and control thread:
bool Interface::terminate()
{
  std::cout << "terminate..." << std::endl;
  // check if control loop is already running
  bool success = true;
  if (!controlAndSensingThread_) {
    for (size_t i = 0; i < 4; ++i) {
      if (controlModes_[i].load() == Disabled) {
        continue;  // we will ignore the disable ones
      }
      success &= setMotorPwm(i, 0);
    }
    return success;
  }

  // get current velocities and estimate shutdown time
  double shutdownTime = 0.0;
  for (size_t i = 0; i < 4; ++i) {
    if (controlModes_[i].load() == Disabled) {
      continue;  // we will ignore the disable ones
    }
    double speed = rotationSpeedReferences_[i];
    double acc = motorAngleControllerParameters_[i].maxRotationAcceleration;
    if (acc < 0.1) {
      acc = 0.1;
    }
    shutdownTime = std::max(speed / acc, shutdownTime);
  }

  // shutdown all motors synchronously
  std::vector<unsigned char> motorPorts;
  std::vector<double> rotationSpeedReferences;
  for (size_t i = 0; i < 4; ++i) {
    if (controlModes_[i].load() == Disabled || Manual) {
      continue;  // we will ignore the disable ones
    }
    motorPorts.push_back(i);
    rotationSpeedReferences.push_back(0.0);
  }
  success = setMotorRotationSpeedReferences(motorPorts,
                                            rotationSpeedReferences);

  // wait for stop completion - maximum twice the calculated time
  bool running = true;
  boost::timer::cpu_timer timer;
  boost::timer::nanosecond_type time_delay(2.0 * shutdownTime * 1.0e9);
  timer.start();
  while (running) {
    running = false;
    for (size_t i = 0; i < 4; ++i) {
      if (controlModes_[i].load() == Disabled || Manual) {
        continue;  // we will ignore the disabled/manual ones
      }
      if (fabs(filteredRotationSpeedReferences_[i].load()) > 1.0e-9) {
        running = true;
      }
    }
    if (timer.elapsed().wall > time_delay && running) {
      std::cout << "shutdown taking longer than expected, forcing..."
                << std::endl;
      break;
    }
  }

  // kill controlAndSensingLoop
  shutdown_.store(true);
  controlAndSensingThread_->join();
  controlAndSensingThread_.reset(NULL);

  // send PWM 0
  for (size_t i = 0; i < 4; ++i) {
    success &= setMotorPwm(i, 0);
  }
  return success;
}

/// immediately stop all motors, stop the polling and control thread (bad for the hardware):
bool Interface::emergencyStop()
{
  // kill controlAndSensingLoop
  shutdown_.store(true);
  if (controlAndSensingThread_) {
    controlAndSensingThread_->join();
  }
  controlAndSensingThread_.reset(NULL);

  // send PWM 0
  bool success = true;
  for (size_t i = 0; i < 4; ++i) {
    if (controlModes_[i].load() == Disabled) {
      continue;  // we will ignore the disable ones
    }
    success &= setMotorPwm(i, 0);
  }

  std::cout << "e-stop requested, sent stop..." << std::endl;

  return success;
}

/// start and stop individual motors:
bool Interface::motorEnable(unsigned char motorPort)
{
  if (motorPort > 3) {
    return false;
  }

  if (controlModes_[motorPort].load() != Disabled) {
    std::cout << "motor " << motorPort << "was already enabled, ignoring."
              << std::endl;
    return false;
  }

  BrickPi.MotorEnable[motorPort] = 1;  //Enable the motor

  // remember state
  controlModes_[motorPort].store(Manual);

  if (!setupSensors())
    return false;

  timestamp_t timestamp;
  if (!updateValues(false, timestamp))
    return false;

  return true;
}
bool Interface::motorDisable(unsigned char motorPort)
{
  if (motorPort > 3) {
    return false;
  }

  if (controlModes_[motorPort].load() == Disabled) {
    std::cout << "motor " << motorPort << "was already disabled, ignoring."
              << std::endl;
    return false;
  }

  // send PWM 0
  bool success = true;
  success &= setMotorPwm(motorPort, 0);

  BrickPi.MotorEnable[motorPort] = 0;  //Disable the motor
  if (!setupSensors())
    return false;

  // remember state
  controlModes_[motorPort].store(Disabled);
  return true;
}
/// activate and deactivate individual sensors:
bool Interface::sensorEnable(unsigned char sensorPort, SensorType sensorType)
{
  if (sensorPort > 3) {
    return false;
  }
  switch (sensorType) {
    case SENSOR_NONE: {
      return sensorDisable(sensorPort);
    }
    case SENSOR_TOUCH: {
      BrickPi.SensorType[sensorPort] = TYPE_SENSOR_TOUCH;
      break;
    }
    case SENSOR_ULTRASONIC: {
      BrickPi.SensorType[sensorPort] = TYPE_SENSOR_ULTRASONIC_CONT;
      ultrasonicEnabled.store(true);
      break;
    }
    default: {
      return false;
    }
  }

  sensorTypes_[sensorPort].store(sensorType);

  if (!setupSensors())
    return false;

  timestamp_t timestamp;
  if (!updateValues(sensorType == SENSOR_ULTRASONIC, timestamp))
    return false;

  return true;
}
bool Interface::sensorDisable(unsigned char sensorPort)
{
  if (sensorPort > 3) {
    return false;
  }
  BrickPi.SensorType[sensorPort] = 0;

  setupSensors();

  // remember state
  sensorTypes_[sensorPort].store(SENSOR_NONE);

  //Check to see if we still have an US sensor enabled
  bool _ultrasonicEnabled = false;
  for (int i = 0; i < 4; i++) {
    if (sensorTypes_[i].load() == SENSOR_ULTRASONIC) {
      _ultrasonicEnabled = true;
    }
  }
  ultrasonicEnabled.store(_ultrasonicEnabled);

  return true;
}

/// low-level motor interface -- overrides controller:
bool Interface::setMotorPwm(unsigned char motorPort, int pwm)
{
  if (motorPort > 3) {
    return false;
  }

  // check range
  if (pwm < -255) {
    std::cout << "pwm below -255 provided, ignoring" << std::endl;
    return false;
  }
  if (pwm > 255) {
    std::cout << "pwm above 255 provided, ignoring" << std::endl;
    return false;
  }

  // put controllers to reset
  pidControllers_[motorPort].reset();

  // check, if the control loop is still running
  if (controlAndSensingThread_) {
    // in this case, we just set manual mode and reference
    controlModes_[motorPort] = Manual;
    pwmReferences_[motorPort] = pwm;
  } else {
    BrickPi.MotorSpeed[motorPort] = pwm;
    boost::lock_guard<boost::mutex> l(commMutex_);
    if (0 == BrickPiUpdateValues()) {
      return true;
    } else {
      return false;
    }
  }

  return true;
}

/// set the controller parameters to non-default values:
bool Interface::setMotorAngleControllerParameters(
    unsigned char motorPort,
    const MotorAngleControllerParameters & motorAngleControllerParameters)
{
  if (motorPort > 3) {
    return false;
  }

  boost::lock_guard<boost::mutex> l(mutex_[motorPort]);  // protect write
  // make sure the user is sensible
  if (motorAngleControllerParameters.maxRotationAcceleration < 0.0) {
    return false;
  }
  if (motorAngleControllerParameters.maxRotationSpeed < 0.0) {
    return false;
  }
  motorAngleControllerParameters_[motorPort] = motorAngleControllerParameters;
  pidControllers_[motorPort].setParamteters(
      motorAngleControllerParameters.pidParameters);
  return true;
}

/*
 * Speed Controller (rotation speed) --
 * this internally also uses the position controller (and its parameters)
 */
/// set a controller speed reference -- overrides low-level setMotorPwm:
bool Interface::setMotorRotationSpeedReference(unsigned char motorPort,
                                               double rotationSpeedReference)
{
  if (motorPort > 3) {
    return false;
  }

  mutex_[motorPort].lock();
  // saturate
  if (rotationSpeedReference
      > motorAngleControllerParameters_[motorPort].maxRotationSpeed) {
    rotationSpeedReference = motorAngleControllerParameters_[motorPort]
        .maxRotationSpeed;
  }
  if (rotationSpeedReference
      < -motorAngleControllerParameters_[motorPort].maxRotationSpeed) {
    rotationSpeedReference = -motorAngleControllerParameters_[motorPort]
        .maxRotationSpeed;
  }

  referenceReached_[motorPort] = false;
  rotationSpeedReferences_[motorPort] = rotationSpeedReference;  // set reference

  // simply use the maximum admissible accelerations in this independent control mode

  filteredMaxRotationAccelerations_[motorPort] =
      motorAngleControllerParameters_[motorPort].maxRotationAcceleration;
  mutex_[motorPort].unlock();

  // get the current angles as a start
  if (controlModes_[motorPort] != Velocity
      && controlModes_[motorPort] != Position) {
    boost::lock_guard<boost::mutex> l(sensMutex_);
    filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
  }

  controlModes_[motorPort] = Velocity;  // reset mode
  return true;
}

bool Interface::setMotorRotationSpeedReferencesPython(
  const boost::python::list &motorPortsList,
  const boost::python::list &rotationSpeedReferencesList)
{
  std::vector<unsigned char> motorPorts;
  std::vector<double> rotationSpeedReferences;
  python_list_to_std_vector(motorPortsList, motorPorts);
  python_list_to_std_vector(rotationSpeedReferencesList, rotationSpeedReferences);
  return setMotorRotationSpeedReferences(motorPorts, rotationSpeedReferences);
}

/// set controller speed references -- overrides low-level setMotorPwm.
/// this version guarantees synchronous operation
bool Interface::setMotorRotationSpeedReferences(
    const std::vector<unsigned char> & motorPorts,
    const std::vector<double> & rotationSpeedReferences)
{
  // sanity check
  if (motorPorts.size() != rotationSpeedReferences.size()) {
    std::cout << "unequal numbers of motor ports and references provided."
              << std::endl;
    return false;
  }

  std::vector<double> rotationSpeedReferencesCopy = rotationSpeedReferences;

  // compute synchronised acceleration/deceleration.
  // we have to figure out what takes the longest according to the set limits
  double dt = 0.0;
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    if (motorPort > 3) {
      return false;
    }

    mutex_[motorPort].lock();
    // saturate
    if (rotationSpeedReferencesCopy.at(i)
        > motorAngleControllerParameters_[motorPort].maxRotationSpeed) {
      rotationSpeedReferencesCopy.at(i) =
          motorAngleControllerParameters_[motorPort].maxRotationSpeed;
    }
    if (rotationSpeedReferencesCopy.at(i)
        < -motorAngleControllerParameters_[motorPort].maxRotationSpeed) {
      rotationSpeedReferencesCopy.at(i) =
          -motorAngleControllerParameters_[motorPort].maxRotationSpeed;
    }

    dt = std::max(
        dt,
        fabs(
            rotationSpeedReferencesCopy.at(i)
                - filteredRotationSpeedReferences_[motorPorts[i]].load())
            / motorAngleControllerParameters_[motorPorts[i]]
                .maxRotationAcceleration);
    mutex_[motorPort].unlock();
  }
  // now we save the individual adjusted limits
  boost::lock_guard<boost::mutex> l(sensMutex_);
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    double acc = (rotationSpeedReferencesCopy.at(i)
        - filteredRotationSpeedReferences_[motorPort].load()) / dt;
    filteredMaxRotationAccelerations_[motorPort] = acc;
    referenceReached_[motorPort] = false;
    rotationSpeedReferences_[motorPort] = rotationSpeedReferencesCopy.at(i);
    if (controlModes_[motorPort] != Velocity
        && controlModes_[motorPort] != Position) {
      //std::cout << "store " << motorAngles_[motorPort] << std::endl;
      filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
    }
    controlModes_[motorPort] = Velocity;  // reset mode
  }

  return true;
}
/// query the current setpoint:
bool Interface::getMotorRotationSpeedReference(unsigned char motorPort,
                                               double & rotationSpeedReference)
{
  if (motorPort > 3) {
    return false;
  }
  rotationSpeedReference = rotationSpeedReferences_[motorPort];
  return true;
}

bool Interface::motorRotationSpeedReferenceReached(unsigned char motorPort)
{
  if (motorPort > 3) {
    return false;
  }
  if (controlModes_[motorPort] != Velocity) {
    std::cout << "motorRotationSpeedReferenceReached will only return true if in Velocity control mode." << std::endl;
    return false;
  }
  return referenceReached_[motorPort];
}

/*
 * Position Controller (angle)
 */
/// set a controller angle reference -- overrides low-level setMotorPwm:
bool Interface::setMotorAngleReference(unsigned char motorPort,
                                       double angleReference)
{
  if (motorPort > 3) {
    return false;
  }
  mutex_[motorPort].lock();
  const double maxRotationSpeed = motorAngleControllerParameters_[motorPort]
      .maxRotationSpeed;
  mutex_[motorPort].unlock();
  return setMotorAngleReference(motorPort, angleReference, maxRotationSpeed);
}
bool Interface::setMotorAngleReference(unsigned char motorPort,
                                       double angleReference,
                                       double rotationSpeedReference)
{
  if (motorPort > 3) {
    return false;
  }

  if (rotationSpeedReference < 0.0) {
    rotationSpeedReference = -rotationSpeedReference;  // ensure positive
  }

  // store angle reference
  inDeceleration_[motorPort] = false;
  referenceReached_[motorPort] = false;
  angleReferences_[motorPort] = angleReference;

  // get the needed direction...
  // first compute the time and distance needed to stop
  const double currentPosition = filteredAngleReferences_[motorPort];
  const double deltaPosition = angleReference - currentPosition;
  const double currentSpeed = filteredRotationSpeedReferences_[motorPort];
  mutex_[motorPort].lock();
  double acc = motorAngleControllerParameters_[motorPort]
      .maxRotationAcceleration;
  mutex_[motorPort].unlock();
  double dec = -acc;
  if (currentSpeed < 0.0) {
    dec = -dec;
  }
  const double dt_stop = -currentSpeed / dec;
  const double ds_stop = -0.5 * dec * dt_stop * dt_stop;

  // we use a negative slope, if
  double sign = 1.0;
  if ((ds_stop + currentPosition) > angleReference) {
    sign = -1.0;
  }
  rotationSpeedReferences_[motorPort] = sign * rotationSpeedReference;  // set speed reference

  /*std::cout << "rotationSpeedReferences_[" << int(motorPort) << "]"
   << rotationSpeedReferences_[motorPort] << std::endl;*/

  // set the acceleration and deceleration
  if (currentSpeed < rotationSpeedReferences_[motorPort]) {
    filteredMaxRotationAccelerations_[motorPort] = acc;
  } else {
    filteredMaxRotationAccelerations_[motorPort] = -acc;
  }
  filteredMaxRotationDecelerations_[motorPort] = -sign * acc;

  // get the current angles as a start
  if (controlModes_[motorPort] != Velocity
      && controlModes_[motorPort] != Position) {
    boost::lock_guard<boost::mutex> l(sensMutex_);
    filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
    //std::cout << "filteredAngleReferences_[motorPort]" << filteredAngleReferences_[motorPort] << std::endl;
  }
  //std::cout << "filteredMaxRotationDecelerations " << filteredMaxRotationDecelerations_[motorPort] << std::endl;
  controlModes_[motorPort] = Position;  // reset mode
  return true;
}
bool Interface::setMotorAngleReferencesPython(
  const boost::python::list & motorPortsList,
  const boost::python::list & angleReferencesList)
{
  std::vector<unsigned char> motorPorts;
  python_list_to_std_vector(motorPortsList, motorPorts);
  std::vector<double> angleReferences;
  python_list_to_std_vector(angleReferencesList, angleReferences);
  return setMotorAngleReferences(motorPorts, angleReferences);
}
/// set controller speed references -- overrides low-level setMotorPwm.
/// this version guarantees synchronous
bool Interface::setMotorAngleReferences(
    const std::vector<unsigned char> & motorPorts,
    const std::vector<double> & angleReferences)
{
  // get the maxima from the parameters
  std::vector<double> rotationSpeeds(motorPorts.size());
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    if (motorPort > 3) {
      return false;
    }
    boost::lock_guard<boost::mutex> l(mutex_[motorPort]);
    rotationSpeeds[i] = motorAngleControllerParameters_[motorPort]
        .maxRotationSpeed;
  }
  return setMotorAngleReferences(motorPorts, angleReferences, rotationSpeeds);
}
bool Interface::setMotorAngleReferencesPython(
  const boost::python::list & motorPortsList,
  const boost::python::list & angleReferencesList,
  const boost::python::list & rotationSpeedReferencesList)
{
  std::vector<unsigned char> motorPorts;
  python_list_to_std_vector(motorPortsList, motorPorts);
  std::vector<double> angleReferences;
  python_list_to_std_vector(angleReferencesList, angleReferences);
  std::vector<double> rotationSpeedReferences;
  python_list_to_std_vector(rotationSpeedReferencesList, rotationSpeedReferences);
  return setMotorAngleReferences(motorPorts, angleReferences, rotationSpeedReferences);
}
bool Interface::setMotorAngleReferences(
    const std::vector<unsigned char> & motorPorts,
    const std::vector<double> & angleReferences,
    const std::vector<double> & rotationSpeedReferences)
{

  // sanity check
  if (motorPorts.size() != angleReferences.size()) {
    return false;
  }
  if (rotationSpeedReferences.size() != angleReferences.size()) {
    return false;
  }

  // check the reqested distances
  std::vector<double> deltaPositions(angleReferences.size());
  double deltaPositionsMax = 1.0e-9;  // avoid division by zero
  boost::lock_guard<boost::mutex> l(sensMutex_);
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    if (motorPort > 3) {
      return false;
    }

    // get the current angles as a start
    if (controlModes_[motorPort] != Velocity
        && controlModes_[motorPort] != Position) {
      filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
    }

    // compute distance and remember maximum
    const double currentPosition = filteredAngleReferences_[motorPort];
    deltaPositions[i] = angleReferences[i] - currentPosition;
    if (fabs(deltaPositions[i]) > deltaPositionsMax) {
      deltaPositionsMax = fabs(deltaPositions[i]);
    }
  }

  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    double angleReference = angleReferences[i];
    double rotationSpeedReference = rotationSpeedReferences[i];

    if (rotationSpeedReference < 0.0) {
      rotationSpeedReference = -rotationSpeedReference;  // ensure positive
    }

    // compute scaling so the motions become synchronised:
    const double scaling = fabs(deltaPositions[i]) / deltaPositionsMax;

    // store angle reference
    inDeceleration_[motorPort] = false;
    referenceReached_[motorPort] = false;
    angleReferences_[motorPort] = angleReference;

    // get the needed direction...
    // first compute the time and distance needed to stop
    const double currentPosition = filteredAngleReferences_[motorPort];
    const double deltaPosition = angleReference - currentPosition;
    const double currentSpeed = filteredRotationSpeedReferences_[motorPort];
    mutex_[motorPort].lock();
    double acc = motorAngleControllerParameters_[motorPort]
        .maxRotationAcceleration * scaling;
    mutex_[motorPort].unlock();
    double dec = -acc;
    if (currentSpeed < 0.0) {
      dec = -dec;
    }
    const double dt_stop = -currentSpeed / dec;
    const double ds_stop = -0.5 * dec * dt_stop * dt_stop;

    // we use a negative slope, if
    double sign = 1.0;
    if ((ds_stop + currentPosition) > angleReference) {
      sign = -1.0;
    }
    rotationSpeedReferences_[motorPort] = sign * rotationSpeedReference
        * scaling;  // set speed reference

    //std::cout << "rotationSpeedReferences_[" << int(motorPort) << "]"
    // << rotationSpeedReferences_[motorPort] << std::endl;

    // set the acceleration and deceleration
    if (currentSpeed < rotationSpeedReferences_[motorPort]) {
      filteredMaxRotationAccelerations_[motorPort] = acc;
    } else {
      filteredMaxRotationAccelerations_[motorPort] = -acc;
    }
    filteredMaxRotationDecelerations_[motorPort] = -sign * acc;

    //std::cout << "filteredMaxRotationDecelerations " << filteredMaxRotationDecelerations_[motorPort] << std::endl;
    controlModes_[motorPort] = Position;  // reset mode
  }
  return true;
}

/// increase a controller angle reference -- overrides low-level setMotorPwm:
bool Interface::increaseMotorAngleReference(unsigned char motorPort,
                                            double angleReferenceIncrease)
{
  if (motorPort > 3) {
    return false;
  }
  // get the current angles as a start
  if (controlModes_[motorPort] != Velocity
      && controlModes_[motorPort] != Position) {
    boost::lock_guard<boost::mutex> l(sensMutex_);
    filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
  }
  // increase
  const double angleRef = filteredAngleReferences_[motorPort];
  return setMotorAngleReference(motorPort, angleRef + angleReferenceIncrease);
}
bool Interface::increaseMotorAngleReference(unsigned char motorPort,
                                            double angleReferenceIncrease,
                                            double rotationSpeedReference)
{
  if (motorPort > 3) {
    return false;
  }
  // get the current angles as a start
  if (controlModes_[motorPort] != Velocity
      && controlModes_[motorPort] != Position) {
    boost::lock_guard<boost::mutex> l(sensMutex_);
    filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
  }
  // increase
  const double angleRef = filteredAngleReferences_[motorPort];
  return setMotorAngleReference(motorPort, angleRef + angleReferenceIncrease,
                                rotationSpeedReference);
}
/// increase controller speed references -- overrides low-level setMotorPwm.
/// this version guarantees synchronous
bool Interface::increaseMotorAngleReferences(
    const std::vector<unsigned char> & motorPorts,
    const std::vector<double> & angleReferenceIncreases)
{
  std::vector<double> angleReferences;
  sensMutex_.lock();
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    // get the current angles as a start
    if (controlModes_[motorPort] != Velocity
        && controlModes_[motorPort] != Position) {
      filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
    }
    // increase
    const double angleRef = filteredAngleReferences_[motorPort];
    angleReferences.push_back(angleRef + angleReferenceIncreases[motorPort]);
  }
  sensMutex_.unlock();
  return setMotorAngleReferences(motorPorts, angleReferences);
}
bool Interface::increaseMotorAngleReferencesPython(
  const boost::python::list & motorPortsList,
  const boost::python::list & angleReferenceIncreasesList)
{
  std::vector<unsigned char> motorPorts;
  python_list_to_std_vector(motorPortsList, motorPorts);
  std::vector<double> angleReferenceIncreases;
  python_list_to_std_vector(angleReferenceIncreasesList, angleReferenceIncreases);
  return increaseMotorAngleReferences(motorPorts, angleReferenceIncreases);
}
bool Interface::increaseMotorAngleReferencesPython(
  const boost::python::list & motorPortsList,
  const boost::python::list & angleReferenceIncreasesList,
  const boost::python::list & rotationSpeedReferencesList)
{
  std::vector<unsigned char> motorPorts;
  python_list_to_std_vector(motorPortsList, motorPorts);
  std::vector<double> angleReferenceIncreases;
  python_list_to_std_vector(angleReferenceIncreasesList, angleReferenceIncreases);
  std::vector<double> rotationSpeedReferences;
  python_list_to_std_vector(rotationSpeedReferencesList, rotationSpeedReferences);
  return increaseMotorAngleReferences(motorPorts, angleReferenceIncreases, rotationSpeedReferences);
}
bool Interface::increaseMotorAngleReferences(
    const std::vector<unsigned char> & motorPorts,
    const std::vector<double> & angleReferenceIncreases,
    const std::vector<double> & rotationSpeedReferences)
{
  std::vector<double> angleReferences;
  sensMutex_.lock();
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts[i];
    // get the current angles as a start
    if (controlModes_[motorPort] != Velocity
        && controlModes_[motorPort] != Position) {
      filteredAngleReferences_[motorPort].store(motorAngles_[motorPort]);
    }
    // increase
    const double angleRef = filteredAngleReferences_[motorPort];
    angleReferences.push_back(angleRef + angleReferenceIncreases[motorPort]);
  }
  sensMutex_.unlock();
  return setMotorAngleReferences(motorPorts, angleReferences,
                                 rotationSpeedReferences);
}

/// query the current setpoint:
bool Interface::getMotorAngleReference(unsigned char motorPort,
                                       double & angleReference)
{
  if (motorPort > 3) {
    return false;
  }
  if (controlModes_[motorPort] == Position) {
    angleReference = angleReferences_[motorPort];
    return true;
  }
  return false;
}
boost::python::list Interface::getMotorAngleReferencesPython(
  const boost::python::list & motorPorts)
{
  boost::python::list l;
  for(int i=0; i<boost::python::len(motorPorts); i++)
  {
    unsigned char motorPort = boost::python::extract<unsigned char>(motorPorts[i]);
    if(motorPort > 3)
    {
      return boost::python::list();
    }
    if (controlModes_[motorPort] == Position) {
      l.append(angleReferences_[motorPort].load() );
    }
    else
    {
      return boost::python::list();
    }
  }
  return l;
}
/// query the current (actual) motor speed
bool Interface::getMotorAngle(unsigned char motorPort, double & angle,
                              timestamp_t &timestamp)
{
  if (motorPort > 3) {
    return false;
  }
  if (controlModes_[motorPort] == Disabled) {
    return false;
  }
  sensMutex_.lock();
  angle = motorAngles_[motorPort];
  timestamp = motorTimestamp;
  sensMutex_.unlock();
  return true;
}
boost::python::tuple Interface::getMotorAnglePython(unsigned char motorPort)
{
  double angle;
  timestamp_t timestamp;
  bool success = getMotorAngle(motorPort, angle, timestamp);
  if (success)
    return boost::python::make_tuple(angle, timestamp);
  else
    return boost::python::tuple();
}
bool Interface::getMotorAngle(unsigned char motorPort, double & angle)
{
  timestamp_t timestamp;
  return getMotorAngle(motorPort, angle, timestamp);
}

bool Interface::getMotorAngles(const std::vector<unsigned char>& motorPorts,
                               std::vector<double> & angles,
                               std::vector<timestamp_t> & timestamps)
{
  angles.resize(motorPorts.size());
  timestamps.resize(motorPorts.size());
  boost::lock_guard<boost::mutex> l(sensMutex_);
  for (size_t i = 0; i < motorPorts.size(); ++i) {
    const unsigned char motorPort = motorPorts.at(i);
    if (motorPort > 3) {
      return false;
    }
    if (controlModes_[motorPort] == Disabled) {
      return false;
    }
    angles[i] = motorAngles_[motorPort];
    timestamps[i] = motorTimestamp;
  }
  return true;
}

boost::python::list Interface::getMotorAnglesPython(
  const boost::python::list &motorPorts)
{
  boost::python::list values;
  boost::lock_guard<boost::mutex> l(sensMutex_);
  for (size_t i = 0; i < boost::python::len(motorPorts); ++i) {
    const unsigned char motorPort = boost::python::extract<unsigned char>(motorPorts[i]);
    if (motorPort > 3) {
      return boost::python::list();
    }
    if (controlModes_[motorPort] == Disabled) {
      return boost::python::list();
    }
    values.append(boost::python::make_tuple(
        motorAngles_[motorPort],
        motorTimestamp.load()
      ));
  }
  return values;
}

bool Interface::getMotorAngles(const std::vector<unsigned char>& motorPorts,
                               std::vector<double> & angles)
{
  std::vector<timestamp_t> unused;
  return getMotorAngles(motorPorts, angles, unused);
}

bool Interface::motorAngleReferenceReached(unsigned char motorPort)
{
  if (motorPort > 3) {
    return false;
  }
  if (controlModes_[motorPort] != Position) {
    std::cout << "motorAngleReferenceReached will only return true if in Position control mode." << std::endl;
    return false;
  }
  return referenceReached_[motorPort];
}

bool Interface::motorAngleReferencesReachedPython(
  const boost::python::list & motorPorts)
{
  bool destinationReached = true;
  for(int i=0; i<boost::python::len(motorPorts); i++)
  {
    unsigned char motorPort = boost::python::extract<unsigned char>(motorPorts[i]);
    if(motorPort > 3)
    {
      std::cout << "motorAngleReferencesReached: ERROR: motorPort out of range." << std::endl;
      return false;
    }
    if (controlModes_[motorPort] != Position) {
      std::cout << "motorAngleReferencesReached: ERROR: not in Position control mode." << std::endl;
      return false;
    }
    destinationReached = destinationReached && referenceReached_[motorPort];
  }
  return destinationReached;
}

bool Interface::filterVelocityMode(double dt, unsigned char motorPort)
{
  boost::lock_guard<boost::mutex> l(mutex_[motorPort]);
  // filter with max. acceleration
  const double speedDiff = rotationSpeedReferences_[motorPort]
      - filteredRotationSpeedReferences_[motorPort];
  const double acc = filteredMaxRotationAccelerations_[motorPort].load();
  if (fabs(speedDiff) / dt
      > motorAngleControllerParameters_[motorPort].maxRotationAcceleration) {
    filteredRotationSpeedReferences_[motorPort].store(
        filteredRotationSpeedReferences_[motorPort].load() + dt * acc);
    // integrate
    filteredAngleReferences_[motorPort].store(
        filteredAngleReferences_[motorPort].load()
            + dt * filteredRotationSpeedReferences_[motorPort]
            + 0.5 * dt * dt * acc);
  } else {
    filteredRotationSpeedReferences_[motorPort].store(
        rotationSpeedReferences_[motorPort]);
    // integrate
    filteredAngleReferences_[motorPort].store(
        filteredAngleReferences_[motorPort].load()
            + dt * filteredRotationSpeedReferences_[motorPort]);
  }

  /*std::cout << "dt: " << dt << "speed: "
   << filteredRotationSpeedReferences_[motorPort] << ", pos: "
   << filteredAngleReferences_[motorPort] << std::endl;*/
  return true;
}

bool Interface::filterPositionMode(double dt, unsigned char motorPort)
{

  boost::lock_guard<boost::mutex> l(mutex_[motorPort]);
  const double currentSpeed = filteredRotationSpeedReferences_[motorPort];
  const double speedDiff = currentSpeed - rotationSpeedReferences_[motorPort];
  const double acc = filteredMaxRotationAccelerations_[motorPort].load();
  double filteredRotationSpeedReference;
  double filteredAngleReference;
  if (fabs(speedDiff) / dt
      > fabs(filteredMaxRotationAccelerations_[motorPort])) {
    // full speed acceleration
    filteredRotationSpeedReference = filteredRotationSpeedReferences_[motorPort]
        .load() + dt * acc;
    // integrate
    filteredAngleReference = filteredAngleReferences_[motorPort].load()
        + dt * filteredRotationSpeedReference + 0.5 * dt * dt * acc;
  } else {
    // reference speed
    filteredRotationSpeedReference = rotationSpeedReferences_[motorPort];
    // integrate
    filteredAngleReference = filteredAngleReferences_[motorPort].load()
        + dt * filteredRotationSpeedReference;
  }

  // check, if deceleration is needed - make it stop in any direction to be on the safe side
  const double angle_ref = angleReferences_[motorPort];
  double dec = filteredMaxRotationDecelerations_[motorPort].load();

  const double dt_stop1 = -filteredRotationSpeedReference / dec;
  if(dt_stop1 < 0.0) {
    dec = -dec;
  }
  const double ds_stop1 = -0.5 * dec * dt_stop1 * dt_stop1;
  const double currentPosition1 = filteredAngleReference;
  const double deltaPosition1 = angle_ref - currentPosition1;
  const double stopAngle1 = currentPosition1 + ds_stop1;

  const double dt_stop0 = -filteredRotationSpeedReferences_[motorPort] / dec;
  if(dt_stop0 < 0.0) {
    dec = -dec;
  }
  const double ds_stop0 = -0.5 * dec * dt_stop0 * dt_stop0;
  const double currentPosition0 = filteredAngleReferences_[motorPort];
  const double deltaPosition0 = angle_ref - currentPosition0;
  const double stopAngle0 = currentPosition0 + ds_stop0;

  //std::cout << stopAngle0 << " " << stopAngle1 << " " << angle_ref << std::endl;

  if ((angle_ref - stopAngle1) * (angle_ref - stopAngle0) <= 0.0) {
    inDeceleration_[motorPort] = true;
  }
  if (inDeceleration_[motorPort]) {
    // make sure no more acceleration
    filteredMaxRotationAccelerations_[motorPort] = 0.0;
    // recompute deceleration
    const double speed = filteredRotationSpeedReferences_[motorPort].load();
    const double diff = (angleReferences_[motorPort]
        - filteredAngleReferences_[motorPort].load());
    if (fabs(diff) < 1.0e-12) {
      // stop
      filteredRotationSpeedReference = 0.0;
      filteredAngleReference = angle_ref;
    } else {
      dec = -speed * speed * 0.5 / diff;
      // recompute speed
      filteredRotationSpeedReference = speed + dt * dec;
      // recompute angle reference
      filteredAngleReference = filteredAngleReferences_[motorPort].load()
          + dt * filteredRotationSpeedReference + 0.5 * dt * dt * dec;
    }
  }
  // check stopping
  const double last_error = angle_ref
      - filteredAngleReferences_[motorPort].load();
  const double this_error = angle_ref - filteredAngleReference;
  if (inDeceleration_[motorPort]
      && filteredRotationSpeedReference
          * filteredRotationSpeedReferences_[motorPort] <= 0.0) {
    filteredRotationSpeedReference = 0.0;
    filteredAngleReference = angle_ref;
  }
  if (inDeceleration_[motorPort]
      && fabs(angle_ref - filteredAngleReference) < 0.5 / TICKS_PER_RAD) {
    filteredRotationSpeedReference = 0.0;
    filteredAngleReference = angle_ref;
  }

  // update
  filteredRotationSpeedReferences_[motorPort].store(
      filteredRotationSpeedReference);
  filteredAngleReferences_[motorPort].store(filteredAngleReference);

  return false;
}

double Interface::deadbandCompensatedPwm(unsigned char motorPort, double pwm)
{
  const double minPwm = motorAngleControllerParameters_[motorPort].minPWM;
  if(fabs(pwm)>=2.0*minPwm)
  {
    return pwm;
  }
  else
  {
    if (pwm>1.0e-12) return 1.0/(4.0*minPwm)*pwm*pwm+minPwm;
    if (pwm<-1.0e-12) return -1.0/(4.0*minPwm)*pwm*pwm-minPwm;
  }
  return 0.0;
  //return ((speedReference>0) - (speedReference<0)) * motorAngleControllerParameters_[motorPort].minPWM
  //    + motorAngleControllerParameters_[motorPort].feedForwardGain * speedReference;
}

bool Interface::controlMotors(timestamp_t timestamp)
{
  bool success = true;

  // compute time situation
  uint64_t now = CurrentTickUs();
  double dt = 0.0;
  if (lastTime_ != 0) {
    dt = double(now - lastTime_) / 1.0e6;
  }
  lastTime_ = now;

  if(loggingEnabled)
  {
    logFile << std::setprecision(7) << (double)timestamp*1.0e-9 << "\t";
  }

  for (size_t i = 0; i < 4; ++i) {
    ControlMode controlMode = controlModes_[i];
    // update angle NO LONGER DONE HERE
    //motorAngles_[i] = double(BrickPi.Encoder[i]) / TICKS_PER_RAD;
    //motorTimestamp = timestamp;
    switch (controlMode) {
      case Disabled:
        // don't do anything
        break;
      case Manual:
        // simply send pwm:
        BrickPi.MotorSpeed[i] = pwmReferences_[i].load();
        break;
      case Velocity: {
        // apply reference filtering
        filterVelocityMode(dt, i);

        // compute error
        double error = filteredAngleReferences_[i].load() - motorAngles_[i];

        if(loggingEnabled)
        {
          logFile << std::setprecision(9)
                  << filteredAngleReferences_[i].load()
                  << "\t" << motorAngles_[i]
                  << "\t";
        }

        // check if reference velocity was reached
        double output = 0.0;
        if ((fabs(filteredRotationSpeedReferences_[i] - rotationSpeedReferences_[i])<1.0e-12) &&
            (fabs(error) < 2.5/TICKS_PER_RAD)) {
          referenceReached_[i] = true;
          pidControllers_[i].reset(); // for continuity
        } else {
          // compute PID output
          output = pidControllers_[i].computeOutput(error, now * 1e3);

          // add feed-forward
          output += filteredRotationSpeedReferences_[i].load()*
              motorAngleControllerParameters_[i].feedForwardGain;

          // dead-band compensation
          output = deadbandCompensatedPwm(i,output);
        }

        // set output -- saturate again for safety
        if (output > 255) {
          output = 255;
        }
        if (output < -255) {
          output = -255;
        }

        //std::cout << "i="<< i
        //    << ", error : " << error
        //    << ", actual angle : " << motorAngles_[i]
        //    << ", angle reference : " << filteredAngleReferences_[i].load()
        //    << ", speed reference : " << this->filteredRotationSpeedReferences_[i].load() << std::endl;

        BrickPi.MotorSpeed[i] = int(output);

        break;
      }
      case Position: {
        // apply reference filtering
        filterPositionMode(dt, i);

        // compute error output
        double error = filteredAngleReferences_[i].load() - motorAngles_[i];

        if(loggingEnabled)
        {
          logFile << std::setprecision(9)
                  << filteredAngleReferences_[i].load()
                  << "\t" << motorAngles_[i]
                  << "\t";
        }

        // check if reference velocity was reached
        double output = 0.0;
        if ((fabs(filteredRotationSpeedReferences_[i]) < 1.0e-12) &&
            (fabs(filteredAngleReferences_[i] - angleReferences_[i]) < 1.0e-5) &&
            (fabs(error) < 2.5/TICKS_PER_RAD)) {
          referenceReached_[i] = true;
          pidControllers_[i].reset(); // for continuity
        } else {
          // compute PID output
          output = pidControllers_[i].computeOutput(error, now * 1e3);

          // add feed-forward
          output += filteredRotationSpeedReferences_[i].load()*
                      motorAngleControllerParameters_[i].feedForwardGain;

          // dead-band compensation
          //std::cout<<output<<"->";
          output = deadbandCompensatedPwm(i,output);
          //std::cout<<output<<std::endl;
        }

        // set output -- saturate again for safety
        if (output > 255) {
          output = 255;
        }
        if (output < -255) {
          output = -255;
        }

        BrickPi.MotorSpeed[i] = int(output);
        //std::cout << "i=" << i << ", error : " << error << ", actual angle : "
        //          << motorAngles_[i] << ", angle reference : "
        //          << filteredAngleReferences_[i].load()
        //          << ", speed reference : "
        //          << filteredRotationSpeedReferences_[i].load()
        //          << std::endl;

        break;
      }
      default:
        break;
    }
  }
  if(loggingEnabled)
  {
    logFile << "\n";
  }
  return success;
}

int Interface::controlAndSensingLoop()
{

  std::cout << "controlAndSensingLoop: running" << std::endl;

  boost::timer::cpu_timer intervalTimer;
  boost::timer::nanosecond_type startTime, lastUltrasonicUpdate = 0;
  intervalTimer.start();
  while (!shutdown_.load()) {
    startTime = intervalTimer.elapsed().wall;

    const bool getUltrasonic = ultrasonicEnabled.load()
        && ((startTime / 1000 - lastUltrasonicUpdate / 1000)
            > ULTRASONIC_INTERVAL_MILLISECONDS * 1000);

    timestamp_t timestamp;
    updateValues(getUltrasonic, timestamp);

    // handle the control of all motors
    controlMotors(timestamp);

    if (getUltrasonic)
      lastUltrasonicUpdate = timestamp;

    // sleep for the rest of the interval
    boost::this_thread::sleep(
        boost::posix_time::microseconds(
            (CONTROL_INTERVAL_SECONDS * 1.0e6
                - (intervalTimer.elapsed().wall / 1000 - startTime / 1000))));
  }

  std::cout << "controlAndSensingLoop: finished" << std::endl;
  return 0;
}

bool Interface::updateSensorValues(bool getUltrasonic, timestamp_t timestamp)
{
  boost::lock_guard<boost::mutex> l(sensMutex_);
  for (int i = 0; i < 4; i++) {
    SensorType type = sensorTypes_[i];
    if(type!=SENSOR_ULTRASONIC || (type==SENSOR_ULTRASONIC && getUltrasonic)) {
    //if (BrickPi.Sensor[i] >= 0) {
      sensorValues_[i] = sensorValue_t(BrickPi.Sensor[i], timestamp);
    }
    motorAngles_[i] = double(BrickPi.Encoder[i]) / TICKS_PER_RAD;
    motorTimestamp = timestamp;
  }
  return true;
}

bool Interface::getSensorValue(unsigned char sensorPort, long &value,
                               timestamp_t &timestamp)
{
  if (sensorPort > 3) {
    return false;
  }
  if (sensorTypes_[sensorPort] == SENSOR_NONE) {
    return false;
  }
  sensMutex_.lock();
  sensorValue_t sense = sensorValues_[sensorPort];
  sensMutex_.unlock();
  if (sense.value < 0) {
    return false;
  } else {
    value = sense.value;
    timestamp = sense.timestamp;
    return true;
  }
}

bool Interface::getSensorValue(unsigned char sensorPort, long &value)
{
  timestamp_t timestamp;
  return getSensorValue(sensorPort, value, timestamp);
}

boost::python::tuple Interface::getSensorValuePython(unsigned char sensorPort)
{
  long value;
  timestamp_t timestamp;
  bool success = getSensorValue(sensorPort, value, timestamp);
  if (success)
    return boost::python::make_tuple(value, timestamp);
  else
    return boost::python::tuple();
}

bool Interface::setupSensors()
{
  int result = 1;
  boost::lock_guard<boost::mutex> l(commMutex_);
  while (result) {
    result = BrickPiSetupSensors();  //Set up the properties of sensors for the BrickPi
  }
  return true;
}

bool Interface::updateValues(bool getUltrasonic, timestamp_t & timestamp)
{
  int result = 1;
  boost::lock_guard<boost::mutex> l(commMutex_);
  while (result) {
    result = BrickPiUpdateValues(getUltrasonic);
    boost::this_thread::sleep(boost::posix_time::milliseconds(1));
  }
  timestamp = timer.elapsed().wall;

  // handle the sensors and encoders
  updateSensorValues(getUltrasonic, timestamp);

  return true;
}

bool Interface::startLogging(std::string fileName)
{
  if(loggingEnabled)
  {
    std::cout << "Logging already enabled!" << std::endl;
    return false;
  }
  logFile.open(fileName.c_str());
  if(logFile.is_open())
  {
    loggingEnabled = true;
    return true;
  }
  else
  {
    std::cout << "Error opening logfile: " << fileName << std::endl;
    return false;
  }
}

bool Interface::stopLogging()
{
  if(loggingEnabled)
  {
    loggingEnabled = false;
    if(logFile.is_open())
    {
      logFile.close();
      return true;
    }
    else
    {
      std::cout << "Logging enabled but logfile not open :S" << std::endl;
      return false;
    }
  }
  else
  {
    std::cout << "Logging already disabled" << std::endl;
    return false;
  }
}

}  // namespace brickpi


#include <boost/test/unit_test.hpp>

#include <scorum/chain/schema/reward_balancer_objects.hpp>
#include <scorum/chain/services/reward_balancer.hpp>
#include <scorum/protocol/config.hpp>
#include <scorum/chain/database/block_tasks/reward_balance_algorithm.hpp>

#include "database_default_integration.hpp"

#include <limits>

using namespace database_fixture;

class dbs_reward_fixture : public database_integration_fixture
{
public:
    dbs_reward_fixture()
        : reward_service(db.obtain_service<dbs_content_reward_scr>())
        , balancer(reward_service)
    {
        open_database();
    }

    dbs_content_reward_scr& reward_service;

    database_ns::reward_balance_algorithm<content_reward_scr_service_i> balancer;

    const asset NULL_BALANCE = asset(0, SCORUM_SYMBOL);
};

BOOST_FIXTURE_TEST_SUITE(reward_service, dbs_reward_fixture)

BOOST_AUTO_TEST_CASE(check_reward_pool_creation)
{
    try
    {
        BOOST_REQUIRE_NO_THROW(reward_service.get());
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_reward_pool_double_creation)
{
    try
    {
        BOOST_REQUIRE(reward_service.is_exists());
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_reward_pool_initial_balance_is_null)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        BOOST_REQUIRE_EQUAL(pool.balance, NULL_BALANCE);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_reward_pool_initial_per_block_reward_is_not_null)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        BOOST_REQUIRE_GT(pool.current_per_block_reward, NULL_BALANCE);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_reward_pool_balance_increasing)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        asset current_balance = pool.balance;
        asset INCREASE_BALANCE(SCORUM_100_PERCENT, SCORUM_SYMBOL);

        BOOST_REQUIRE_EQUAL(balancer.increase_ballance(INCREASE_BALANCE), current_balance + INCREASE_BALANCE);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_reward_pool_per_block_reward_decreases_balance)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        asset initial_balance = pool.balance;

        asset block_reward = balancer.take_block_reward();

        BOOST_REQUIRE_EQUAL(pool.balance + block_reward, initial_balance);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_automatic_reward_increasing)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        asset current_per_block_reward = pool.current_per_block_reward;

        asset threshold_balance
            = asset(current_per_block_reward * SCORUM_REWARD_INCREASE_THRESHOLD_IN_DAYS * SCORUM_BLOCKS_PER_DAY
                    * (SCORUM_100_PERCENT / SCORUM_ADJUST_REWARD_PERCENT + 1));

        BOOST_REQUIRE_EQUAL(balancer.increase_ballance(threshold_balance), threshold_balance);

        for (int i = 1; i < SCORUM_100_PERCENT / SCORUM_ADJUST_REWARD_PERCENT; ++i)
        {
            asset balanced_per_block_reward
                = current_per_block_reward * (SCORUM_100_PERCENT + SCORUM_ADJUST_REWARD_PERCENT) / SCORUM_100_PERCENT;

            current_per_block_reward = balancer.take_block_reward();

            // before current_per_block_reward becomes 20 we increase reward by 1
            BOOST_REQUIRE_EQUAL(current_per_block_reward, balanced_per_block_reward + 1);
        }

        // after current_per_block_reward becomes 20 we increase reward by by 5%
        asset balanced_per_block_reward
            = current_per_block_reward * (SCORUM_100_PERCENT + SCORUM_ADJUST_REWARD_PERCENT) / SCORUM_100_PERCENT;

        current_per_block_reward = balancer.take_block_reward();

        BOOST_REQUIRE_EQUAL(current_per_block_reward, balanced_per_block_reward);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_automatic_reward_decreasing)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        int k = (SCORUM_100_PERCENT / SCORUM_ADJUST_REWARD_PERCENT + 1);

        asset threshold_balance = asset(pool.current_per_block_reward * SCORUM_GUARANTED_REWARD_SUPPLY_PERIOD_IN_DAYS
                                        * SCORUM_BLOCKS_PER_DAY * k);

        reward_service.update([&](content_reward_balancer_scr_object& rp) { rp.current_per_block_reward *= k; });

        BOOST_REQUIRE_EQUAL(balancer.increase_ballance(threshold_balance), threshold_balance);

        asset initial_per_block_reward = balancer.take_block_reward();
        asset current_per_block_reward = balancer.take_block_reward();

        BOOST_REQUIRE_EQUAL(current_per_block_reward,
                            initial_per_block_reward
                                - (initial_per_block_reward * SCORUM_ADJUST_REWARD_PERCENT) / SCORUM_100_PERCENT);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_CASE(check_automatic_reward_decreasing_with_int_arithmetic_correction)
{
    try
    {
        const content_reward_balancer_scr_object& pool = reward_service.get();

        int k = 2;

        asset threshold_balance = asset(pool.current_per_block_reward * SCORUM_GUARANTED_REWARD_SUPPLY_PERIOD_IN_DAYS
                                        * SCORUM_BLOCKS_PER_DAY * k);

        reward_service.update([&](content_reward_balancer_scr_object& rp) { rp.current_per_block_reward *= k; });

        BOOST_REQUIRE_EQUAL(balancer.increase_ballance(threshold_balance), threshold_balance);

        asset initial_per_block_reward = balancer.take_block_reward();
        asset current_per_block_reward = balancer.take_block_reward();

        BOOST_REQUIRE_EQUAL(current_per_block_reward, initial_per_block_reward - 1);
    }
    FC_LOG_AND_RETHROW()
}

BOOST_AUTO_TEST_SUITE_END()


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "itkTractsToDWIImageFilter.h"
#include <boost/progress.hpp>
#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>
#include <itkImageRegionIteratorWithIndex.h>
#include <itkResampleImageFilter.h>
#include <itkNearestNeighborInterpolateImageFunction.h>
#include <itkBSplineInterpolateImageFunction.h>
#include <itkCastImageFilter.h>
#include <itkImageFileWriter.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkWindowedSincInterpolateImageFunction.h>
#include <itkResampleDwiImageFilter.h>
#include <itkKspaceImageFilter.h>
#include <itkDftImageFilter.h>
#include <itkAddImageFilter.h>

namespace itk
{

template< class PixelType >
TractsToDWIImageFilter< PixelType >::TractsToDWIImageFilter()
    : m_CircleDummy(false)
    , m_VolumeAccuracy(10)
    , m_Upsampling(1)
    , m_NumberOfRepetitions(1)
    , m_EnforcePureFiberVoxels(false)
    , m_InterpolationShrink(1000)
    , m_FiberRadius(0)
    , m_SignalScale(25)
    , m_kOffset(0)
    , m_tLine(1)
    , m_UseInterpolation(false)
    , m_SimulateRelaxation(true)
    , m_tInhom(50)
    , m_TE(100)
    , m_FrequencyMap(NULL)
    , m_EddyGradientStrength(0.001)
    , m_SimulateEddyCurrents(false)
    , m_Spikes(0)
    , m_SpikeAmplitude(1)
{
    m_Spacing.Fill(2.5); m_Origin.Fill(0.0);
    m_DirectionMatrix.SetIdentity();
    m_ImageRegion.SetSize(0, 10);
    m_ImageRegion.SetSize(1, 10);
    m_ImageRegion.SetSize(2, 10);
}

template< class PixelType >
TractsToDWIImageFilter< PixelType >::~TractsToDWIImageFilter()
{

}

template< class PixelType >
TractsToDWIImageFilter< PixelType >::DoubleDwiType::Pointer TractsToDWIImageFilter< PixelType >::DoKspaceStuff( std::vector< DoubleDwiType::Pointer >& images )
{
    // create slice object
    ImageRegion<2> sliceRegion;
    sliceRegion.SetSize(0, m_UpsampledImageRegion.GetSize()[0]);
    sliceRegion.SetSize(1, m_UpsampledImageRegion.GetSize()[1]);
    Vector< double, 2 > sliceSpacing;
    sliceSpacing[0] = m_UpsampledSpacing[0];
    sliceSpacing[1] = m_UpsampledSpacing[1];

    // frequency map slice
    SliceType::Pointer fMap = NULL;
    if (m_FrequencyMap.IsNotNull())
    {
        fMap = SliceType::New();
        ImageRegion<2> region;
        region.SetSize(0, m_UpsampledImageRegion.GetSize()[0]);
        region.SetSize(1, m_UpsampledImageRegion.GetSize()[1]);
        fMap->SetLargestPossibleRegion( region );
        fMap->SetBufferedRegion( region );
        fMap->SetRequestedRegion( region );
        fMap->Allocate();
    }

    DoubleDwiType::Pointer newImage = DoubleDwiType::New();
    newImage->SetSpacing( m_Spacing );
    newImage->SetOrigin( m_Origin );
    newImage->SetDirection( m_DirectionMatrix );
    newImage->SetLargestPossibleRegion( m_ImageRegion );
    newImage->SetBufferedRegion( m_ImageRegion );
    newImage->SetRequestedRegion( m_ImageRegion );
    newImage->SetVectorLength( images.at(0)->GetVectorLength() );
    newImage->Allocate();

    MatrixType transform = m_DirectionMatrix;
    for (int i=0; i<3; i++)
        for (int j=0; j<3; j++)
        {
            if (j<2)
                transform[i][j] *= m_UpsampledSpacing[j];
            else
                transform[i][j] *= m_Spacing[j];
        }

    std::vector< int > spikeVolume;
    for (int i=0; i<m_Spikes; i++)
        spikeVolume.push_back(rand()%images.at(0)->GetVectorLength());
    std::sort (spikeVolume.begin(), spikeVolume.end());
    std::reverse (spikeVolume.begin(), spikeVolume.end());

    boost::progress_display disp(images.at(0)->GetVectorLength()*images.at(0)->GetLargestPossibleRegion().GetSize(2));
    for (int g=0; g<images.at(0)->GetVectorLength(); g++)
    {
        std::vector< int > spikeSlice;
        while (!spikeVolume.empty() && spikeVolume.back()==g)
        {
            spikeSlice.push_back(rand()%images.at(0)->GetLargestPossibleRegion().GetSize(2));
            spikeVolume.pop_back();
        }
        std::sort (spikeSlice.begin(), spikeSlice.end());
        std::reverse (spikeSlice.begin(), spikeSlice.end());

        for (int z=0; z<images.at(0)->GetLargestPossibleRegion().GetSize(2); z++)
        {
            std::vector< SliceType::Pointer > compartmentSlices;
            std::vector< double > t2Vector;

            for (int i=0; i<images.size(); i++)
            {
                DiffusionSignalModel<double>* signalModel;
                if (i<m_FiberModels.size())
                    signalModel = m_FiberModels.at(i);
                else
                    signalModel = m_NonFiberModels.at(i-m_FiberModels.size());

                SliceType::Pointer slice = SliceType::New();
                slice->SetLargestPossibleRegion( sliceRegion );
                slice->SetBufferedRegion( sliceRegion );
                slice->SetRequestedRegion( sliceRegion );
                slice->SetSpacing(sliceSpacing);
                slice->Allocate();
                slice->FillBuffer(0.0);

                // extract slice from channel g
                for (int y=0; y<images.at(0)->GetLargestPossibleRegion().GetSize(1); y++)
                    for (int x=0; x<images.at(0)->GetLargestPossibleRegion().GetSize(0); x++)
                    {
                        SliceType::IndexType index2D; index2D[0]=x; index2D[1]=y;
                        DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;

                        slice->SetPixel(index2D, images.at(i)->GetPixel(index3D)[g]);

                        if (fMap.IsNotNull() && i==0)
                            fMap->SetPixel(index2D, m_FrequencyMap->GetPixel(index3D));
                    }

                compartmentSlices.push_back(slice);
                t2Vector.push_back(signalModel->GetT2());
            }

            // create k-sapce (inverse fourier transform slices)
            itk::Size<2> outSize; outSize.SetElement(0, m_ImageRegion.GetSize(0)); outSize.SetElement(1, m_ImageRegion.GetSize(1));
            itk::KspaceImageFilter< SliceType::PixelType >::Pointer idft = itk::KspaceImageFilter< SliceType::PixelType >::New();
            idft->SetCompartmentImages(compartmentSlices);
            idft->SetT2(t2Vector);
            idft->SetkOffset(m_kOffset);
            idft->SettLine(m_tLine);
            idft->SetTE(m_TE);
            idft->SetTinhom(m_tInhom);
            idft->SetSimulateRelaxation(m_SimulateRelaxation);
            idft->SetSimulateEddyCurrents(m_SimulateEddyCurrents);
            idft->SetEddyGradientMagnitude(m_EddyGradientStrength);
            idft->SetZ((double)z-(double)images.at(0)->GetLargestPossibleRegion().GetSize(2)/2.0);
            idft->SetDirectionMatrix(transform);
            idft->SetDiffusionGradientDirection(m_FiberModels.at(0)->GetGradientDirection(g));
            idft->SetFrequencyMap(fMap);
            idft->SetSignalScale(m_SignalScale);
            idft->SetOutSize(outSize);
            int numSpikes = 0;
            while (!spikeSlice.empty() && spikeSlice.back()==z)
            {
                numSpikes++;
                spikeSlice.pop_back();
            }
            idft->SetSpikes(numSpikes);
            idft->SetSpikeAmplitude(m_SpikeAmplitude);
            idft->Update();

            ComplexSliceType::Pointer fSlice;
            fSlice = idft->GetOutput();

            for (int i=0; i<m_KspaceArtifacts.size(); i++)
                fSlice = m_KspaceArtifacts.at(i)->AddArtifact(fSlice);

            // fourier transform slice
            SliceType::Pointer newSlice;
            itk::DftImageFilter< SliceType::PixelType >::Pointer dft = itk::DftImageFilter< SliceType::PixelType >::New();
            dft->SetInput(fSlice);
            dft->Update();
            newSlice = dft->GetOutput();

            // put slice back into channel g
            for (int y=0; y<fSlice->GetLargestPossibleRegion().GetSize(1); y++)
                for (int x=0; x<fSlice->GetLargestPossibleRegion().GetSize(0); x++)
                {
                    DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;
                    SliceType::IndexType index2D; index2D[0]=x; index2D[1]=y;

                    DoubleDwiType::PixelType pix3D = newImage->GetPixel(index3D);
                    pix3D[g] = newSlice->GetPixel(index2D);
                    newImage->SetPixel(index3D, pix3D);
                }

            ++disp;
        }
    }
    return newImage;
}

//template< class PixelType >
//TractsToDWIImageFilter< PixelType >::ComplexSliceType::Pointer TractsToDWIImageFilter< PixelType >::RearrangeSlice(ComplexSliceType::Pointer slice)
//{
//    ImageRegion<2> region = slice->GetLargestPossibleRegion();
//    ComplexSliceType::Pointer rearrangedSlice = ComplexSliceType::New();
//    rearrangedSlice->SetLargestPossibleRegion( region );
//    rearrangedSlice->SetBufferedRegion( region );
//    rearrangedSlice->SetRequestedRegion( region );
//    rearrangedSlice->Allocate();

//    int xHalf = region.GetSize(0)/2;
//    int yHalf = region.GetSize(1)/2;

//    for (int y=0; y<region.GetSize(1); y++)
//        for (int x=0; x<region.GetSize(0); x++)
//        {
//            SliceType::IndexType idx;
//            idx[0]=x; idx[1]=y;
//            vcl_complex< double > pix = slice->GetPixel(idx);

//            if( idx[0] <  xHalf )
//                idx[0] = idx[0] + xHalf;
//            else
//                idx[0] = idx[0] - xHalf;

//            if( idx[1] <  yHalf )
//                idx[1] = idx[1] + yHalf;
//            else
//                idx[1] = idx[1] - yHalf;

//            rearrangedSlice->SetPixel(idx, pix);
//        }

//    return rearrangedSlice;
//}

template< class PixelType >
void TractsToDWIImageFilter< PixelType >::GenerateData()
{
    // check input data
    if (m_FiberBundle.IsNull())
        itkExceptionMacro("Input fiber bundle is NULL!");

    int numFibers = m_FiberBundle->GetNumFibers();
    if (numFibers<=0)
        itkExceptionMacro("Input fiber bundle contains no fibers!");

    if (m_FiberModels.empty())
        itkExceptionMacro("No diffusion model for fiber compartments defined!");

    if (m_NonFiberModels.empty())
        itkExceptionMacro("No diffusion model for non-fiber compartments defined!");

    int baselineIndex = m_FiberModels[0]->GetFirstBaselineIndex();
    if (baselineIndex<0)
        itkExceptionMacro("No baseline index found!");

    // check k-space undersampling
    if (m_Upsampling<1)
        m_Upsampling = 1;

    if (m_TissueMask.IsNotNull())
    {
        // use input tissue mask
        m_Spacing = m_TissueMask->GetSpacing();
        m_Origin = m_TissueMask->GetOrigin();
        m_DirectionMatrix = m_TissueMask->GetDirection();
        m_ImageRegion = m_TissueMask->GetLargestPossibleRegion();

        if (m_Upsampling>1.00001)
        {
            MITK_INFO << "Adding ringing artifacts. Image upsampling factor: " << m_Upsampling;
            ImageRegion<3> region = m_ImageRegion;
            region.SetSize(0, m_ImageRegion.GetSize(0)*m_Upsampling);
            region.SetSize(1, m_ImageRegion.GetSize(1)*m_Upsampling);
            itk::Vector<double> spacing = m_Spacing;
            spacing[0] /= m_Upsampling;
            spacing[1] /= m_Upsampling;
            itk::RescaleIntensityImageFilter<ItkUcharImgType,ItkUcharImgType>::Pointer rescaler = itk::RescaleIntensityImageFilter<ItkUcharImgType,ItkUcharImgType>::New();
            rescaler->SetInput(0,m_TissueMask);
            rescaler->SetOutputMaximum(100);
            rescaler->SetOutputMinimum(0);
            rescaler->Update();

            itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::Pointer resampler = itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::New();
            resampler->SetInput(rescaler->GetOutput());
            resampler->SetOutputParametersFromImage(m_TissueMask);
            resampler->SetSize(region.GetSize());
            resampler->SetOutputSpacing(spacing);
            resampler->Update();
            m_TissueMask = resampler->GetOutput();
        }
        MITK_INFO << "Using tissue mask";
    }

    // initialize output dwi image
    typename OutputImageType::Pointer outImage = OutputImageType::New();
    outImage->SetSpacing( m_Spacing );
    outImage->SetOrigin( m_Origin );
    outImage->SetDirection( m_DirectionMatrix );
    outImage->SetLargestPossibleRegion( m_ImageRegion );
    outImage->SetBufferedRegion( m_ImageRegion );
    outImage->SetRequestedRegion( m_ImageRegion );
    outImage->SetVectorLength( m_FiberModels[0]->GetNumGradients() );
    outImage->Allocate();
    typename OutputImageType::PixelType temp;
    temp.SetSize(m_FiberModels[0]->GetNumGradients());
    temp.Fill(0.0);
    outImage->FillBuffer(temp);

    // is input slize size a power of two?
    int x=m_ImageRegion.GetSize(0); int y=m_ImageRegion.GetSize(1);
    if ( x%2 == 1 )
        x += 1;
    if ( y%2 == 1 )
        y += 1;

    // if not, adjust size and dimension (needed for FFT); zero-padding
    if (x!=m_ImageRegion.GetSize(0))
        m_ImageRegion.SetSize(0, x);
    if (y!=m_ImageRegion.GetSize(1))
        m_ImageRegion.SetSize(1, y);

    // apply undersampling to image parameters
    m_UpsampledSpacing = m_Spacing;
    m_UpsampledImageRegion = m_ImageRegion;
    m_UpsampledSpacing[0] /= m_Upsampling;
    m_UpsampledSpacing[1] /= m_Upsampling;
    m_UpsampledImageRegion.SetSize(0, m_ImageRegion.GetSize()[0]*m_Upsampling);
    m_UpsampledImageRegion.SetSize(1, m_ImageRegion.GetSize()[1]*m_Upsampling);

    // everything from here on is using the upsampled image parameters!!!
    if (m_TissueMask.IsNull())
    {
        m_TissueMask = ItkUcharImgType::New();
        m_TissueMask->SetSpacing( m_UpsampledSpacing );
        m_TissueMask->SetOrigin( m_Origin );
        m_TissueMask->SetDirection( m_DirectionMatrix );
        m_TissueMask->SetLargestPossibleRegion( m_UpsampledImageRegion );
        m_TissueMask->SetBufferedRegion( m_UpsampledImageRegion );
        m_TissueMask->SetRequestedRegion( m_UpsampledImageRegion );
        m_TissueMask->Allocate();
        m_TissueMask->FillBuffer(1);
    }

    // resample frequency map
    if (m_FrequencyMap.IsNotNull())
    {
        itk::ResampleImageFilter<ItkDoubleImgType, ItkDoubleImgType>::Pointer resampler = itk::ResampleImageFilter<ItkDoubleImgType, ItkDoubleImgType>::New();
        resampler->SetInput(m_FrequencyMap);
        resampler->SetOutputParametersFromImage(m_FrequencyMap);
        resampler->SetSize(m_UpsampledImageRegion.GetSize());
        resampler->SetOutputSpacing(m_UpsampledSpacing);
        resampler->Update();
        m_FrequencyMap = resampler->GetOutput();
    }

    // initialize volume fraction images
    m_VolumeFractions.clear();
    for (int i=0; i<m_FiberModels.size()+m_NonFiberModels.size(); i++)
    {
        ItkDoubleImgType::Pointer tempimg = ItkDoubleImgType::New();
        tempimg->SetSpacing( m_UpsampledSpacing );
        tempimg->SetOrigin( m_Origin );
        tempimg->SetDirection( m_DirectionMatrix );
        tempimg->SetLargestPossibleRegion( m_UpsampledImageRegion );
        tempimg->SetBufferedRegion( m_UpsampledImageRegion );
        tempimg->SetRequestedRegion( m_UpsampledImageRegion );
        tempimg->Allocate();
        tempimg->FillBuffer(0);
        m_VolumeFractions.push_back(tempimg);
    }

    // resample fiber bundle for sufficient voxel coverage
    double segmentVolume = 0.0001;
    float minSpacing = 1;
    if(m_UpsampledSpacing[0]<m_UpsampledSpacing[1] && m_UpsampledSpacing[0]<m_UpsampledSpacing[2])
        minSpacing = m_UpsampledSpacing[0];
    else if (m_UpsampledSpacing[1] < m_UpsampledSpacing[2])
        minSpacing = m_UpsampledSpacing[1];
    else
        minSpacing = m_UpsampledSpacing[2];
    FiberBundleType fiberBundle = m_FiberBundle->GetDeepCopy();
    fiberBundle->ResampleFibers(minSpacing/m_VolumeAccuracy);
    double mmRadius = m_FiberRadius/1000;
    if (mmRadius>0)
        segmentVolume = M_PI*mmRadius*mmRadius*minSpacing/m_VolumeAccuracy;

    // generate double images to work with because we don't want to lose precision
    // we use a separate image for each compartment model
    std::vector< DoubleDwiType::Pointer > compartments;
    for (int i=0; i<m_FiberModels.size()+m_NonFiberModels.size(); i++)
    {
        DoubleDwiType::Pointer doubleDwi = DoubleDwiType::New();
        doubleDwi->SetSpacing( m_UpsampledSpacing );
        doubleDwi->SetOrigin( m_Origin );
        doubleDwi->SetDirection( m_DirectionMatrix );
        doubleDwi->SetLargestPossibleRegion( m_UpsampledImageRegion );
        doubleDwi->SetBufferedRegion( m_UpsampledImageRegion );
        doubleDwi->SetRequestedRegion( m_UpsampledImageRegion );
        doubleDwi->SetVectorLength( m_FiberModels[0]->GetNumGradients() );
        doubleDwi->Allocate();
        DoubleDwiType::PixelType pix;
        pix.SetSize(m_FiberModels[0]->GetNumGradients());
        pix.Fill(0.0);
        doubleDwi->FillBuffer(pix);
        compartments.push_back(doubleDwi);
    }

    double interpFact = 2*atan(-0.5*m_InterpolationShrink);
    double maxVolume = 0;

    MITK_INFO << "Generating signal of " << m_FiberModels.size() << " fiber compartments";
    vtkSmartPointer<vtkPolyData> fiberPolyData = fiberBundle->GetFiberPolyData();
    boost::progress_display disp(numFibers);
    for( int i=0; i<numFibers; i++ )
    {
        vtkCell* cell = fiberPolyData->GetCell(i);
        int numPoints = cell->GetNumberOfPoints();
        vtkPoints* points = cell->GetPoints();

        if (numPoints<2)
            continue;

        for( int j=0; j<numPoints; j++)
        {
            double* temp = points->GetPoint(j);
            itk::Point<float, 3> vertex = GetItkPoint(temp);
            itk::Vector<double> v = GetItkVector(temp);

            itk::Vector<double, 3> dir(3);
            if (j<numPoints-1)
                dir = GetItkVector(points->GetPoint(j+1))-v;
            else
                dir = v-GetItkVector(points->GetPoint(j-1));

            itk::Index<3> idx;
            itk::ContinuousIndex<float, 3> contIndex;
            m_TissueMask->TransformPhysicalPointToIndex(vertex, idx);
            m_TissueMask->TransformPhysicalPointToContinuousIndex(vertex, contIndex);

            if (!m_UseInterpolation)    // use nearest neighbour interpolation
            {
                if (!m_TissueMask->GetLargestPossibleRegion().IsInside(idx) || m_TissueMask->GetPixel(idx)<=0)
                    continue;

                // generate signal for each fiber compartment
                for (int k=0; k<m_FiberModels.size(); k++)
                {
                    DoubleDwiType::Pointer doubleDwi = compartments.at(k);
                    m_FiberModels[k]->SetFiberDirection(dir);
                    DoubleDwiType::PixelType pix = doubleDwi->GetPixel(idx);
                    pix += segmentVolume*m_FiberModels[k]->SimulateMeasurement();
                    doubleDwi->SetPixel(idx, pix );
                    if (pix[baselineIndex]>maxVolume)
                        maxVolume = pix[baselineIndex];
                }

                continue;
            }

            double frac_x = contIndex[0] - idx[0];
            double frac_y = contIndex[1] - idx[1];
            double frac_z = contIndex[2] - idx[2];
            if (frac_x<0)
            {
                idx[0] -= 1;
                frac_x += 1;
            }
            if (frac_y<0)
            {
                idx[1] -= 1;
                frac_y += 1;
            }
            if (frac_z<0)
            {
                idx[2] -= 1;
                frac_z += 1;
            }

            frac_x = atan((0.5-frac_x)*m_InterpolationShrink)/interpFact + 0.5;
            frac_y = atan((0.5-frac_y)*m_InterpolationShrink)/interpFact + 0.5;
            frac_z = atan((0.5-frac_z)*m_InterpolationShrink)/interpFact + 0.5;

            // use trilinear interpolation
            itk::Index<3> newIdx;
            for (int x=0; x<2; x++)
            {
                frac_x = 1-frac_x;
                for (int y=0; y<2; y++)
                {
                    frac_y = 1-frac_y;
                    for (int z=0; z<2; z++)
                    {
                        frac_z = 1-frac_z;

                        newIdx[0] = idx[0]+x;
                        newIdx[1] = idx[1]+y;
                        newIdx[2] = idx[2]+z;

                        double frac = frac_x*frac_y*frac_z;

                        // is position valid?
                        if (!m_TissueMask->GetLargestPossibleRegion().IsInside(newIdx) || m_TissueMask->GetPixel(newIdx)<=0)
                            continue;

                        // generate signal for each fiber compartment
                        for (int k=0; k<m_FiberModels.size(); k++)
                        {
                            DoubleDwiType::Pointer doubleDwi = compartments.at(k);
                            m_FiberModels[k]->SetFiberDirection(dir);
                            DoubleDwiType::PixelType pix = doubleDwi->GetPixel(newIdx);
                            pix += segmentVolume*frac*m_FiberModels[k]->SimulateMeasurement();
                            doubleDwi->SetPixel(newIdx, pix );
                            if (pix[baselineIndex]>maxVolume)
                                maxVolume = pix[baselineIndex];
                        }
                    }
                }
            }
        }
        ++disp;
    }

    MITK_INFO << "Generating signal of " << m_NonFiberModels.size() << " non-fiber compartments";
    ImageRegionIterator<ItkUcharImgType> it3(m_TissueMask, m_TissueMask->GetLargestPossibleRegion());
    boost::progress_display disp3(m_TissueMask->GetLargestPossibleRegion().GetNumberOfPixels());
    double voxelVolume = m_UpsampledSpacing[0]*m_UpsampledSpacing[1]*m_UpsampledSpacing[2];

    double fact = 1;
    if (m_FiberRadius<0.0001)
        fact = voxelVolume/maxVolume;

    while(!it3.IsAtEnd())
    {
        DoubleDwiType::IndexType index = it3.GetIndex();

        if (it3.Get()>0)
        {
            // get fiber volume fraction
            DoubleDwiType::Pointer fiberDwi = compartments.at(0);
            DoubleDwiType::PixelType fiberPix = fiberDwi->GetPixel(index); // intra axonal compartment
            if (fact>1) // auto scale intra-axonal if no fiber radius is specified
            {
                fiberPix *= fact;
                fiberDwi->SetPixel(index, fiberPix);
            }
            double f = fiberPix[baselineIndex];

            if (f>voxelVolume || f>0 && m_EnforcePureFiberVoxels)  // more fiber than space in voxel?
            {
                fiberDwi->SetPixel(index, fiberPix*voxelVolume/f);

                for (int i=1; i<m_FiberModels.size(); i++)
                {
                    DoubleDwiType::PixelType pix; pix.Fill(0.0);
                    compartments.at(i)->SetPixel(index, pix);
                    m_VolumeFractions.at(i)->SetPixel(index, 1);
                }
            }
            else
            {
                m_VolumeFractions.at(0)->SetPixel(index, f/voxelVolume);

                double nonf = voxelVolume-f;    // non-fiber volume
                double inter = 0;
                if (m_FiberModels.size()>1)
                    inter = nonf * f/voxelVolume;   // intra-axonal fraction of non fiber compartment scales linearly with f
                double other = nonf - inter;        // rest of compartment
                double singleinter = inter/(m_FiberModels.size()-1);

                // adjust non-fiber and intra-axonal signal
                for (int i=1; i<m_FiberModels.size(); i++)
                {
                    DoubleDwiType::Pointer doubleDwi = compartments.at(i);
                    DoubleDwiType::PixelType pix = doubleDwi->GetPixel(index);
                    if (pix[baselineIndex]>0)
                        pix /= pix[baselineIndex];
                    pix *= singleinter;
                    doubleDwi->SetPixel(index, pix);
                    m_VolumeFractions.at(i)->SetPixel(index, singleinter/voxelVolume);
                }
                for (int i=0; i<m_NonFiberModels.size(); i++)
                {
                    DoubleDwiType::Pointer doubleDwi = compartments.at(i+m_FiberModels.size());
                    DoubleDwiType::PixelType pix = doubleDwi->GetPixel(index) + m_NonFiberModels[i]->SimulateMeasurement()*other*m_NonFiberModels[i]->GetWeight();
                    doubleDwi->SetPixel(index, pix);
                    m_VolumeFractions.at(i+m_FiberModels.size())->SetPixel(index, other/voxelVolume*m_NonFiberModels[i]->GetWeight());
                }
            }
        }
        ++it3;
        ++disp3;
    }

    // do k-space stuff
    DoubleDwiType::Pointer doubleOutImage;
    if (m_Spikes>0 || m_FrequencyMap.IsNotNull() || !m_KspaceArtifacts.empty() || m_kOffset>0 || m_SimulateRelaxation || m_SimulateEddyCurrents || m_Upsampling>1.00001)
    {
        MITK_INFO << "Adjusting complex signal";
        doubleOutImage = DoKspaceStuff(compartments);
        m_SignalScale = 1;
    }
    else
    {
        MITK_INFO << "Summing compartments";
        doubleOutImage = compartments.at(0);

        for (int i=1; i<compartments.size(); i++)
        {
            itk::AddImageFilter< DoubleDwiType, DoubleDwiType, DoubleDwiType>::Pointer adder = itk::AddImageFilter< DoubleDwiType, DoubleDwiType, DoubleDwiType>::New();
            adder->SetInput1(doubleOutImage);
            adder->SetInput2(compartments.at(i));
            adder->Update();
            doubleOutImage = adder->GetOutput();
        }
    }

    MITK_INFO << "Finalizing image";
    unsigned int window = 0;
    unsigned int min = itk::NumericTraits<unsigned int>::max();
    ImageRegionIterator<OutputImageType> it4 (outImage, outImage->GetLargestPossibleRegion());
    DoubleDwiType::PixelType signal; signal.SetSize(m_FiberModels[0]->GetNumGradients());
    boost::progress_display disp4(outImage->GetLargestPossibleRegion().GetNumberOfPixels());
    while(!it4.IsAtEnd())
    {
        ++disp4;
        typename OutputImageType::IndexType index = it4.GetIndex();
        signal = doubleOutImage->GetPixel(index)*m_SignalScale;

        if (m_NoiseModel->GetNoiseVariance() > 0)
        {
            DoubleDwiType::PixelType accu = signal; accu.Fill(0.0);
            for (int i=0; i<m_NumberOfRepetitions; i++)
            {
                DoubleDwiType::PixelType temp = signal;
                m_NoiseModel->AddNoise(temp);
                accu += temp;
            }
            signal = accu/m_NumberOfRepetitions;
        }

        for (int i=0; i<signal.Size(); i++)
        {
            if (signal[i]>0)
                signal[i] = floor(signal[i]+0.5);
            else
                signal[i] = ceil(signal[i]-0.5);

            if (!m_FiberModels.at(0)->IsBaselineIndex(i) && signal[i]>window)
                window = signal[i];
            if (!m_FiberModels.at(0)->IsBaselineIndex(i) && signal[i]<min)
                min = signal[i];
        }
        it4.Set(signal);
        ++it4;
    }
    window -= min;
    unsigned int level = window/2 + min;
    m_LevelWindow.SetLevelWindow(level, window);
    this->SetNthOutput(0, outImage);
}

template< class PixelType >
itk::Point<float, 3> TractsToDWIImageFilter< PixelType >::GetItkPoint(double point[3])
{
    itk::Point<float, 3> itkPoint;
    itkPoint[0] = point[0];
    itkPoint[1] = point[1];
    itkPoint[2] = point[2];
    return itkPoint;
}

template< class PixelType >
itk::Vector<double, 3> TractsToDWIImageFilter< PixelType >::GetItkVector(double point[3])
{
    itk::Vector<double, 3> itkVector;
    itkVector[0] = point[0];
    itkVector[1] = point[1];
    itkVector[2] = point[2];
    return itkVector;
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToDWIImageFilter< PixelType >::GetVnlVector(double point[3])
{
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = point[0];
    vnlVector[1] = point[1];
    vnlVector[2] = point[2];
    return vnlVector;
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToDWIImageFilter< PixelType >::GetVnlVector(Vector<float,3>& vector)
{
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = vector[0];
    vnlVector[1] = vector[1];
    vnlVector[2] = vector[2];
    return vnlVector;
}

}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Andreas Bertsch $
// $Authors: Andreas Bertsch $
// --------------------------------------------------------------------------

#include <OpenMS/COMPARISON/SPECTRA/ZhangSimilarityScore.h>
#include <OpenMS/DATASTRUCTURES/ListUtils.h>
#include <boost/math/special_functions/erf.hpp>

#include <cmath>

using namespace std;

namespace OpenMS
{
  ZhangSimilarityScore::ZhangSimilarityScore() :
    PeakSpectrumCompareFunctor()
  {
    setName(ZhangSimilarityScore::getProductName());
    defaults_.setValue("tolerance", 0.2, "defines the absolute (in Da) or relative (in ppm) tolerance");
    defaults_.setValue("is_relative_tolerance", "false", "If set to true, the tolerance is interpreted as relative");
    defaults_.setValidStrings("is_relative_tolerance", ListUtils::create<String>("true,false"));
    defaults_.setValue("use_linear_factor", "false", "if true, the intensities are weighted with the relative m/z difference");
    defaults_.setValidStrings("use_linear_factor", ListUtils::create<String>("true,false"));
    defaults_.setValue("use_gaussian_factor", "false", "if true, the intensities are weighted with the relative m/z difference using a gaussian");
    defaults_.setValidStrings("use_gaussian_factor", ListUtils::create<String>("true,false"));
    defaultsToParam_();
  }

  ZhangSimilarityScore::ZhangSimilarityScore(const ZhangSimilarityScore & source) :
    PeakSpectrumCompareFunctor(source)
  {
  }

  ZhangSimilarityScore::~ZhangSimilarityScore()
  {
  }

  ZhangSimilarityScore & ZhangSimilarityScore::operator=(const ZhangSimilarityScore & source)
  {
    if (this != &source)
    {
      PeakSpectrumCompareFunctor::operator=(source);
    }
    return *this;
  }

  double ZhangSimilarityScore::operator()(const PeakSpectrum & spec) const
  {
    return operator()(spec, spec);
  }

  double ZhangSimilarityScore::operator()(const PeakSpectrum & s1, const PeakSpectrum & s2) const
  {
    const double tolerance = (double)param_.getValue("tolerance");
    bool use_linear_factor = param_.getValue("use_linear_factor").toBool();
    bool use_gaussian_factor = param_.getValue("use_gaussian_factor").toBool();
    double score(0), sum(0), sum1(0), sum2(0) /*, squared_sum1(0), squared_sum2(0)*/;

    // TODO remove parameter
    if (param_.getValue("is_relative_tolerance").toBool() )
    {
      throw Exception::NotImplemented(__FILE__, __LINE__, __PRETTY_FUNCTION__);
    }

    for (PeakSpectrum::ConstIterator it1 = s1.begin(); it1 != s1.end(); ++it1)
    {
      sum1 += it1->getIntensity();
      /*
for (PeakSpectrum::ConstIterator it2 = s1.begin(); it2 != s1.end(); ++it2)
{
  if (abs(it1->getPosition()[0] - it2->getPosition()[0]) <= 2 * tolerance)
  {
    squared_sum1 += it1->getIntensity() * it2->getIntensity();
  }
}*/
    }

/*
        UInt i_left(0);
        for (Size i = 0; i != s1.size(); ++i)
        {
            sum1 += s1[i].getIntensity();
            for (Size j = i_left; j != s1.size(); ++j)
            {
                double pos1(s1[i].getPosition()[0]), pos2(s1[j].getPosition()[0]);
                if (abs(pos1 - pos2) <= 2 * tolerance)
                {
                    squared_sum1 += s1[i].getIntensity() * s1[j].getIntensity();
                }
                else
                {
                    if (pos2 > pos1)
                    {
                        break;
                    }
                    else
                    {
                        i_left = i;
                    }
                }
            }
        }*/

/*
    i_left = 0;
    for (Size i = 0; i != s2.size(); ++i)
    {
      sum2 += s2[i].getIntensity();
      for (Size j = i_left; j != s2.size(); ++j)
      {
        double pos1(s2[i].getPosition()[0]), pos2(s2[j].getPosition()[0]);
        if (abs(pos1 - pos2) <= 2 * tolerance)
        {
          squared_sum1 += s2[i].getIntensity() * s2[j].getIntensity();
        }
        else
        {
          if (pos2 > pos1)
          {
            break;
          }
          else
          {
            i_left = i;
          }
        }
      }
    }*/

    for (PeakSpectrum::ConstIterator it1 = s2.begin(); it1 != s2.end(); ++it1)
    {
      sum2 += it1->getIntensity();
      /*
for (PeakSpectrum::ConstIterator it2 = s2.begin(); it2 != s2.end(); ++it2)
{
  if (abs(it1->getPosition()[0] - it2->getPosition()[0]) <= 2 * tolerance)
  {
    squared_sum2 += it1->getIntensity() * it2->getIntensity();
  }
}
      */
    }

    Size j_left(0);
    for (Size i = 0; i != s1.size(); ++i)
    {
      for (Size j = j_left; j != s2.size(); ++j)
      {
        double pos1(s1[i].getMZ()), pos2(s2[j].getMZ());
        if (fabs(pos1 - pos2) < tolerance)
        {
          //double factor((tolerance - fabs(pos1 - pos2)) / tolerance);
          double factor = 1.0;

          if (use_linear_factor || use_gaussian_factor)
          {
            factor = getFactor_(tolerance, fabs(pos1 - pos2), use_gaussian_factor);
          }
          sum += sqrt(s1[i].getIntensity() * s2[j].getIntensity() * factor);
        }
        else
        {
          if (pos2 > pos1)
          {
            break;
          }
          else
          {
            j_left = j;
          }
        }
      }
    }

    /*
for (PeakSpectrum::ConstIterator it1 = s1.begin(); it1 != s1.end(); ++it1)
{
  for (PeakSpectrum::ConstIterator it2 = s2.begin(); it2 != s2.end(); ++it2)
  {
    if (abs(it1->getPosition()[0] - it2->getPosition()[0]) <= 2 * tolerance)
    {
      sum += sqrt(it1->getIntensity() * it2->getIntensity());
    }
  }
}*/

    score = sum / (sqrt(sum1 * sum2));

    return score;

  }

  double ZhangSimilarityScore::getFactor_(double mz_tolerance, double mz_difference, bool is_gaussian) const
  {
    double factor(0.0);

    if (is_gaussian)
    {
      static const double denominator = mz_tolerance * 3.0 * sqrt(2.0);
      factor = boost::math::erfc(mz_difference / denominator);
      //cerr << "Factor: " << factor << " " << mz_tolerance << " " << mz_difference << endl;
    }
    else
    {
      factor = (mz_tolerance - mz_difference) / mz_tolerance;
    }
    return factor;
  }

}