// triad_display.cpp
// Wyatt Newman, 8/16
// node to assist display of triads (axes) in rviz
// this node subscribes to topic "triad_display_pose", from which it receives geometry_msgs/PoseStamped poses
// it uses this info to populate and publish axes, using whatever frame_id is in the pose header
// To see the result, add a "Marker" display in rviz and subscribe to the marker topic "/triad_display"
// Can test this display node with the test node: "triad_display_test_node", which generates moving poses
// corresponding to a marker origin spiraling up in z

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
//#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <tf_conversions/tf_eigen.h>

//some globals...
geometry_msgs::Point vertex1;
geometry_msgs::PoseStamped g_stamped_pose;
Eigen::Affine3d g_affine_marker_pose;

// create arrow markers; do this 3 times to create a triad (frame)
visualization_msgs::Marker arrow_marker_x; //this one for the x axis
visualization_msgs::Marker arrow_marker_y; //this one for the y axis
visualization_msgs::Marker arrow_marker_z; //this one for the y axis

//udpdate_arrows() set the frame and

void update_arrows() {
    geometry_msgs::Point origin, arrow_x_tip, arrow_y_tip, arrow_z_tip;
    Eigen::Matrix3d R;
    Eigen::Quaterniond quat;
    quat.x() = g_stamped_pose.pose.orientation.x;
    quat.y() = g_stamped_pose.pose.orientation.y;
    quat.z() = g_stamped_pose.pose.orientation.z;
    quat.w() = g_stamped_pose.pose.orientation.w;
    R = quat.toRotationMatrix();
    Eigen::Vector3d x_vec, y_vec, z_vec;
    double veclen = 0.2; //make the arrows this long
    x_vec = R.col(0) * veclen;
    y_vec = R.col(1) * veclen;
    z_vec = R.col(2) * veclen;

    //update the arrow markers w/ new pose:
    origin = g_stamped_pose.pose.position;
    arrow_x_tip = origin;
    arrow_x_tip.x += x_vec(0);
    arrow_x_tip.y += x_vec(1);
    arrow_x_tip.z += x_vec(2);
    arrow_marker_x.points.clear();
    arrow_marker_x.points.push_back(origin);
    arrow_marker_x.points.push_back(arrow_x_tip);
    arrow_marker_x.header = g_stamped_pose.header;

    arrow_y_tip = origin;
    arrow_y_tip.x += y_vec(0);
    arrow_y_tip.y += y_vec(1);
    arrow_y_tip.z += y_vec(2);

    arrow_marker_y.points.clear();
    arrow_marker_y.points.push_back(origin);
    arrow_marker_y.points.push_back(arrow_y_tip);
    arrow_marker_y.header = g_stamped_pose.header;

    arrow_z_tip = origin;
    arrow_z_tip.x += z_vec(0);
    arrow_z_tip.y += z_vec(1);
    arrow_z_tip.z += z_vec(2);

    arrow_marker_z.points.clear();
    arrow_marker_z.points.push_back(origin);
    arrow_marker_z.points.push_back(arrow_z_tip);
    arrow_marker_z.header = g_stamped_pose.header;
}

//init persistent params of markers, then variable coords

void init_markers() {
    //initialize stamped pose for at a legal (if boring) pose
    g_stamped_pose.header.stamp = ros::Time::now();
    g_stamped_pose.header.frame_id = "world";
    g_stamped_pose.pose.position.x = 0;
    g_stamped_pose.pose.position.y = 0;
    g_stamped_pose.pose.position.z = 0;
    g_stamped_pose.pose.orientation.x = 0;
    g_stamped_pose.pose.orientation.y = 0;
    g_stamped_pose.pose.orientation.z = 0;
    g_stamped_pose.pose.orientation.w = 1;

    //the following parameters only need to get set once
    arrow_marker_x.type = visualization_msgs::Marker::ARROW;
    arrow_marker_x.action = visualization_msgs::Marker::ADD; //create or modify marker
    arrow_marker_x.ns = "triad_namespace";
    arrow_marker_x.lifetime = ros::Duration(); //never delete
    // make the arrow thin
    arrow_marker_x.scale.x = 0.01;
    arrow_marker_x.scale.y = 0.01;
    arrow_marker_x.scale.z = 0.01;
    arrow_marker_x.color.r = 1.0; // red, for the x axis
    arrow_marker_x.color.g = 0.0;
    arrow_marker_x.color.b = 0.0;
    arrow_marker_x.color.a = 1.0;
    arrow_marker_x.id = 0;
    arrow_marker_x.header = g_stamped_pose.header;

    //y and z arrow params are the same, except for colors
    arrow_marker_y = arrow_marker_x;
    arrow_marker_y.color.r = 0.0;
    arrow_marker_y.color.g = 1.0; //green for y axis
    arrow_marker_y.color.b = 0.0;
    arrow_marker_y.color.a = 1.0;
    arrow_marker_y.id = 1;

    arrow_marker_z = arrow_marker_x;
    arrow_marker_z.id = 2;
    arrow_marker_z.color.r = 0.0;
    arrow_marker_z.color.g = 0.0;
    arrow_marker_z.color.b = 1.0; //blue for z axis
    arrow_marker_z.color.a = 1.0;
    //set the poses of the arrows based on g_stamped_pose
    update_arrows();
}

void poseCB(const geometry_msgs::PoseStamped &pose_msg) {
    ROS_DEBUG("got pose message");
    //ROS_INFO("got pose message");
    g_stamped_pose.header = pose_msg.header;
    g_stamped_pose.pose = pose_msg.pose;

}

int main(int argc, char** argv) {
    ros::init(argc, argv, "triad_display"); // this will be the node name;
    ros::NodeHandle nh;

    // subscribe to stamped-pose publications
    ros::Subscriber pose_sub = nh.subscribe("triad_display_pose", 1, poseCB);
    ros::Publisher vis_pub = nh.advertise<visualization_msgs::Marker>("triad_display", 1);
    init_markers();

    ros::Rate timer(20); //timer to run at 20 Hz

    while (ros::ok()) {
        update_arrows();
        vis_pub.publish(arrow_marker_x); //publish the marker
        ros::Duration(0.01).sleep();
        vis_pub.publish(arrow_marker_y); //publish the marker
        ros::Duration(0.01).sleep();
        vis_pub.publish(arrow_marker_z); //publish the marker
        ros::spinOnce(); //let callbacks perform an update
        timer.sleep();
    }
}


//
//  Copyright Toon Knapen
//
// Distributed under 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)
//

#include <iostream>
#include <fstream>
#include <string>
#include <stdlib.h>
#include <iomanip>

#include "blas.hpp"
#include <boost/numeric/bindings/blas/blas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>

template < typename ValueType, typename MatrixType, typename VectorType >
void test_gemv(std::ostream& os, int runs, int runs_i, int size, int size_i, char c, ValueType alpha, ValueType beta, MatrixType &a, VectorType& x, VectorType &y_native, VectorType &y_toblas)
{
  typedef typename VectorType::value_type value_type ;

  boost::timer t ;
  if ( c == boost::numeric::bindings::traits::NO_TRANSPOSE )    for(int i = 0 ; i < runs_i ; ++i ) y_native += alpha * numerics::prod( a, x) ;
  else if ( c == boost::numeric::bindings::traits::TRANSPOSE )  for(int i = 0 ; i < runs_i ; ++i ) y_native += alpha * numerics::prod( trans(a), x) ;
  else if ( c == boost::numeric::bindings::traits::CONJUGATE )  for(int i = 0 ; i < runs_i ; ++i ) y_native += alpha * numerics::prod( herm(a), x) ;
  else assert( 0 ) ;

  report< value_type >( os, runs, runs_i, size_i, t.elapsed() );

  t.restart() ;
  for(int i = 0 ; i < runs_i ; ++i ) boost::numeric::bindings::blas::gemv( c, alpha, a, x, beta, y_toblas ) ;

  report< value_type >( os, runs, runs_i, size_i, t.elapsed() );

  check( y_native.begin(), y_native.end(), y_toblas.begin() );
}

template < typename T >
struct gemv_matrix_vector_vector
{
  void operator()(std::ostream& os, int size, int size_i, int runs, int runs_i)
  {
    runs_i = std::max( 1, runs_i / size_i ) ;

    T alpha = 1 / ( size_i * 10 ), beta = 1.0 ;
    random_initialise( alpha );
    numerics::matrix< T, numerics::column_major > a(size_i,size_i) ;
    random_initialise_matrix( a ) ;
    numerics::vector< T > x( size_i );
    random_initialise_vector( x) ;
    numerics::vector< T > y_native( x ) ;
    numerics::vector< T > y_toblas( x ) ;

    test_gemv( os, runs, runs_i, size, size_i, boost::numeric::bindings::traits::NO_TRANSPOSE, alpha, beta, a, x, y_native, y_toblas );
  }
};

template < typename T >
struct gemv_trans_matrix_vector_vector
{
  void operator()(std::ostream& os, int size, int size_i, int runs, int runs_i)
  {
    runs_i = std::max( 1, runs_i / size_i ) ;

    T alpha = 1 / ( size_i * 10 ), beta = 1.0 ;
    random_initialise( alpha );
    numerics::matrix< T, numerics::column_major > a(size_i,size_i) ;
    random_initialise_matrix( a ) ;
    numerics::vector< T > x( size_i );
    random_initialise_vector( x) ;
    numerics::vector< T > y_native( x ) ;
    numerics::vector< T > y_toblas( x ) ;

    test_gemv( os, runs, runs_i, size, size_i, boost::numeric::bindings::traits::TRANSPOSE, alpha, beta, a, x, y_native, y_toblas );
  }
};

template < typename T >
struct gemv_conj_matrix_vector_vector
{
  void operator()(std::ostream& os, int size, int size_i, int runs, int runs_i)
  {
    runs_i = std::max( 1, runs_i / size_i ) ;

    T alpha = 1 / ( size_i * 10 ), beta = 1.0 ;
    random_initialise( alpha );
    numerics::matrix< T, numerics::column_major > a(size_i,size_i) ;
    random_initialise_matrix( a ) ;
    numerics::vector< T > x( size_i );
    random_initialise_vector( x) ;
    numerics::vector< T > y_native( x ) ;
    numerics::vector< T > y_toblas( x ) ;

    test_gemv( os, runs, runs_i, size, size_i, boost::numeric::bindings::traits::CONJUGATE, alpha, beta, a, x, y_native, y_toblas );
  }
};

template < typename T >
struct gemv_matrix_range_vector_vector
{
  void operator()(std::ostream& os, int size, int size_i, int runs, int runs_i)
  {
    runs_i = std::max( 1, runs_i / size_i ) ;

    T alpha = 1 / ( size_i * 10 ), beta = 1.0 ;
    random_initialise( alpha );
    numerics::matrix< T, numerics::column_major > a(size_i * 2,size_i * 2) ;
    random_initialise_matrix( a ) ;
    int start = size_i / 2 ;
    int stop = start + size_i ;
    numerics::matrix_range< numerics::matrix< T, numerics::column_major > > mr( a, numerics::range( start, stop ), numerics::range( start, stop ) ) ;
    numerics::vector< T > x( size_i );
    random_initialise_vector( x) ;
    numerics::vector< T > y_native( x ) ;
    numerics::vector< T > y_toblas( x ) ;

    test_gemv( os, runs, runs_i, size, size_i, boost::numeric::bindings::traits::NO_TRANSPOSE, alpha, beta, mr, x, y_native, y_toblas );
  }
};

int main (int argc, char *argv [])
{
  int runs = 1 ; // 10000000 ;
  int stop  = 10 ; // 10000 ;

  switch ( argc ) {
  case 3:
    stop = atoi( argv[2] ) ;
  case 2:
    runs = atoi( argv[1] ) ;
  case 1:
  default: {}
  }

  int start = 1 ;
  int step  = 50 ;

  std::cerr << "\npeak float\n";
  peak<float> () ( runs );

  std::cerr << "\npeak double\n";
  peak<double> () ( runs );

  std::cerr << "\nstd:complex<float>\n";
  peak<std::complex<float> > () ( runs );

  std::cerr << "\nstd:complex<double>\n";
  peak<std::complex<double> > () ( runs );

  if (argc > 1) {
    int scale = atoi(argv [1]);
    runs *= scale ;
  }

  {
    {
      std::cerr <<         "gemv_matrix_vector_vector_double" << std::endl ;
      std::ofstream stream("gemv_matrix_vector_vector_double");
      loop( stream, start, step, stop, runs, gemv_matrix_vector_vector<double>() ) ;
    }

    {
      std::cerr <<         "gemv_matrix_vector_vector_double_complex" << std::endl ;
      std::ofstream stream("gemv_matrix_vector_vector_double_complex");
      loop( stream, start, step, stop, runs, gemv_matrix_vector_vector<std::complex<double> >() ) ;
    }

    {
      std::cerr <<         "gemv_trans_matrix_vector_vector_double" << std::endl ;
      std::ofstream stream("gemv_trans_matrix_vector_vector_double");
      loop( stream, start, step, stop, runs, gemv_trans_matrix_vector_vector<double>() ) ;
    }

    {
      std::cerr <<         "gemv_trans_matrix_vector_vector_double_complex" << std::endl ;
      std::ofstream stream("gemv_trans_matrix_vector_vector_double_complex");
      loop( stream, start, step, stop, runs, gemv_trans_matrix_vector_vector<std::complex<double> >() ) ;
    }

    {
      std::cerr <<         "gemv_conj_matrix_vector_vector_double_complex" << std::endl ;
      std::ofstream stream("gemv_conj_matrix_vector_vector_double_complex");
      loop( stream, start, step, stop, runs, gemv_conj_matrix_vector_vector<std::complex<double> >() ) ;
    }

    {
      std::cerr <<         "gemv_matrix_range_vector_vector_double" << std::endl ;
      std::ofstream stream("gemv_matrix_range_vector_vector_double");
      loop( stream, start, step, stop, runs, gemv_matrix_range_vector_vector<double>() ) ;
    }

    {
      std::cerr <<         "gemv_matrix_range_vector_vector_double_complex" << std::endl ;
      std::ofstream stream("gemv_matrix_range_vector_vector_double_complex");
      loop( stream, start, step, stop, runs, gemv_matrix_range_vector_vector<std::complex<double> >() ) ;
    }
  }

  return 0 ;
}


#ifndef _maps_Utils_hpp_
#define _maps_Utils_hpp_

#include <pcl/point_cloud.h>
#include <Eigen/Geometry>
#include <vector>

namespace maps {

class Utils {

public:

  template<typename PointType>
  static Eigen::Isometry3f
  getPose(const pcl::PointCloud<PointType>& iCloud) {
    Eigen::Isometry3f xform = Eigen::Isometry3f::Identity();
    xform.linear() = iCloud.sensor_orientation_.matrix();
    Eigen::Vector4f position = iCloud.sensor_origin_;
    xform.translation() = position.head<3>();
    return xform;
  }

  template<typename PointType>
  static void
  setPose(const Eigen::Isometry3f iPose, pcl::PointCloud<PointType>& oCloud) {
    for (int i = 0; i < 3; ++i) {
      oCloud.sensor_origin_[i] = iPose.translation()[i];
    }
    oCloud.sensor_origin_[3] = 1;
    oCloud.sensor_orientation_ = iPose.rotation();
  }

  template<typename PointType1, typename PointType2>
  static void copyPose(const pcl::PointCloud<PointType1>& iCloud,
                       pcl::PointCloud<PointType2>& oCloud) {
    oCloud.sensor_origin_ = iCloud.sensor_origin_;
    oCloud.sensor_orientation_ = iCloud.sensor_orientation_;
  }

  template<typename PointType>
  static bool crop(const pcl::PointCloud<PointType>& iCloud,
                   pcl::PointCloud<PointType>& oCloud,
                   const std::vector<Eigen::Vector4f>& iPlanes) {
    std::vector<int> survivors;
    if (!crop(iCloud, survivors, iPlanes)) {
      return false;
    }
    oCloud.resize(iCloud.size());
    for (int i = 0; i < survivors.size(); ++i) {
      oCloud[i] = iCloud[survivors[i]];
    }
    oCloud.resize(survivors.size());
    oCloud.width = oCloud.size();
    oCloud.height = 1;
    oCloud.is_dense = false;
    copyPose(iCloud, oCloud);
    return true;
  }

  template<typename PointType>
  static bool crop(const pcl::PointCloud<PointType>& iCloud,
                   std::vector<int>& oSurvivors,
                   const std::vector<Eigen::Vector4f>& iPlanes) {
    oSurvivors.clear();
    oSurvivors.reserve(iCloud.size());
    for (int i = 0; i < iCloud.size(); ++i) {
      PointType pt = iCloud[i];
      bool survived = true;
      for (int j = 0; j < iPlanes.size(); ++j) {
        Eigen::Vector4f plane = iPlanes[j].cast<float>();
        if (plane[0]*pt.x + plane[1]*pt.y + plane[2]*pt.z < -plane[3]) {
          survived = false;
          continue;
        }
      }
      if (survived) {
        oSurvivors.push_back(i);
      }
    }
    return true;
  }

  template<typename PointType>
  static bool crop(const pcl::PointCloud<PointType>& iCloud,
                   pcl::PointCloud<PointType>& oCloud,
                   const Eigen::Vector3f& iMin, const Eigen::Vector3f& iMax) {
    std::vector<int> survivors;
    if (!crop(iCloud, survivors, iMin, iMax)) {
      return false;
    }
    oCloud.resize(iCloud.size());
    for (int i = 0; i < survivors.size(); ++i) {
      oCloud[i] = iCloud[survivors[i]];
    }
    oCloud.resize(survivors.size());
    oCloud.width = oCloud.size();
    oCloud.height = 1;
    oCloud.is_dense = false;
    copyPose(iCloud, oCloud);
    return true;
  }

  template<typename PointType>
  static bool crop(const pcl::PointCloud<PointType>& iCloud,
                   std::vector<int>& oSurvivors,
                   const Eigen::Vector3f& iMin, const Eigen::Vector3f& iMax) {
    oSurvivors.clear();
    oSurvivors.reserve(iCloud.size());
    for (int i = 0; i < iCloud.size(); ++i) {
      PointType pt = iCloud[i];
      if ((pt.x >= iMin[0]) && (pt.x <= iMax[0]) &&
          (pt.y >= iMin[1]) && (pt.y <= iMax[1]) &&
          (pt.z >= iMin[2]) && (pt.z <= iMax[2])) {
        oSurvivors.push_back(i);
      }
    }
    return true;
  }

  static bool clipRay(const Eigen::Vector3f& iOrigin,
                      const Eigen::Vector3f& iEndPoint,
                      const Eigen::Vector3f& iBoxMin,
                      const Eigen::Vector3f& iBoxMax,
                      Eigen::Vector3f& oOrigin,
                      Eigen::Vector3f& oEndPoint,
                      float& oMinT, float& oMaxT);

  static bool clipRay(const Eigen::Vector3f& iOrigin,
                      const Eigen::Vector3f& iEndPoint,
                      const std::vector<Eigen::Vector4f>& iPlanes,
                      Eigen::Vector3f& oOrigin,
                      Eigen::Vector3f& oEndPoint,
                      float& oMinT, float& oMaxT);

  static std::vector<Eigen::Vector4f>
  planesFromBox(const Eigen::Vector3f& iBoundMin,
                const Eigen::Vector3f& iBoundMax);

  static bool polyhedronFromPlanes(const std::vector<Eigen::Vector4f>& iPlanes,
                                   std::vector<Eigen::Vector3f>& oVertices,
                                   std::vector<std::vector<int> >& oFaces,
                                   const double iTol=1e-5);

  static std::vector<Eigen::Vector4f>
  planesFromPolyhedron(const std::vector<Eigen::Vector3f>& iVertices,
                       const std::vector<std::vector<int> >& iFaces);

  static bool isOrthographic(const Eigen::Matrix4f& iMatrix);
  static bool composeViewMatrix(Eigen::Projective3f& oMatrix,
                                const Eigen::Matrix3f& iCalib,
                                const Eigen::Isometry3f& iPose,
                                const bool iIsOrthographic);
  static bool factorViewMatrix(const Eigen::Projective3f& iMatrix,
                               Eigen::Matrix3f& oCalib,
                               Eigen::Isometry3f& oPose,
                               bool& oIsOrthographic);

  static uint64_t rand64();
};

}

#endif


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

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

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIRECITON_CODE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIRECITON_CODE_HPP

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Index, typename Point1, typename Point2>
inline int sign_of_difference(Point1 const& point1, Point2 const& point2)
{
    return
        math::equals(geometry::get<Index>(point1), geometry::get<Index>(point2))
        ?
        0
        :
        (geometry::get<Index>(point1) > geometry::get<Index>(point2) ? 1 : -1);
}

// Gives sense of direction for point p, collinear w.r.t. segment (a,b)
// Returns -1 if p goes backward w.r.t (a,b), so goes from b in direction of a
// Returns 1 if p goes forward, so extends (a,b)
// Returns 0 if p is equal with b, or if (a,b) is degenerate
// Note that it does not do any collinearity test, that should be done before
template <typename Point1, typename Point2>
inline int direction_code(Point1 const& segment_a, Point1 const& segment_b,
                          const Point2& p)
{
    // Suppose segment = (4 3,4 4) and p =(4 2)
    // Then sign_a1 = 1 and sign_p1 = 1 -> goes backward -> return -1

    int const sign_a0 = sign_of_difference<0>(segment_b, segment_a);
    int const sign_a1 = sign_of_difference<1>(segment_b, segment_a);

    if (sign_a0 == 0 && sign_a1 == 0)
    {
        return 0;
    }

    int const sign_p0 = sign_of_difference<0>(segment_b, p);
    int const sign_p1 = sign_of_difference<1>(segment_b, p);

    if (sign_p0 == 0 && sign_p1 == 0)
    {
        return 0;
    }

    return sign_a0 == sign_p0 && sign_a1 == sign_p1 ? -1 : 1;
}

} // namespace detail
#endif //DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIRECITON_CODE_HPP


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include <math.h>

namespace caffe {

// 这些公式参考这些：https://blog.csdn.net/seven_first/article/details/47378697
template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

/*********new added start***************************************************************************/

// 从这往后新加的代码
template <>
void caffe_clip<int>(const int n, int* y){
// NOT IMPLEMENTED
}

template <>
void caffe_clip<unsigned int>(const int n, unsigned int* y){
// NOT IMPLEMENTED
}

template <>
void caffe_clip<float>(const int n, float* y){
  for(int i=0; i<n; i++){
	const float x = y[i];
	y[i] = (x>1) - (x<1) + x * (x<=1 && x>=-1);
  }
}
/* 实现的功能：
       { -1, x<-1
f(x) = {  x, |x|<=1
       {  1, x>1
*/
template <>
void caffe_clip<double>(const int n, double* y){
  for(int i=0; i<n; i++){
	const double x = y[i];
	y[i] = (x>1) - (x<1) + x * (x<=1 && x>=-1);
  }
}

template <>
void caffe_quantize<int>(const int N, const int left, const int right, const int* X, int *Y){
// NOT IMPLEMENTED
}

template <>
void caffe_quantize<unsigned>(const int N, const unsigned left, const unsigned right,
const unsigned*X, unsigned* Y){
// NOT IMPLEMENTED
}

template <>
void caffe_quantize<float>(const int N, const float left, const float right, const float* X, float* Y){
  for(int i=0; i<N; i++){
	const float x = X[i];
	float idx = (x >= 0) ? floor(log2(x)) : floor(log2(-x));
	idx = (idx<left)*left + (idx>=left && idx<=right)*idx + (idx>right)*right;
	float sign = (x>=0) ? 1.0f : -1.0f;
	float p_up = sign * x / pow(2.0f, idx) - 1;
	float idx_rand = idx + (p_up >= 0.5);

	Y[i] = sign * pow(2.0f, idx_rand);
  }
}

template <>
void caffe_quantize<double>(const int N, const double left, const double right, const double* X, double* Y){
// 参数含义： N：元素个数 left：   right：     X：   Y：
  for(int i=0; i<N; i++){
	const double x = X[i];
	double idx = (x >= 0) ? floor(log2(x)) : floor(log2(-x)); // 看绝对值超过1，置1，否则为0
	idx = (idx<left)*left + (idx>=left && idx<=right)*idx + (idx>right)*right;
	double sign = (x>=0) ? 1.0d : -1.0d;
	double p_up = sign * x / pow(2.0d, idx) - 1;
	double idx_rand = idx + (p_up >= 0.5);

	Y[i] = sign * pow(2.0d, idx_rand);
  }
}

template<>
void caffe_cpu_ternary<int>(const int N, const int delta, const int* X, int* Y){
// NOT IMPLEMENTED
}

template<>
void caffe_cpu_ternary<unsigned>(const int N, const unsigned delta, const unsigned* X, unsigned* Y){
// NOT IMPLEMENTED
}

template<>
void caffe_cpu_ternary<float>(const int N, const float delta, const float* X, float* Y){
	for(int i=0; i<N; i++){
		float x = X[i];
		Y[i] = (x>delta) - (x<-delta);
	}
}

template<>
void caffe_cpu_ternary<double>(const int N, const double delta, const double* X, double* Y){
	for(int i=0; i<N; i++){
		double x = X[i];
		Y[i] = (x>delta) - (x<-delta);
	}
}

///*## 我写的量化公式 start ---- ####################################################################*/
///*
//bit_width:量化的位宽
//
//*/
//template<>
//void caffe_cpu_multi_bit_quantization<int>(const int N, const int bit_width, const int* X, int* Y){
//// NOT IMPLEMENTED
//}
//
//template<>
//void caffe_cpu_multi_bit_quantization<unsigned>(const int N, const int bit_width, const unsigned* X, unsigned* Y){
//// NOT IMPLEMENTED
//}
//
//template<>
//void caffe_cpu_multi_bit_quantization<float>(const int N, const int bit_width, const float* X, float* Y){
//	for(int i=0; i<N; i++){
//        caffe_clip(N, X);
//		float x = X[i];
//		Y[i] = round((pow(2,bit_width)-1.0)*x)/(pow(2,bit_width)-1.0);
//	}
//}
//
//template<>
//void caffe_cpu_multi_bit_quantization<double>(const int N, const int bit_width, const double* X, double* Y){
//	for(int i=0; i<N; i++){
//		caffe_clip(N, X);
//		double x = X[i];
//		Y[i] = round((pow(2,bit_width)-1.0)*x)/(pow(2,bit_width)-1.0);
//	}
//}
///*## 我写的量化公式 end ---- ####################################################################*/

/*********new added end********************************************************************************/
template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

/*
template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}*/

template <>
int caffe_cpu_dot<int>(const int n, const int* x, const int* y){
  // return caffe_cpu_strided_dot(n, x, 1, y, 1);
  return 0;
// NOT IMPLEMENTED
}

template <>
unsigned caffe_cpu_dot<unsigned>(const int n, const unsigned* x, const unsigned* y){
  // return caffe_cpu_strided_dot(n, x, 1, y, 1);
  return 0;
// NOT IMPLEMENTED
}

template <>
float caffe_cpu_dot<float>(const int n, const float* x, const float* y){
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template <>
double caffe_cpu_dot<double>(const int n, const double* x, const double* y){
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template <>
int caffe_cpu_asum<int>(const int n, const int* x){
return 0;
// NOT IMPLEMENTED YET
}

template <>
unsigned caffe_cpu_asum<unsigned>(const int n, const unsigned* x){
return 0;
// NOT IMPLEMENTED YET
}

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}
template <>
void caffe_cpu_scale<int>(const int n, const int alpha, const int* x, int* y){
// NOT IMPLEMENTED YET
}

template <>
void caffe_cpu_scale<unsigned>(const int n, const unsigned alpha, const unsigned* x, unsigned* y){
// NOT IMPLEMENTED YET
}
template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


/*
  High Performance Astrophysical Reconstruction and Processing (HARP)
  (c) 2014-2015, The Regents of the University of California,
  through Lawrence Berkeley National Laboratory.  See top
  level LICENSE file for details.
*/

#ifndef HARP_LINALG_HPP
#define HARP_LINALG_HPP

#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/storage.hpp>

#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/lapack.hpp>
#include <boost/numeric/bindings/views.hpp>

namespace harp {

  typedef enum {
    EIG_NONE,
    EIG_SQRT,
    EIG_INVSQRT,
    EIG_INV
  } eigen_op;

  typedef boost::numeric::ublas::matrix < double, boost::numeric::ublas::column_major > matrix_double;

  typedef boost::numeric::ublas::compressed_matrix < double, boost::numeric::ublas::row_major > matrix_double_sparse;

  typedef boost::numeric::ublas::vector < double > vector_double;

  typedef boost::numeric::ublas::matrix < uint8_t, boost::numeric::ublas::column_major > matrix_mask;

  typedef boost::numeric::ublas::vector < uint8_t > vector_mask;

  typedef boost::numeric::ublas::matrix < float, boost::numeric::ublas::column_major > matrix_float;

  typedef boost::numeric::ublas::compressed_matrix < float, boost::numeric::ublas::row_major > matrix_float_sparse;

  typedef boost::numeric::ublas::vector < float > vector_float;

  void check_column_major ( boost::numeric::ublas::column_major_tag );

  void check_column_major ( boost::numeric::ublas::row_major_tag );

  template < class M >
  void check_column_major ( boost::numeric::ublas::matrix_expression < M > const & matrix ) {
    typedef typename M::orientation_category orientation_category;
    check_column_major ( orientation_category() );
    return;
  }

  void eigen_decompose ( matrix_double const & invcov, vector_double & D, matrix_double & W, bool regularize );

  void eigen_compose ( eigen_op op, vector_double const & D, matrix_double const & W, matrix_double & out );

  void column_norm ( matrix_double const & mat, vector_double & S );

  void apply_norm ( vector_double const & S, matrix_double & mat );

  void apply_inverse_norm ( vector_double const & S, matrix_double & mat );

  void apply_norm ( vector_double const & S, vector_double & vec );

  void apply_inverse_norm ( vector_double const & S, vector_double & vec );

  void norm ( vector_double const & D, matrix_double const & W, vector_double & S );

  void sparse_mv_trans ( matrix_double_sparse const & AT, vector_double const & in, vector_double & out );

}

#endif


// Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#include "rotationalinertia.hpp"
#include <Eigen/Core>

namespace KDL
{
	RotationalInertia::RotationalInertia(double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz)
	{
        data[0]=Ixx;
        data[1]=data[3]=Ixy;
        data[2]=data[6]=Ixz;
        data[4]=Iyy;
        data[5]=data[7]=Iyz;
        data[8]=Izz;

	}

	RotationalInertia::~RotationalInertia()
	{
	}

	Vector RotationalInertia::operator*(const Vector& omega) const {
		// Complexity : 9M+6A
        Vector result;
        Eigen::Map<Eigen::Vector3d>(result.data) = Eigen::Map<const Eigen::Matrix3d>(this->data) * Eigen::Map<const Eigen::Vector3d>(omega.data);
        return result;
 	}

    RotationalInertia operator*(double a, const RotationalInertia& I){
        RotationalInertia result;
        Eigen::Map<Eigen::Matrix3d>(result.data) = a * Eigen::Map<const Eigen::Matrix3d>(I.data);
        return result;
    }

    RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){
        RotationalInertia result;
        Eigen::Map<Eigen::Matrix3d>(result.data) = Eigen::Map<const Eigen::Matrix3d>(Ia.data) + Eigen::Map<const Eigen::Matrix3d>(Ib.data);
        return result;
    }
}


/* Copyright (c) 2013-2014 Fabian Schuiki */
#define BOOST_TEST_MODULE gamma
#include "gamma/integer.hpp"
#include "gamma/vector.hpp"
#include "gamma/matrix.hpp"
#include "gamma/fixed_point.hpp"
#include "gamma/transform/translation.hpp"
#include "gamma/transform/x_rotation.hpp"
#include "gamma/transform/y_rotation.hpp"
#include "gamma/transform/z_rotation.hpp"
#include "gamma/transform/axial_rotation.hpp"
#include "gamma/transform/perspective.hpp"
#include "gamma/transform/orientation.hpp"
#include "gamma/transform/lookat.hpp"
#include "gamma/mvp.hpp"
#include <boost/test/unit_test.hpp>

using namespace gma::convenience;

BOOST_AUTO_TEST_CASE(integer)
{
	#define check(_type, _bits, _target)\
		BOOST_CHECK_EQUAL((int)(gma::integer::_type<_bits>::requested_bits), _bits);\
		BOOST_CHECK_EQUAL((int)(gma::integer::_type<_bits>::bits), _target*8);\
		BOOST_CHECK_EQUAL(sizeof(gma::integer::_type<_bits>::type), _target);

	#define check_pair(bits, target)\
		check(signed_integer, bits, target);\
		check(unsigned_integer, bits, target);

	#define check_eight(bits, target)\
		check_pair(bits+1, target);\
		check_pair(bits+2, target);\
		check_pair(bits+3, target);\
		check_pair(bits+4, target);\
		check_pair(bits+5, target);\
		check_pair(bits+6, target);\
		check_pair(bits+7, target);\
		check_pair(bits+8, target);

	check_eight(0, 1);
	check_eight(8, 2);
	check_eight(16, 4);
	check_eight(24, 4);
	check_eight(32, 8);
	check_eight(40, 8);
	check_eight(48, 8);
	check_eight(56, 8);

	#undef check
	#undef check_pair
	#undef check_eight
}

BOOST_AUTO_TEST_CASE(sizes)
{
	#define check(type, target) BOOST_CHECK_EQUAL(sizeof(type), target)

	#define check_vector(dim)\
		check(vector ## dim ## b, 1*dim);\
		check(vector ## dim ## i, 4*dim);\
		check(vector ## dim ## f, 4*dim);\
		check(vector ## dim ## d, 8*dim);

	#define check_matrix(dim)\
		check(matrix ## dim ## b, 1*dim*dim);\
		check(matrix ## dim ## i, 4*dim*dim);\
		check(matrix ## dim ## f, 4*dim*dim);\
		check(matrix ## dim ## d, 8*dim*dim);

	check_vector(2);
	check_vector(3);
	check_vector(4);

	check_matrix(2);
	check_matrix(3);
	check_matrix(4);

	#undef check
	#undef check_vector
	#undef check_matrix
}

BOOST_AUTO_TEST_CASE(matrix_column_major)
{
	#define verify_column_major(type, dim) {\
		type m;\
		m.m10 = 1;\
		m.m01 = 2;\
		BOOST_CHECK(m.a[0][1] == 1 && m.a[1][0] == 2);\
	}

	#define verify_column_major_matrices(dim)\
		verify_column_major(matrix ## dim ## b, dim);\
		verify_column_major(matrix ## dim ## i, dim);\
		verify_column_major(matrix ## dim ## f, dim);\
		verify_column_major(matrix ## dim ## d, dim);

	verify_column_major_matrices(2);
	verify_column_major_matrices(3);
	verify_column_major_matrices(4);

	#undef verify_column_major
	#undef verify_column_major_matrices
}

/// This test tries to trigger an overflow of a fixed_point's underlying
/// storage by multiplying two large numbers.
BOOST_AUTO_TEST_CASE(fixed_point_overflow)
{
	gma::fixed_point<24,8> a0, a1, a2;
	a0.v = 0x00008000;
	a1.v = 0x00010000;
	a2.v = 0x0007ff00;

	BOOST_CHECK_EQUAL((a0*a0).v, 0x80*0x80 * 0x100);
	BOOST_CHECK_EQUAL((a1*a1).v, 0x100*0x100 * 0x100);
	BOOST_CHECK_EQUAL((a2*a2).v, 0x7ff*0x7ff * 0x100);

	BOOST_CHECK_EQUAL((int)a0, 0x80);
	BOOST_CHECK_EQUAL((int)a1, 0x100);
	BOOST_CHECK_EQUAL((int)a2, 0x7ff);
}

BOOST_AUTO_TEST_CASE(fixed_point_rounding)
{
	gma::fixed_point<24,8> a0(0x200,8), a1(0x199,8), a2(0x201,8), b0(0x180,8), b1(0x17f,8), b2(0x181,8);

	BOOST_REQUIRE(a0.v == 0x200);
	BOOST_REQUIRE(a1.v == 0x199);
	BOOST_REQUIRE(a2.v == 0x201);

	BOOST_REQUIRE(b0.v == 0x180);
	BOOST_REQUIRE(b1.v == 0x17f);
	BOOST_REQUIRE(b2.v == 0x181);

	BOOST_CHECK_EQUAL(a0.floor().v, 0x200); BOOST_CHECK_EQUAL(a1.floor().v, 0x100);	BOOST_CHECK_EQUAL(a2.floor().v, 0x200);
	BOOST_CHECK_EQUAL(b0.floor().v, 0x100); BOOST_CHECK_EQUAL(b1.floor().v, 0x100);	BOOST_CHECK_EQUAL(b2.floor().v, 0x100);

	BOOST_CHECK_EQUAL(a0.round().v, 0x200); BOOST_CHECK_EQUAL(a1.round().v, 0x200);	BOOST_CHECK_EQUAL(a2.round().v, 0x200);
	BOOST_CHECK_EQUAL(b0.round().v, 0x200); BOOST_CHECK_EQUAL(b1.round().v, 0x100);	BOOST_CHECK_EQUAL(b2.round().v, 0x200);

	BOOST_CHECK_EQUAL(a0.ceil().v, 0x200); BOOST_CHECK_EQUAL(a1.ceil().v, 0x200);	BOOST_CHECK_EQUAL(a2.ceil().v, 0x300);
	BOOST_CHECK_EQUAL(b0.ceil().v, 0x200); BOOST_CHECK_EQUAL(b1.ceil().v, 0x200);	BOOST_CHECK_EQUAL(b2.ceil().v, 0x200);
}


/**
 * @file   NMAAnalyzer.hpp
 * @author see AUTHORS
 * @brief  NMAAnalyzer header file.
 */

#ifndef NONLINEARFITTER_HPP
#define NONLINEARFITTER_HPP

#include <math.h>

#include <Eigen/Dense>

/**
 * @class NonlinearFitter
 */
class NonlinearFitter {
public:
    /**
     * @param x
     * @param y
     * @param yerr2
     */
    void exponential_fit(Eigen::VectorXd & x, Eigen::VectorXd & y, Eigen::VectorXd & yerr2); //length == ndat/S

    /**
     * @return a
     */
    double get_a();

    /**
     * @return b
     */
    double get_b();

    /**
     * @return error for a
     */
    double get_err_a();

    /**
     * @return error for b
     */
    double get_err_b();
private:
    double a = 0;
    double b = 0;
    double err_a = 0;
    double err_b = 0;
    bool calculation_completed = false;
};

#endif

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4


/******************************************************************************

  Basic linear algebra subprograms.

  Copyright (c) 2011 - 2013
  Dzmitry Hlindzich <dzmitry.hlindzich@ziti.uni-heidelberg.de>
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 THE AUTHOR OR CONTRIBUTORS 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.

*******************************************************************************/

#ifndef BLAS_EXTENSIONS_HPP_F74A6974_6444_4C40_BFE7_75ADEC15B7E6_
#define BLAS_EXTENSIONS_HPP_F74A6974_6444_4C40_BFE7_75ADEC15B7E6_

#include <iostream>
#include <vector>
#include <cmath>
#include <boost/format.hpp>

// Suppress boost::numeric::ublas C4127 warning under MSVC.
#ifdef _MSC_VER
#   pragma warning(push)
#   pragma warning(disable:4127)
#endif // _MSC_VER

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

#ifdef _MSC_VER
#   pragma warning(pop)
#endif // _MSC_VER

#include "bo/math/functions.hpp"

// Dirty hack: add stream operator for matrix<T> type into boost namespace.
namespace boost {
namespace numeric {
namespace ublas {

// Prints formatted matrix to the given stream. T must support operator<<.
template <typename T>
std::ostream& operator<<(std::ostream& os, const matrix<T>& obj)
{
    // Print full vertex info.
    os << boost::format("%1%x%2% boost::numeric::ublas::matrix, object %3$#x, %4% bytes: ")
          % obj.size1() % obj.size2() % &obj % sizeof(obj) << std::endl;

    for (std::size_t i = 0; i < obj.size1(); ++i)
    {
        for (std::size_t j = 0; j < obj.size2(); ++j)
        {
            os << "\t " << obj(i, j);
        }
        os << std::endl;
    }

    os << boost::format("end of object %1$#x.") % &obj << std::endl;

    return os;
}

} // namespace ublas
} // namespace numeric
} // namespace boost

namespace bo {
namespace math {

using namespace boost::numeric::ublas;

// Matrix inversion routine.
// Use lu_factorize and lu_substitute in uBLAS to invert a matrix.
template<class T>
bool invert_matrix(const matrix<T>& input, matrix<T>& inverse)
{
    // Create a working copy of the input
    matrix<T> A(input);

    // Create a permutation matrix for the LU-factorization
    permutation_matrix<std::size_t> pm(A.size1());

    // Perform LU-factorization
    size_t res = lu_factorize(A, pm);
    if (res != 0)
        return false;

    // Create identity matrix of "inverse"
    inverse.assign(identity_matrix<T> (A.size1()));

    // Backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);

    return true;
}

// Define the sign of the determinant using the given permutation matrix.
inline
int determinant_sign(const permutation_matrix<std::size_t>& pm)
{
    int pm_sign=1;
    std::size_t size = pm.size();
    for (std::size_t i = 0; i < size; ++i)
    {
        if (i != pm(i))
        {
            // swap_rows would swap a pair of rows here, so we change sign
            pm_sign *= -1;
        }
    }

    return pm_sign;
}

// Calculate the determinant of the input matrix.
template<class T>
double determinant(const matrix<T>& input)
{
    // create a working copy of the input
    matrix<T> A(input);

    // create a permutation matrix for the LU-factorization
    permutation_matrix<std::size_t> pm(A.size1());

    // perform LU-factorization
    int res = lu_factorize(A, pm);

    double det=1.0;

    if (res != 0 )
    {
        det = 0.0;
    }
    else
    {
        // multiply by elements on diagonal
        for(int i = 0; i < A.size1(); ++i)
            det *= A(i,i);
        det = det * determinant_sign( pm );
    }

    return det;
}

// Eigenvector decomposition for real symmetric matrices. Returns a vector of the
// eigenvalues, sorted in non-decreasing order. The corresponding eigenvectors are
// stored in the columns of the matrix expr.
template <class E>
std::vector<typename E::value_type> eigen_symmetric(matrix_expression<E>& expr)
{
    // Cache the underlying matrix type.
    typedef typename E::value_type T;

    // Evaluate and cache the matrix_expression.
    typename E::closure_type A(expr());

    // Allocate the return vector.
    const int n(static_cast<int>(A.size1()));
    std::vector<T> d(n);

    // Initialize the return vector.
    for (int j = 0; j < n; ++j)
    {
        d[j] = A(n - 1, j);
    }

    BOOST_ASSERT(A.size2() == (unsigned)n);

    // Check for a square matrix.
    if (A.size2() != (unsigned)n)
    {
        return d;
    }

    std::vector<T> e(n, T(0));

    // Iterating.
    for (int i = n - 1; i > 0; --i)
    {
        T scale(0);

        for (int k = 0; k < i; ++k)
        {
            scale += std::abs(d[k]);
        }

        if (scale == T(0))
        {
            e[i] = d[i - 1];

            for (int j = 0; j < i; ++j)
            {
                d[j] = A(i - 1, j);
                A(i, j) = A(j, i) = T(0);
            }

            d[i] = T(0);
        }
        else
        {
            T h(0);
            T invscale = T(1.0 / scale);

            for (int k = 0; k < i; ++k)
            {
                d[k] *= invscale;
                h += square(d[k]);
            }

            T f = d[i - 1];
            T g = (f > 0) ? -std::sqrt(h) : std::sqrt(h);
            e[i] = scale * g;
            h -= f * g;
            d[i - 1] = f - g;

            for (int j = 0; j < i; ++j)
            {
                e[j] = T(0);
            }

            for (int j = 0; j < i; ++j)
            {
                f = d[j];
                A(j, i) = f;
                g = e[j] + f * A(j, j);

                for (int k = j+1; k < i; k++)
                {
                    g += A(k, j) * d[k];
                    e[k] += A(k, j) * f;
                }

                e[j] = g;
            }

            f = T(0);
            T invh = T(1.0 / h);

            for (int j = 0; j < i; ++j)
            {
                e[j] *= invh;
                f += e[j] * d[j];
            }

            T hh = f / (h + h);

            for (int j = 0; j < i; j++)
            {
                e[j] -= hh * d[j];
            }

            for (int j = 0; j < i; j++)
            {
                f = d[j];
                g = e[j];

                for (int k = j; k < i; k++)
                {
                    A(k, j) -= f * e[k] + g * d[k];
                }

                d[j] = A(i - 1, j);
                A(i, j) = T(0);
            }

            d[i] = h;
        }
    }

    // I put on my robe and wizard hat...
    for (int i = 0; i < n - 1; ++i)
    {
        A(n - 1, i) = A(i, i);
        A(i, i) = 1;

        T h = d[i+1];

        if (h != T(0))
        {
            T invh = T(1.0 / h);

            for (int k = 0; k <= i; ++k)
            {
                d[k] = A(k, i + 1) * invh;
            }

            for (int j = 0; j <= i; ++j)
            {
                T g(0);

                for (int k = 0; k <= i; ++k)
                {
                    g += A(k, i + 1) * A(k, j);
                }

                for (int k = 0; k <= i; ++k)
                {
                    A(k, j) -= g * d[k];
                }

            }
        }

        for (int k = 0; k <= i; ++k)
        {
            A(k, i + 1) = T(0);
        }
    }

    for (int j = 0; j < n; ++j)
    {
        d[j] = A(n - 1, j);
        A(n - 1, j) = T(0);
    }

    A(n - 1, n - 1) = 1;

    // QL.
    for (int i = 1; i < n; ++i)
    {
        e[i - 1] = e[i];
    }

    e[n - 1] = T(0);
    T f(0), tmp(0);
    const T eps = T(std::pow(T(2), -52));

    for (int l = 0; l < n; l++)
    {
        tmp = std::max(tmp, std::fabs(d[l]) + std::fabs(e[l]));
        int m = l;

        while (m < n)
        {
            if (std::fabs(e[m]) <= eps * tmp)
                break;
            ++m;
        }

        if (m > l)
        {
            do
            {
                T g = d[l];
                T p = (d[l + 1] - g) / (e[l] + e[l]);
                T r = T(std::sqrt(square(p) + T(1)));

                if (p < T(0))
                {
                    r = -r;
                }

                d[l] = e[l] / (p + r);
                d[l + 1] = e[l] * (p + r);
                T dl1 = d[l + 1];
                T h = g - d[l];

                for (int i = l + 2; i < n; ++i)
                {
                    d[i] -= h;
                }

                f += h;
                p = d[m];
                T c(1), c2(1), c3(1);

                T el1 = e[l + 1];
                T s(0), s2(0);

                for (int i = m - 1; i >= l; --i)
                {
                    c3 = c2;
                    c2 = c;
                    s2 = s;
                    g = c * e[i];
                    h = c * p;

                    r = T(std::sqrt(square(p) + square(e[i])));

                    e[i + 1] = s * r;
                    s = e[i] / r;
                    c = p / r;
                    p = c * d[i] - s * g;
                    d[i + 1] = h + s * (c * g + s * d[i]);

                    for (int k = 0; k < n; ++k)
                    {
                        h = A(k, i + 1);
                        A(k, i + 1) = s * A(k, i) + c * h;
                        A(k, i) = c * A(k, i) - s * h;
                    }
                }

                p = -s * s2 * c3 * el1 * e[l] / dl1;

                e[l] = s * p;
                d[l] = c * p;

            }
            while (std::fabs(e[l]) > eps * tmp);
        }

        d[l] += f;
        e[l] = T(0);
    }

    // Sort.
    for (int i = 0; i < n - 1; ++i)
    {
        int k = i;
        T p = d[i];

        for (int j = i + 1; j < n; ++j)
        {
            if (d[j] < p)
            {
                k = j;
                p = d[j];
            }
        }

        if (k == i) continue;

        d[k] = d[i];
        d[i] = p;

        for (int j = 0; j < n; ++j)
        {
            p = A(j, i);
            A(j, i) = A(j, k);
            A(j, k) = p;
        }
    }

    return d;
}

// Computes the L1 norm of matrix. Uses std::abs(T), which implies T is a built-in type
// or a custom type, for which abs() is provided in std namespace.
template <class T>
T l1_norm(const matrix<T>& A)
{
    T sum(0);

    for (std::size_t i = 0; i < A.size1(); ++i)
        for (std::size_t j = 0; j < A.size2(); ++j)
            sum += std::abs(A(i, j));

    return sum;
}

} // namespace math
} // namespace bo

#endif // BLAS_EXTENSIONS_HPP_F74A6974_6444_4C40_BFE7_75ADEC15B7E6_


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>

#define XGBOOST_CUSTOMIZE_MSG_
#include <xgboost/src/io/simple_fmatrix-inl.hpp>
#include <xgboost/src/learner/learner-inl.hpp>
#include <xgboost/src/io/simple_dmatrix-inl.hpp>

using namespace xgboost;
using namespace xgboost::learner;
using namespace xgboost::io;

constexpr double DELTA = 1e-7;

namespace xgboost {
namespace utils {
  void HandleAssertError(const char *msg) {
    fprintf(stderr, "AssertError:%s\n", msg);
    exit(-1);
  }
  void HandleCheckError(const char *msg) {
    throw std::runtime_error(msg);
  }
  void HandlePrint(const char *msg) {
    printf("%s", msg);
  }
 }
}

struct decision_tree_test {

  void set_options(BoostLearner& model, std::string obj) {
    model.SetParam("eta", "1"); // learning_rate
    model.SetParam("max_depth", "1");
    model.SetParam("gamma", "0.0"); // min loss reduction
    model.SetParam("min_child_weight", "0.0"); // min child weight
    model.SetParam("lambda", "0.0"); // regularizer
    model.SetParam("objective", obj.c_str());
  }

 public:
  void test_regression() {
    DMatrixSimple data;
    data.info.labels = {-1, 1};
    data.AddRow({RowBatch::Entry(0, 1)});
    data.AddRow({RowBatch::Entry(0, -1)});

    BoostLearner gbm;
    set_options(gbm, "reg:linear");
    gbm.SetCacheData({&data});
    gbm.InitModel();
    gbm.CheckInit(&data);
    gbm.UpdateOneIter(0, data);

    std::vector<float> preds;
    bool output_margin = true;
    bool pred_leaf = false;
    gbm.Predict(data, output_margin, &preds, 0, pred_leaf);

    // Base scores (B): 0.5, 0.5
    // Gradients (G): 1.5, -0.5
    // Hessians (H): 1, 1
    // Leaf weights (W) = -G / H : -1.5, 0.5
    // Preds = B + learning_rate * W
    TS_ASSERT_DELTA(preds[0], -1.0, DELTA);
    TS_ASSERT_DELTA(preds[1], 1.0, DELTA);
  }

  void test_classifier() {
    DMatrixSimple data;
    data.info.labels = {0, 1};
    data.AddRow({RowBatch::Entry(0, 1)});
    data.AddRow({RowBatch::Entry(0, -1)});

    BoostLearner gbm;
    set_options(gbm, "binary:logistic");
    gbm.SetCacheData({&data});
    gbm.InitModel();
    gbm.CheckInit(&data);
    gbm.UpdateOneIter(0, data);

    std::vector<float> preds;
    bool output_margin = true;
    bool pred_leaf = false;
    gbm.Predict(data, output_margin, &preds, 0, pred_leaf);

    // Base scores (B): 0.0, 0.0
    // Gradients (G): 0.5, -0.5
    // Hessians (H): 0.25, 0.25
    // Leaf weights (W) = -G / H : -2, 2
    // Preds = B + learning_rate * W
    TS_ASSERT_DELTA(preds[0], -2.0, DELTA);
    TS_ASSERT_DELTA(preds[1], 2.0, DELTA);
  }

  void test_multiclass_classifier() {
    DMatrixSimple data;
    data.info.labels = {0, 1, 2};
    data.AddRow({RowBatch::Entry(0, 1)});
    data.AddRow({RowBatch::Entry(0, 0)});
    data.AddRow({RowBatch::Entry(0, -1)});

    BoostLearner gbm;
    set_options(gbm, "multi:softmax");
    gbm.SetParam("num_class", "3");
    gbm.SetParam("max_depth", "2");
    gbm.SetCacheData({&data});
    gbm.InitModel();
    gbm.CheckInit(&data);
    gbm.UpdateOneIter(0, data);

    std::vector<float> preds;
    bool output_margin = true;
    bool pred_leaf = false;
    gbm.Predict(data, output_margin, &preds, 0, pred_leaf);

    // Base scores (B): 0.5
    // Gradients (G): (-2/3, 1/3, 1/3), (1/3, -2/3, 1/3), (1/3, 1/3, -2/3)
    // Hessians (H): 0.444
    // Leaf weights (W) = -G / H : (1.5, -.75, -.75), (-.75, 1.5, -.75), ...
    // Preds = B + learning_rate * W
    TS_ASSERT_DELTA(preds[0], 2.0, DELTA);
    TS_ASSERT_DELTA(preds[1], -.25, DELTA);
    TS_ASSERT_DELTA(preds[2], -.25, DELTA);
    TS_ASSERT_DELTA(preds[3], -.25, DELTA);
    TS_ASSERT_DELTA(preds[4], 2.0, DELTA);
    TS_ASSERT_DELTA(preds[5], -.25, DELTA);
    TS_ASSERT_DELTA(preds[6], -.25, DELTA);
    TS_ASSERT_DELTA(preds[7], -.25, DELTA);
    TS_ASSERT_DELTA(preds[8], 2.0, DELTA);
  }
};

BOOST_FIXTURE_TEST_SUITE(_decision_tree_test, decision_tree_test)
BOOST_AUTO_TEST_CASE(test_regression) {
  decision_tree_test::test_regression();
}
BOOST_AUTO_TEST_CASE(test_classifier) {
  decision_tree_test::test_classifier();
}
BOOST_AUTO_TEST_CASE(test_multiclass_classifier) {
  decision_tree_test::test_multiclass_classifier();
}
BOOST_AUTO_TEST_SUITE_END()


#ifndef MATHTOOLBOX_L_BFGS_HPP
#define MATHTOOLBOX_L_BFGS_HPP

#include <Eigen/Core>
#include <functional>

namespace mathtoolbox
{
    namespace optimization
    {
        void RunLBfgs(const Eigen::VectorXd&                                        x_init,
                      const std::function<double(const Eigen::VectorXd&)>&          f,
                      const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& g,
                      const double                                                  epsilon,
                      const unsigned int                                            max_num_iterations,
                      Eigen::VectorXd&                                              x_star,
                      unsigned int&                                                 num_iterations);
    }
} // namespace mathtoolbox

#endif // MATHTOOLBOX_L_BFGS_HPP


#include <boost/simd/include/functions/shuffle.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/mpl/int.hpp>
#include <iostream>

using boost::mpl::int_;
using boost::simd::pack;
using boost::simd::shuffle;

struct half_half
{
  template<class Index, class Cardinal>
  struct apply  : int_< Index::value < Cardinal::value/2
                      ? Index::value
                      : Index::value+Cardinal::value
                      >
  {};
};

int main()
{
  pack<float,4> f(1,2,3,4), g(10,20,30,40);

  // Gather the first half of f and the second half of g: [1 2 30 40]
  std::cout << shuffle<half_half>(f,g) << "\n";
}


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. 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.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// This class defines the FundamentalMatrixRansacModel class, which
// is derived from the abstract base class RansacModel.
//
///////////////////////////////////////////////////////////////////////////////

#include <iostream>

#include <algorithm>
#include <Eigen/Core>
#include <gflags/gflags.h>
#include <vector>

#include "ransac_problem.h"
#include "fundamental_matrix_ransac_problem.h"
#include "../math/random_generator.h"
#include "../geometry/eight_point_algorithm_solver.h"
#include "../geometry/fundamental_matrix_solver_options.h"

namespace bsfm {

// ------------ FundamentalMatrixRansacModel methods ------------ //

// Default constructor.
FundamentalMatrixRansacModel::FundamentalMatrixRansacModel()
    : F_(Matrix3d::Identity()), error_(0.0) {}

FundamentalMatrixRansacModel::FundamentalMatrixRansacModel(
    const Matrix3d& F)
    : F_(F), error_(0.0) {}

// Destructor.
FundamentalMatrixRansacModel::~FundamentalMatrixRansacModel() {}

// Return model error.
double FundamentalMatrixRansacModel::Error() const {
  return error_;
}

// Evaluate model on a single data element and update error.
bool FundamentalMatrixRansacModel::IsGoodFit(
    const FeatureMatch& data_point,
    double error_tolerance) const {
  const double error = EvaluateEpipolarCondition(data_point);

  // Test squared error against the provided tolerance.
  if (error * error < error_tolerance) {
    return true;
  }
  return false;
}

double FundamentalMatrixRansacModel::EvaluateEpipolarCondition(
    const FeatureMatch& match) const {
  // Construct vectors for 2D keypoints in match.
  Vector3d kp1, kp2;
  kp1 << match.feature1_.u_, match.feature1_.v_, 1;
  kp2 << match.feature2_.u_, match.feature2_.v_, 1;

  // Compute deviation from the epipolar condition.
  const double epipolar_condition = kp2.transpose() * F_ * kp1;
  return epipolar_condition;
}

// ------------ FundamentalMatrixRansacProblem methods ------------ //

// RansacProblem constructor.
FundamentalMatrixRansacProblem::FundamentalMatrixRansacProblem() {}

// RansacProblem destructor.
FundamentalMatrixRansacProblem::~FundamentalMatrixRansacProblem() {}

// Subsample the data.
std::vector<FeatureMatch> FundamentalMatrixRansacProblem::SampleData(
    unsigned int num_samples) {
  // Randomly shuffle the entire dataset and take the first elements.
  std::random_shuffle(data_.begin(), data_.end());

  // Make sure we don't over step.

  if (static_cast<size_t>(num_samples) > data_.size()) {
    VLOG(1) << "Requested more RANSAC data samples than are available. "
               "Returning all data.";
    num_samples = data_.size();
  }

  // Get samples.
  std::vector<FeatureMatch> samples(
      data_.begin(), data_.begin() + static_cast<size_t>(num_samples));

  return samples;
}

// Return all data that was not sampled.
std::vector<FeatureMatch> FundamentalMatrixRansacProblem::RemainingData(
    unsigned int num_sampled_previously) const {
  // In Sample(), the data was shuffled and we took the first
  // 'num_sampled_previously' elements. Here, take the remaining elements.
  if (num_sampled_previously >= data_.size()) {
    VLOG(1) << "No remaining RANSAC data to sample.";
    return std::vector<FeatureMatch>();
  }

  return std::vector<FeatureMatch>(
      data_.begin() + num_sampled_previously, data_.end());
}

// Fit a model to the provided data using the 8-point algorithm.
FundamentalMatrixRansacModel FundamentalMatrixRansacProblem::FitModel(
    const std::vector<FeatureMatch>& input_data) const {
  // Create an empty fundamental matrix.
  Matrix3d F;

  // Run the 8-point algorithm with default options.
  EightPointAlgorithmSolver solver;
  FundamentalMatrixSolverOptions options;
  solver.SetOptions(options);

  if (solver.ComputeFundamentalMatrix(input_data, F)) {
    // Create a new RansacModel using the computed fundamental matrix.
    FundamentalMatrixRansacModel model_out(F);

    // Record sum of squared error over all matches.
    model_out.error_ = 0.0;
    for (const auto& feature_match : input_data) {
      const double error = model_out.EvaluateEpipolarCondition(feature_match);
      model_out.error_ += error * error;
    }

    return model_out;
  }
  // Set a large error - we didn't find a model that fits the data.
  FundamentalMatrixRansacModel model_out(Matrix3d::Identity());
  model_out.error_ = std::numeric_limits<double>::infinity();
  return model_out;
}

}  //\namespace bsfm


//---------------------------------------------------------------------------//
//!
//! \file   tstMomentumConversion.cpp
//! \author Alex Robinson
//! \brief  The momentum conversion unit tests
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>

// FRENSIE Includes
#include "Utility_MomentumUnits.hpp"
#include "Utility_RawPhysicalConstants.hpp"

using namespace Utility::Units;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the momentum units can be converted
BOOST_AUTO_TEST_CASE( convert )
{
  boost::units::quantity<Utility::Units::AtomicMomentum> atomic_momentum_q( 1.0*mec_momentum );

  boost::units::quantity<Utility::Units::MeCMomentum> mec_momentum_q( 1.0*atomic_momentum );

  BOOST_CHECK_CLOSE_FRACTION(
		atomic_momentum_q.value(),
		Utility::RawPhysicalConstants::inverse_fine_structure_constant,
		1e-15 );
  BOOST_CHECK_CLOSE_FRACTION(
			mec_momentum_q.value(),
			Utility::RawPhysicalConstants::fine_structure_constant,
			1e-15 );
}

//---------------------------------------------------------------------------//
// end tstMomentumConversion.cpp
//---------------------------------------------------------------------------//


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_BICGSTAB_INCLUDE
#define ITL_BICGSTAB_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/utility/exception.hpp>
#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

///  Bi-Conjugate Gradient Stabilized
template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	   class Preconditioner, class Iteration >
int bicgstab(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
	     const Preconditioner& M, Iteration& iter)
{
  typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
  typedef HilbertSpaceX                                       Vector;
  mtl::vampir_trace<7004> tracer;

  Scalar     rho_1(0), rho_2(0), alpha(0), beta(0), gamma, omega(0);
  Vector     p(resource(x)), phat(resource(x)), s(resource(x)), shat(resource(x)),
             t(resource(x)), v(resource(x)), r(resource(x)), rtilde(resource(x));

  r = b - A * x;
  rtilde = r;

  while (! iter.finished(r)) {
    ++iter;
    rho_1 = dot(rtilde, r);
    MTL_THROW_IF(rho_1 == 0.0, unexpected_orthogonality());

    if (iter.first())
      p = r;
    else {
      MTL_THROW_IF(omega == 0.0, unexpected_orthogonality());
      beta = (rho_1 / rho_2) * (alpha / omega);
      p = r + beta * (p - omega * v);
    }
    phat = solve(M, p);
    v = A * phat;

    gamma = dot(rtilde, v);
    MTL_THROW_IF(gamma == 0.0, unexpected_orthogonality());

    alpha = rho_1 / gamma;
    s = r - alpha * v;

    if (iter.finished(s)) {
      x += alpha * phat;
      break;
    }
    shat = solve(M, s);
    t = A * shat;
    omega = dot(t, s) / dot(t, t);

    x += omega * shat + alpha * phat;
    r = s - omega * t;

    rho_2 = rho_1;
  }
  return iter;
}

/// Solver class for BiCGStab method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class bicgstab_solver
  : public base_solver< bicgstab_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< bicgstab_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit bicgstab_solver(const LinearOperator& A) : base(A), L(A)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Construct solver from a linear operator and (left) preconditioner
    bicgstab_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return bicgstab(this->A, x, b, L, iter);
    }

  private:
    Preconditioner        L;
};

} // namespace itl

#endif // ITL_BICGSTAB_INCLUDE


#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <cstdlib>
#include <ctime>
#include <chrono>
#include "host.h"
#include "Signal_Loader.h"
#include "filesIO.h"
#include "convolution.h"

using namespace std;

template <typename T>
void zero_padding(std::vector<T> &x, int N)
{
	for (int i = 0; i < N; i++)
	{
		x.push_back(0);
	}
}

void create_output_file(string output_path)
{
	fstream output_file;
	output_file.open(output_path, std::ios::out);
	if (!output_file) {
		std::cerr << "FILE FAILED TO OPEN!" << std::endl;
		exit(-1);
	}
	output_file.close();
}

template <typename T>
void init(vector<T> &x, vector<T> &h, vector<T>&y_linear, vector<T>&y_circular, int &Nh, int N, string filter_path, string output_path)
{
	load_samples(h, filter_path);
	Nh = h.size();
	//zero_padding(h, Nh);
	zero_padding(x, Nh-1);
	zero_padding(y_linear, 2 * N - 1);
	zero_padding(y_circular, N - (Nh - 1));
	create_output_file(output_path);
}

void overlapsave()
{
	string filter_path = "D:\\MOJE\\Projekty\\OpenCL\\OverlapSaveGPU\\OverlapSaveGPU\\Filter.csv";
	string output_path = "output.csv";

	vector<float> h;
	vector<float> x;
	vector<float> y_linear;
	vector<float> y_circular;
	int N = 1024;
	int Nh;
	int Fs = 20000;
	init(x, h, y_linear, y_circular, Nh, N, filter_path, output_path);
	Signal_Loader<float> w(x, Fs);
	boost::thread t(w);
	//circular_convolution(x, h, y_linear, y_circular, N);
	//for (int i = 0; i < y_circular.size(); i++)
	//{
	//	cout << y_circular[i] << endl;
	//}

	int block_number = 0;
	int iteration = 0;
	int block_is_full;

	while (true)
	{
		cout << "";		// (workaround) for some reason without this line, program go to following if despite
		if(x.size() >= N)
		{
			cout << x.size() << "      " << 1024 << "        " << (x.size() >= 1024) << endl;

			circular_convolution(x, h, y_linear, y_circular, N);
			//save_samples(y_circular, output_path);
			x.erase(x.begin(), x.begin() + N-(Nh-1));
			block_number += 1;
		}
	}
	t.join();
}

int main()
{
	srand(time(NULL));
	overlapsave();
	return 0;
}


/**
 * \ file AttackReleaseFilter.cpp
 */

#include <ATK/Dynamic/AttackReleaseFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1024*64;

TEST(AttackRelease, attack_test)
{
  ATK::AttackReleaseFilter<double> filter;
  filter.set_attack(0.5);
  ASSERT_EQ(filter.get_attack(), 0.5);
}

TEST(AttackRelease, attack_range_test)
{
  ATK::AttackReleaseFilter<double> filter;
  ASSERT_THROW(filter.set_attack(-0.000001), ATK::RuntimeError);
}

TEST(AttackRelease, attack_range2_test)
{
  ATK::AttackReleaseFilter<double> filter;
  ASSERT_THROW(filter.set_attack(1.000001), ATK::RuntimeError);
}

TEST(AttackRelease, release_test)
{
  ATK::AttackReleaseFilter<double> filter;
  filter.set_release(0.5);
  ASSERT_EQ(filter.get_release(), 0.5);
}

TEST(AttackRelease, release_range_test)
{
  ATK::AttackReleaseFilter<double> filter;
  ASSERT_THROW(filter.set_release(-0.000001), ATK::RuntimeError);
}

TEST(AttackRelease, release_range2_test)
{
  ATK::AttackReleaseFilter<double> filter;
  ASSERT_THROW(filter.set_release(1.000001), ATK::RuntimeError);
}

TEST(AttackReleaseFilter, triangle_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[i] = i / 48000;
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[PROCESSSIZE/2 + i] = (PROCESSSIZE/2 - i) / 48000;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AttackReleaseFilter<double> filter(1);
  filter.set_attack(std::exp(-1./(48000 * 1e-3)));
  filter.set_release(std::exp(-1./(48000 * 100e-3)));
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    ASSERT_GE(data[i], outdata[i]);
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    ASSERT_GE(outdata[PROCESSSIZE / 2 + i], outdata[PROCESSSIZE / 2 + i - 1]);
  }
}


#include <Eigen/Dense>
#include <vector>
#include <cassert>
#include <cmath>
#include "day_6.hpp"

// We will be using these a lot so for convenience
// make it so we don't have to put Eigen:: in front of them.
using Eigen::MatrixXd;
using Eigen::Vector3d;

/*! \brief Returns the value of the chi tensor for 3 orbital indices on the same atom
 */
double chi_on_atom(int o1, int o2, int o3, double model_dipole)
{
    if(o1 == o2 and o3 == 0)
        return 1.0;
    if(o1 == o3 && o3 > 0 && o2 == 0)
        return model_dipole;
    if(o2 == o3 && o3 > 0 && o1 == 0)
        return model_dipole;
    return 0.0;
}

/*! \brief Returns the atom index part of an atomic orbital index
 */
int atom(int ao_index)
{
    // Division of two integers always results in an integer in C++
    return ao_index / orbitals_per_atom;
}

/*! \brief Returns the atomic orbital index for a given atom index and orbital type
 */
int ao_index(int atom_p, int orb_p)
{
    return atom_p*orbitals_per_atom + orb_p;
}

/*! \brief Returns the orbital type of an atomic orbital index
 */
int orb(int ao_index)
{
    return ao_index % orbitals_per_atom;
}

MatrixXd calculate_fock_matrix_fast(MatrixXd hamiltonian_matrix,
                                    MatrixXd interaction_matrix,
                                    MatrixXd density_matrix,
                                    double model_dipole)
{
    // Number of degrees of freedon
    const size_t ndof = hamiltonian_matrix.rows();
    MatrixXd fock_matrix(hamiltonian_matrix); // Calls copy constructor

    // Potential term
    for(size_t p = 0; p < ndof; p++)
    {
        for(int orb_q = 0; orb_q < orbitals_per_atom; orb_q++)
        {
            int q = ao_index(atom(p), orb_q); // p & q on same atom

            for(int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
            {
                int t = ao_index(atom(p), orb_t); // p & t on same atom
                double chi_pqt = chi_on_atom(orb(p), orb_q, orb_t, model_dipole);

                for(size_t r = 0; r < ndof; r++)
                {
                    for(int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
                    {
                        int s = ao_index(atom(r), orb_s); // r & s on same atom
                        for(int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
                        {
                            int u = ao_index(atom(r), orb_u); // r & u on same atom
                            double chi_rsu = chi_on_atom(orb(r), orb_s, orb_u, model_dipole);
                            fock_matrix(p,q) += 2.0 * chi_pqt * chi_rsu * interaction_matrix(t,u) * density_matrix(r,s);
                        }
                    }
                }
            }
        }
    }

    // Exchange term
    for(size_t p = 0; p < ndof; p++)
    {
        for(int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
        {
            int s = ao_index(atom(p), orb_s); // p & s on same atom
            for(int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
            {
                int u = ao_index(atom(p), orb_u); // p & u on same atom
                double chi_psu = chi_on_atom(orb(p), orb_s, orb_u, model_dipole);

                for(size_t q = 0; q < ndof; q++)
                {
                    for(int orb_r = 0; orb_r < orbitals_per_atom; orb_r++)
                    {
                        int r = ao_index(atom(q), orb_r); // q & r on same atom
                        for(int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
                        {
                            int t = ao_index(atom(q), orb_t);
                            double chi_rqt = chi_on_atom(orb_r, orb(q), orb_t, model_dipole);
                            fock_matrix(p,q) -= chi_rqt * chi_psu * interaction_matrix(t,u) * density_matrix(r,s);
                        }
                    }
                }
            }
        }
    }

    return fock_matrix;
}

double calculate_energy_mp2(std::vector<double> E_occ, std::vector<double> E_virt, MatrixXd v_tilde_flat)
{
    size_t num_occ = E_occ.size();
    size_t num_virt = E_virt.size();

    double energy_mp2 = 0.0;

    // The v_tilde matrix has been flattened from 4 indices to 2 indices.
    // The dimensions of the flattened tensor should be nvirt*nocc x nvirt*nocc
    size_t nrow = v_tilde_flat.rows(); // should be nvirt*nocc
    size_t ncol = v_tilde_flat.cols(); // should be nvirt*nocc
    assert(nrow == num_virt*num_occ);
    assert(ncol == num_virt*num_occ);

    for(size_t a = 0; a < num_virt; a++)
    for(size_t i = 0; i < num_occ; i++)
    {
        size_t idx_1 = a*num_occ+i;
        for(size_t b = 0; b < num_virt; b++)
        for(size_t j = 0; j < num_occ; j++)
        {
            size_t idx_2 = b*num_occ+j;
            size_t idx_3 = a*num_occ+j;
            size_t idx_4 = b*num_occ+i;

            double numerator = 2.0 * pow(v_tilde_flat(idx_1, idx_2), 2) - v_tilde_flat(idx_1, idx_2)*v_tilde_flat(idx_3, idx_4);
            double denominator = E_virt[a] + E_virt[b] - E_occ[i] - E_occ[j];
            energy_mp2 -= numerator/denominator;
        }
    }

    return energy_mp2;
}


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticMarginalBetaFunction.hpp
//! \author Alex Robinson
//! \brief  Free gas elastic marginal beta function declaration.
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_FREE_GAS_ELASTIC_MARGINAL_BETA_FUNCTION_HPP
#define DATA_GEN_FREE_GAS_ELASTIC_MARGINAL_BETA_FUNCTION_HPP

// Std Lib Includes
#include <list>

// Boost Includes
#include <boost/function.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "DataGen_FreeGasElasticSAlphaBetaFunction.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_Tuple.hpp"

namespace DataGen{

//! The Free gas elastic marginal beta function
class FreeGasElasticMarginalBetaFunction
{

public:

  //! Constructor
  FreeGasElasticMarginalBetaFunction(
	      const Teuchos::RCP<Utility::OneDDistribution>&
	      zero_temp_elastic_cross_section,
              const Teuchos::RCP<MonteCarlo::NuclearScatteringAngularDistribution>&
	      cm_scattering_distribution,
	      const double A,
	      const double kT,
	      const double E );

  //! Destructor
  ~FreeGasElasticMarginalBetaFunction()
  { /* ... */ }

  //! Set the beta and energy values
  void setIndependentVariables( const double E );

  //! Get the lower beta limit
  double getBetaMin() const;

  //! Get the normalization constant
  double getNormalizationConstant() const;

  //! Evaluate the marginal PDF
  double operator()( const double beta );

  //! Evaluate the marginal CDF
  double evaluateCDF( const double beta );

private:

  // Update the cached values
  void updateCachedValues();

  // Function that represents the integral of S(alpha,beta) over all alpha
  // multiplied by exp(-beta/2)
  double integratedSAlphaBetaFunction( const double beta );

  // The integration gkq_set for integrating over alpha values
  Utility::GaussKronrodIntegrator d_alpha_gkq_set;

  // The integration gkq_set for integrating over beta values
  Utility::GaussKronrodIntegrator d_beta_gkq_set;

  // The free gas elastic S(alpha,beta) function
  FreeGasElasticSAlphaBetaFunction d_sab_function;

  // The energy value (MeV)
  double d_E;

  // The atomic weight ratio
  double d_A;

  // The temperature (MeV)
  double d_kT;

  // The beta min value
  double d_beta_min;

  // The normalization constant
  double d_norm_constant;

  // Cached CDF values (first = beta, second = CDF)
  std::list<Utility::Pair<double,double> > d_cached_cdf_values;
};

} // end DataGen namespace

#endif // end DATA_GEN_FREE_GAS_ELASTIC_MARGINAL_BETA_FUNCTION_HPP

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticMarginalBetaFunction.hpp
//---------------------------------------------------------------------------//


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "tudat/astro/aerodynamics/equilibriumWallTemperature.h"
#include "tudat/astro/aerodynamics/aerodynamics.h"

namespace tudat
{
namespace unit_tests
{

using namespace aerodynamics;

BOOST_AUTO_TEST_SUITE( test_fay_riddell_heat_flux )

//! Test if equilibrium temperature is properly computed for variety of cases.
BOOST_AUTO_TEST_CASE( testEquilibriumTemperature )
{
    double airSpeed, noseRadius, wallEmissivity, equilibriumWallTemperature,
            adiabaticWallTemperature;
    double airDensity = 1.0E-5;
    double airTemperature = 300.0;
    for( unsigned int i = 0; i <= 10; i++ )
    {
        airSpeed = 1.0E3 + static_cast< double >( i ) * 1.0E3;
        double machNumber = airSpeed / 300.0;

        for( unsigned int j = 0; j <= 10; j++ )
        {
            noseRadius = 0.001 + static_cast< double >( j ) * 0.1;
            for( unsigned int k = 0; k < 10; k++ )
            {
                wallEmissivity = 0.1 + static_cast< double >( k ) * 0.1;
                adiabaticWallTemperature
                        = computeAdiabaticWallTemperature( airTemperature , machNumber );

                std::function< double( const double ) > heatTransferFunction = std::bind(
                            &computeFayRiddellHeatFlux, airDensity, airSpeed, airTemperature, noseRadius, std::placeholders::_1 );

                equilibriumWallTemperature =
                        computeEquilibiumWallTemperature( heatTransferFunction, wallEmissivity, adiabaticWallTemperature );

                BOOST_CHECK_CLOSE_FRACTION(
                            wallEmissivity * electromagnetism::computeBlackbodyRadiationIntensity( equilibriumWallTemperature ),
                            heatTransferFunction( equilibriumWallTemperature ), 1.0E-12 );

            }
        }
    }

}

//! Test if Fay-Ridell heat flux functions produce consistent results for variety of cases.
BOOST_AUTO_TEST_CASE( testFayRiddellHeatFluxConsistency )
{
    double airSpeed, noseRadius, equilibriumWallTemperature,
            adiabaticWallTemperature;
    double airDensity = 1.0E-5;
    double airTemperature = 300.0;
    double wallEmissivity = 0.7;
    double heatFlux1, heatFlux2, heatFlux3;
    for( unsigned int i = 0; i <= 10; i++ )
    {
        airSpeed = 1.0E3 + static_cast< double >( i ) * 1.0E3;
        double machNumber = airSpeed / 300.0;

        for( unsigned int j = 0; j <= 10; j++ )
        {
            noseRadius = 0.001 + static_cast< double >( j ) * 0.1;

            adiabaticWallTemperature
                    = computeAdiabaticWallTemperature( airTemperature , machNumber );

            std::function< double( const double ) > heatTransferFunction = std::bind(
                        &computeFayRiddellHeatFlux, airDensity, airSpeed, airTemperature, noseRadius, std::placeholders::_1 );

            heatFlux1 = computeEquilibriumFayRiddellHeatFlux(
                        airDensity, airSpeed, airTemperature, machNumber, noseRadius, wallEmissivity );
            heatFlux2 = computeEquilibriumHeatflux(
                        heatTransferFunction, wallEmissivity, adiabaticWallTemperature );

            equilibriumWallTemperature =
                    computeEquilibiumWallTemperature( heatTransferFunction, wallEmissivity, adiabaticWallTemperature );
            heatFlux3 = computeFayRiddellHeatFlux(
                        airDensity, airSpeed, airTemperature, noseRadius, equilibriumWallTemperature );

            BOOST_CHECK_CLOSE_FRACTION(
                        heatFlux1, heatFlux2, 4.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION(
                        heatFlux1, heatFlux3, 4.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION(
                        heatFlux2, heatFlux3, 4.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }
}

//! Test if Fay-Ridell heat flux functions produce correct results for limit cases
BOOST_AUTO_TEST_CASE( testFayRiddellHeatFluxFunctions )
{
    double airSpeed = 6.0E3;
    double noseRadius = 0.1;
    double airDensity = 1.0E-5;
    double airTemperature = 300.0;

    double computedHeatFlux  = computeFayRiddellHeatFlux(
                airDensity, airSpeed, airTemperature, noseRadius, airTemperature );

    double expectedHeatFlux = 0.5 * FAY_RIDDEL_HEAT_FLUX_CONSTANT * std::pow(
                airSpeed, 3.0 ) * std::pow( airDensity, 0.5 ) * std::pow( noseRadius, -0.5 );
    BOOST_CHECK_CLOSE_FRACTION(
                computedHeatFlux, expectedHeatFlux, 4.0 * std::numeric_limits< double >::epsilon( ) );

    computedHeatFlux  = computeFayRiddellHeatFlux(
                    0.0, airSpeed, airTemperature, noseRadius, airTemperature );
    BOOST_CHECK_SMALL( std::fabs( computedHeatFlux ), std::numeric_limits< double >::epsilon( ) );

    computedHeatFlux  = computeFayRiddellHeatFlux(
                    airDensity, 0.0, airTemperature, noseRadius, airTemperature );
    BOOST_CHECK_SMALL( std::fabs( computedHeatFlux ), std::numeric_limits< double >::epsilon( ) );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat