// Copyright (C) 2020 Chris Richardson (chris@bpi.cam.ac.uk)
// Copyright (C) 2021 Athena Elafrou (ae488@cam.ac.uk)
// SPDX-License-Identifier:    MIT

#include "read_petsc.h"
#include "L2GMap.h"
#include "Matrix.h"
#include <Eigen/Sparse>
#include <cassert>
#include <fstream>
#include <iostream>
#include <map>
#include <memory>
#include <vector>

// Divide size into N ~equal chunks
std::vector<std::int64_t> owner_ranges(int size, std::int64_t N)
{
  // Compute number of items per process and remainder
  const std::int64_t n = N / size;
  const std::int64_t r = N % size;

  // Compute local range
  std::vector<std::int64_t> ranges;
  for (int rank = 0; rank < (size + 1); ++rank) {
    if (rank < r)
      ranges.push_back(rank * (n + 1));
    else
      ranges.push_back(rank * n + r);
  }

  return ranges;
}

//-----------------------------------------------------------------------------
spmv::Matrix<double> spmv::read_petsc_binary_matrix(MPI_Comm comm,
                                                    std::string filename,
                                                    bool symmetric,
                                                    CommunicationModel cm)
{
  auto A = std::make_shared<Eigen::SparseMatrix<double, Eigen::RowMajor>>();
  auto A_remote
      = std::make_shared<Eigen::SparseMatrix<double, Eigen::RowMajor>>();

  int mpi_rank;
  MPI_Comm_rank(comm, &mpi_rank);
  int mpi_size;
  MPI_Comm_size(comm, &mpi_size);

  std::map<std::int32_t, std::int32_t> col_indices;
  std::vector<std::int64_t> row_ranges, col_ranges;
  std::int64_t ncols_local, nrows_local;

  std::ifstream file(filename.c_str(),
                     std::ios::in | std::ios::binary | std::ios::ate);
  if (!file.is_open())
    throw std::runtime_error("Could not open file");

  // Get first 4 ints from file
  std::vector<char> memblock(16);
  file.seekg(0, std::ios::beg);
  file.read(memblock.data(), 16);

  char* ptr = memblock.data();
  for (int i = 0; i < 4; ++i) {
    std::swap(*ptr, *(ptr + 3));
    std::swap(*(ptr + 1), *(ptr + 2));
    ptr += 4;
  }

  std::int32_t* int_data = (std::int32_t*)(memblock.data());
  int id = int_data[0];
  if (id != 1211216)
    throw std::runtime_error("Bad signature in PETSc Matrix file");

  int nrows = int_data[1];
  int ncols = int_data[2];
  int nnz_tot = int_data[3];
  row_ranges = owner_ranges(mpi_size, nrows);
  col_ranges = owner_ranges(mpi_size, ncols);

  if (mpi_rank == 0)
    std::cout << "Read matrix file: " << filename << ": " << nrows << "x"
              << ncols << " = " << nnz_tot << "\n";

  nrows_local = row_ranges[mpi_rank + 1] - row_ranges[mpi_rank];
  ncols_local = col_ranges[mpi_rank + 1] - col_ranges[mpi_rank];

  auto A_diagonal
      = std::make_shared<Eigen::Matrix<double, Eigen::Dynamic, 1>>(nrows_local);
  A_diagonal->setZero();

  // Reset memory block and read nnz per row for all rows
  memblock.resize(nrows * 4);
  ptr = memblock.data();
  file.read(memblock.data(), nrows * 4);
  std::vector<std::int32_t> nnz(nrows);
  std::int64_t nnz_sum = 0;
  for (int i = 0; i < nrows; ++i) {
    std::swap(*ptr, *(ptr + 3));
    std::swap(*(ptr + 1), *(ptr + 2));
    nnz[i] = *((std::int32_t*)ptr);
    nnz_sum += nnz[i];
    ptr += 4;
  }
  assert(nnz_sum == nnz_tot);

  // Get offset and size for data
  std::int64_t nnz_offset = 0;
  std::int64_t nnz_size = 0;
  for (std::int64_t i = 0; i < row_ranges[mpi_rank]; ++i)
    nnz_offset += nnz[i];
  for (std::int64_t i = row_ranges[mpi_rank]; i < row_ranges[mpi_rank + 1]; ++i)
    nnz_size += nnz[i];

  std::streampos value_data_pos
      = file.tellg() + (std::streampos)(nnz_tot * 4 + nnz_offset * 8);

  // Read col indices for each row
  memblock.resize(nnz_size * 4);
  ptr = memblock.data();
  file.seekg(nnz_offset * 4, std::ios::cur);
  file.read(memblock.data(), nnz_size * 4);

  std::int32_t c = 0;
  for (std::int64_t col = col_ranges[mpi_rank]; col < col_ranges[mpi_rank + 1];
       ++col) {
    col_indices.insert({col, c});
    ++c;
  }

  // Map other columns
  for (std::int64_t row = row_ranges[mpi_rank]; row < row_ranges[mpi_rank + 1];
       ++row) {
    for (std::int64_t j = 0; j < nnz[row]; ++j) {
      std::swap(*ptr, *(ptr + 3));
      std::swap(*(ptr + 1), *(ptr + 2));
      col_indices.insert({*((std::int32_t*)ptr), -1});
      ptr += 4;
    }
  }
  // Ensure they are labelled in ascending order
  for (auto& q : col_indices)
    if (q.second == -1) {
      q.second = c;
      ++c;
    }

  A->resize(nrows_local, col_indices.size());
  if (symmetric || cm == CommunicationModel::p2p_nonblocking
      || cm == CommunicationModel::collective_nonblocking)
    A_remote->resize(nrows_local, col_indices.size());

  // Read values
  std::vector<char> valuedata(nnz_size * 8);
  file.seekg(value_data_pos, std::ios::beg);
  file.read(valuedata.data(), nnz_size * 8);
  file.close();

  // Pointer to values
  char* vptr = valuedata.data();
  ptr = memblock.data();
  for (std::int64_t global_row = row_ranges[mpi_rank];
       global_row < row_ranges[mpi_rank + 1]; ++global_row) {
    for (std::int64_t j = 0; j < nnz[global_row]; ++j) {
      std::swap(*vptr, *(vptr + 7));
      std::swap(*(vptr + 1), *(vptr + 6));
      std::swap(*(vptr + 2), *(vptr + 5));
      std::swap(*(vptr + 3), *(vptr + 4));
      double val = *((double*)vptr);
      vptr += 8;

      // Look up column local index
      std::int32_t col = col_indices[*((std::int32_t*)ptr)];

      if (symmetric) {
        std::int32_t global_col = *((std::int32_t*)ptr);
        // If element is in local column range, insert only if it's on or
        // below main diagonal
        if (col < ncols_local && global_row > global_col)
          A->insert(global_row - row_ranges[mpi_rank], col) = val;
        // If element on main diagonal, store separately
        if (col < ncols_local && global_row == global_col)
          (*A_diagonal)(global_row - row_ranges[mpi_rank]) = val;
        // If element is out of local column range, always insert
        if (col >= ncols_local)
          A_remote->insert(global_row - row_ranges[mpi_rank], col) = val;
      } else {
        if (cm == CommunicationModel::p2p_nonblocking
            || cm == CommunicationModel::collective_nonblocking) {
          // If element is in local column range, insert in "local" block
          if (col < ncols_local)
            A->insert(global_row - row_ranges[mpi_rank], col) = val;
          // Otherwise, store in "remote" block
          else
            A_remote->insert(global_row - row_ranges[mpi_rank], col) = val;
        } else {
          A->insert(global_row - row_ranges[mpi_rank], col) = val;
        }
      }
      ptr += 4;
    }
  }

  A->makeCompressed();
  if (symmetric || cm == CommunicationModel::p2p_nonblocking
      || cm == CommunicationModel::collective_nonblocking)
    A_remote->makeCompressed();

  std::vector<std::int64_t> ghosts(col_indices.size() - ncols_local);
  for (auto& q : col_indices)
    if (q.first < col_ranges[mpi_rank] or q.first >= col_ranges[mpi_rank + 1])
      ghosts[q.second - ncols_local] = q.first;

  auto col_map = std::make_shared<spmv::L2GMap>(comm, ncols_local, ghosts, cm);
  auto row_map = std::make_shared<spmv::L2GMap>(comm, nrows_local,
                                                std::vector<std::int64_t>());

  if (symmetric)
    return spmv::Matrix<double>(A, A_remote, A_diagonal, col_map, row_map,
                                nnz_tot);
  else if (col_map->overlapping())
    return spmv::Matrix<double>(A, A_remote, col_map, row_map);
  else
    return spmv::Matrix<double>(A, col_map, row_map);
}
//-----------------------------------------------------------------------------
Eigen::VectorXd spmv::read_petsc_binary_vector(MPI_Comm comm,
                                               std::string filename)
{
  Eigen::VectorXd vec;

  int mpi_rank;
  MPI_Comm_rank(comm, &mpi_rank);
  int mpi_size;
  MPI_Comm_size(comm, &mpi_size);

  std::ifstream file(filename.c_str(),
                     std::ios::in | std::ios::binary | std::ios::ate);
  if (file.is_open()) {
    // Get first 2 ints from file
    std::vector<char> memblock(8);
    file.seekg(0, std::ios::beg);
    file.read(memblock.data(), 8);

    char* ptr = memblock.data();
    for (int i = 0; i < 2; ++i) {
      std::swap(*ptr, *(ptr + 3));
      std::swap(*(ptr + 1), *(ptr + 2));
      ptr += 4;
    }

    std::int32_t* int_data = (std::int32_t*)(memblock.data());
    int id = int_data[0];
    if (id != 1211214)
      throw std::runtime_error("Bad signature in PETSc Vector file");

    int nrows = int_data[1];
    std::vector<std::int64_t> ranges = owner_ranges(mpi_size, nrows);

    if (mpi_rank == 0)
      std::cout << "Read vector file: " << filename << ": " << nrows << "\n";

    std::int64_t nrows_local = ranges[mpi_rank + 1] - ranges[mpi_rank];

    vec.resize(nrows_local);

    std::streampos value_data_pos
        = file.tellg() + (std::streampos)(ranges[mpi_rank] * 8);

    // Read values
    std::vector<char> valuedata(nrows_local * 8);
    file.seekg(value_data_pos, std::ios::beg);
    file.read(valuedata.data(), nrows_local * 8);
    file.close();

    // Pointer to values
    char* vptr = valuedata.data();

    for (std::int64_t row = ranges[mpi_rank]; row < ranges[mpi_rank + 1];
         ++row) {
      std::swap(*vptr, *(vptr + 7));
      std::swap(*(vptr + 1), *(vptr + 6));
      std::swap(*(vptr + 2), *(vptr + 5));
      std::swap(*(vptr + 3), *(vptr + 4));
      double val = *((double*)vptr);
      vptr += 8;
      vec[row - ranges[mpi_rank]] = val;
    }
  } else
    throw std::runtime_error("Could not open file");

  return vec;
}
//-----------------------------------------------------------------------------


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

#ifndef SKETCHSORT_HPP
#define SKETCHSORT_HPP

// for boost
#include <boost/pool/pool.hpp>
#include <boost/pool/object_pool.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>

#include <iostream>
#include <string>
#include <vector>
#include <list>
#include <map>
#include <cmath>
#include <cstring>
#include <iterator>
#include <fstream>
#include <strstream>
#include <sstream>
#include <cstdio>
#include <cstdlib>
#include <set>
#include <stdint.h>
#include <time.h>

#include <boost/random.hpp>

using namespace boost::numeric::ublas;  // boost::numeric::ublas
using namespace boost::numeric;         // boost::numeric

#ifndef M_PI
#define M_PI 3.14159265358979323846/* pi */
#define M_PI_2 1.57079632679489661923/* pi/2 */
#define M_PI_4 0.78539816339744830962/* pi/4 */
#endif

struct pstat {
  int start;
  int end;
};

struct params {
  unsigned int numblocks;
  unsigned int numchunks;
  unsigned int projectDim;
  unsigned int chunk_dist;
  unsigned int chunks;
  unsigned int num_seq;
  unsigned int seq_len;
  unsigned int chunk_len;
  unsigned int start_chunk;
  unsigned int end_chunk;
  unsigned int cchunk;
  unsigned int *counter;
  pstat        *pos;
  pstat        *pchunks;
  float         cosDist;
  std::vector<unsigned int> ids;
  std::vector<int> blocks;
  std::ostream *os;
};

class SketchSort {
  boost::pool<> *p;
  std::vector<boost::numeric::ublas::vector<float> > fvs;
  std::vector<float> norms;
  uint8_t num_char;
  unsigned int dim;

  uint64_t numSort;
  uint64_t numHamDist;
  uint64_t numCosDist;

  void readFeature(const char *fname);
  void centeringData();
  void preComputeNorms();
  int projectVectors(unsigned int projectDim, std::vector<uint8_t*> &sig, params &param);
  void report(std::vector<uint8_t*> &sig, int l, int r, params &param);
  void sort(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, params &param);
  void radixsort(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, params &param);
  void insertionSort(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, params &param);
  bool calc_chunk_hamdist(uint8_t *seq1, uint8_t *seq2, const params &param);
  bool calc_hamdist(uint8_t *seq1, uint8_t *seq2, const params &param);
  bool check_canonical(uint8_t *seq1, uint8_t *seq2, const params &param);
  bool check_chunk_canonical(uint8_t *seq1, uint8_t *seq2, const params &param);
  float checkCos(unsigned int id1, unsigned int id2);
  double calcMissingEdgeRatio(params &param);
  void classify(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, int bpos, params &param);
  void multi_classification(std::vector<uint8_t*> &sig, int maxind, int l, int r, params &param);
  void refinement();
  void insertKnnList(unsigned int from_id, unsigned int to_id, float cosDist);
  void decideParameters(float _missingratio, params &param);
 public:
  SketchSort() {};
  void run(const char *fname, const char *oname,
	   unsigned int _numblocks,
           unsigned int _dist,
	   float        _cosDist,
	   unsigned int _numchunks,
	   bool         _autoFlag,
	   float        _missingratio,
	   bool         _centering);

};

#endif // SKETCHSORT_HPP


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

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

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

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

/*! \file qle/termstructures/dynamicoptionletvolatilitystructure.hpp
    \brief dynamic optionlet volatility structure
    \ingroup termstructures
*/

#pragma once

#include <qle/termstructures/dynamicstype.hpp>

#include <ql/termstructures/volatility/optionlet/optionletvolatilitystructure.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Converts OptionletVolatilityStructure with fixed reference date into a floating reference date term structure.
/*! Different ways of reacting to time decay can be specified.

    \warning No checks are performed that the supplied OptionletVolatilityStructure has a fixed reference date

        \ingroup termstructures
*/

class DynamicOptionletVolatilityStructure : public OptionletVolatilityStructure {
public:
    DynamicOptionletVolatilityStructure(const boost::shared_ptr<OptionletVolatilityStructure>& source,
                                        Natural settlementDays, const Calendar& calendar,
                                        ReactionToTimeDecay decayMode = ConstantVariance);

protected:
    //! \name OptionletVolatilityStructure interface
    //@{
    boost::shared_ptr<SmileSection> smileSectionImpl(Time optionTime) const;
    Volatility volatilityImpl(Time optionTime, Rate strike) const;
    //@}

    //! \name VolatilityTermStructure interface
    //@{
    Rate minStrike() const;
    Rate maxStrike() const;
    //@}

    //! \name VolatilityTermStructure interface
    //@{
    Date maxDate() const;
    //@}

    //! \name Observer interface
    //@{
    void update();
    //@}

    //! Override the default implementations in OptionletVolatilityStructure
    VolatilityType volatilityType() const;
    Real displacement() const;

private:
    const boost::shared_ptr<OptionletVolatilityStructure> source_;
    ReactionToTimeDecay decayMode_;
    const Date originalReferenceDate_;
    const VolatilityType volatilityType_;
    const Real displacement_;
};

inline VolatilityType DynamicOptionletVolatilityStructure::volatilityType() const { return volatilityType_; }

inline Real DynamicOptionletVolatilityStructure::displacement() const { return displacement_; }
} // namespace QuantExt


//////////////////////////////////////////////////////////////////////////////
// data::unit::survival::responses::response.hpp							//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DATA_UNIT_SURVIVAL_RESPONSES_RESPONSE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DATA_UNIT_SURVIVAL_RESPONSES_RESPONSE_HPP_ER_2010
#include <boost/statistics/detail/data/field/survival/fields/is_failure.hpp>
#include <boost/statistics/detail/data/field/survival/fields/event_time.hpp>
#include <boost/statistics/detail/data/unit/framework/impl.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace data{
namespace unit{
namespace survival{

    typedef data::unit::impl<
        field::is_failure,
        field::float_valued::event_time
    > response;

}// unit
}// data
}// detail
}// statistics
}// boost

#endif


/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#ifndef COMMON_INCLUDE_COMMON_TYPES_OBJECT_HPP_
#define COMMON_INCLUDE_COMMON_TYPES_OBJECT_HPP_

#include <pcl/io/io.h>  // pcl::copyPointCloud
#include <Eigen/Core>
#include <memory>
#include "common/types/feature.hpp"
#include "common/types/type.h"

namespace autosense {

using Eigen::Vector3d;

typedef PointCloud PolygonType;
typedef PointDCloud PolygonDType;

struct alignas(16) Object {
    Object() {
        ground_center = Vector3d::Zero();
        velocity = Vector3d::Zero();
        direction = Vector3d(1, 0, 0);

        cloud.reset(new PointICloud);

        // type = UNKNOWN;
        // type_probs.resize(MAX_OBJECT_TYPE, 0);

        /*
         *
         * | 0.01  0    0   |
         * |  0   0.01  0   |
         * |  0    0   0.01 |
         */
        position_uncertainty << 0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01;
        velocity_uncertainty << 0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01;
    }

    /**
     * @brief deep copy of Object
     * @param rhs
     */
    void clone(const Object& rhs) {
        *this = rhs;
        // TODO(gary): pay attention to point when deep copy
        this->cloud.reset(new PointICloud);
        pcl::copyPointCloud<PointI, PointI>(*(rhs.cloud), *cloud);
    }

    void setId(IdType id) { this->id = id; }

    /*std::string ToString() const
    {
        // StrCat supports 9 arguments at most.
        return StrCat(StrCat("Object[id: ", id,
                             ", "
                                     "track_id: ",
                             track_id,
                             ", "
                                     "cloud_size: ",
                             cloud->size(),
                             ", "
                                     "direction: ",
                             Print(direction.transpose()), ", "),
                      StrCat("center: ", Print(center.transpose()),
                             ", "
                                     "velocity: ",
                             Print(velocity.transpose()),
                             ", "
                                     "width: ",
                             width,
                             ", "
                                     "length: ",
                             length, ", "),
                      StrCat("height: ", height,
                             ", "
                                     "polygon_size: ",
                             polygon.size(),
                             ", "
                                     "type: ",
                             type,
                             ", "
                                     "is_background: ",
                             is_background, "]"));
    }*/

    /*void Serialize(PerceptionObstacle *pb_obj) const
    {
        CHECK(pb_obj != NULL);
        pb_obj->set_id(track_id);
        pb_obj->set_theta(theta);

        Point *obj_center = pb_obj->mutable_position();
        obj_center->set_x(center(0));
        obj_center->set_y(center(1));
        obj_center->set_z(center(2));

        Point *obj_velocity = pb_obj->mutable_velocity();
        obj_velocity->set_x(velocity(0));
        obj_velocity->set_y(velocity(1));
        obj_velocity->set_z(velocity(2));

        pb_obj->set_length(length);
        pb_obj->set_width(width);
        pb_obj->set_height(height);

        for (auto point : polygon.points) {
            Point *p = pb_obj->add_polygon_point();
            p->set_x(point.x);
            p->set_y(point.y);
            p->set_z(point.z);
        }

        if (FLAGS_is_serialize_point_cloud) {
            for (auto point : cloud->points) {
                pb_obj->add_point_cloud(point.x);
                pb_obj->add_point_cloud(point.y);
                pb_obj->add_point_cloud(point.z);
            }
        }

        pb_obj->set_tracking_time(tracking_time);
        pb_obj->set_type(static_cast<PerceptionObstacle::Type>(type));
        pb_obj->set_timestamp(latest_tracked_time);  // in seconds.
    }*/

    /*void Deserialize(const PerceptionObstacle& pb_obs)
    {
        track_id = pb_obs.id();
        theta = pb_obs.theta();

        center(0) = pb_obs.position().x();
        center(1) = pb_obs.position().y();
        center(2) = pb_obs.position().z();

        velocity(0) = pb_obs.velocity().x();
        velocity(1) = pb_obs.velocity().y();
        velocity(2) = pb_obs.velocity().z();

        length = pb_obs.length();
        width = pb_obs.width();
        height = pb_obs.height();

        polygon.clear();
        for (int idx = 0; idx < pb_obs.polygon_point_size(); ++idx) {
            const auto& p = pb_obs.polygon_point(idx);
            pcl_util::PointD point;
            point.x = p.x();
            point.y = p.y();
            point.z = p.z();
            polygon.push_back(point);
        }

        tracking_time = pb_obs.tracking_time();
        latest_tracked_time = pb_obs.timestamp();
        type = static_cast<ObjectType>(pb_obs.type());
    }*/

    /*std::string ToString() const
    {
        std::ostringstream oss;
        oss << "sensor_type: " << GetSensorType(sensor_type)
            << ", timestamp:" << GLOG_TIMESTAMP(timestamp)
            << ", sensor2world_pose:\n";
        oss << sensor2world_pose << "\n, objects: " << objects.size() << " < ";
        for (auto obj : objects) {
            oss << "\n" << obj->ToString();
        }
        oss << " >]";
        return oss.str();
    }*/

    //---------------------- basic information
    // object id per frame
    IdType id = -1;
    // point cloud of the object
    PointICloudPtr cloud;
    // convex hull of the object
    PolygonDType polygon;
    /*
     * @note Apollo's Object Coordinate
     *          |x
     *      C   |   D-----------
     *          |              |
     *  y---------------     length
     *          |              |
     *      B   |   A-----------
     */
    // oriented boundingbox information: main direction vector(x, y, 0)
    Eigen::Vector3d direction;
    // the yaw angle, theta = 0.0 <=> direction = (1, 0, 0)
    double yaw_rad = 0.0;
    // ground center of the object (cx, cy, z_min)
    Eigen::Vector3d ground_center;
    // size of the oriented bbox, length is the size in the main direction
    double length = 0.0;
    double width = 0.0;
    double height = 0.0;

    //---------------------- classification information
    ObjectType type;
    // foreground score/probability
    float score = 0.0;
    // fg/bg flag
    bool is_background = false;
    // Object classification type.
    // ObjectType type;
    // Probability of each type, used for track type.
    // std::vector<float> type_probs;

    //---------------------- tracking information
    // shape feature used for tracker-observation match
    Feature shape_features;
    /// @note one tracker maintaina tracked trajectory
    IdType tracker_id = 0;
    // tracking state
    // stable anchor_point during time, e.g., barycenter
    Eigen::Vector3d anchor_point;
    Eigen::Vector3d velocity;
    // age of the tracked object
    double tracking_time = 0.0;
    double latest_tracked_time = 0.0;
    // noise covariance matrix for uncertainty of position and velocity
    Eigen::Matrix3d position_uncertainty;
    Eigen::Matrix3d velocity_uncertainty;
};

typedef std::shared_ptr<Object> ObjectPtr;
typedef std::shared_ptr<const Object> ObjectConstPtr;

}  // namespace autosense

#endif  // COMMON_INCLUDE_COMMON_TYPES_OBJECT_HPP_


#include <sparse.h>
#include <sparse_fill.h>     // 2009-08-04 Kenny: Damn this header is part of the sparse unit tests but not sparse. The reason is that it uses some ugly Boost MPL stuff
#include <prox_math_policy.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(mass_block_jac);
BOOST_AUTO_TEST_CASE(mass_block_test_case)
{
  typedef prox::MathPolicy<float>           math_policy;
  typedef math_policy::diagonal_mass_type   mass_matrix_type;
  typedef math_policy::value_traits         value_traits;

  mass_matrix_type A(1);
  sparse::fill(A(0,0), 0.0f);   // 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'

  BOOST_CHECK_EQUAL( A(0,0)[0] , value_traits::zero()  );
  BOOST_CHECK_EQUAL( A(0,0)[1] , value_traits::one()   );
  BOOST_CHECK_EQUAL( A(0,0)[2] , value_traits::two()   );
  BOOST_CHECK_EQUAL( A(0,0)[3] , value_traits::three() );
  BOOST_CHECK_EQUAL( A(0,0)[4] , value_traits::four()  );
  BOOST_CHECK_EQUAL( A(0,0)[5] , value_traits::numeric_cast(5) );
  BOOST_CHECK_EQUAL( A(0,0)[6] , value_traits::numeric_cast(6) );

  A(0,0)[0] = value_traits::numeric_cast(2.5);
  mass_matrix_type B(A);

  BOOST_CHECK_EQUAL( B(0,0)[0] , value_traits::numeric_cast(2.5));
  BOOST_CHECK_EQUAL( B(0,0)[1] , value_traits::one()            );
  BOOST_CHECK_EQUAL( B(0,0)[2] , value_traits::two()            );
  BOOST_CHECK_EQUAL( B(0,0)[3] , value_traits::three()          );
  BOOST_CHECK_EQUAL( B(0,0)[4] , value_traits::four()           );
  BOOST_CHECK_EQUAL( B(0,0)[5] , value_traits::numeric_cast(5) );
  BOOST_CHECK_EQUAL( B(0,0)[6] , value_traits::numeric_cast(6) );

  sparse::inverse(A);

  BOOST_CHECK_EQUAL( A(0,0)[0] , 1/value_traits::numeric_cast(2.5) );
  BOOST_CHECK_EQUAL( A(0,0)[1] , value_traits::one()   );
  BOOST_CHECK_EQUAL( A(0,0)[2] , value_traits::numeric_cast(-3)   );
  BOOST_CHECK_EQUAL( A(0,0)[3] , value_traits::two() );
  BOOST_CHECK_EQUAL( A(0,0)[4] , value_traits::three()  );
  BOOST_CHECK_EQUAL( A(0,0)[5] , value_traits::numeric_cast(-1) );
  BOOST_CHECK_EQUAL( A(0,0)[6] , value_traits::zero() );
}

BOOST_AUTO_TEST_SUITE_END();


#include "Demos/Utils/Config.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Demos/Simulation/SimulationModel.h"
#include "Demos/Simulation/TimeStepController.h"
#include <iostream>

#define _USE_MATH_DEFINES
#include "math.h"

// Enable memory leak detection
#ifdef _DEBUG
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void createRod();
void render ();
void reset();
void cleanup();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);

void TW_CALL setBendAndTwistStiffness(const void *value, void *clientData);
void TW_CALL getBendAndTwistStiffness(void *value, void *clientData);

void TW_CALL setRestDarbouxX(const void *value, void *clientData);
void TW_CALL setRestDarbouxY(const void *value, void *clientData);
void TW_CALL setRestDarbouxZ(const void *value, void *clientData);

void TW_CALL getRestDarbouxX(void *value, void *clientData);
void TW_CALL getRestDarbouxY(void *value, void *clientData);
void TW_CALL getRestDarbouxZ(void *value, void *clientData);

void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);

SimulationModel model;
TimeStepController sim;

const int numberOfPoints = 32;
bool doPause = false;
std::vector<unsigned int> selectedBodies;
Eigen::Vector3f oldMousePos;
Eigen::Vector3f restDarboux;
// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	// OpenGL
	MiniGL::init (argc, argv, 1024, 768, 0, 0, "Rigid body demo");
	MiniGL::initLights ();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);

	buildModel ();

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3f (5.0, -10.0, 30.0), Vector3f (5.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_FLOAT, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &sim, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation ");

	TwAddVarCB(MiniGL::getTweakBar(), "BendAndTwistKs", TW_TYPE_FLOAT, setBendAndTwistStiffness, getBendAndTwistStiffness, &model, " label='Bend and Twist Stiffness'  min=0.0 max = 1.0 step=0.1 precision=2 group=BendTwist ");

	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxX", TW_TYPE_FLOAT, setRestDarbouxX, getRestDarbouxX, &model, " label='Rest Darboux X'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxY", TW_TYPE_FLOAT, setRestDarbouxY, getRestDarbouxY, &model, " label='Rest Darboux Y'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxZ", TW_TYPE_FLOAT, setRestDarbouxZ, getRestDarbouxZ, &model, " label='Rest Darboux Z'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	glutMainLoop ();

	cleanup ();

	return 0;
}

void cleanup()
{
	delete TimeManager::getCurrent();
}

void reset()
{
	model.reset();
	sim.reset();
	TimeManager::getCurrent()->setTime(0.0);
}

void mouseMove(int x, int y)
{
	Eigen::Vector3f mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Eigen::Vector3f diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const float h = tm->getTimeStepSize();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	for (size_t j = 0; j < selectedBodies.size(); j++)
	{
		rb[selectedBodies[j]]->getVelocity() += 1.0f / h * diff;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{

 	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void buildModel()
{

	TimeManager::getCurrent()->setTimeStepSize(0.002f);

	sim.setDamping(0.001f);

	model.setElasticRodBendAndTwistStiffness(0.5f);
	model.setElasticRodStretchStiffness(1.0f);
	restDarboux = Eigen::Vector3f(0.0f, 0.0f, 0.0f);

	createRod();
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 8; i++)
		sim.step(model);
}

void render()
{
	MiniGL::coordinateSystem();

	// Draw sim model

	ParticleData &particles = model.getParticles();
	ParticleData &ghostParticles = model.getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float pointColor[4] = { 0.1f, 0.1f, 0.5f, 1 };
	float ghostPointColor[4] = { 0.1f, 0.1f, 0.1f, 0.5f };
	float edgeColor[4] = { 0.0f, 0.6f, 0.2f, 1 };

	for (size_t i = 0; i < numberOfPoints; i++)
	{
		MiniGL::drawSphere(particles.getPosition(i), 0.2f, pointColor);
	}

	for (size_t i = 0; i < numberOfPoints-1; i++)
	{
		MiniGL::drawSphere(ghostParticles.getPosition(i), 0.1f, ghostPointColor);
		MiniGL::drawVector(particles.getPosition(i), particles.getPosition(i + 1), 0.2f, edgeColor);
	}

	for (size_t i = 0; i < constraints.size(); i++)
	{
		if (constraints[i]->getTypeId() == ElasticRodBendAndTwistConstraint::TYPE_ID)
		{
			((ElasticRodBendAndTwistConstraint*)constraints[i])->m_restDarbouxVector = restDarboux;
		}
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create the elastic rod model
*/
void createRod()
{
	ParticleData &particles = model.getParticles();
	ParticleData &ghostParticles = model.getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	//centreline points
	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		particles.addVertex(Eigen::Vector3f((float)i*1.0f, 0.0f, 0.0f));
	}

	//edge ghost points
	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		ghostParticles.addVertex(Eigen::Vector3f((float)i*1.0f + 0.5f, 1.0f, 0.0f));
	}

	//lock two first particles and first ghost point
	particles.setMass(0, 0.0f);
	particles.setMass(1, 0.0f);
	ghostParticles.setMass(0, 0.0f);

	for (unsigned int i = 0; i < numberOfPoints - 1; i++)
	{
		model.addElasticRodEdgeConstraint(i, i + 1, i);

		if (i < numberOfPoints - 2)
		{
			//  Single rod element:
			//      D   E		//ghost points
			//		|	|
			//  --A---B---C--	// rod points
			int pA = i;
			int pB = i + 1;
			int pC = i + 2;
			int pD = i;
			int pE = i + 1;
			model.addElasticRodBendAndTwistConstraint(pA, pB, pC, pD, pE);
		}
	}
}

void TW_CALL setBendAndTwistStiffness(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	model.setElasticRodBendAndTwistStiffness(val);
}

void TW_CALL getBendAndTwistStiffness(void *value, void *clientData)
{
	*(float *)(value) = model.getElasticRodBendAndTwistStiffness();
}

void TW_CALL setRestDarbouxX(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	restDarboux[0] = val;
}

void TW_CALL setRestDarbouxY(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	restDarboux[1] = val;
}

void TW_CALL setRestDarbouxZ(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	restDarboux[2] = val;
}

void TW_CALL getRestDarbouxX(void *value, void *clientData)
{
	*(float *)(value) = restDarboux[0];
}

void TW_CALL getRestDarbouxY(void *value, void *clientData)
{
	*(float *)(value) = restDarboux[1];
}

void TW_CALL getRestDarbouxZ(void *value, void *clientData)
{
	*(float *)(value) = restDarboux[2];
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const float val = *(const float *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(float *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepController*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepController*)clientData)->getVelocityUpdateMethod();
}


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

#include <sstream>
#include <iostream>
#include <cmath>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>

#include <unsupported/Eigen/NonLinearOptimization>

#include <OpenMS/MATH/STATISTICS/GammaDistributionFitter.h>

// #define GAMMA_DISTRIBUTION_FITTER_VERBOSE
// #undef  GAMMA_DISTRIBUTION_FITTER_VERBOSE

namespace OpenMS
{
  namespace Math
  {
    GammaDistributionFitter::GammaDistributionFitter() :
      init_param_(1.0, 5.0)
    {
    }

    GammaDistributionFitter::~GammaDistributionFitter()
    {
    }

    void GammaDistributionFitter::setInitialParameters(const GammaDistributionFitResult& param)
    {
      init_param_ = param;
    }

    struct GammaFunctor
    {
      int inputs() const { return m_inputs; }
      int values() const { return m_values; }

      GammaFunctor(unsigned dimensions, const std::vector<DPosition<2> >* data) :
        m_inputs(dimensions), m_values(data->size()), m_data(data) {}

      int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
      {

        double b = x(0);
        double p = x(1);

        UInt i = 0;

        // gamma distribution is only defined for positive parameter values
        if (b > 0.0 && p > 0.0)
        {
          for (std::vector<DPosition<2> >::const_iterator it = m_data->begin(); it != m_data->end(); ++it, ++i)
          {
            double the_x = it->getX();
            fvec(i) =  std::pow(b, p) / boost::math::tgamma(p) * std::pow(the_x, p - 1) * std::exp(-b * the_x) - it->getY();
          }
        }
        else
        {
          for (std::vector<DPosition<2> >::const_iterator it = m_data->begin(); it != m_data->end(); ++it, ++i)
          {
            fvec(i) = -it->getY();
          }
        }

        return 0;
      }

      // compute Jacobian matrix for the different parameters
      int df(const Eigen::VectorXd& x, Eigen::MatrixXd& J)
      {

        double b = x(0);
        double p = x(1);

        UInt i = 0;
        // gamma distribution is only defined for positive parameter values
        if (b > 0.0 && p > 0.0)
        {
          for (std::vector<DPosition<2> >::const_iterator it = m_data->begin(); it != m_data->end(); ++it, ++i)
          {
            double the_x = it->getX();

            // partial deviation regarding b
            double part_dev_b = std::pow(the_x, p - 1) * std::exp(-the_x * b) / boost::math::tgamma(p) * (p * std::pow(b, p - 1) - the_x * std::pow(b, p));
            J(i, 0) = part_dev_b;

            // partial deviation regarding p
            double factor = std::exp(-b * the_x) * std::pow(the_x, p - 1) * std::pow(b, p) / std::pow(boost::math::tgamma(p), 2);
            double argument = (std::log(b) + std::log(the_x)) * boost::math::tgamma(p) - boost::math::tgamma(p) * boost::math::digamma(p);
            double part_dev_p = factor * argument;
            J(i, 1) = part_dev_p;
          }
        }
        else
        {
          for (std::vector<DPosition<2> >::const_iterator it = m_data->begin(); it != m_data->end(); ++it, ++i)
          {
            J(i, 0) = 0.0;
            J(i, 1) = 0.0;
          }
        }
        return 0;
      }

      const int m_inputs, m_values;
      const std::vector<DPosition<2> >* m_data;
    };

    GammaDistributionFitter::GammaDistributionFitResult GammaDistributionFitter::fit(const std::vector<DPosition<2> >& input)
    {
      Eigen::VectorXd x_init(2);
      x_init << init_param_.b, init_param_.p;
      GammaFunctor functor(2, &input);
      Eigen::LevenbergMarquardt<GammaFunctor> lmSolver(functor);
      Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);

      //the states are poorly documented. after checking the source, we believe that
      //all states except NotStarted, Running and ImproperInputParameters are good
      //termination states.
      if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-GammaDistributionFitter", "Could not fit the gamma distribution to the data");
      }

#ifdef GAMMA_DISTRIBUTION_FITTER_VERBOSE
      std::stringstream formula;
      formula << "f(x)=" << "(" << x_init(0) << " ** " << x_init(1) << ") / gamma(" << x_init(1) << ") * x ** (" << x_init(1) << " - 1) * exp(- " << x_init(0) << " * x)";
      std::cout << formula.str() << std::endl;
#endif

      return GammaDistributionFitResult(x_init(0), x_init(1));
    }

  } //namespace Math
} // namespace OpenMS


#ifndef STAN_MATH_PRIM_SCAL_PROB_NEG_BINOMIAL_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_NEG_BINOMIAL_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>
#include <stan/math/prim/scal/meta/max_size.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/random/negative_binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

// NegBinomial(n|alpha, beta)  [alpha > 0;  beta > 0;  n >= 0]
template <bool propto, typename T_n, typename T_shape, typename T_inv_scale>
typename return_type<T_shape, T_inv_scale>::type neg_binomial_lpmf(
    const T_n& n, const T_shape& alpha, const T_inv_scale& beta) {
  typedef typename stan::partials_return_type<T_n, T_shape, T_inv_scale>::type
      T_partials_return;

  static const char* function = "neg_binomial_lpmf";

  if (size_zero(n, alpha, beta))
    return 0.0;

  T_partials_return logp(0.0);
  check_nonnegative(function, "Failures variable", n);
  check_positive_finite(function, "Shape parameter", alpha);
  check_positive_finite(function, "Inverse scale parameter", beta);
  check_consistent_sizes(function, "Failures variable", n, "Shape parameter",
                         alpha, "Inverse scale parameter", beta);

  if (!include_summand<propto, T_shape, T_inv_scale>::value)
    return 0.0;

  using std::log;
  using std::log;

  scalar_seq_view<T_n> n_vec(n);
  scalar_seq_view<T_shape> alpha_vec(alpha);
  scalar_seq_view<T_inv_scale> beta_vec(beta);
  size_t size = max_size(n, alpha, beta);

  operands_and_partials<T_shape, T_inv_scale> ops_partials(alpha, beta);

  size_t len_ab = max_size(alpha, beta);
  VectorBuilder<true, T_partials_return, T_shape, T_inv_scale> lambda(len_ab);
  for (size_t i = 0; i < len_ab; ++i)
    lambda[i] = value_of(alpha_vec[i]) / value_of(beta_vec[i]);

  VectorBuilder<true, T_partials_return, T_inv_scale> log1p_beta(length(beta));
  for (size_t i = 0; i < length(beta); ++i)
    log1p_beta[i] = log1p(value_of(beta_vec[i]));

  VectorBuilder<true, T_partials_return, T_inv_scale> log_beta_m_log1p_beta(
      length(beta));
  for (size_t i = 0; i < length(beta); ++i)
    log_beta_m_log1p_beta[i] = log(value_of(beta_vec[i])) - log1p_beta[i];

  VectorBuilder<true, T_partials_return, T_inv_scale, T_shape>
      alpha_times_log_beta_over_1p_beta(len_ab);
  for (size_t i = 0; i < len_ab; ++i)
    alpha_times_log_beta_over_1p_beta[i]
        = value_of(alpha_vec[i])
          * log(value_of(beta_vec[i]) / (1.0 + value_of(beta_vec[i])));

  VectorBuilder<!is_constant_struct<T_shape>::value, T_partials_return, T_shape>
      digamma_alpha(length(alpha));
  if (!is_constant_struct<T_shape>::value) {
    for (size_t i = 0; i < length(alpha); ++i)
      digamma_alpha[i] = digamma(value_of(alpha_vec[i]));
  }

  VectorBuilder<!is_constant_struct<T_shape>::value, T_partials_return,
                T_inv_scale>
      log_beta(length(beta));
  if (!is_constant_struct<T_shape>::value) {
    for (size_t i = 0; i < length(beta); ++i)
      log_beta[i] = log(value_of(beta_vec[i]));
  }

  VectorBuilder<!is_constant_struct<T_inv_scale>::value, T_partials_return,
                T_shape, T_inv_scale>
      lambda_m_alpha_over_1p_beta(len_ab);
  if (!is_constant_struct<T_inv_scale>::value) {
    for (size_t i = 0; i < len_ab; ++i)
      lambda_m_alpha_over_1p_beta[i]
          = lambda[i]
            - (value_of(alpha_vec[i]) / (1.0 + value_of(beta_vec[i])));
  }

  for (size_t i = 0; i < size; i++) {
    if (alpha_vec[i] > 1e10) {  // reduces numerically to Poisson
      if (include_summand<propto>::value)
        logp -= lgamma(n_vec[i] + 1.0);
      if (include_summand<propto, T_shape, T_inv_scale>::value)
        logp += multiply_log(n_vec[i], lambda[i]) - lambda[i];

      if (!is_constant_struct<T_shape>::value)
        ops_partials.edge1_.partials_[i]
            += n_vec[i] / value_of(alpha_vec[i]) - 1.0 / value_of(beta_vec[i]);
      if (!is_constant_struct<T_inv_scale>::value)
        ops_partials.edge2_.partials_[i]
            += (lambda[i] - n_vec[i]) / value_of(beta_vec[i]);
    } else {  // standard density definition
      if (include_summand<propto, T_shape>::value)
        if (n_vec[i] != 0)
          logp += binomial_coefficient_log(
              n_vec[i] + value_of(alpha_vec[i]) - 1.0, n_vec[i]);
      if (include_summand<propto, T_shape, T_inv_scale>::value)
        logp += alpha_times_log_beta_over_1p_beta[i] - n_vec[i] * log1p_beta[i];

      if (!is_constant_struct<T_shape>::value)
        ops_partials.edge1_.partials_[i]
            += digamma(value_of(alpha_vec[i]) + n_vec[i]) - digamma_alpha[i]
               + log_beta_m_log1p_beta[i];
      if (!is_constant_struct<T_inv_scale>::value)
        ops_partials.edge2_.partials_[i]
            += lambda_m_alpha_over_1p_beta[i]
               - n_vec[i] / (value_of(beta_vec[i]) + 1.0);
    }
  }
  return ops_partials.build(logp);
}

template <typename T_n, typename T_shape, typename T_inv_scale>
inline typename return_type<T_shape, T_inv_scale>::type neg_binomial_lpmf(
    const T_n& n, const T_shape& alpha, const T_inv_scale& beta) {
  return neg_binomial_lpmf<false>(n, alpha, beta);
}

}  // namespace math
}  // namespace stan
#endif


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#define GRAPHBLAS_LOGGING_LEVEL 0

#include <functional>
#include <iostream>
#include <vector>

#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE apply_binop_test_suite

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
// apply standard matrix
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdmat_test_bad_dimension)
{
    // | 1 1 - - |
    // | 1 2 2 - |
    // | - 2 3 3 |
    // | - - 3 4 |

    // Build some sparse matrices.
    IndexArrayType i    = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j    = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v          = {1, 1, 1, 2, 2, 2, 3, 3, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i, j, v);

    // bind1st
    {
        Matrix<double, DirectedMatrixTag> C(3, 4);
        BOOST_CHECK_THROW(
            (apply(C,
                   NoMask(),
                   NoAccumulate(),
                   Times<double>(),
                   A,
                   2.0)),
            DimensionException);
    }
    {
        Matrix<double, DirectedMatrixTag> C(4, 3);
        BOOST_CHECK_THROW(
            (apply(C,
                   NoMask(),
                   NoAccumulate(),
                   Times<double>(),
                   2.0,
                   A)),
            DimensionException);
    }

    {
        Matrix<double, DirectedMatrixTag> C(4, 4);
        Matrix<bool> M(3, 4);
        BOOST_CHECK_THROW(
            (apply(C,
                   M,
                   NoAccumulate(),
                   Times<double>(),
                   2.0,
                   A)),
            DimensionException);
    }
    {
        Matrix<double, DirectedMatrixTag> C(4, 4);
        Matrix<bool> M(4, 3);
        BOOST_CHECK_THROW(
            (apply(C,
                   M,
                   NoAccumulate(),
                   Times<double>(),
                   2.0,
                   A)),
            DimensionException);
    }

    // bind 2nd
    {
        Matrix<double, DirectedMatrixTag> C(3, 4);
        BOOST_CHECK_THROW(
            (apply(C,
                   NoMask(),
                   NoAccumulate(),
                   Times<double>(),
                   A,
                   2.0)),
            DimensionException);
    }
    {
        Matrix<double, DirectedMatrixTag> C(4, 3);
        BOOST_CHECK_THROW(
            (apply(C,
                   NoMask(),
                   NoAccumulate(),
                   Times<double>(),
                   A,
                   2.0)),
            DimensionException);
    }

    {
        Matrix<double, DirectedMatrixTag> C(4, 4);
        Matrix<bool> M(3, 4);
        BOOST_CHECK_THROW(
            (apply(C,
                   M,
                   NoAccumulate(),
                   Times<double>(),
                   A,
                   2.0)),
            DimensionException);
    }
    {
        Matrix<double, DirectedMatrixTag> C(4, 4);
        Matrix<bool> M(4, 3);
        BOOST_CHECK_THROW(
            (apply(C,
                   M,
                   NoAccumulate(),
                   Times<double>(),
                   A,
                   2.0)),
            DimensionException);
    }

}

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

BOOST_AUTO_TEST_CASE(sparse_apply_matrix_nomask_noaccum)
{
    std::vector<std::vector<double>> matA = {{8, 1, 6},
                                             {3, 5, 7},
                                             {4, 9, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> mA(matA, 0);

    std::vector<std::vector<double>> matC = {{1, 2, 3},
                                             {4, 5, 6},
                                             {7, 8, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);

    std::vector<std::vector<double>> matAnswer = {{-8, -1, -6},
                                                  {-3, -5, -7},
                                                  {-4, -9,  0}};

    grb::Matrix<double, grb::DirectedMatrixTag> answer(matAnswer, 0);

    grb::apply(mC,
               grb::NoMask(),
               grb::NoAccumulate(),
               grb::Times<double>(),
               -1.0,
               mA);

    BOOST_CHECK_EQUAL(mC, answer);

    grb::apply(mC,
               grb::NoMask(),
               grb::NoAccumulate(),
               grb::Times<double>(),
               mA,
               -1.0);

    BOOST_CHECK_EQUAL(mC, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(sparse_apply_matrix_mask_merge_noaccum)
{
    std::vector<std::vector<double>> matA = {{8, 1, 6},
                                             {3, 5, 7},
                                             {4, 9, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> mA(matA, 0);

    std::vector<std::vector<double>> matC = {{1, 2, 3},
                                             {4, 5, 6},
                                             {7, 8, 0}};

    std::vector<std::vector<bool>> matMask = {{true, false, false},
                                              {true, true,  false,},
                                              {true, true,  true}};
    grb::Matrix<bool, grb::DirectedMatrixTag> mask(matMask, false);

    std::vector<std::vector<double>> matAnswer = {{-8, 2,  3},
                                                  {-3, -5, 6},
                                                  {-4, -9, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> answer(matAnswer, 0);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);

        grb::apply(mC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   -1.0,
                   mA);

        BOOST_CHECK_EQUAL(mC, answer);
    }
    {
        grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);

        grb::apply(mC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   mA,
                   -1.0);

        BOOST_CHECK_EQUAL(mC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(sparse_apply_matrix_mask_replace_noaccum)
{
    std::vector<std::vector<double>> matA = {{8, 1, 6},
                                             {3, 5, 7},
                                             {4, 9, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> mA(matA, 0);

    std::vector<std::vector<double>> matC = {{1, 2, 3},
                                             {4, 5, 6},
                                             {7, 8, 0}};

    std::vector<std::vector<bool>> matMask = {{true, false, false},
                                              {true, true,  false,},
                                              {true, true,  true}};
    grb::Matrix<bool, grb::DirectedMatrixTag> mask(matMask, false);

    std::vector<std::vector<double>> matAnswer = {{-8, 0,  0},
                                                  {-3, -5, 0},
                                                  {-4, -9, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> answer(matAnswer, 0);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);
        grb::apply(mC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   -1.0,
                   mA,
                   grb::REPLACE);

        BOOST_CHECK_EQUAL(mC, answer);
    }
    {
        grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);
        grb::apply(mC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   mA,
                   -1.0,
                   grb::REPLACE);

        BOOST_CHECK_EQUAL(mC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdmat_test_noaccum)
{
    // Build input matrix
    // | 1 1 - - |
    // | 1 2 2 - |
    // | - 2 3 3 |

    std::vector<std::vector<double>> Atmp = {{1, 1, 0, 0},
                                             {1, 2, 2, 0},
                                             {0, 2, 3, 3}};
    Matrix<double, DirectedMatrixTag> A(Atmp, 0.0);

    std::vector<std::vector<double>> Ctmp = {{9, 9, 9, 9},
                                             {9, 9, 9, 9},
                                             {9, 9, 9, 9}};

    std::vector<std::vector<uint8_t>> mask = {{1, 1, 1, 1},
                                              {0, 1, 1, 1},
                                              {0, 0, 1, 1}};
    Matrix<uint8_t> M(mask, 0);

    {
        std::vector<std::vector<double>> ans = {{1.5, 1.5,   0,   0},
                                                {1.5, 2.5, 2.5,   0},
                                                {  0, 2.5, 3.5, 3.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(3, 4);
        apply(C,
              NoMask(),
              NoAccumulate(),
              Plus<double>(),
              0.5,
              A);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(3, 4);
        apply(C2,
              NoMask(),
              NoAccumulate(),
              Plus<double>(),
              A,
              0.5);
        BOOST_CHECK_EQUAL(C2, answer);
    }

    // Mask, replace
    {
        std::vector<std::vector<double>> ans = {{1.5, 1.5,   0,   0},
                                                {  0, 2.5, 2.5,   0},
                                                {  0,   0, 3.5, 3.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              M,
              NoAccumulate(),
              Plus<double>(),
              0.5,
              A,
              REPLACE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              M,
              NoAccumulate(),
              Plus<double>(),
              A,
              0.5,
              REPLACE);
        BOOST_CHECK_EQUAL(C2, answer);
    }

    // Mask, merge
    {
        std::vector<std::vector<double>> ans = {{1.5, 1.5,   0,   0},
                                                {  9, 2.5, 2.5,   0},
                                                {  9,   9, 3.5, 3.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              M,
              NoAccumulate(),
              Plus<double>(),
              0.5,
              A,
              MERGE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              M,
              NoAccumulate(),
              Plus<double>(),
              A,
              0.5,
              MERGE);
        BOOST_CHECK_EQUAL(C2, answer);
    }

    // complement(Mask), replace
    {
        std::vector<std::vector<double>> ans = {{  0,   0,   0,   0},
                                                {1.5,   0,   0,   0},
                                                {  0, 2.5,   0,   0}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              complement(M),
              NoAccumulate(),
              Plus<double>(),
              0.5,
              A,
              REPLACE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              complement(M),
              NoAccumulate(),
              Plus<double>(),
              A,
              0.5,
              REPLACE);
        BOOST_CHECK_EQUAL(C2, answer);
    }
    // complement(Mask), merge
    {
        std::vector<std::vector<double>> ans = {{  9,   9,   9,   9},
                                                {1.5,   9,   9,   9},
                                                {  0, 2.5,   9,   9}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              complement(M),
              NoAccumulate(),
              Plus<double>(),
              0.5,
              A,
              MERGE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              complement(M),
              NoAccumulate(),
              Plus<double>(),
              A,
              0.5,
              MERGE);
        BOOST_CHECK_EQUAL(C2, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(sparse_apply_matrix_nomask_plus_accum)
{
    std::vector<std::vector<double>> matA = {{8, 1, 6},
                                             {3, 5, 7},
                                             {4, 9, 0}};
    grb::Matrix<double, grb::DirectedMatrixTag> mA(matA, 0);

    std::vector<std::vector<double>> matC = {{1, 2, 3},
                                             {4, 5, 6},
                                             {7, 8, 0}};

    std::vector<std::vector<double>> matAnswer = {{-7, 1, -3},
                                                  { 1, 0, -1},
                                                  { 3,-1,  1337}};
    // NOTE: The accum results in an explicit zero
    grb::Matrix<double, grb::DirectedMatrixTag> answer(matAnswer, 1337);

    {
        grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);
        grb::apply(mC,
                   grb::NoMask(),
                   grb::Plus<double>(),
                   grb::Times<double>(),
                   -1.0,
                   mA);

        BOOST_CHECK_EQUAL(mC, answer);
    }
    {
        grb::Matrix<double, grb::DirectedMatrixTag> mC(matC, 0);
        grb::apply(mC,
                   grb::NoMask(),
                   grb::Plus<double>(),
                   grb::Times<double>(),
                   mA,
                   -1.0);

        BOOST_CHECK_EQUAL(mC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdmat_test_plus_accum)
{
    // Build input matrix
    // | 1 1 - - |
    // | 1 2 2 - |
    // | - 2 3 3 |

    std::vector<std::vector<double>> Atmp = {{1, 1, 0, 0},
                                             {1, 2, 2, 0},
                                             {0, 2, 3, 3}};
    Matrix<double, DirectedMatrixTag> A(Atmp, 0.0);

    std::vector<std::vector<double>> Ctmp = {{9, 9, 9, 0},
                                             {9, 9, 0, 0},
                                             {9, 0, 0, 0}};

    std::vector<std::vector<uint8_t>> mask = {{1, 1, 1, 1},
                                              {0, 1, 1, 1},
                                              {0, 0, 1, 1}};
    Matrix<uint8_t> M(mask, 0);

    {
        std::vector<std::vector<double>> ans = {{9.5,  9.5,    9,   0},
                                                {9.5, 10.0,  1.0,   0},
                                                {  9,  1.0,  1.5, 1.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0.);
        apply(C,
              NoMask(),
              Plus<double>(),
              Times<double>(),
              0.5,
              A);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0.);
        apply(C2,
              NoMask(),
              Plus<double>(),
              Times<double>(),
              A,
              0.5);
        BOOST_CHECK_EQUAL(C2, answer);
    }

    // Mask, replace
    {
        std::vector<std::vector<double>> ans = {{9.5,  9.5,    9,   0},
                                                {  0, 10.0,  1.0,   0},
                                                {  0,    0,  1.5, 1.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              M,
              Plus<double>(),
              Times<double>(),
              0.5,
              A,
              REPLACE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              M,
              Plus<double>(),
              Times<double>(),
              A,
              0.5,
              REPLACE);
        BOOST_CHECK_EQUAL(C2, answer);
    }
    // Mask, merge
    {
        std::vector<std::vector<double>> ans = {{9.5,  9.5,    9,   0},
                                                {  9, 10.0,  1.0,   0},
                                                {  9,    0,  1.5, 1.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              M,
              Plus<double>(),
              Times<double>(),
              0.5,
              A,
              MERGE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              M,
              Plus<double>(),
              Times<double>(),
              A,
              0.5,
              MERGE);
        BOOST_CHECK_EQUAL(C2, answer);
    }

    // complement(Mask), replace
    {
        std::vector<std::vector<double>> ans = {{  0,    0,    0,   0},
                                                {9.5,    0,    0,   0},
                                                {  9,  1.0,    0,   0}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              complement(M),
              Plus<double>(),
              Times<double>(),
              0.5,
              A,
              REPLACE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              complement(M),
              Plus<double>(),
              Times<double>(),
              A,
              0.5,
              REPLACE);
        BOOST_CHECK_EQUAL(C2, answer);
    }
    // complement(Mask), merge
    {
        std::vector<std::vector<double>> ans = {{  9,    9,    9,   0},
                                                {9.5,    9,    0,   0},
                                                {  9,  1.0,    0,   0}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(Ctmp, 0);
        apply(C,
              complement(M),
              Plus<double>(),
              Times<double>(),
              0.5,
              A,
              MERGE);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(Ctmp, 0);
        apply(C2,
              complement(M),
              Plus<double>(),
              Times<double>(),
              A,
              0.5,
              MERGE);
        BOOST_CHECK_EQUAL(C2, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdmat_test_second_accum)
{
    // Build input matrix
    // | 1 1 - - |
    // | 1 2 2 - |
    // | - 2 3 3 |
    // | - - 3 4 |

    IndexArrayType i = {0, 0, 1, 1, 1, 2, 2, 2, 3, 3};
    IndexArrayType j = {0, 1, 0, 1, 2, 1, 2, 3, 2, 3};
    std::vector<double> v = {1, 1, 1, 2, 2, 2, 3, 3, 3, 4};
    Matrix<double, DirectedMatrixTag> A(4, 4);
    A.build(i, j, v);

    std::vector<double> v_answer = {0.5, 0.5, 0.5, 1.0, 1.0,
                                    1.0, 1.5, 1.5, 1.5, 2.0};

    Matrix<double, DirectedMatrixTag> answer(4, 4);
    answer.build(i, j, v_answer);

    Matrix<double, DirectedMatrixTag> C(4, 4);
    apply(C,
          NoMask(),
          Second<double>(),
          Times<double>(),
          0.5,
          A);
    BOOST_CHECK_EQUAL(C, answer);

    Matrix<double, DirectedMatrixTag> C2(4, 4);
    apply(C2,
          NoMask(),
          Second<double>(),
          Times<double>(),
          A,
          0.5);
    BOOST_CHECK_EQUAL(C2, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdmat_test_noaccum_transpose)
{
    // Build input matrix
    // | 1 1 - - |T
    // | 1 2 2 - |
    // | - 2 3 3 |

    std::vector<std::vector<double>> Atmp = {{1, 1, 0},
                                             {1, 2, 2},
                                             {0, 2, 3},
                                             {0, 0, 3}};
    Matrix<double, DirectedMatrixTag> A(Atmp, 0.0);

    std::vector<std::vector<double>> Ctmp = {{9, 9, 9, 9},
                                             {9, 9, 9, 9},
                                             {9, 9, 9, 9}};

    std::vector<std::vector<uint8_t>> mask = {{1, 1, 1, 1},
                                              {0, 1, 1, 1},
                                              {0, 0, 1, 1}};
    Matrix<uint8_t> M(mask, 0);

    {
        std::vector<std::vector<double>> ans = {{0.5, 0.5,   0,   0},
                                                {0.5, 1.0, 1.0,   0},
                                                {0,   1.0, 1.5, 1.5}};
        Matrix<double, DirectedMatrixTag> answer(ans, 0.);

        Matrix<double, DirectedMatrixTag> C(3, 4);
        apply(C,
              NoMask(),
              NoAccumulate(),
              Times<double>(),
              transpose(A),
              0.5);
        BOOST_CHECK_EQUAL(C, answer);

        Matrix<double, DirectedMatrixTag> C2(3, 4);
        apply(C2,
              NoMask(),
              NoAccumulate(),
              Times<double>(),
              transpose(A),
              0.5);
        BOOST_CHECK_EQUAL(C2, answer);
    }
}

//****************************************************************************
// Apply standard vector
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdvec_test_bad_dimension)
{
    // | - 2 3 3 |

    // Build some sparse vectors.
    std::vector<double> v = {0, 2, 3, 3};
    Vector<double> u(v, 0.0);

    {
        Vector<double> w(3);
        BOOST_CHECK_THROW(
            (apply(w,
                   NoMask(),
                   NoAccumulate(),
                   Times<double>(),
                   0.5,
                   u)),
            DimensionException);

        BOOST_CHECK_THROW(
            (apply(w,
                   NoMask(),
                   NoAccumulate(),
                   Times<double>(),
                   u,
                   0.5)),
            DimensionException);
    }

    {
        Vector<double> w(4);
        Vector<bool> m(3);
        BOOST_CHECK_THROW(
            (apply(w,
                   m,
                   NoAccumulate(),
                   Times<double>(),
                   0.5,
                   u)),
            DimensionException);
        BOOST_CHECK_THROW(
            (apply(w,
                   m,
                   NoAccumulate(),
                   Times<double>(),
                   u,
                   0.5)),
            DimensionException);
    }
}

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

BOOST_AUTO_TEST_CASE(sparse_apply_vector_nomask_noaccum)
{
    std::vector<double> vecA = {8, 0, 6, 0};
    grb::Vector<double> vA(vecA, 0);

    std::vector<double> vecC = {1, 2, 0, 0};

    std::vector<double> vecAnswer = {-8, 0, -6,  0};
    grb::Vector<double> answer(vecAnswer, 0);

    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   -1.0,
                   vA);

        BOOST_CHECK_EQUAL(vC, answer);
    }
    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   vA,
                   -1.0);

        BOOST_CHECK_EQUAL(vC, answer);
    }
}

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

BOOST_AUTO_TEST_CASE(sparse_apply_vector_nomask_noaccum_bool)
{
    std::vector<bool> vecA = {true, false, true, false};
    grb::Vector<bool> vA(vecA, false);

    std::vector<bool> vecC = {true, true, false, false};

    std::vector<bool> vecAnswer = {true, false, true, false};
    grb::Vector<bool> answer(vecAnswer, false);

    {
        grb::Vector<bool> vC(vecC, false);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::LogicalXor<bool>(),
                   false,
                   vA);

        BOOST_CHECK_EQUAL(vC, answer);
    }
    {
        grb::Vector<bool> vC(vecC, false);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::LogicalXor<bool>(),
                   vA,
                   false);

        BOOST_CHECK_EQUAL(vC, answer);
    }
}

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

BOOST_AUTO_TEST_CASE(sparse_apply_vector_nomask_noaccum_int)
{
    std::vector<int> vecA = {1, 0, 1, 0};
    grb::Vector<int> vA(vecA, 0);

    std::vector<int> vecC = {1, 1, 0, 0};

    std::vector<int> vecAnswer = {2, 0, 2, 0};
    grb::Vector<int> answer(vecAnswer, 0);

    {
        grb::Vector<int> vC(vecC, 0);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Plus<int>(),
                   1,
                   vA);

        BOOST_CHECK_EQUAL(vC, answer);
    }
    {
        grb::Vector<int> vC(vecC, 0);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::NoAccumulate(),
                   grb::Plus<int>(),
                   vA,
                   1);

        BOOST_CHECK_EQUAL(vC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(sparse_apply_vector_mask_merge_noaccum)
{
    std::vector<double> vecA = {8, 8, 0, 0, 6, 6, 0, 0};
    grb::Vector<double> vA(vecA, 0);

    std::vector<double> vecC = {1, 1, 2, 2, 0, 0, 0, 0};

    std::vector<bool> vecMask = {true, false, true, false, true, false, true, false};
    grb::Vector<bool> mask(vecMask, false);

    std::vector<double> vecAnswer = {-8, 1, 0, 2, -6, 0, 0, 0};
    grb::Vector<double> answer(vecAnswer, 0);

    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   -1.0,
                   vA);

        BOOST_CHECK_EQUAL(vC, answer);
    }
    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   vA,
                   -1.0);

        BOOST_CHECK_EQUAL(vC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(sparse_apply_vector_mask_replace_noaccum)
{
    std::vector<double> vecA = {8, 8, 0, 0, 6, 6, 0, 0};
    grb::Vector<double> vA(vecA, 0);

    std::vector<double> vecC = {1, 1, 2, 2, 0, 0, 0, 0};

    std::vector<bool> vecMask = {true, false, true, false, true, false, true, false};
    grb::Vector<bool> mask(vecMask, false);

    std::vector<double> vecAnswer = {-8, 0, 0, 0, -6, 0, 0, 0};
    grb::Vector<double> answer(vecAnswer, 0);

    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   -1.0,
                   vA,
                   REPLACE);

        BOOST_CHECK_EQUAL(vC, answer);
    }

    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   mask,
                   grb::NoAccumulate(),
                   grb::Times<double>(),
                   vA,
                   -1.0,
                   REPLACE);

        BOOST_CHECK_EQUAL(vC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdvec_test_noaccum)
{
    // Build input vector
    // | 1 2 - - |'

    std::vector<double> utmp  = {4, 5, 2, 3, 0, 0};
    Vector<double> u(utmp, 0.0);

    std::vector<double> wtmp  = {9, 0, 9, 0, 9, 0};
    std::vector<uint8_t> mask = {0, 0, 1, 1, 1, 1};
    Vector<uint8_t> m(mask, 0);

    {
        std::vector<double> ans = {-4., -5., -2., -3.,   0,   0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(6);
        apply(w,
              NoMask(),
              NoAccumulate(),
              Times<double>(),
              -1.,
              u);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(6);
        apply(w2,
              NoMask(),
              NoAccumulate(),
              Times<double>(),
              u,
              -1.);
        BOOST_CHECK_EQUAL(w2, answer);
    }

    // Mask, replace
    {
        std::vector<double> ans = {0, 0, -2., -3.,   0,   0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              m,
              NoAccumulate(),
              Times<double>(),
              -1.,
              u,
              REPLACE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              m,
              NoAccumulate(),
              Times<double>(),
              u,
              -1.,
              REPLACE);
        BOOST_CHECK_EQUAL(w2, answer);
    }
    // Mask, merge
    {
        std::vector<double> ans = {9, 0, -2., -3., 0, 0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              m,
              NoAccumulate(),
              Times<double>(),
              -1.,
              u,
              MERGE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              m,
              NoAccumulate(),
              Times<double>(),
              u,
              -1.,
              MERGE);
        BOOST_CHECK_EQUAL(w2, answer);
    }

    // complement(Mask), replace
    {
        std::vector<double> ans = {-4., -5., 0, 0,   0,   0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              complement(m),
              NoAccumulate(),
              Times<double>(),
              -1.,
              u,
              REPLACE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              complement(m),
              NoAccumulate(),
              Times<double>(),
              u,
              -1.,
              REPLACE);
        BOOST_CHECK_EQUAL(w2, answer);
    }
    // complement(Mask), merge
    {
        std::vector<double> ans = {-4., -5., 9, 0, 9,  0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              complement(m),
              NoAccumulate(),
              Times<double>(),
              -1.,
              u,
              MERGE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              complement(m),
              NoAccumulate(),
              Times<double>(),
              u,
              -1.,
              MERGE);
        BOOST_CHECK_EQUAL(w2, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(sparse_apply_vector_accum)
{
    std::vector<double> vecA = {8, 0, 6, 0};
    grb::Vector<double> vA(vecA, 0);

    std::vector<double> vecC = {8, 2, 0, 0};

    // NOTE: The accum results in an explicit zero
    std::vector<double> vecAnswer = {0, 2, -6,  666};
    grb::Vector<double> answer(vecAnswer, 666);

    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::Plus<double>(),
                   grb::Times<double>(),
                   -1.,
                   vA);

        BOOST_CHECK_EQUAL(vC, answer);
    }

    {
        grb::Vector<double> vC(vecC, 0);
        grb::apply(vC,
                   grb::NoMask(),
                   grb::Plus<double>(),
                   grb::Times<double>(),
                   vA,
                   -1.);

        BOOST_CHECK_EQUAL(vC, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(apply_stdvec_test_plus_accum)
{
    // Build input vector
    // | 1 2 - - |'

    std::vector<double> utmp  = {4, 5, 2, 3, 0, 0};
    Vector<double> u(utmp, 0.0);

    std::vector<double> wtmp  = {9, 0, 9, 0, 9, 0};
    std::vector<uint8_t> mask = {0, 0, 1, 1, 1, 1};
    Vector<uint8_t> m(mask, 0);

    {
        std::vector<double> ans = {5., -5., 7., -3.,  9.,   0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              NoMask(),
              Plus<double>(),
              Times<double>(),
              -1.,
              u);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              NoMask(),
              Plus<double>(),
              Times<double>(),
              u,
              -1.);
        BOOST_CHECK_EQUAL(w2, answer);
    }

    // Mask, replace
    {
        std::vector<double> ans = {0, 0, 7., -3.,  9.,   0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              m,
              Plus<double>(),
              Times<double>(),
              -1.,
              u,
              REPLACE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              m,
              Plus<double>(),
              Times<double>(),
              u,
              -1.,
              REPLACE);
        BOOST_CHECK_EQUAL(w2, answer);
    }
    // Mask, merge
    {
        std::vector<double> ans = {9, 0, 7., -3., 9, 0};
        Vector<double> answer(ans, 0.);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              m,
              Plus<double>(),
              Times<double>(),
              -1.,
              u,
              MERGE);
        BOOST_CHECK_EQUAL(w2, answer);

        Vector<double> w(wtmp, 0);
        apply(w,
              m,
              Plus<double>(),
              Times<double>(),
              u,
              -1.,
              MERGE);
        BOOST_CHECK_EQUAL(w, answer);
    }

    // complement(Mask), replace
    {
        std::vector<double> ans = {5., -5., 0, 0,   0,   0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              complement(m),
              Plus<double>(),
              Times<double>(),
              -1.,
              u,
              REPLACE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              complement(m),
              Plus<double>(),
              Times<double>(),
              u,
              -1.,
              REPLACE);
        BOOST_CHECK_EQUAL(w2, answer);
    }
    // complement(Mask), merge
    {
        std::vector<double> ans = {5., -5., 9, 0, 9,  0};
        Vector<double> answer(ans, 0.);

        Vector<double> w(wtmp, 0);
        apply(w,
              complement(m),
              Plus<double>(),
              Times<double>(),
              -1.,
              u,
              MERGE);
        BOOST_CHECK_EQUAL(w, answer);

        Vector<double> w2(wtmp, 0);
        apply(w2,
              complement(m),
              Plus<double>(),
              Times<double>(),
              u,
              -1.,
              MERGE);
        BOOST_CHECK_EQUAL(w2, answer);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include "specex_linalg.h"
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/basic_text_oarchive.hpp>
#include <time.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/random.hpp>

using namespace std;

int main() {

  harp::vector_double x(4);
  x(1)=1;
  x[2]=2;

  if(0) {
  boost::archive::xml_oarchive xml_oa ( cout );
  xml_oa << BOOST_SERIALIZATION_NVP(x);

  boost::archive::text_oarchive text_oa ( cout );
  text_oa << BOOST_SERIALIZATION_NVP(x);

  boost::archive::text_oarchive basic_text_oa ( cout );
  basic_text_oa << BOOST_SERIALIZATION_NVP(x);
  }

  {

    //ublas::mapped_vector<double> sv (4000);
    //ublas::compressed_vector<double> sv (4000);
     cout << "==================" << endl;
  cout << " SPARSE VECTORS   " << endl;
  cout << "==================" << endl;

  int n = 8000;
  //ublas::coordinate_vector<double> sv (n);
  //ublas::mapped_vector<double> sv (n);
  ublas::compressed_vector<double> sv (n);
  //ublas::vector<double> sv (n);
  harp::vector_double v (n);
  harp::vector_double v2 (n);
  double w = 0.2;
  v.clear();
  v2.clear();
  sv.clear();

  for (unsigned i = 0; i < sv.size (); i ++) {
    v2(i)=i;
  }
  for (unsigned i = 0; i < sv.size (); i += 10) {
    sv(i) = i;
    v(i)=i;
  }

  {
    harp::matrix_double a(10,10);
    a.clear();
    harp::vector_double h = ublas::project(v2,ublas::range(0,10));
    cout << h << endl;
    specex::syr(1,ublas::project(h,ublas::range(0,5)),a);
    cout << a << endl;
    a.clear();
    specex::syr(1,ublas::project(h,ublas::range(5,10)),a);
    cout << a << endl;

    exit(12);
  }

    // see http://www.boost.org/doc/libs/1_38_0/libs/numeric/ublas/doc/blas.htm

  harp::matrix_double m(n,n);

  int N=500;
  {
    m.clear();
    clock_t tstart = clock();
    for(int i=0;i<N;i++)
      specex::syr(w,v,m);
    clock_t tstop = clock();
    cout << "#1 n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
  }

  //ublas::coordinate_matrix<double> sm (n,n);
  ublas::compressed_matrix<double> sm (n,n);
  //ublas::triangular_matrix<double,  ublas::lower> sm (n,n); // crashed
  //ublas::mapped_matrix<double> sm (n,n);
  //ublas::matrix<double> sm (n,n);
  {

    clock_t  tstart = clock();
    //ublas::outer_prod(sv,sv);
    clock_t tstop = clock();
    cout << "#1 n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
  }
  {

    clock_t  tstart = clock();
    //ublas::outer_prod(sv,sv);

    //ublas::triangular_adaptor<ublas::compressed_matrix<double>,ublas::lower> tsm(sm);
    for(int i=0;i<N;i++)
      ublas::noalias(sm) += w*ublas::outer_prod(sv,sv);
    //ublas::noalias(m) += ublas::sparse_prod(sv,sv);

    //ublas::blas_3::srk(m,1,1,v);

    clock_t tstop = clock();
    cout << "#2 n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
  }
  harp::matrix_double dm = m - sm;
  for(unsigned i = 0; i < 100;i++) {
    cout << "m dm ("<< i << ") =" << m(i,i) << " " << dm(i,i) << endl;
  }

  if(0) {
    harp::matrix_double m(n,n); m.clear();
    clock_t tstart = clock();
    for(int i=0;i<N;i++)
      specex::syr(w,v2,m);
    clock_t tstop = clock();
    cout << "#3 n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
  }

    return 0;

}
  cout << "==================" << endl;
  cout << " LINEAR SYSTEMS   " << endl;
  cout << "==================" << endl;

  typedef boost::ecuyer1988 base_generator_type;
  base_generator_type generator(42u);
  boost::uniform_real < double > dist ( 0.0, 1.0 );
  boost::variate_generator < base_generator_type&, boost::uniform_real < double > > uni ( generator, dist );

  int ndata = 12;
  int nparams = 10;
  vector<harp::vector_double> h;
  for( int i=0; i<ndata; i++) {
    harp::vector_double hi(nparams);
    for(int p=0; p<nparams ;p++) {
      hi(p)=uni(); // stupid number
    }
    //cout << "data " << i << " h=" << hi << endl;
    h.push_back(hi);
  }

  harp::vector_double true_params(nparams);
  for(int p=0; p<nparams ;p++) {
    true_params(p)=p;
  }

  harp::matrix_double A(nparams,nparams);
  harp::vector_double B(nparams);
  A.clear();
  B.clear();

  harp::vector_double data(ndata);
  for( int i=0; i<ndata; i++) {
    data[i]=specex::dot(true_params,h[i]);
    double w=uni()+1; // stupid number
    specex::syr(w,h[i],A);
    specex::axpy(w*data(i),h[i],B);
  }

  //cout << "A=" << A << endl;
  //cout << "B=" << B << endl;

  int status = specex::cholesky_solve(A,B);
  cout << "cholesky_solve status = " << status << endl;
  harp::vector_double diff = B-true_params;
  cout << "parameter residuals = " << diff << endl;
  harp::vector_double res(ndata);
  for( int i=0; i<ndata; i++) {
    res(i) = data(i)-specex::dot(B,h[i]);
  }
  cout << "data residuals = " << res << endl;

  cout << "==================" << endl;
  cout << " PROJECTIONS   " << endl;
  cout << "==================" << endl;

  harp::vector_double one(16);
  harp::vector_double zero(16);
  zero.clear();

  for(size_t i=0;i<one.size();i++) one[i]=i+1;
  for(int j=0;j<4;j++)
    ublas::project(zero,ublas::range(4*j,4*(j+1))) += pow(10,j)* ublas::project(one,ublas::range(4*j,4*(j+1)));
  cout << zero << endl;

  return 0;
  harp::vector_double y(12);
  for(int i=7;i<7+4;i++) y[i]=1;

  try {
    //cout << specex::dot(x,y) << endl; // this throws an exception

    ublas::range r(0,1);
    cout << specex::dot(x,ublas::project(y,ublas::range(0,x.size()))) << endl;
    cout << specex::dot(x,ublas::project(y,ublas::range(7,7+x.size()))) << endl;

    y.clear();
    size_t x_size = x.size();
    for(size_t k=0;(k+1)*x_size<=y.size();k++) {
      ublas::project(y,ublas::range(k*x_size,(k+1)*x_size)) = (k+1)*x;
    }

    cout << y << endl;
    cout << ublas::inner_prod (y,y) << endl;
    cout << specex::dot(y,y) << endl;
    cout << ublas::outer_prod (y,y) << endl;

    cout << "==================" << endl;
    cout << " SPARSE VECTORS   " << endl;
    cout << "==================" << endl;

    //ublas::mapped_vector<double> sv (8000);
    ublas::compressed_vector<double> sv (8000);
    //ublas::coordinate_vector<double> sv (8000);
    harp::vector_double v (8000);

    if(0) {

      cout << sv << endl;
      // for coordinate vector
      //for(ublas::coordinate_vector<double>::iterator it=sv.begin();it!=sv.end();++it) {
      //cout << *it << " ";
      //}
      //cout << endl;
    }

    double res1=0;
    {
      clock_t tstart = clock();
      ublas::vector<double> v2(v.size());
      ublas::matrix<double> m = ublas::outer_prod(v,v); boost::numeric::bindings::blas::gemv(1,m,v,0,v2);
      //boost::numeric::bindings::blas::gemv(1,(ublas::matrix<double>&)ublas::outer_prod(v,v),v,0,v2);
      //cout << v2 << endl;
      clock_t tstop = clock();
      res1=ublas::inner_prod(v,v2);
      cout << res1 << endl;
      cout << "n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
    }
    if(1){ // ok
      clock_t tstart = clock();
      ublas::vector<double> v2(sv.size());
      ublas::matrix<double> m = ublas::outer_prod(sv,sv); boost::numeric::bindings::blas::gemv(1,m,v,0,v2);
      double res2 = ublas::inner_prod(sv,v2);
      clock_t tstop = clock();
      cout << res2 << " " << res2/res1-1 << endl;
      cout << "n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
    }
    if(1){ // ok
      clock_t tstart = clock();
      double res2 = ublas::inner_prod(sv,ublas::prod(ublas::outer_prod(sv,sv),sv));
      clock_t tstop = clock();
      cout << res2 << " " << res2/res1-1 << endl;
      cout << "n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
    }

    // same with "pre-existing" matrix
    {
      ublas::matrix<double, ublas::column_major> m(v.size(),v.size());
      clock_t tstart = clock();
      //m += ublas::outer_prod(sv,sv); // terribly slow
      // ublas::add(m,ublas::outer_prod(sv,sv)); // doesn't compile
      boost::numeric::bindings::blas::syr(1.,v,boost::numeric::bindings::lower(m)); // compiles, and fast, but fills only part of the matrix !!
      //boost::numeric::bindings::blas::syr(1.,sv,boost::numeric::bindings::lower(m)); // doesn't compile

      //specex::syr(1.,v,m);
      double res2 = ublas::inner_prod(sv,ublas::prod(m,sv));
      clock_t tstop = clock();
      cout << res2 << " " << res2/res1-1 << endl;
      cout << "n clocks = " << tstop-tstart << " " << float(tstop-tstart)/float(CLOCKS_PER_SEC) << endl;
    }

  }catch(harp::exception e) {
    cout << "ohoh " << e.what() << endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE CircuitFinder test suite
// #include <boost/test/unit_test.hpp> // changes every so often...
#include <boost/test/included/unit_test.hpp>
#include <SmartPeak/graph/CircuitFinder.h>

#include <SmartPeak/ml/Link.h>
#include <SmartPeak/ml/Node.h>

#include <vector>
#include <iostream>

using namespace SmartPeak;
using namespace std;

BOOST_AUTO_TEST_SUITE(circuitFinder)

BOOST_AUTO_TEST_CASE(test)
{
	std::list<int>* A1;
	A1 = new std::list<int>[5];
	A1[0].push_back(2);
	A1[1].push_back(2); A1[1].push_back(3); A1[1].push_back(4);
	A1[2].push_back(5);
	A1[3].push_back(3);
	A1[4].push_back(1);

	CircuitFinder CF1(A1, 5);
	CF1.run();

	BOOST_CHECK_EQUAL(CF1.getCycles()[0].first, 5);
	BOOST_CHECK_EQUAL(CF1.getCycles()[1].first, 5);
	BOOST_CHECK_EQUAL(CF1.getCycles()[2].first, 2);
	BOOST_CHECK_EQUAL(CF1.getCycles()[0].second, 1);
	BOOST_CHECK_EQUAL(CF1.getCycles()[1].second, 1);
	BOOST_CHECK_EQUAL(CF1.getCycles()[2].second, 2);

	std::list<int>* A2;
	A2 = new std::list<int>[6];
	A2[0].push_back(2); A2[0].push_back(5);
	A2[1].push_back(3);
	A2[2].push_back(1); A2[2].push_back(2); A2[2].push_back(4); A2[2].push_back(6);
	A2[3].push_back(5);
	A2[4].push_back(2);
	A2[5].push_back(4);

	CircuitFinder CF2(A2, 6);
	CF2.run();

	BOOST_CHECK_EQUAL(CF2.getCycles()[0].first, 3);
	BOOST_CHECK_EQUAL(CF2.getCycles()[1].first, 3);
	BOOST_CHECK_EQUAL(CF2.getCycles()[2].first, 3);
	BOOST_CHECK_EQUAL(CF2.getCycles()[3].first, 5);
	BOOST_CHECK_EQUAL(CF2.getCycles()[4].first, 5);
	BOOST_CHECK_EQUAL(CF2.getCycles()[0].second, 1);
	BOOST_CHECK_EQUAL(CF2.getCycles()[1].second, 1);
	BOOST_CHECK_EQUAL(CF2.getCycles()[2].second, 2);
	BOOST_CHECK_EQUAL(CF2.getCycles()[3].second, 2);
	BOOST_CHECK_EQUAL(CF2.getCycles()[4].second, 2);
}

BOOST_AUTO_TEST_SUITE_END()