#include <common_robotics_utilities/serialization.hpp>

#include <cstdint>
#include <utility>
#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace serialization
{
constexpr uint64_t SerializedSizeVector2d()
{
  return static_cast<uint64_t>(sizeof(double) * 2);
}

constexpr uint64_t SerializedSizeVector3d()
{
  return static_cast<uint64_t>(sizeof(double) * 3);
}

constexpr uint64_t SerializedSizeVector4d()
{
  return static_cast<uint64_t>(sizeof(double) * 4);
}

constexpr uint64_t SerializedSizeQuaterniond()
{
  return static_cast<uint64_t>(sizeof(double) * 4);
}

constexpr uint64_t SerializedSizeIsometry3d()
{
  return static_cast<uint64_t>(sizeof(double) * 16);
}

uint64_t SerializeMatrixXd(
    const Eigen::MatrixXd& value, std::vector<uint8_t>& buffer)
{
  const uint64_t start_buffer_size = buffer.size();
  // First, write uint64_t rows and cols headers
  const uint64_t rows = static_cast<uint64_t>(value.rows());
  SerializeMemcpyable<uint64_t>(rows, buffer);
  const uint64_t cols = static_cast<uint64_t>(value.cols());
  SerializeMemcpyable<uint64_t>(cols, buffer);
  // Expand the buffer to handle everything
  const size_t serialized_length = sizeof(double) * rows * cols;
  const size_t previous_buffer_size = buffer.size();
  buffer.resize(previous_buffer_size + serialized_length, 0x00);
  // Serialize the contained items
  memcpy(&buffer[previous_buffer_size], value.data(), serialized_length);
  // Figure out how many bytes were written
  const uint64_t end_buffer_size = buffer.size();
  const uint64_t bytes_written = end_buffer_size - start_buffer_size;
  return bytes_written;
}

uint64_t SerializeVectorXd(
    const Eigen::VectorXd& value, std::vector<uint8_t>& buffer)
{
  return SerializeMemcpyableVectorLike<double, Eigen::VectorXd>(value, buffer);
}

uint64_t SerializeVector2d(
    const Eigen::Vector2d& value, std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeVector2d(), 0x00);
  memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector2d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeVector2d();
}

uint64_t SerializeVector3d(
    const Eigen::Vector3d& value, std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeVector3d(), 0x00);
  memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector3d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeVector3d();
}

uint64_t SerializeVector4d(
    const Eigen::Vector4d& value, std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeVector4d(), 0x00);
  memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector4d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeVector4d();
}

uint64_t SerializeQuaterniond(
    const Eigen::Quaterniond& value, std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeQuaterniond(), 0x00);
  memcpy(&temp_buffer.front(),
         value.coeffs().data(),
         SerializedSizeQuaterniond());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeQuaterniond();
}

uint64_t SerializeIsometry3d(
    const Eigen::Isometry3d& value, std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeIsometry3d(), 0x00);
  memcpy(&temp_buffer.front(),
         value.matrix().data(),
         SerializedSizeIsometry3d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeIsometry3d();
}

Deserialized<Eigen::MatrixXd> DeserializeMatrixXd(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  uint64_t current_position = starting_offset;
  // Load the rows and cols header
  const Deserialized<uint64_t> deserialized_rows
      = DeserializeMemcpyable<uint64_t>(buffer, current_position);
  const uint64_t rows = deserialized_rows.Value();
  current_position += deserialized_rows.BytesRead();
  const Deserialized<uint64_t> deserialized_cols
      = DeserializeMemcpyable<uint64_t>(buffer, current_position);
  const uint64_t cols = deserialized_cols.Value();
  current_position += deserialized_cols.BytesRead();
  // Deserialize the items
  const size_t serialized_length = sizeof(double) * rows * cols;
  if ((current_position + serialized_length) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  Eigen::MatrixXd deserialized = Eigen::MatrixXd::Zero(rows, cols);
  memcpy(deserialized.data(), &buffer[current_position], serialized_length);
  current_position += serialized_length;
  // Figure out how many bytes were read
  const uint64_t bytes_read = current_position - starting_offset;
  return MakeDeserialized(deserialized, bytes_read);
}

Deserialized<Eigen::VectorXd> DeserializeVectorXd(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  return DeserializeMemcpyableVectorLike<double, Eigen::VectorXd>(
      buffer, starting_offset);
}

Deserialized<Eigen::Vector2d> DeserializeVector2d(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  if (starting_offset >= buffer.size())
  {
    throw std::invalid_argument(
        "starting_offset is outside the provided buffer");
  }
  if ((starting_offset + SerializedSizeVector2d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Vector2d temp_value;
  memcpy(temp_value.data(), &buffer[starting_offset], SerializedSizeVector2d());
  return MakeDeserialized(temp_value, SerializedSizeVector2d());
}

Deserialized<Eigen::Vector3d> DeserializeVector3d(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  if (starting_offset >= buffer.size())
  {
    throw std::invalid_argument(
        "starting_offset is outside the provided buffer");
  }
  if ((starting_offset + SerializedSizeVector3d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Vector3d temp_value;
  memcpy(temp_value.data(), &buffer[starting_offset], SerializedSizeVector3d());
  return MakeDeserialized(temp_value, SerializedSizeVector3d());
}

Deserialized<Eigen::Vector4d> DeserializeVector4d(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  if (starting_offset >= buffer.size())
  {
    throw std::invalid_argument(
        "starting_offset is outside the provided buffer");
  }
  if ((starting_offset + SerializedSizeVector4d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Vector4d temp_value;
  memcpy(temp_value.data(), &buffer[starting_offset], SerializedSizeVector4d());
  return MakeDeserialized(temp_value, SerializedSizeVector4d());
}

Deserialized<Eigen::Quaterniond> DeserializeQuaterniond(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  if (starting_offset >= buffer.size())
  {
    throw std::invalid_argument(
        "starting_offset is outside the provided buffer");
  }
  if ((starting_offset + SerializedSizeQuaterniond()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Quaterniond temp_value;
  memcpy(temp_value.coeffs().data(),
         &buffer[starting_offset],
         SerializedSizeQuaterniond());
  return MakeDeserialized(temp_value, SerializedSizeQuaterniond());
}

Deserialized<Eigen::Isometry3d> DeserializeIsometry3d(
    const std::vector<uint8_t>& buffer, const uint64_t starting_offset)
{
  if (starting_offset >= buffer.size())
  {
    throw std::invalid_argument(
        "starting_offset is outside the provided buffer");
  }
  if ((starting_offset + SerializedSizeIsometry3d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Isometry3d temp_value;
  memcpy(temp_value.matrix().data(),
         &buffer[starting_offset],
         SerializedSizeIsometry3d());
  return MakeDeserialized(temp_value, SerializedSizeIsometry3d());
}
}  // namespace serialization
}  // namespace common_robotics_utilities


/**
 *  biascorrection.h
 *  express
 *
 *  Created by Adam Roberts on 4/5/11.
 *  Copyright 2011 Adam Roberts. All rights reserved.
 *
 */

#include <algorithm>
#include <boost/assign.hpp>
#include <cassert>

#include "biascorrection.h"

#include "fragments.h"
#include "frequencymatrix.h"
#include "main.h"
#include "sequence.h"
#include "targets.h"

using namespace std;

// size of bias window
const int WINDOW = 21;
// center position in window
const int CENTER = 11;
// number of bases on either side of center
const int SURROUND = 10;

SeqWeightTable::SeqWeightTable(size_t window_size, size_t order, double alpha)
    : _order(order),
      _observed(order, window_size, window_size, alpha),
      _expected(order, window_size, order+1, EPSILON) {
}

SeqWeightTable::SeqWeightTable(size_t window_size, size_t order,
                               string param_file_name, string identifier)
    : _order(order),
      _observed(order, window_size, window_size, 0),
      _expected(order, window_size, order+1, 0) {

  //TODO: Allow for orders and window sizes to be read from param file.

  ifstream infile (param_file_name.c_str());
  const size_t BUFF_SIZE = 99999;
  char line_buff[BUFF_SIZE];
  if (!infile.is_open()) {
    logger.severe("Unable to open parameter file '%s'.",
                  param_file_name.c_str());
  }

  do {
    infile.getline (line_buff, BUFF_SIZE, '\n');
  } while (strncmp(line_buff, identifier.c_str(), identifier.size()));
  infile.getline (line_buff, BUFF_SIZE, '\n');

  do {
    infile.getline (line_buff, BUFF_SIZE, '\n');
  } while (strcmp(line_buff, "\tObserved Conditional Probabilities"));
  infile.getline (line_buff, BUFF_SIZE, '\n');

  //FG
  for (size_t k = 0; k < (size_t)WINDOW; ++k) {
    infile.getline (line_buff, BUFF_SIZE, '\n');
    char *p = strtok(line_buff, "\t");
    for (size_t i=0; i < pow(4.0, (double)min(order, k)); ++i) {
      for (size_t j=0; j < NUM_NUCS; ++j) {
        p = strtok(NULL, "\t");
        _observed.update(k, i, j, log(strtod(p,NULL)));
      }
    }
  }

  infile.getline (line_buff, BUFF_SIZE, '\n');
  infile.getline (line_buff, BUFF_SIZE, '\n');
  //BG
  for (size_t k = 0; k < order+1; ++k) {
    infile.getline (line_buff, BUFF_SIZE, '\n');
    char *p = strtok(line_buff, "\t");
    for (size_t i=0; i < pow(4.0, (double)min(order, k)); ++i) {
      for (size_t j=0; j < NUM_NUCS; ++j) {
        p = strtok(NULL, "\t");
        _expected.update(k, i, j, log(strtod(p,NULL)));
      }
    }
  }
}

void SeqWeightTable::copy_observed(const SeqWeightTable& other) {
  _observed = other._observed;
}

void SeqWeightTable::copy_expected(const SeqWeightTable& other) {
  _expected = other._expected;
}

void SeqWeightTable::increment_expected(const Sequence& seq, double mass,
                                        const vector<double>& fl_cdf) {
  _expected.fast_learn(seq, mass, fl_cdf);
}

void SeqWeightTable::normalize_expected() {
  _expected.calc_marginals();
}

void SeqWeightTable::increment_observed(const Sequence& seq, size_t i,
                                        double mass) {
  int left = (int)i - SURROUND;
  _observed.update(seq, left, mass);
}

double SeqWeightTable::get_weight(const Sequence& seq, size_t i) const {
  int left = (int)i - SURROUND;
  return _observed.seq_prob(seq, left) - _expected.seq_prob(seq, left);
}

void SeqWeightTable::append_output(ofstream& outfile) const {
  char buff[200];
  string header = "";
  for (int i = 0; i < WINDOW; i++) {
    sprintf(buff, "\t%d", i-CENTER+1);
    header += buff;
  }
  header += '\n';

  outfile << "\tObserved Marginal Distribution\n" << header;

  for (size_t j = 0; j < NUM_NUCS; j++) {
    outfile << NUCS[j] << ":\t";
    for (int i = 0; i < WINDOW; i++) {
      outfile << scientific << sexp(_observed.marginal_prob(i,j)) << "\t";
    }
    outfile<<endl;
  }

  outfile << "\tObserved Conditional Probabilities\nPosition\t";

  for (size_t j = 0; j < pow((double)NUM_NUCS, (double)_order+1); j++) {
    string s = "->";
    s += NUCS[j & 3];
    size_t cond = j >> 2;
    for (size_t k = 0; k < _order; ++k) {
      s = NUCS[cond & 3] + s;
      cond = cond >> 2;
    }
    outfile << s << '\t';
  }
  outfile << endl;

  for (int i = 0; i < WINDOW; i++) {
    outfile << i-SURROUND << ":\t";
    for (size_t j = 0; j < pow((double)NUM_NUCS, (double)_order+1); j++) {
      outfile << scientific << sexp(_observed.transition_prob(i,j>>2,j&3))
              << "\t";
    }
    outfile<<endl;
  }

  outfile << "\tBackground Conditional Probabilities\nPosition\t";

  for (size_t j = 0; j < pow((double)NUM_NUCS, (double)_order+1); j++) {
    string s = "->";
    s += NUCS[j & 3];
    size_t cond = j >> 2;
    for (size_t k = 0; k < _order; ++k) {
      s = NUCS[cond & 3] + s;
      cond = cond >> 2;
    }
    outfile << s << '\t';
  }
  outfile << endl;

  for (size_t i = 0; i < _order+1; i++) {
    outfile << i+1 << ":\t";
    for (size_t j = 0; j <  pow((double)NUM_NUCS, (double)_order+1); j++) {
      outfile << scientific << sexp(_expected.transition_prob(i,j>>2,j&3))
              << "\t";
    }
    outfile<<endl;
  }
}

BiasBoss::BiasBoss(size_t order, double alpha)
    : _order(order),
      _5_seq_bias(WINDOW, order, alpha),
      _3_seq_bias(WINDOW, order, alpha){
}

BiasBoss::BiasBoss(size_t order, string param_file_name)
    : _order(order),
      _5_seq_bias(WINDOW, order, param_file_name, ">5"),
      _3_seq_bias(WINDOW, order, param_file_name, ">3") {
}

void BiasBoss::copy_observations(const BiasBoss& other) {
  _5_seq_bias.copy_observed(other._5_seq_bias);
  _3_seq_bias.copy_observed(other._3_seq_bias);
}

void BiasBoss::copy_expectations(const BiasBoss& other) {
  _5_seq_bias.copy_expected(other._5_seq_bias);
  _3_seq_bias.copy_expected(other._3_seq_bias);
}

void BiasBoss::update_expectations(const Target& targ, double mass,
                                   const vector<double>& fl_cdf) {
  if (mass == LOG_0) {
    return;
  }

  if (direction != R) {
    const Sequence& seq_fwd = targ.seq(0);
    _5_seq_bias.increment_expected(seq_fwd, mass, fl_cdf);
  }
  if (direction != F) {
    const Sequence& seq_rev = targ.seq(1);
    _3_seq_bias.increment_expected(seq_rev, mass, fl_cdf);
  }
}

void BiasBoss::normalize_expectations() {
  _5_seq_bias.normalize_expected();
  _3_seq_bias.normalize_expected();
}

void BiasBoss::update_observed(const FragHit& hit, double normalized_mass)
{
  assert (hit.pair_status() != PAIRED || (int)hit.length() > WINDOW);

  const Sequence& t_seq_fwd = hit.target()->seq(0);
  const Sequence& t_seq_rev = hit.target()->seq(1);

  if (hit.pair_status() != RIGHT_ONLY) {
    _5_seq_bias.increment_observed(t_seq_fwd, hit.left(), normalized_mass);
  }
  if (hit.pair_status() != LEFT_ONLY) {
    _3_seq_bias.increment_observed(t_seq_rev, t_seq_rev.length()-hit.right(),
                                   normalized_mass);
  }
}

double BiasBoss::get_target_bias(std::vector<float>& start_bias,
                                 std::vector<float>& end_bias,
                                 const Target& targ) const {
  double tot_start = LOG_0;
  double tot_end = LOG_0;

  const Sequence& t_seq_fwd = targ.seq(0);
  const Sequence& t_seq_rev = targ.seq(1);

  for (size_t i = 0; i < targ.length(); ++i) {
    start_bias[i] = _5_seq_bias.get_weight(t_seq_fwd, i);
    end_bias[targ.length()-i-1] = _3_seq_bias.get_weight(t_seq_rev, i);
    tot_start = log_add(tot_start, start_bias[i]);
    tot_end = log_add(tot_end, end_bias[i]);
  }

  double avg_bias = (tot_start + tot_end) - (2*log((double)targ.length()));
  assert(!isnan(avg_bias));
  return avg_bias;
}

void BiasBoss::append_output(ofstream& outfile) const {
  outfile << ">5' Sequence-Specific Bias\n";
  _5_seq_bias.append_output(outfile);
  outfile << ">3' Sequence-Specific Bias\n";
  _3_seq_bias.append_output(outfile);
}


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

#include <vector>
#include <boost/numeric/mtl/mtl.hpp>
#include "SubElementAssembler.h"
#include "ScalableQuadrature.h"
#include "SubPolytope.h"

namespace compositeFEM
{

  SubElementAssembler::SubElementAssembler(Operator* op,
      const FiniteElemSpace* rowFeSpace_,
      const FiniteElemSpace* colFeSpace_)
    : StandardAssembler(op, NULL, NULL, NULL, NULL, rowFeSpace_, colFeSpace_)
  {
    /**
     * Create a scalable quadrature for subassembler and replace the original
     * quadrature of the subassembler with the scalable quadrature.
     */

    if (zeroOrderAssembler)
    {
      checkQuadratures();
      zeroOrderScalableQuadrature =
        new ScalableQuadrature(zeroOrderAssembler->getQuadrature());
      zeroOrderAssembler->setQuadrature(zeroOrderScalableQuadrature);
    }
    else
    {
      zeroOrderScalableQuadrature = NULL;
    }

    if (firstOrderAssemblerGrdPsi)
    {
      checkQuadratures();
      firstOrderGrdPsiScalableQuadrature =
        new ScalableQuadrature(firstOrderAssemblerGrdPsi->getQuadrature());
      firstOrderAssemblerGrdPsi->setQuadrature(firstOrderGrdPsiScalableQuadrature);
    }
    else
    {
      firstOrderGrdPsiScalableQuadrature = NULL;
    }

    if (firstOrderAssemblerGrdPhi)
    {
      checkQuadratures();
      firstOrderGrdPhiScalableQuadrature =
        new ScalableQuadrature(firstOrderAssemblerGrdPhi->getQuadrature());
      firstOrderAssemblerGrdPhi->setQuadrature(firstOrderGrdPhiScalableQuadrature);
    }
    else
    {
      firstOrderGrdPhiScalableQuadrature = NULL;
    }

    if (secondOrderAssembler)
    {
      checkQuadratures();
      secondOrderScalableQuadrature =
        new ScalableQuadrature(secondOrderAssembler->getQuadrature());
      secondOrderAssembler->setQuadrature(secondOrderScalableQuadrature);
    }
    else
    {
      secondOrderScalableQuadrature = NULL;
    }
  }

  void SubElementAssembler::scaleQuadratures(const SubElInfo& subElInfo)
  {
    if (zeroOrderAssembler)
    {
      zeroOrderScalableQuadrature->scaleQuadrature(subElInfo);
    }
    if (firstOrderAssemblerGrdPsi)
    {
      firstOrderGrdPsiScalableQuadrature->scaleQuadrature(subElInfo);
    }
    if (firstOrderAssemblerGrdPhi)
    {
      firstOrderGrdPhiScalableQuadrature->scaleQuadrature(subElInfo);
    }
    if (secondOrderAssembler)
    {
      secondOrderScalableQuadrature->scaleQuadrature(subElInfo);
    }
  }

  void SubElementAssembler::getSubElementVector(SubElInfo* subElInfo,
      const ElInfo* elInfo,
      DenseVector<double>& userVec)
  {
    /**
     * Manipulate the quadratures of the SubAssemblers for subelement.
     */
    scaleQuadratures(*subElInfo);

    calculateElementVector(elInfo, userVec);

    /**
     * The integration has been performed with a quadrature living on element. The
     * determinant of element has been used instead of the determinant of subelement. Thus
     * the result must be corrected with respect to subelement.
     */
    double corrFactor = subElInfo->getDet() / fabs(elInfo->getDet());
    for (int i = 0; i < nRow; i++)
      userVec[i] *= corrFactor;
  }

  void SubElementAssembler::getSubElementMatrix(SubElInfo* subElInfo,
      const ElInfo* elInfo,
      ElementMatrix& userMat)
  {
    /**
     * Manipulate the quadratures of the SubAssemblers for subelement.
     */
    scaleQuadratures(*subElInfo);

    /**
     * Integrate using the manipulated quadrature.
     */
    calculateElementMatrix(elInfo, userMat);

    /**
     * The integration has been performed with a quadrature living on element. The
     * determinant of element has been used instead of the determinant of subelement.
     * Thus the result must be corrected with respect to subelement.
     */
    double corrFactor = subElInfo->getDet() / fabs(elInfo->getDet());
    for (int i = 0; i < nRow; i++)
    {
      for (int j = 0; j < nCol; j++)
      {
        userMat[i][j] *= corrFactor;
      }
    }
  }

  void SubElementAssembler::getSubPolytopeVector(SubPolytope* subPolytope,
      SubElementAssembler* subElementAssembler,
      const ElInfo* elInfo,
      DenseVector<double>& subPolVec)
  {
    /// Note: There is no reset of subPolVec.
    std::vector<SubElInfo*>::iterator it;
    DenseVector<double> subElVec(nRow);

    /// Assemble for each subelement of subpolytope.
    for (it = subPolytope->getSubElementsBegin();
         it != subPolytope->getSubElementsEnd();
         it++)
    {
      set_to_zero(subElVec);
      subElementAssembler->getSubElementVector(*it, elInfo, subElVec);

      /// Add results for subelement to total result for subpolytope.
      subPolVec += subElVec;
    }
  }

  void SubElementAssembler::getSubPolytopeMatrix(SubPolytope* subPolytope,
      SubElementAssembler* subElementAssembler,
      const ElInfo* elInfo,
      ElementMatrix& subPolMat)
  {
    /**
     * Note: There is no reset of subPolMat.
     */
    std::vector<SubElInfo*>::iterator it;
    ElementMatrix subElMat(nRow, nCol);

    /**
     * Assemble for each subelement of subpolytope.
     */
    for (it = subPolytope->getSubElementsBegin();
         it != subPolytope->getSubElementsEnd();
         it++)
    {
      set_to_zero(subElMat);
      subElementAssembler->getSubElementMatrix(*it, elInfo, subElMat);

      /**
       * Add results for subelement to total result for subpolytope.
       */
      for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
          subPolMat[i][j] += subElMat[i][j];
    }
  }

}


#ifndef BOX_HPP_4WFLKG9Q
#define BOX_HPP_4WFLKG9Q

#include <assert.h>

#include <Eigen/Core>
#include <Eigen/Dense>

namespace ca
{
namespace scrollgrid
{

/**
 * Describes an axis-aligned volume in space.
 */
template<typename Scalar, int Dim>
class Box {
 public:
  typedef Eigen::Matrix<Scalar, Dim, 1> Vec;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  Box() {
    center_.setZero();
    radius_.setZero();
    bounds_[0].setZero();
    bounds_[1].setZero();
  }

  Box(const Vec& min_pt,
      const Vec& max_pt) :
      center_( (min_pt+max_pt)/2 ),
      radius_( (max_pt-min_pt)/2 )
  {
    bounds_[0] = min_pt;
    bounds_[1] = max_pt;
  }

  Box(const Box& other) :
      center_(other.center_),
      radius_(other.radius_)
  {
    bounds_[0] = other.bounds_[0];
    bounds_[1] = other.bounds_[1];
  }

  Box& operator=(const Box& other) {
    if (this==&other) { return *this; }
    center_ = other.center_;
    radius_ = other.radius_;
    bounds_[0] = other.bounds_[0];
    bounds_[1] = other.bounds_[1];
    return *this;
  }

  const Vec& min_pt() const { return bounds_[0]; }

  const Vec& max_pt() const { return bounds_[1]; }

  const Vec& bound(int ix) const {
    assert (ix >= 0);
    assert (ix < 2);
    return bounds_[ix];
  }

  void set_bound(int ix, const Vec& v) {
    assert (ix >= 0);
    assert (ix < 2);
    bounds_[ix] = v;
    center_ = (bounds_[0]+bounds_[1])/2;
    radius_ = (bounds_[0]-bounds_[1])/2;
  }

  void set_min_pt(const Vec& min_pt) {
    bounds_[0] = min_pt;
    center_ = (bounds_[0]+bounds_[1])/2;
    radius_ = (bounds_[0]-bounds_[1])/2;
  }

  void set_max_pt(const Vec& max_pt) {
    bounds_[1] = max_pt;
    center_ = (bounds_[0]+bounds_[1])/2;
    radius_ = (bounds_[0]-bounds_[1])/2;
  }

  const Vec& center() const {
    return center_;
  }

  const Vec& radius() const {
    return radius_;
  }

  void set_center(const Vec& v) {
    Vec delta = v - center_;
    this->translate(delta);
  }

  void set_radius(const Vec& v) {
    radius_ = v;
    bounds_[0] = center_ - radius_;
    bounds_[1] = center_ + radius_;
  }

  void translate(const Vec& v) {
    center_ += v;
    bounds_[0] += v;
    bounds_[1] += v;
  }

  bool contains(const Vec& v) const {
    return (bounds_[0].array() <= v.array()).all() &&
           (v.array() <= bounds_[1].array()).all();
  }

#if 0
  template<typename Scalar2>
  Box<Scalar2, Dim> cast() {
    Box<Scalar2, Dim> ret(bounds_[0].template cast<Scalar2>(),
                          bounds_[1].template cast<Scalar2>());
    return ret;
  }
#endif

 private:

  Vec center_;
  Vec radius_;
  Vec bounds_[2];
};

}
} /* ca */

#endif /* end of include guard: BOX_HPP_4WFLKG9Q */


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file test/sensitivityanalysis.hpp
    \brief Sensitivity analysis tests
    \ingroup tests
*/

#pragma once

#include <boost/test/unit_test.hpp>

namespace testsuite {

//! Sensitivity analysis tests
/*!
  \ingroup tests
*/
class SensitivityAnalysisTest {
public:
    //! Test that shifting all shift curve tenor points by DELTA yields DELTA shifts at all tenor points of the an
    // underlying curve
    static void test1dShiftsSparse();
    //! Test that shifting all shift curve tenor points by DELTA yields DELTA shifts at all tenor points of the an
    // underlying curve, the shift curve is more granular here than the underlying curve
    static void test1dShiftsGranular();
    //! Test that shifting all 2-d shift tenor points by DELTA yields DELTA shifts at all 2-d grid points of the
    // underlying data
    static void test2dShifts();
    //! Test that the portfolio shows the expected sensitivity (regression test - "None" observation mode)
    static void testPortfolioSensitivityNoneObs();
    //! Test that the portfolio shows the expected sensitivity (regression test - "Disable" observation mode)
    static void testPortfolioSensitivityDisableObs();
    //! Test that the portfolio shows the expected sensitivity (regression test - "Defer" observation mode)
    static void testPortfolioSensitivityDeferObs();
    //! Test that the portfolio shows the expected sensitivity (regression test - "Unregister" observation mode)
    static void testPortfolioSensitivityUnregisterObs();
    //! Test computed delta/gamma/vega values for an FX option against analytic results
    static void testFxOptionDeltaGamma();
    //! Test computed delta/gamma/vega values for an Equity option against analytic results
    static void testEquityOptionDeltaGamma();
    //! Test computed cross gamma against archived values (smoke test only - no external benchmark)
    static void testCrossGamma();
    static boost::unit_test_framework::test_suite* suite();
};
} // namespace testsuite


#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif
#include <boost/test/unit_test.hpp>

#include <memory>
#include <exception>
#include <utility>

#define GLM_FORCE_RADIANS
#include "glm/glm.hpp"
#include "glm/gtc/quaternion.hpp"

#include "GlrInclude.hpp"

BOOST_AUTO_TEST_SUITE(sceneNode)

BOOST_AUTO_TEST_CASE(createDeleteSceneNodes)
{

}

BOOST_AUTO_TEST_CASE(sceneNodeMovement)
{
    auto node = std::unique_ptr< glr::BasicSceneNode >( new glr::BasicSceneNode(glr::Id(1), nullptr) );

	// Scale test
	node->setScale(0.1f, 0.1f, 0.1f);
	glm::vec3 scale = node->getScale();

	BOOST_CHECK_EQUAL( scale.x, 0.1f );
	BOOST_CHECK_EQUAL( scale.y, 0.1f );
	BOOST_CHECK_EQUAL( scale.z, 0.1f );

	// Position test
	node->setPosition(14.0f, 100.0f, 2.0f);
	glm::vec3 pos = node->getPosition();

	BOOST_CHECK_EQUAL( pos.x, 14.0f );
	BOOST_CHECK_EQUAL( pos.y, 100.0f );
	BOOST_CHECK_EQUAL( pos.z, 2.0f );

	// Translation test
	node->setPosition(100.0f, 100.0f, 100.0f);
	node->translate(1.0f, 1.0f, 1.0f);
	pos = node->getPosition();

	BOOST_CHECK_EQUAL( pos.x, 101.0f );
	BOOST_CHECK_EQUAL( pos.y, 101.0f );
	BOOST_CHECK_EQUAL( pos.z, 101.0f );

	node->translate( glm::vec3(-2.0f, -2.0f, -2.0f) );
	pos = node->getPosition();

	BOOST_CHECK_EQUAL( pos.x, 99.0f );
	BOOST_CHECK_EQUAL( pos.y, 99.0f );
	BOOST_CHECK_EQUAL( pos.z, 99.0f );

	// Rotation test
	float errorMargin = 0.000005f;
	node->rotate( 90.0f, glm::vec3(1.0f, 0.0f, 0.0f) );
	node->rotate( 90.0f, glm::vec3(0.0f, 1.0f, 0.0f) );
	node->rotate( 90.0f, glm::vec3(0.0f, 0.0f, 1.0f) );

	glm::quat expectedQuat = glm::quat(0.707107f, 0.0f, 0.707107f, 0.0f);
	auto quat = node->getOrientation();

	BOOST_CHECK_CLOSE_FRACTION( quat.w, expectedQuat.w, errorMargin );
	BOOST_CHECK(std::fabs(expectedQuat.x - quat.x) < errorMargin);
	BOOST_CHECK_CLOSE_FRACTION( quat.y, expectedQuat.y, errorMargin );
	BOOST_CHECK(std::fabs(expectedQuat.z - quat.z) < errorMargin);

	// TODO: think of more tests?
}

BOOST_AUTO_TEST_CASE(sceneNodeLookAt)
{
	auto n = std::unique_ptr< glr::BasicSceneNode >( new glr::BasicSceneNode(glr::Id(1), nullptr) );

	auto pos = n->getPosition();
	BOOST_CHECK( pos == glm::vec3(0,0,0) );

	auto q = n->getOrientation();
	BOOST_CHECK( q == glm::quat(1,0,0,0) );

	n->lookAt( glm::vec3(1,1,1) );
	q = n->getOrientation();

	float errorMargin = 0.000005f;
	BOOST_CHECK_CLOSE_FRACTION( q.w, 0.364705, errorMargin );
	BOOST_CHECK_CLOSE_FRACTION( q.x, -0.115917, errorMargin );
	BOOST_CHECK_CLOSE_FRACTION( q.y, 0.880476, errorMargin );
	BOOST_CHECK_CLOSE_FRACTION( q.z, -0.279848, errorMargin );

	pos = n->getPosition();
	BOOST_CHECK( pos == glm::vec3(0,0,0) );
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_SIGN_HPP
#define BOOST_MATH_TOOLS_SIGN_HPP

#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

namespace boost{ namespace math{

template <class T>
inline int sign BOOST_NO_MACRO_EXPAND(const T& z)
{
   return (z == 0) ? 0 : (z < 0) ? -1 : 1;
}

template <class T>
inline int signbit BOOST_NO_MACRO_EXPAND(const T& z)
{
   return (z < 0) ? 1 : 0;
}

template <class T>
inline T copysign BOOST_NO_MACRO_EXPAND(const T& x, const T& y)
{
   BOOST_MATH_STD_USING
   return fabs(x) * boost::math::sign(y);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_SIGN_HPP