/////////////////////////////////////////////////////////////////////////////////////////////////
/// \file  /utils/lmyengine/block_mat.cpp
///
/// \brief  Implementation file for block recursive matrix class
///
/////////////////////////////////////////////////////////////////////////////////////////////////

#include <vector>
#include <list>
#include <string>
#include <numeric>
#include <cassert>
#include <algorithm>
#include <cmath>
#include <sstream>
#include "formic/utils/openmp.h"

#include <boost/format.hpp>
#include <boost/shared_ptr.hpp>

#include "formic/utils/matrix.h"
#include "formic/utils/mpi_interface.h"
#include "block_mat.h"
#include "formic/utils/lmyengine/block_detail.h"

//////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief  Function that contracts each block's previous update components with derivative ratios
///
/// \param[in]   dr        derivative ratio(bare or energy) for this sample
/// \param[in]   ou_mat    matrix holds old updates, size num_old_update * num_variables
/// \param[out]  cont_vec  contracted results
///
//////////////////////////////////////////////////////////////////////////////////////////////////
void cqmc::engine::LMBlockerMatData::prep_block_ou_contractions(const std::vector<double> & dr,
                                                                const formic::Matrix<double> & ou_mat,
                                                                std::vector<formic::ColVec<double> > & cont_vec) {

  // loop over blocks
  for (int b = 0; b < this->nb(); b++) {

    const int ibeg = 1 + m_block_beg.at(b);
    const int len = m_block_len.at(b);

    formic::ColVec<double> & cont = cont_vec.at(b);
    for (int i = 0; i < cont.size(); i++)
      cont.at(i) = 0.0;

    for (int k = 0; k < m_nou; k++) {
      for (int j = ibeg; j < ibeg+len; j++) {
        cont.at(k) += dr.at(j) * ou_mat.at(j-1,k);
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief  Function that create a matrix in the basis of a block of variables and older updates
///         from a different block
///
/// \param[in]   b     block index
/// \param[in]   x     block index
/// \param[out]  mat   output matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
void cqmc::engine::LMBlockerMatData::prep_lm_block_plus_other_ou_matrix(const int b, const int x, formic::Matrix<double> & mat) {

  // first check that if x and b is the same block
  if ( x == b )
    throw formic::Exception("b and x must be different in prep_lm_block_plus_other_ou_matrix");

  // get the length of this block
  const int len = this->bl(b);

  // get the dimension of this matrix
  const int dim = 1 + len + m_nou;

  const int y = x - ( x > b ? 1 : 0 );

  // size the output matrix correctly
  mat.reset(dim, dim);

  // <wfn|wfn> element
  mat.at(0,0) = m_ww[0];
  //std::cout << "in prep_lm_block_plus_other_ou_matrix mat(0,0) is " << m_ww << std::endl;

  // <wfn|var>
  for (int i = 0; i < len; i++)
    mat.at(0, 1+i) = m_wv[0].at(b).at(i);

  // <var|wfn>
  for (int i = 0; i < len; i++)
    mat.at(1+i, 0) = m_vw[0].at(b).at(i);

  // <var|var>
  for (int i = 0; i < len; i++) {
    for (int j = 0; j < len; j++) {
      mat.at(1+i,1+j) = m_vv[0].at(b).at(i,j);
    }
  }

  // <wfn|old_update>
  for (int k = 0; k < m_nou; k++)
    mat.at(0, 1+len+k) = m_wo[0].at(x).at(k);

  // <old_update_wfn>
  for (int k = 0; k < m_nou; k++)
    mat.at(1+len+k, 0) = m_ow[0].at(x).at(k);

  // <var|old_update>
  for (int i = 0; i < len; i++) {
    for (int k = 0; k < m_nou; k++) {
      mat.at(1+i, 1+len+k) = m_vo[0].at(b).at(i, y*m_nou+k);
    }
  }

  // <old_update|var>
  for (int k = 0; k < m_nou; k++) {
    for (int i = 0; i < len; i++) {
      mat.at(1+len+k, 1+i) = m_ov[0].at(b).at(y*m_nou+k, i);
    }
  }

  // <old_update|old_update>
  for (int k = 0; k < m_nou; k++) {
    for (int l = 0; l < m_nou; l++) {
      mat.at(1+len+k, 1+len+l) = m_oo[0].at(x).at(k,l);
    }
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief  Function that accumulates contribution to block matrices for this sample
///
/// \param[in]   d        weight for this sample
/// \param[in]   lr       left vector for outer product
/// \param[in]   rr       right vector for outer product
/// \param[in]   ou_mat   matrix storing the old update coefficients
///
////////////////////////////////////////////////////////////////////////////////////////////////
void cqmc::engine::LMBlockerMatData::acc(const double d, const std::vector<double> & lr, const std::vector<double> &rr, const formic::Matrix<double> & ou_mat) {

  // get the thread number
  int myThread = omp_get_thread_num();

  // check to see if the input vector is of the correct size
  if ( lr.size() != 1 + this->tot_rows(m_vv[myThread]) )
    throw formic::Exception("lr vector length was %i but should be %i LMBlockerMatData::acc") % lr.size() % ( 1 + this->tot_rows(m_vv[myThread]) );
  if ( rr.size() != 1 + this->tot_rows(m_vv[myThread]) )
    throw formic::Exception("rr vector length was %i but should be %i LMBlockerMatData::acc") % rr.size() % ( 1 + this->tot_rows(m_vv[myThread]) );

  // prep intermediates for old updates
  this->prep_block_ou_contractions(lr, ou_mat, m_boulr[myThread]);
  this->prep_block_ou_contractions(rr, ou_mat, m_bourr[myThread]);

  // <wfn|wfn>
  m_ww[myThread] += d * lr.at(0) * rr.at(0);

  // loop over blocks
  for (int b = 0; b < this->nb(); b++) {

    // beginning index
    const int ibeg = 1 + m_block_beg.at(b);

    // block length
    const int len = m_block_len.at(b);

    for (int i = 0; i < len; i++) {

      // <wfn|var>
      m_wv[myThread].at(b).at(i) += d * lr.at(0) * rr.at(ibeg + i);

      // <var|wfn>
      m_vw[myThread].at(b).at(i) += d * lr.at(ibeg + i) * rr.at(0);

    }

    for (int k = 0; k < m_nou; k++) {

      // <wfn|old_updates>
      m_wo[myThread].at(b).at(k) += d * lr.at(0) * m_bourr[myThread].at(b).at(k);

      // <old_updates|wfn>
      m_ow[myThread].at(b).at(k) += d * m_boulr[myThread].at(b).at(k) * rr.at(0);
    }

    // <var|var>
    { formic::Matrix<double> & vv_mat = m_vv[myThread].at(b);
      for (int i = 0; i < len; i++) {
        for (int j = 0; j < len; j++) {
          vv_mat.at(i,j) += d * lr.at(ibeg+i) * rr.at(ibeg+j);
        }
      }
    }

    // <var|old_update> note this is for var in the current block and old-update in all other blocks
    { formic::Matrix<double> & vo_mat = m_vo[myThread].at(b);
      for (int x = 0, y = 0; x < this->nb(); x++) {
        if ( x == b )
          continue;
        const formic::ColVec<double> & rr_vec = m_bourr[myThread].at(x);
        for (int o = 0; o < m_nou; o++) {
          for (int i = 0; i < len; i++) {
            vo_mat.at(i,y*m_nou+o) += d * lr.at(ibeg+i) * rr_vec.at(o);
           }
         }
         y++;
       }
     }

     // <old_update|var> note this is for var in the current block and old-update in all other blocks
     { formic::Matrix<double> & ov_mat = m_ov[myThread].at(b);
       for (int i = 0; i < len; i++) {
         for (int x = 0, y = 0; x < this->nb(); x++) {
           if ( x == b )
             continue;
           const formic::ColVec<double> & lr_vec = m_boulr[myThread].at(x);
           for (int k = 0; k < m_nou; k++) {
             ov_mat.at(m_nou*y+k, i) += d * lr_vec.at(k) * rr.at(ibeg+i);
           }
           y++;
         }
       }
     }

     // <old_update|old_update>
     { formic::Matrix<double> & oo_mat = m_oo[myThread].at(b);
       for (int k = 0; k < m_nou; k++) {
         for (int l = 0; l < m_nou; l++) {
           oo_mat.at(k,l) += d * m_boulr[myThread].at(b).at(k) * m_bourr[myThread].at(b).at(l);
         }
       }
     }
   }
}

////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief  Function that finalizes the accumulation by dividing matrices total weight
///
////////////////////////////////////////////////////////////////////////////////////////////////
void cqmc::engine::LMBlockerMatData::finalize(const double total_weight) {

  // get the number of threads
  int NumThreads = omp_get_max_threads();

  // sum over threads
  for (int ip = 1; ip < NumThreads; ip++) {
    m_ww[0] += m_ww[ip];
    for (int b = 0; b < this->nb(); b++) {
      m_wv[0].at(b) += m_wv[ip].at(b);
      m_vw[0].at(b) += m_vw[ip].at(b);
      m_wo[0].at(b) += m_wo[ip].at(b);
      m_ow[0].at(b) += m_ow[ip].at(b);
      m_ov[0].at(b) += m_ov[ip].at(b);
      m_vo[0].at(b) += m_vo[ip].at(b);
      m_oo[0].at(b) += m_oo[ip].at(b);
    }
  }

  // <wfn|wfn>
  m_ww[0] /= total_weight;

  // loop over blocks
  for (int b = 0; b < this->nb(); b++) {
    m_wv[0].at(b) /= total_weight;
    m_vw[0].at(b) /= total_weight;
    m_vv[0].at(b) /= total_weight;
    m_wo[0].at(b) /= total_weight;
    m_ow[0].at(b) /= total_weight;
    m_ov[0].at(b) /= total_weight;
    m_vo[0].at(b) /= total_weight;
    m_oo[0].at(b) /= total_weight;
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief  Function that finalizes the accumulation by dividing matrices total weight
///         across the whole processes
///
////////////////////////////////////////////////////////////////////////////////////////////////
void cqmc::engine::LMBlockerMatData::mpi_finalize(const double total_weight) {

  // get rank number and number of ranks
  int my_rank = formic::mpi::rank();

  // get the number of threads
  int NumThreads = omp_get_max_threads();

  // loop over blocks
  //for (int b = 0; b < this->nb(); b++) {
  //  // print out to debug
  //  for (int i = 0; i < m_ow.at(b).size(); i++) {
  //    std::cout << m_ow.at(b).at(i) << "   ";
  //  }
  //  std::cout << std::endl;
  //}
  //std::cout << std::endl;

  // sum over threads
  for (int ip = 1; ip < NumThreads; ip++) {
    m_ww[0] += m_ww[ip];
    for (int b = 0; b < this->nb(); b++) {
      m_wv[0].at(b) += m_wv[ip].at(b);
      m_vw[0].at(b) += m_vw[ip].at(b);
      m_vv[0].at(b) += m_vv[ip].at(b);
      m_wo[0].at(b) += m_wo[ip].at(b);
      m_ow[0].at(b) += m_ow[ip].at(b);
      m_ov[0].at(b) += m_ov[ip].at(b);
      m_vo[0].at(b) += m_vo[ip].at(b);
      m_oo[0].at(b) += m_oo[ip].at(b);
    }
  }

  // <wfn|wfn>
  double m_ww_tot = 0.0;
  formic::mpi::reduce(&m_ww[0], &m_ww_tot, 1, MPI_SUM);
  //MPI_Reduce(&m_ww, &m_ww_tot, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  m_ww[0] = m_ww_tot / total_weight;

  // get space for MPI reduce
  std::vector<formic::ColVec<double> > m_wv_tot, m_vw_tot, m_wo_tot, m_ow_tot;
  std::vector<formic::Matrix<double> > m_vv_tot, m_vo_tot, m_ov_tot, m_oo_tot;
  for (int i = 0; i < this->nb(); i++) {
    m_wv_tot.push_back(formic::ColVec<double>(this->bl(i), 0.0));
    m_vw_tot.push_back(formic::ColVec<double>(this->bl(i), 0.0));
    m_vv_tot.push_back(formic::Matrix<double>(this->bl(i), this->bl(i), 0.0));
    m_wo_tot.push_back(formic::ColVec<double>(m_nou, 0.0));
    m_ow_tot.push_back(formic::ColVec<double>(m_nou, 0.0));
    m_ov_tot.push_back(formic::Matrix<double>((this->nb()-1)*m_nou, this->bl(i), 0.0));
    m_vo_tot.push_back(formic::Matrix<double>(this->bl(i), (this->nb()-1)*m_nou, 0.0));
    m_oo_tot.push_back(formic::Matrix<double>(m_nou, m_nou, 0.0));
  }

  // do MPI reduce
  for (int i = 0; i < this->nb(); i++) {
    formic::mpi::reduce(&m_wv[0].at(i).at(0), &m_wv_tot.at(i).at(0), this->bl(i), MPI_SUM);
    formic::mpi::reduce(&m_vw[0].at(i).at(0), &m_vw_tot.at(i).at(0), this->bl(i), MPI_SUM);
    formic::mpi::reduce(&m_vv[0].at(i).at(0,0), &m_vv_tot.at(i).at(0,0), m_vv[0].at(i).size(), MPI_SUM);
    formic::mpi::reduce(&m_wo[0].at(i).at(0), &m_wo_tot.at(i).at(0), m_nou, MPI_SUM);
    formic::mpi::reduce(&m_ow[0].at(i).at(0), &m_ow_tot.at(i).at(0), m_nou, MPI_SUM);
    formic::mpi::reduce(&m_ov[0].at(i).at(0,0), &m_ov_tot.at(i).at(0,0), m_ov[0].at(i).size(), MPI_SUM);
    formic::mpi::reduce(&m_vo[0].at(i).at(0,0), &m_vo_tot.at(i).at(0,0), m_vo[0].at(i).size(), MPI_SUM);
    formic::mpi::reduce(&m_oo[0].at(i).at(0,0), &m_oo_tot.at(i).at(0,0), m_oo[0].at(i).size(), MPI_SUM);
  }

  // evaluate the average across all processors
  if ( my_rank == 0 ) {

    // loop over blocks
    for (int b = 0; b < this->nb(); b++) {
      m_vw[0].at(b) = m_vw_tot.at(b) / total_weight;
      m_wv[0].at(b) = m_wv_tot.at(b) / total_weight;
      m_vv[0].at(b) = m_vv_tot.at(b) / total_weight;
      m_ow[0].at(b) = m_ow_tot.at(b) / total_weight;
      m_wo[0].at(b) = m_wo_tot.at(b) / total_weight;
      m_vo[0].at(b) = m_vo_tot.at(b) / total_weight;
      m_ov[0].at(b) = m_ov_tot.at(b) / total_weight;
      m_oo[0].at(b) = m_oo_tot.at(b) / total_weight;
      // print out to debug
      //for (int i = 0; i < m_ow.at(b).size(); i++) {
      //  std::cout << m_ow.at(b).at(i) << "   ";
      //}
      //std::cout << std::endl;
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief  Function that resets the matrices
///
/////////////////////////////////////////////////////////////////////////////////////////////////
void cqmc::engine::LMBlockerMatData::reset(const int nv, const int nblock, const int nou) {

  m_nou = nou;

  // get block information
  cqmc::engine::brlm_get_block_info(nv, nblock, m_block_beg, m_block_end, m_block_len);

  // get the maximum number of threads
  int NumThreads = omp_get_max_threads();

  m_ww.assign(NumThreads, 0.0);

  m_wv.clear();
  m_wv.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_wv[ip].push_back(formic::ColVec<double>(this->bl(i), 0.0));
    }
  }

  m_vw.clear();
  m_vw.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_vw[ip].push_back(formic::ColVec<double>(this->bl(i), 0.0));
    }
  }

  m_vv.clear();
  m_vv.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_vv[ip].push_back(formic::Matrix<double>(this->bl(i), this->bl(i), 0.0));
    }
  }

  m_wo.clear();
  m_wo.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_wo[ip].push_back(formic::ColVec<double>(m_nou, 0.0));
    }
  }

  m_ow.clear();
  m_ow.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_ow[ip].push_back(formic::ColVec<double>(m_nou, 0.0));
    }
  }

  m_vo.clear();
  m_vo.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_vo[ip].push_back(formic::Matrix<double>(this->bl(i), (this->nb()-1)*m_nou, 0.0));
    }
  }

  m_ov.clear();
  m_ov.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_ov[ip].push_back(formic::Matrix<double>((this->nb()-1)*m_nou, this->bl(i), 0.0));
    }
  }

  m_oo.clear();
  m_oo.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_oo[ip].push_back(formic::Matrix<double>(m_nou, m_nou, 0.0));
    }
  }

  m_boulr.clear();
  m_bourr.clear();
  m_boulr.resize(NumThreads);
  m_bourr.resize(NumThreads);
  for (int ip = 0; ip < NumThreads; ip++) {
    for (int i = 0; i < this->nb(); i++) {
      m_boulr[ip].push_back(formic::ColVec<double>(m_nou, 0.0));
      m_bourr[ip].push_back(formic::ColVec<double>(m_nou, 0.0));
    }
  }
}


#pragma once

#include <boost/range/irange.hpp>
#include <stdexcept>
#include <vector>
#include <string>
#include <fmt/format.h>
#include <fmt/printf.h>

#include "TransformationFunctionArgs.hh"

namespace TypeClasses{
    class Range {
    public:
        /// End is inclusive
        Range(int begin)                          : m_begin(begin), m_end(begin), m_step(1u) {}
        Range(int begin, int end, size_t step=1u) : m_begin(begin), m_end(end), m_step(step) {}
        Range(const Range& other) = default;

        bool singular() const { return m_end == m_begin; }

        int getBegin() const { return m_begin; }
        int getEnd()   const { return m_end; }

        size_t getBeginAbs(size_t size, bool strict=true) const {
            size_t begin, end;
            getRangeAbs(size, begin, end, strict);
            return begin;
        }
        size_t getEndAbs(size_t size, bool strict=true)   const {
            size_t begin, end;
            getRangeAbs(size, begin, end, strict);
            return end;
        }

        decltype(auto) iterate(size_t size, bool strict=true) const {
            size_t begin, end;
            getRangeAbs(size, begin, end, strict);
            if(begin==-1lu){
                return boost::irange(begin, end, m_step);
            }
            return boost::irange(begin, end+1, m_step);
        }

        decltype(auto) iterateSafe(size_t size) const { return iterate(size, false); }

        std::vector<size_t> vector(size_t size, bool strict=true) const {
            auto it=iterate(size, strict);
            return std::vector<size_t>(it.begin(), it.end());
        }

        void dump(){
            printf("Range %i->%i, step %zu", m_begin, m_end, m_step);
            if(singular()){
                fmt::print(" (singular)");
            }
        }

        void dump(size_t size){
            printf("Range %i->%i, step %zu", m_begin, m_end, m_step);
            if(singular()){
                fmt::print(" (singular)");
            }
            size_t begin, end;
            getRangeAbs(size, begin, end, false);
            fmt::print(" for size {}: {}->{}", size, begin, end);
        }
    private:
        void getRangeAbs(int size, size_t &begin, size_t &end, bool strict=true) const {
            int absbegin = m_begin>=0 ? m_begin : size+m_begin;
            int absend   = m_end>=0   ? m_end   : size+m_end;

            bool error=absend<absbegin && !((m_begin>=0) ^ (m_end>=0));
            if (absend<0 || absbegin>=size || absend<absbegin){
                begin=end=-1lu;
                error|=strict;
            }
            else{
                if (absend>=size) {
                    absend=size-1;
                }
                if (absbegin<0){
                    absbegin=0;
                }
                begin=absbegin;
                end=absend;
            }

            //printf("Convert %i->%i (%i): %i->%i or %zu->%zu (error: %i)\n", m_begin, m_end, size, absbegin, absend, begin, end, (int)error);

            if(error){
                throw std::runtime_error(fmt::format("Invalid range {}->{} for size {}: {}->{}", m_begin, m_end, size, absbegin, absend));
            }
        }

        int m_begin;
        int m_end;
        size_t m_step;
    };

    template<typename FloatType>
    class TypeClassT {
    public:
        using TypesFunctionArgs = TransformationTypes::TypesFunctionArgsT<FloatType,FloatType>;
        virtual void processTypes(TypesFunctionArgs& fargs) = 0;
        virtual ~TypeClassT() = default;
    };

    template<typename FloatType>
    class CheckSameTypesT : public TypeClassT<FloatType> {
    private:
        using BaseClass = TypeClassT<FloatType>;
        using SelfClass = CheckSameTypesT<FloatType>;

    public:
        using TypesFunctionArgs = typename BaseClass::TypesFunctionArgs;

        CheckSameTypesT(Range argsrange, const std::string& what="") : m_argsrange(argsrange) {
            if(what=="" or what=="all"){
                m_comparison = ComparisonType::All;
            }
            else if (what=="shape"){
                m_comparison = ComparisonType::Shape;
            }
            else if (what=="kind"){
                m_comparison = ComparisonType::Kind;
            }
            else{
                throw std::runtime_error(fmt::format("Unknown comparison type: {}", what));
            }
        }
        CheckSameTypesT(const SelfClass& other) = default;

        void processTypes(TypesFunctionArgs& fargs){
            auto& args = fargs.args;
            const DataType* compare_to=nullptr;
            size_t first;
            for(auto aidx: m_argsrange.iterate(args.size())){
                if(!compare_to){
                    compare_to = &args[aidx];
                    first = aidx;
                    continue;
                }
                bool eq = compare(args[aidx], *compare_to);
                if (!eq) {
                    auto msg = fmt::format("Transformation {0}: all inputs should have same {3}, {1} and {2} differ", args.name(), first, aidx, m_comparison==ComparisonType::All?"type":"shape");
                    compare_to->dump();
                    args[aidx].dump();
                    throw args.error(args[aidx], msg);
                }
            }
        }

        bool compare(const DataType& dt1, const DataType& dt2) const {
            switch(m_comparison){
                case ComparisonType::All:
                return dt1 == dt2;
                break;

                case ComparisonType::Shape:
                return dt1.shape == dt2.shape;
                break;

                case ComparisonType::Kind:
                return dt1.kind == dt2.kind;
                break;

                default:
                assert(false);
                return false;
            }
        }

        void dump(){
            const char* names[] = {"types", "shapes", "kinds"};
            fmt::print("TypeClass check same {}: args ", names[static_cast<size_t>(m_comparison)]);
            m_argsrange.dump();
        }
    private:
        Range m_argsrange;

        enum class ComparisonType {
            All = 0,
            Shape = 1,
            Kind = 2,
        };

        ComparisonType m_comparison;
    };

    template<typename FloatType>
    class CheckNdimT : public TypeClassT<FloatType> {
    private:
        using BaseClass = TypeClassT<FloatType>;
        using SelfClass = CheckNdimT<FloatType>;

    public:
        using TypesFunctionArgs = typename BaseClass::TypesFunctionArgs;

        CheckNdimT(size_t ndim, Range argsrange={0,-1}) : m_argsrange(argsrange), m_ndim(ndim) { }
        CheckNdimT(const SelfClass& other) = default;

        void processTypes(TypesFunctionArgs& fargs){
            auto& args = fargs.args;
            for(auto aidx: m_argsrange.iterate(args.size())){
                if(args[aidx].shape.size()!=m_ndim){
                    auto msg = fmt::format("Transformation {0}: input {1} should have dimension {2}, got {3}", args.name(), aidx, m_ndim, args[aidx].shape.size());
                    throw args.error(args[aidx], msg);
                }
            }
        }

        void dump(){
            fmt::print("TypeClass to check ndim is {} ", m_ndim);
            m_argsrange.dump();
        }
    private:
        Range m_argsrange;
        size_t m_ndim;
    };

    template<typename FloatType>
    class CheckKindT : public TypeClassT<FloatType> {
    private:
        using BaseClass = TypeClassT<FloatType>;
        using SelfClass = CheckKindT<FloatType>;

    public:
        using TypesFunctionArgs = typename BaseClass::TypesFunctionArgs;

        CheckKindT(DataKind kind, Range argsrange={0,-1}) : m_kind(kind), m_argsrange(argsrange) { }
        CheckKindT(const SelfClass& other) = default;

        void processTypes(TypesFunctionArgs& fargs){
            static const char* names[] = {"Undefined", "Points", "Hist"};

            auto& args = fargs.args;
            for(auto aidx: m_argsrange.iterate(args.size())){
                if(args[aidx].kind!=m_kind){
                    auto msg = fmt::format("Transformation {0}: input {1} should have kind {2}, got {3}", args.name(), aidx, names[int(m_kind)], names[int(args[aidx].kind)]);
                    throw args.error(args[aidx], msg);
                }
            }
        }

        void dump(){
            fmt::print("TypeClass to check kind is {} ", size_t(m_kind));
            m_argsrange.dump();
        }
    private:
        DataKind m_kind;
        Range m_argsrange;
    };

    template<typename FloatType>
    class PassTypeT : public TypeClassT<FloatType> {
    private:
        using BaseClass = TypeClassT<FloatType>;
        using SelfClass = PassTypeT<FloatType>;

    public:
        using TypesFunctionArgs = typename BaseClass::TypesFunctionArgs;

        PassTypeT(Range argsrange, Range retsrange) : m_argsrange(argsrange), m_retsrange(retsrange) {
            if( !m_argsrange.singular() ){
                throw std::runtime_error(fmt::format("Expect singular argsrange, got {}->{}", m_argsrange.getBegin(), m_argsrange.getEnd()));
            }
        }
        PassTypeT(const SelfClass& other) = default;

        void processTypes(TypesFunctionArgs& fargs){
            auto& args = fargs.args;
            auto& rets = fargs.rets;
            auto& typeToPass=args[m_argsrange.getBeginAbs(args.size())];
            for(auto ridx: m_retsrange.iterateSafe(rets.size())){
                rets[ridx]=typeToPass;
            }
        }

        void dump(){
            fmt::print("TypeClass pass type: args ");
            m_argsrange.dump();
            fmt::print("rets ");
            m_retsrange.dump();
        }
    private:
        Range m_argsrange;
        Range m_retsrange;
    };

    template<typename FloatType>
    class SetPointsT : public TypeClassT<FloatType> {
    private:
        using BaseClass = TypeClassT<FloatType>;
        using SelfClass = SetPointsT<FloatType>;

    public:
        using TypesFunctionArgs = typename BaseClass::TypesFunctionArgs;

        SetPointsT(size_t size, Range retsrange={0,-1}) : m_shape({size}), m_retsrange(retsrange) { }
        SetPointsT(size_t sizex, size_t sizey, Range retsrange={0,-1}) : m_shape({sizex, sizey}), m_retsrange(retsrange) { }
        SetPointsT(const SelfClass& other) = default;

        void processTypes(TypesFunctionArgs& fargs){
            auto& rets = fargs.rets;
            for(auto ridx: m_retsrange.iterateSafe(rets.size())){
                rets[ridx].points().shape(m_shape);
            }
        }

        void dump(){
            fmt::print("TypeClass set points: rets ");
            m_retsrange.dump();
        }
    private:
        std::vector<size_t> m_shape;
        Range m_retsrange;
    };

    template<typename FloatType>
    class PassEachTypeT : public TypeClassT<FloatType> {
    private:
        using BaseClass = TypeClassT<FloatType>;
        using SelfClass = PassEachTypeT<FloatType>;

    public:
        using TypesFunctionArgs = typename BaseClass::TypesFunctionArgs;

        PassEachTypeT(Range argsrange={0,-1}, Range retsrange={0,-1}) : m_argsrange(argsrange), m_retsrange(retsrange) { }
        PassEachTypeT(const SelfClass& other) = default;

        void processTypes(TypesFunctionArgs& fargs){
            auto& args = fargs.args;
            auto& rets = fargs.rets;

            auto arange = m_argsrange.iterateSafe(args.size());
            auto rrange = m_retsrange.iterateSafe(rets.size());

            auto rit = rrange.begin();
            for(auto aidx: arange){
                if(rit==rrange.end()){
                    break;
                    //error(args.size(), rets.size());
                }
                rets[*rit]=args[aidx];
                std::advance(rit, 1);
            }
            //if(rit!=rrange.end()){
                //error(args.size(), rets.size());
            //}
        }

        void dump(){
            fmt::print("TypeClass pass each type: args ");
            m_argsrange.dump();
            fmt::print("rets ");
            m_retsrange.dump();
        }
    private:
        void error(size_t nargs, size_t nrets){
            fmt::print("Args: ");
            m_argsrange.dump(nargs);
            fmt::print("\nRets: ");
            m_retsrange.dump(nrets);
            fmt::print("\n");
            throw std::runtime_error("Inconsistent ranges\n");
        }
        Range m_argsrange;
        Range m_retsrange;
    };
}


// Copyright (c) 2010 Satoshi Nakamoto
// Copyright (c) 2009-2012 The Bitcoin developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "main.h"
#include "bitcoinrpc.h"
#include <boost/lexical_cast.hpp>

using namespace json_spirit;
using namespace std;

void ScriptPubKeyToJSON(const CScript& scriptPubKey, Object& out, unsigned char cUnit);

double GetDifficulty(const CBlockIndex* blockindex)
{
    // Floating point number that is a multiple of the minimum difficulty,
    // minimum difficulty = 1.0.
    if (blockindex == NULL)
    {
        if (pindexBest == NULL)
            return 1.0;
        else
            blockindex = GetLastBlockIndex(pindexBest, false);
    }

    int nShift = (blockindex->nBits >> 24) & 0xff;

    double dDiff =
        (double)0x0000ffff / (double)(blockindex->nBits & 0x00ffffff);

    while (nShift < 29)
    {
        dDiff *= 256.0;
        nShift++;
    }
    while (nShift > 29)
    {
        dDiff /= 256.0;
        nShift--;
    }

    return dDiff;
}

void TxToJSON(const CTransaction& tx, Object& txdata)
{
    // tx data
    txdata.push_back(Pair("txid", tx.GetHash().ToString().c_str()));
    txdata.push_back(Pair("version", (int)tx.nVersion));
    txdata.push_back(Pair("locktime", (int)tx.nLockTime));
    txdata.push_back(Pair("is_coinbase", tx.IsCoinBase()));
    txdata.push_back(Pair("is_coinstake", tx.IsCoinStake()));

    // add inputs
    Array vins;
    BOOST_FOREACH(const CTxIn& txin, tx.vin)
    {
        Object vin;

        if (txin.prevout.IsNull())
        {
            vin.push_back(Pair("coinbase", HexStr(txin.scriptSig).c_str()));
        }
        else
        {
            vin.push_back(Pair("txid", txin.prevout.hash.ToString().c_str()));
            vin.push_back(Pair("vout", (int)txin.prevout.n));
        }

        vin.push_back(Pair("sequence", (boost::uint64_t)txin.nSequence));

        vins.push_back(vin);
    }
    txdata.push_back(Pair("vin", vins));

    // add outputs
    Array vouts;
    int n = 0;
    BOOST_FOREACH(const CTxOut& txout, tx.vout)
    {
        Object vout;

        std::vector<CTxDestination> addresses;
        txnouttype txtype;
        int nRequired;

        vout.push_back(Pair("value", ValueFromAmount(txout.nValue)));
        vout.push_back(Pair("n", n));

        Object scriptpubkey;

        scriptpubkey.push_back(Pair("asm", txout.scriptPubKey.ToString()));
        scriptpubkey.push_back(Pair("hex", HexStr(txout.scriptPubKey.begin(), txout.scriptPubKey.end())));

        if (ExtractDestinations(txout.scriptPubKey, txtype, addresses, nRequired))
        {
            scriptpubkey.push_back(Pair("type", GetTxnOutputType(txtype)));
            scriptpubkey.push_back(Pair("reqSig", nRequired));

            Array addrs;
            BOOST_FOREACH(const CTxDestination& addr, addresses)
                addrs.push_back(CBitcoinAddress(addr, tx.cUnit).ToString());
            scriptpubkey.push_back(Pair("addresses", addrs));
        }
        else
        {
            scriptpubkey.push_back(Pair("type", GetTxnOutputType(TX_NONSTANDARD)));
        }

        vout.push_back(Pair("scriptPubKey",scriptpubkey));

        vouts.push_back(vout);
        n++;
    }
    txdata.push_back(Pair("vout", vouts));
}

Object blockToJSON(const CBlock& block, const CBlockIndex* blockindex, bool fTxInfo, bool fTxDetails)
{
    Object result;
    result.push_back(Pair("hash", block.GetHash().GetHex()));
    CMerkleTx txGen(block.vtx[0]);
    txGen.SetMerkleBranch(&block);
    result.push_back(Pair("confirmations", (int)txGen.GetDepthInMainChain()));
    result.push_back(Pair("size", (int)::GetSerializeSize(block, SER_NETWORK, PROTOCOL_VERSION)));
    result.push_back(Pair("height", blockindex->nHeight));
    result.push_back(Pair("version", block.nVersion));
    result.push_back(Pair("merkleroot", block.hashMerkleRoot.GetHex()));
    result.push_back(Pair("time", DateTimeStrFormat(block.GetBlockTime())));
    result.push_back(Pair("nonce", (boost::uint64_t)block.nNonce));
    result.push_back(Pair("bits", HexBits(block.nBits)));
    result.push_back(Pair("difficulty", GetDifficulty(blockindex)));
    result.push_back(Pair("mint", ValueFromAmount(blockindex->nMint)));

    if (blockindex->pprev)
        result.push_back(Pair("previousblockhash", blockindex->pprev->GetBlockHash().GetHex()));
    if (blockindex->pnext)
        result.push_back(Pair("nextblockhash", blockindex->pnext->GetBlockHash().GetHex()));

    result.push_back(Pair("flags", strprintf("%s%s", blockindex->IsProofOfStake()? "proof-of-stake" : "proof-of-work", blockindex->GeneratedStakeModifier()? " stake-modifier": "")));
    result.push_back(Pair("proofhash", blockindex->IsProofOfStake()? blockindex->hashProofOfStake.GetHex() : blockindex->GetBlockHash().GetHex()));
    result.push_back(Pair("entropybit", (int)blockindex->GetStakeEntropyBit()));
    result.push_back(Pair("modifier", strprintf("%016"PRI64x, blockindex->nStakeModifier)));
    result.push_back(Pair("modifierchecksum", strprintf("%08x", blockindex->nStakeModifierChecksum)));

    Array txinfo;
    BOOST_FOREACH (const CTransaction& tx, block.vtx)
    {
        if (fTxInfo && !fTxDetails)
        {
            txinfo.push_back(tx.ToStringShort());
            txinfo.push_back(DateTimeStrFormat(tx.nTime));
            BOOST_FOREACH(const CTxIn& txin, tx.vin)
                txinfo.push_back(txin.ToStringShort());
            BOOST_FOREACH(const CTxOut& txout, tx.vout)
                txinfo.push_back(txout.ToStringShort());
        }
        else if (fTxDetails)
        {
            Object txdata;
            TxToJSON(tx, txdata);
            txinfo.push_back(txdata);
        }
        else
            txinfo.push_back(tx.GetHash().GetHex());
    }
    result.push_back(Pair("tx", txinfo));

    result.push_back(Pair("coinagedestroyed", (boost::int64_t)blockindex->nCoinAgeDestroyed));
    result.push_back(Pair("vote", voteToJSON(blockindex->vote)));
    Array parkRateResults;
    BOOST_FOREACH(const CParkRateVote& parkRateResult, blockindex->vParkRateResult)
        parkRateResults.push_back(parkRateVoteToJSON(parkRateResult));
    result.push_back(Pair("parkrates", parkRateResults));
    if (blockindex->vElectedCustodian.size() > 0)
    {
        Object electedCustodians;
        BOOST_FOREACH(const CCustodianVote& custodianVote, blockindex->vElectedCustodian)
        {
            electedCustodians.push_back(Pair(custodianVote.GetAddress().ToString(), (double)custodianVote.nAmount / COIN));
        }
        result.push_back(Pair("electedcustodians", electedCustodians));
    }

    return result;
}

Value getblockcount(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getblockcount\n"
            "Returns the number of blocks in the longest block chain.");

    return nBestHeight;
}

Value getdifficulty(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getdifficulty\n"
            "Returns difficulty as a multiple of the minimum difficulty.");

    Object obj;
    obj.push_back(Pair("proof-of-work",        GetDifficulty()));
    obj.push_back(Pair("proof-of-stake",       GetDifficulty(GetLastBlockIndex(pindexBest, true))));
    obj.push_back(Pair("search-interval",      (int)nLastCoinStakeSearchInterval));
    return obj;
}

Value settxfee(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 1)
        throw runtime_error(
            "settxfee is disabled\n");

    return "settxfee is disabled";
}

Value getrawmempool(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getrawmempool\n"
            "Returns all transaction ids in memory pool.");

    vector<uint256> vtxid;
    mempool.queryHashes(vtxid);

    Array a;
    BOOST_FOREACH(const uint256& hash, vtxid)
        a.push_back(hash.ToString());

    return a;
}

Value getblockhash(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 1)
        throw runtime_error(
            "getblockhash <index>\n"
            "Returns hash of block in best-block-chain at <index>.");

    int nHeight = params[0].get_int();
    if (nHeight < 0 || nHeight > nBestHeight)
        throw runtime_error("Block number out of range.");

    CBlockIndex* pblockindex = FindBlockByHeight(nHeight);
    return pblockindex->phashBlock->GetHex();
}

Value getblock(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 3)
        throw runtime_error(
            "getblock <hash> [txinfo] [txdetails]\n"
            "txinfo optional to print more detailed tx info\n"
            "txdetails optional to print even more detailed tx info\n"
            "Returns details of a block with given block-hash.");

    std::string strHash = params[0].get_str();
    uint256 hash(strHash);

    if (mapBlockIndex.count(hash) == 0)
        throw JSONRPCError(RPC_INVALID_ADDRESS_OR_KEY, "Block not found");

    CBlock block;
    CBlockIndex* pblockindex = mapBlockIndex[hash];
    block.ReadFromDisk(pblockindex);

    bool fTxInfo = params.size() > 1 ? params[1].get_bool() : false;
    bool fTxDetails = params.size() > 2 ? params[2].get_bool() : false;

    return blockToJSON(block, pblockindex, fTxInfo, fTxDetails);
}

Value gettxoutsetinfo(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "gettxoutsetinfo\n"
            "Returns statistics about the unspent transaction output set.");

    Object ret;

    CCoinsStats stats;
    if (pcoinsTip->GetStats(stats)) {
        ret.push_back(Pair("height", (boost::int64_t)stats.nHeight));
        ret.push_back(Pair("bestblock", stats.hashBlock.GetHex()));
        ret.push_back(Pair("transactions", (boost::int64_t)stats.nTransactions));
        ret.push_back(Pair("txouts", (boost::int64_t)stats.nTransactionOutputs));
        ret.push_back(Pair("bytes_serialized", (boost::int64_t)stats.nSerializedSize));
        ret.push_back(Pair("hash_serialized", stats.hashSerialized.GetHex()));
        ret.push_back(Pair("total_amount", ValueFromAmount(stats.nTotalAmount)));
    }
    return ret;
}

Value gettxout(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 2 || params.size() > 3)
        throw runtime_error(
            "gettxout <txid> <n> [includemempool=true]\n"
            "Returns details about an unspent transaction output.");

    Object ret;

    std::string strHash = params[0].get_str();
    uint256 hash(strHash);
    int n = params[1].get_int();
    bool fMempool = true;
    if (params.size() > 2)
        fMempool = params[2].get_bool();

    CCoins coins;
    if (fMempool) {
        LOCK(mempool.cs);
        CCoinsViewMemPool view(*pcoinsTip, mempool);
        if (!view.GetCoins(hash, coins))
            return Value::null;
        mempool.pruneSpent(hash, coins); // TODO: this should be done by the CCoinsViewMemPool
    } else {
        if (!pcoinsTip->GetCoins(hash, coins))
            return Value::null;
    }
    if (n<0 || (unsigned int)n>=coins.vout.size() || coins.vout[n].IsNull())
        return Value::null;

    ret.push_back(Pair("bestblock", pcoinsTip->GetBestBlock()->GetBlockHash().GetHex()));
    if ((unsigned int)coins.nHeight == MEMPOOL_HEIGHT)
        ret.push_back(Pair("confirmations", 0));
    else
        ret.push_back(Pair("confirmations", pcoinsTip->GetBestBlock()->nHeight - coins.nHeight + 1));
    ret.push_back(Pair("value", ValueFromAmount(coins.vout[n].nValue)));
    Object o;
    ScriptPubKeyToJSON(coins.vout[n].scriptPubKey, o, coins.cUnit);
    ret.push_back(Pair("scriptPubKey", o));
    ret.push_back(Pair("version", coins.nVersion));
    ret.push_back(Pair("coinbase", coins.fCoinBase));

    return ret;
}

CBlockIndex *getOptionalBlockHeight(const Array& params, const int blockHeightIndex)
{
    CBlockIndex *pindex = pindexBest;
    if (params.size() > 0)
    {
        int nHeight = params[0].get_int();

        if (nHeight < 0 || nHeight > nBestHeight)
            throw runtime_error("Invalid height\n");

        for (int i = nBestHeight; i > nHeight; i--)
            pindex = pindex->pprev;
    }
    return pindex;
}

Value getprotocolinfo(const Array& params, bool fHelp)
{
    if (fHelp || params.size() > 1)
        throw runtime_error(
            "getprotocolinfo [<block height>]\n"
            "Returns an object containing the current active protocol and votes.");

    Object obj;

    CBlockIndex *pindex = getOptionalBlockHeight(params, 0);

    obj.push_back(Pair("version",       pindex->nProtocolVersion));
    obj.push_back(Pair("max_version",    PROTOCOL_VERSION));
    obj.push_back(Pair("switch_percent", (double)PROTOCOL_SWITCH_REQUIRE_VOTES/PROTOCOL_SWITCH_COUNT_VOTES*100));

    map<int, int> mapProtocolVotes;
    for (unsigned int i = 0; i < PROTOCOL_SWITCH_COUNT_VOTES && pindex; i++, pindex = pindex->pprev)
        mapProtocolVotes[pindex->vote.nVersionVote]++;

    Object protocolVotesObject;
    BOOST_FOREACH(const PAIRTYPE(int, int)& protocolVote, mapProtocolVotes)
    {
        string version = boost::lexical_cast<std::string>(protocolVote.first);
        Object voteDetails;
        voteDetails.push_back(Pair("votes", protocolVote.second));
        voteDetails.push_back(Pair("percentage", (double)protocolVote.second/PROTOCOL_SWITCH_COUNT_VOTES*100));
        protocolVotesObject.push_back(Pair(version, voteDetails));
    }
    obj.push_back(Pair("protocol_votes", protocolVotesObject));

    return obj;
}


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <TwoDLib.hpp>
#include <vector>
#include <iostream>

using namespace std;
using namespace TwoDLib;
/*
BOOST_AUTO_TEST_CASE(MeshTreeSimple){
	Mesh mesh("aexpoverview.mesh");
	MeshTree tree(mesh);

	Point p = tree.FindNearest(Point(-55.0,20.0));
	double d_large = 1e10;
	int i_d = 0;
	int j_d = 0;
	int k = 0;
	for (int i = 0; i < mesh.NrQuadrilateralStrips(); i++)
		for (int j = 0; j < mesh.NrCellsInStrip(i); j++){
			vector<Point> vec_p = mesh.Quad(i,j).Points();
			for (auto it = vec_p.begin(); it != vec_p.end(); it++){
				double d = ( (*it)[0] - p[0])*( (*it)[0] - p[0]) + ((*it)[1] - p[1])*((*it)[1] - p[1]);
				if (d < d_large){
					d_large = d;
					i_d = i;
					j_d = j;
					k = it - vec_p.begin();
				}
			}
		}

	BOOST_REQUIRE( mesh.Quad(i_d,j_d).Points()[k][0] == p[0] );
	BOOST_REQUIRE( mesh.Quad(i_d,j_d).Points()[k][1] == p[1] );
	std::cout << p[0] << std::endl;
	std::cout << p[1] << std::endl;
}

BOOST_AUTO_TEST_CASE(MeshNearest)
{
	Mesh mesh("aexpoverview.mesh");
	std::cout << "Mesh created" << std::endl;
	MeshTree tree(mesh);

	// uncomment this to see the  kdtree code at work; we don't use it in the project atm
//	vector<Point> vec_p = tree.FindNearestN(Point(-55.0,20.0),5.0);
//	for (auto it = vec_p.begin(); it != vec_p.end(); it++)
//		std::cout << (*it)[0] << " " << (*it)[1] << std::endl;

}
*/
/*
BOOST_AUTO_TEST_CASE(KdSimple){
#define rand1() (rand() / (double)RAND_MAX)
#define rand_pt(v) { v.x[0] = rand1(); v.x[1] = rand1(); v.x[2] = rand1(); }

    int i;
    kd_node_t wp[] = {
        {{2, 3}}, {{5, 4}}, {{9, 6}}, {{4, 7}}, {{8, 1}}, {{7, 2}}
    };
    int len = sizeof(wp) / sizeof(wp[1]);
    struct kd_node_t testNode = {{9, 2}};
    struct kd_node_t *root, *found, *million;
    double best_dist;

    char buff[len*sizeof(kd_node_t)*sizeof(char)];
    root = make_tree(buff, wp, len, 0, 2);
    const int N = 1000000;
    int visited = 0;
    found = 0;
    nearest(root, &testNode, 0, 2, &found, &best_dist, &visited);

    // this is demo code and can be commented out to get a demonstration of the kdtree
//    printf(">> WP tree\nsearching for (%g, %g)\n"
//           "found (%g, %g) dist %g\nseen %d nodes\n\n",
//           testNode.x[0], testNode.x[1],
//            found->x[0], found->x[1], sqrt(best_dist), visited);

    million =(struct kd_node_t*) calloc(N, sizeof(struct kd_node_t));
    srand(time(0));
    for (i = 0; i < N; i++) rand_pt(million[i]);

    char* large_buffer = new  char[N*sizeof(kd_node_t)];
    root = make_tree(large_buffer, million, N, 0, 3);
    rand_pt(testNode);

    visited = 0;
    found = 0;
    nearest(root, &testNode, 0, 3, &found, &best_dist,&visited);

//    printf(">> Million tree\nsearching for (%g, %g, %g)\n"
//            "found (%g, %g, %g) dist %g\nseen %d nodes\n",
//            testNode.x[0], testNode.x[1], testNode.x[2],
//            found->x[0], found->x[1], found->x[2],
//            sqrt(best_dist), visited);
*/
    /* search many random points in million tree to see average behavior.
       tree size vs avg nodes visited:
       10      ~  7
       100     ~ 16.5
       1000        ~ 25.5
       10000       ~ 32.8
       100000      ~ 38.3
       1000000     ~ 42.6
       10000000    ~ 46.7              */
/*    int sum = 0, test_runs = 100000;
    for (i = 0; i < test_runs; i++) {
        found = 0;
        visited = 0;
        rand_pt(testNode);
        nearest(root, &testNode, 0, 3, &found, &best_dist,&visited);
        sum += visited;
    }
//    printf("\n>> Million tree\n"
//            "visited %d nodes for %d random findings (%f per lookup)\n",
//            sum, test_runs, sum/(double)test_runs);

    free(million);
    free(large_buffer);
}

*/


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 2, 2> test_double_2_2(const Eigen::Matrix<double, 2, 2>& M) { return M; }
void export_double_2_2() { boost::python::def("test_double_2_2", test_double_2_2); }


/*!
 * @file swp_surfvol_cubic_primary.cpp
 * @author Robert Patterson
 *
  Project:        sweepc (population balance solver)
  Sourceforge:    http://sourceforge.net/projects/mopssuite

  Copyright (C) 2013 Robert I A Patterson

  File purpose:
    Implementation of a particle the approximates cuboidal crystals by
    storing their surface area and volume
    @brief Implementation of cuboidal crystals described by volume and surface area.

  Licence:
    This file is part of "sweepc".

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

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

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Prof Markus Kraft
    Dept of Chemical Engineering
    University of Cambridge
    New Museums Site
    Pembroke Street
    Cambridge
    CB2 3RA
    UK

    Email:       mk306@cam.ac.uk
    Website:     http://como.cheng.cam.ac.uk
*/

#include "swp_surfvol_cubic_primary.h"

#include "swp_aggmodel_type.h"
#include "swp_model_factory.h"

#include <stdexcept>
#include <boost/random/poisson_distribution.hpp>

using namespace Sweep;
using namespace Sweep::AggModels;

// CONSTRUCTORS AND DESTRUCTORS.

// Default constructor (protected).
SurfVolCubicPrimary::SurfVolCubicPrimary(void)
{
}

// Initialising constructor.
SurfVolCubicPrimary::SurfVolCubicPrimary(double time, const Sweep::ParticleModel &model)
: Primary(time, model)
{
}

// Copy constructor.
SurfVolCubicPrimary::SurfVolCubicPrimary(const SurfVolCubicPrimary &copy)
{
    *this = copy;
}

// Stream-reading constructor.
SurfVolCubicPrimary::SurfVolCubicPrimary(std::istream &in, const Sweep::ParticleModel &model)
{
    Deserialize(in, model);
}

// Default destructor.
SurfVolCubicPrimary::~SurfVolCubicPrimary()
{
    releaseMem();
}

// OPERATOR OVERLOADS.

// Assignment operator (Primary RHS).
SurfVolCubicPrimary &SurfVolCubicPrimary::operator=(const Primary &rhs)
{
    operator=(dynamic_cast<const SurfVolCubicPrimary&>(rhs));
    return *this;
}

// AGGREGATION MODEL.

// Returns the aggregation model which this primary describes.
AggModels::AggModelType SurfVolCubicPrimary::AggID(void) const {return AggModels::SurfVolCubic_ID;}

// BASIC DERIVED PARTICLE PROPERTIES.

// Calculates the derived properties from the unique properties.  This
// function is broadly similar to the version in the spherical Primary
// class except that it uses the surface-volume model.  Therefore the
// surface area is not altered and the collision diameter is calculated
// using the arithmetic mean function.
//
// Mobility diameter:
// This calculation is based on the work of Rogak et al., 1993 Aer. Sci.
// Tech. 18:25-47, who give the calculation of dmob in the FM, SF and
// transition regime.
void SurfVolCubicPrimary::UpdateCache(void)
{
    // Store the old surface area, this value may be 0 if the particle is in
    // the process of initialisation
    double s = m_surf;

    // Pretend that the primary is spherical and set the cache
    // accordingly.
    Primary::UpdateCache();

    m_surf = std::max(s, m_surf);

    // Calculate diameters.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;

    // Calculate mobility diameter
    m_dmob = PP_Diameter();
    if (false) {
        // SF regime mobility diameter
        m_dmob *= 0.9 * sqrt(m_pmodel->GetFractDim() / (m_pmodel->GetFractDim() + 2));
        m_dmob *= pow(PP_Count(), (1.0/m_pmodel->GetFractDim()));
    } else {
        // FM regime mobility diameter
        m_dmob *= sqrt(0.802*(PP_Count()-1) + 1);
    }

    if (m_dmob < m_diam) m_dmob = m_diam;
}

// Returns the number of primary particles if the aggregate is assumed
// to consist of mono-sized primaries.
unsigned int SurfVolCubicPrimary::PP_Count(void) const
{
    // Note the minimum number of primary particles must be 1.
    return std::max(1u, (unsigned int)((m_surf * m_surf * m_surf) /
                                       (36.0 * PI * m_vol * m_vol)));
}

// Returns the primary particle diameter if the aggregate is assumed
// to consist of mono-sized primaries.
double SurfVolCubicPrimary::PP_Diameter(void) const
{
    // This should always be <= equiv. sphere diameter.
    return 6.0 * m_vol / m_surf;
}

// OPERATIONS.

// Adjusts the primary with the given composition and
// tracker values changes n times.  If the particle cannot be adjust
// n times, then this function returns the number of times
// it was adjusted.
unsigned int SurfVolCubicPrimary::Adjust(const fvector &dcomp, const fvector &dvalues, rng_type &rng,
                                    unsigned int n)
{
    // Store initial surface and volume
    double surfOld = m_surf;
    double volOld  = m_vol;

    // Adjust the particle assuming that it is spherical.
    n = Primary::Adjust(dcomp, dvalues, rng, n);

    // Calculate change in volume.
    double dvol = 0.0;
    for (unsigned int i=0; i!=dcomp.size(); ++i) {
        dvol += dcomp[i] * m_pmodel->Components(i)->MolWt() /
                m_pmodel->Components(i)->Density();
    }
    dvol *= (double)n / NA;

    // Calculate change in surface area.
    double invRadius = 0.0;
    if (dvol > 0.0) {
        // Inverse growth radius.
        invRadius = sqrt(4.0 * PI / surfOld);
    } else {
        // Inverse oxidation radius.
        invRadius = surfOld / (3.0 * volOld);
    }

    // Save new surface area.
    double s = surfOld + (2.0 * dvol * invRadius);

    // Set correct surface area, which was incorrectly set by
    // Primary::Adjust.
    m_surf    = s;

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return n;
}

/*!
 * Combines this primary with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 *
 * \return      Reference to the current instance after rhs has been added
 */
SurfVolCubicPrimary &SurfVolCubicPrimary::Coagulate(const Primary &rhs, rng_type &rng)

{
    // Store the resultant surface area.
    double s = m_surf + rhs.SurfaceArea();

    // Perform the coagulation.
    Primary::Coagulate(rhs, rng);

    // One sixth of the surface area is assumed to be lost as a result of two faces, one
    // from each incoming particle covering each other.  More sophisticated formulae
    // could be implemented here.
    m_surf = s * 0.8333333333333333333;

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return *this;
}

/*!
 * Combines this primary with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 *
 * \return      Reference to the current instance after rhs has been added
 */
SurfVolCubicPrimary &SurfVolCubicPrimary::Fragment(const Primary &rhs, rng_type &rng)

{
    // Store the resultant surface area.
    double s = m_surf + rhs.SurfaceArea();

    // Perform the coagulation.
    Primary::Fragment(rhs, rng);

    // One sixth of the surface area is assumed to be lost as a result of two faces, one
    // from each incoming particle covering each other.  More sophisticated formulae
    // could be implemented here.
    m_surf = s * 0.8333333333333333333;

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return *this;
}

// READ/WRITE/COPY.

//! Returns a copy of the model data.
SurfVolCubicPrimary *const SurfVolCubicPrimary::Clone(void) const
{
    return new SurfVolCubicPrimary(*this);
}

// Writes the object to a binary stream.
void SurfVolCubicPrimary::Serialize(std::ostream &out) const
{
    if (out.good()) {
        // Output base class.
        Primary::Serialize(out);
    } else {
        throw std::invalid_argument("Output stream not ready "
                                    "(Sweep, SurfVolCubicPrimary::Serialize).");
    }
}

// Reads the object from a binary stream.
void SurfVolCubicPrimary::Deserialize(std::istream &in, const Sweep::ParticleModel &model)
{
    if (in.good()) {
        // Read base class.
        Primary::Deserialize(in, model);
    } else {
        throw std::invalid_argument("Input stream not ready "
                                    "(Sweep, SurfVolCubicPrimary::Deserialize).");
    }
}


#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <map>
#include <algorithm>
#include <stdint.h>

#include "gflags/gflags.h"
#include "glog/logging.h"
#include <boost/shared_ptr.hpp>

#include "caffe/caffe.hpp"
#include "caffe/proto/caffe.pb.h"
#include "caffe/blob.hpp"
#include "caffe/util/benchmark.hpp"
#include "caffe/util/io.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/layer.hpp"
#include "caffe/layers/roi_align_layer.hpp"
#include "caffe/filler.hpp"

using namespace std;
using namespace caffe;

void NCHWToNHWC(const float* input, float* output, const int channels, const int height, const int width) {
  for (int c = 0; c < channels; ++c) {
    for (int h = 0; h < height; ++h) {
      for (int w = 0; w < width; ++w) {
        const int input_offset = (c * height + h) * width + w;
        const int output_offset = (h * width + w) * channels + c;
        output[output_offset] = input[input_offset];
      }
    }
  }
}

void equal_NHWC_NCHW(const float* input, const float* output, const int channels, const int height, const int width) {
  for (int c = 0; c < channels; ++c) {
    for (int h = 0; h < height; ++h) {
      for (int w = 0; w < width; ++w) {
        const int input_offset = (c * height + h) * width + w;
        const int output_offset = (h * width + w) * channels + c;
        const float input_val = input[input_offset];
        const float output_val = output[output_offset];
        if (fabs(input_val - output_val) > 1e-3) {
          LOG(INFO) << "[ERROR] (c,h,w) = " << c << "," << h << "," << w << ", "
                    << "Input is : " << input_val << ", while Output is : " << output_val;
        } else {
          LOG(INFO) << "[   OK] (c,h,w) = " << c << "," << h << "," << w << ", "
                    << "Input is : " << input_val << ", and Output is : " << output_val;
        }
      }
    }
  }
}

int main(int nargc, char** args) {
  // network NCHW
  caffe::Caffe::SetDevice(0);
  caffe::Caffe::set_mode(caffe::Caffe::CPU);
  boost::shared_ptr<caffe::Net<float> > net_NCHW_, net_NHWC_;
  const std::string network_proto = "/home/zhangming/work/repository/test/testToNHWC/models/net_NCHW.prototxt";
  const std::string caffe_model = "/home/zhangming/work/repository/test/testToNHWC/models/net_NCHW.caffemodel";
  const std::string network_proto_NHWC = "/home/zhangming/work/repository/test/testToNHWC/models/net_NHWC.prototxt";
  const std::string caffe_model_NHWC = "/home/zhangming/work/repository/test/testToNHWC/models/net_NHWC.caffemodel";
  net_NCHW_.reset(new caffe::Net<float>(network_proto, caffe::TEST));
  net_NHWC_.reset(new caffe::Net<float>(network_proto_NHWC, caffe::TEST));
  net_NCHW_->CopyTrainedLayersFrom(caffe_model);
  net_NHWC_->CopyTrainedLayersFrom(caffe_model_NHWC);

  // input for NCHW
  const int input_channels = 3;
  const int input_height = 224;
  const int input_width = 224;
  Blob<float>* input_blob = net_NCHW_->input_blobs()[0];
  input_blob->Reshape(1,input_channels,input_height,input_width);
  const int input_width_NCHW = input_blob->shape(3);
  const int input_height_NCHW = input_blob->shape(2);
  const int input_channels_NCHW = input_blob->shape(1);
  LOG(INFO) << "Input dim (NCHW): " << input_height_NCHW << " x " << input_width_NCHW << ", Input channels: " << input_channels_NCHW;
  // input for NHWC
  Blob<float>* input_blob_NHWC = net_NHWC_->input_blobs()[0];
  input_blob_NHWC->Reshape(1,input_height,input_width,input_channels);
  const int input_width_NHWC = input_blob_NHWC->shape(2);
  const int input_height_NHWC = input_blob_NHWC->shape(1);
  const int input_channels_NHWC = input_blob_NHWC->shape(3);
  LOG(INFO) << "Input dim (NHWC): " << input_height_NHWC << " x " << input_width_NHWC << ", Input channels: " << input_channels_NHWC;

  // input for NCHW
  LOG(INFO) << "Input data for NCHW.";
  FillerParameter filler_param;
  filler_param.set_std(0.5);
  GaussianFiller<float> filler(filler_param);
  filler.Fill(input_blob);
  LOG(INFO) << "Input data for NHWC.";
  NCHWToNHWC(input_blob->cpu_data(), input_blob_NHWC->mutable_cpu_data(), input_channels, input_height, input_width);

  // Forward
  net_NCHW_->Forward();
  net_NHWC_->Forward();

  // get the feature blobs
  const std::string feature_name = "pool5";
  Blob<float> output_NCHW, output_NHWC;
  const boost::shared_ptr<Blob<float> > feature = net_NCHW_->blob_by_name(feature_name.c_str());
  const Blob<float>* f_b = feature.get();
  output_NCHW.ReshapeLike(*f_b);
  output_NCHW.ShareData(*f_b);
  const boost::shared_ptr<Blob<float> > feature_NHWC = net_NHWC_->blob_by_name(feature_name.c_str());
  const Blob<float>* f_b_NHWC = feature_NHWC.get();
  output_NHWC.ReshapeLike(*f_b_NHWC);
  output_NHWC.ShareData(*f_b_NHWC);

  // check if it's equal in [NCHW] & [NHWC]
  equal_NHWC_NCHW(output_NCHW.cpu_data(), output_NHWC.cpu_data(), output_NCHW.shape(1), output_NCHW.shape(2), output_NCHW.shape(3));

  LOG(INFO) << "Finished.";
  return 0;
}


#if HAVE_CONFIG_H
#include "config.h"
#endif

#include <getopt.h>
#include <stdlib.h>
#include <math.h>
#include <limits>
#include <string>
#include <map>
#include <set>
#include <queue>
#include <iterator>
#include <iostream>

#include <sys/time.h>

#ifdef AMOS_HAVE_OPENMP
#include <omp.h>
#endif

#ifdef AMOS_HAVE_BOOST
#include <boost/version.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/strong_components.hpp>

#include <boost/random.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/betweenness_centrality.hpp>

#if BOOST_VERSION / 100000 <= 1 && BOOST_VERSION / 100 % 1000 < 48
#include <boost/interprocess/detail/atomic.hpp>
#define BOOST_ATOMIC_ADD boost::interprocess::detail::atomic_add32
#define BOOST_ATOMIC_CAS boost::interprocess::detail::atomic_cas32
#else
#include <boost/interprocess/detail/atomic.hpp>
#define BOOST_ATOMIC_ADD boost::interprocess::ipcdetail::atomic_add32
#define BOOST_ATOMIC_CAS boost::interprocess::ipcdetail::atomic_cas32
#endif

#endif //AMOS_HAVE_BOOST

#include "datatypes_AMOS.hh"
#include "Bank_AMOS.hh"
#include "BankStream_AMOS.hh"

#include "Contig_AMOS.hh"
#include "ContigEdge_AMOS.hh"

#include "Utilities_Bundler.hh"

using namespace std;
using namespace HASHMAP;
using namespace AMOS;
using namespace Bundler;

// constants for repeat resolution
static const uint32_t MIN_PARALLEL_SIZE  = 500000000;
static const uint32_t PARALLEL_SCALING   = 100;
static const int32_t  MAX_REPEAT_SIZE    =   10000;
static const int32_t  MAX_REPEAT_STDEV   =       3;
static const double   MAX_REPEAT_COV     =       0.02;
static const double   MAX_COVERAGE_ERROR =       0.05;
static const int32_t  MAX_K              =       4;

#ifdef AMOS_HAVE_BOOST
static const uint32_t MAX_VALUE     = numeric_limits<uint32_t>::max();
#endif

HASHMAP::hash_map<AMOS::ID_t, int32_t, HASHMAP::hash<AMOS::ID_t>, HASHMAP::equal_to<AMOS::ID_t> > *Bundler::cte2weight = NULL;

struct config {
   string      bank;
   int32_t     debug;
   int32_t     redundancy;
   bool        doAgressiveRepeatFinding;
   bool        noPathRepeats;
   bool        noCoverageRepeats;
   double      maxRepeatCoverage;
   bool        weighted;
   bool        test;
   int32_t     numThreads;
   int32_t     kPath;
   bool        runOriginal;
};
config globals;
void printHelpText() {
   cerr <<
    "\n"
    "Find repetitive contigs and mark them as repeats\n"
    "\n"
    "USAGE:\n"
    "\n"
    "MarkRepeats -b[ank] <bank_name> [-agressive] [-redundancy] [-noPathRepeats] [-noCoverageRepeats]\n"
    "The -redundancy option specifies the minimum number of links between two contigs before they will be scaffolded\n"
    "The -aggressive option will identify repeats using only the size and coverage stat (i.e. global coverage) and not the graph structure of the contigs for the coverage repeat algorithm\n"
    "The -noPathRepeats and -noCoverageRepeats will control which repeat algorithms are run. The default is to run both.\n"
       << endl;
}

bool GetOptions(int argc, char ** argv) {
   int option_index = 0;
   static struct option long_options[] = {
    {"help",               0, 0, 'h'},
    {"h",                  0, 0, 'h'},
    {"b",                  1, 0, 'b'},
    {"bank",               1, 0, 'b'},
    {"agressive",          0, 0, 'R'},
    {"redundancy",         1, 0, 'p'},
    {"debug",              1, 0, 'd'},
    {"noPathRepeats",      0, 0, 'r'},
    {"noCoverageRepeats",  0, 0, 'c'},
    {"weightedPath",       0, 0, 'w'},
    {"test",               0, 0, 'T'},
    {"threads",            1, 0, 't'},
    {"kPath",              1, 0, 'k'},
    {"original",        0, 0, 'o'},
    {0, 0, 0, 0}
  };

   globals.debug = 0;
   globals.doAgressiveRepeatFinding = false;
   globals.redundancy = 1;
   globals.noPathRepeats = false;
   globals.noCoverageRepeats = false;
   globals.maxRepeatCoverage = MAX_REPEAT_COV;
   globals.test = false;
   globals.weighted = false;
   globals.numThreads = 1;
   globals.kPath = 0;
   globals.runOriginal = false;

   int c;
   ifstream repeatsFile;
   ID_t repeatID;

   while ((c = getopt_long_only(argc, argv, "", long_options, &option_index))!= -1){
      switch (c){
      case 'h':
         printHelpText();
         break;
      case 'b':
         globals.bank = string(optarg);
         break;
      case 'p':
         globals.redundancy = atoi(optarg);
         break;
       case 'R':
         globals.doAgressiveRepeatFinding = true;
         break;
       case 'r':
         globals.noPathRepeats = true;
         break;
       case 'c':
         globals.noCoverageRepeats = true;
         break;
       case 'd':
         globals.debug = atoi(optarg);
         break;
       case 'w':
         globals.weighted = true;
         break;
       case 'T':
         globals.test = true;
         break;
      case 't':
         globals.numThreads = atoi(optarg);
         if (globals.numThreads <= 0) globals.numThreads = 1;
#ifdef AMOS_HAVE_OPENMP
         if (globals.numThreads > omp_get_max_threads()) { globals.numThreads = omp_get_max_threads(); }
#endif
         if (globals.debug > 1) { cerr << "The threads is set to " << globals.numThreads << endl; }
         break;
      case 'k':
         globals.kPath = atoi(optarg);
         if (globals.kPath < 0) globals.kPath = 0;
         if (globals.kPath > MAX_K) globals.kPath = MAX_K;
         break;
      case 'o':
         globals.runOriginal = true;
         break;
      case '?':
         return false;
      }
   }

   // print summary
   if (globals.debug >= 1) {
      cerr << "Options summary:" << endl;
      cerr << "Bank = \t" << globals.bank << endl;
      cerr << "Redundancy = \t" << globals.redundancy << endl;
      cerr << "AgressiveRepats = \t" << globals.doAgressiveRepeatFinding << endl;
   }

   return true;
} // GetOptions

double inline W (uint32_t k, uint32_t d, uint32_t w, uint32_t** sigma) {
   if (k == 0) {
      return pow(sigma[0][w], d);
   } else {
      uint32_t sum = 0;
      uint32_t i = 0;
      for (i = 1; i <=k; i++) {
         sum = sum - sigma[i][w] * W(k - i, d - 1, w, sigma);
      }
      return sum;
   }
}

#ifdef AMOS_HAVE_BOOST
double* findShortestPathRepeatsParallel(Graph &g, int K) {
  if (K > MAX_K) {
     cerr << "Error: Maximum allowed k-max path is " << MAX_K << endl;
     K = MAX_K;
  }
  K++;

  int32_t      numVertices = boost::num_vertices(g);
  int32_t      numEdges = boost::num_edges(g);
  double *       B     = new double[numVertices];
  double **      delta = new double*[K];
  uint32_t **    sigma = new uint32_t*[K];
  uint32_t *     sigmaSums = new uint32_t[numVertices];
  uint32_t *     dist  = new uint32_t[numVertices];
  uint32_t **    S     = new uint32_t*[numVertices];
  uint32_t *     S_count = new uint32_t[numVertices];
  uint32_t **    child_count = new uint32_t*[K];
  uint32_t ***   child   = new uint32_t**[K];

  for (int k = 0; k < K; k++) {
     child_count[k] = new uint32_t[numVertices];
     child[k] = new uint32_t*[numVertices];
     sigma[k] = new uint32_t[numVertices];
     delta[k] = new double[numVertices];
  }

  Vertex v,w;
  EdgeIterator out_i, out_end;
  uint32_t sw, distW, deltaW, p;
  int j, d, di, dj;
  double dsw;

  #pragma omp parallel for private(j) schedule(static) collapse(2)
  for (int k = 0; k < K; k++) {
     for (j = 0; j < numVertices; j++) {
        child[k][j] = new uint32_t[numEdges];
        if (k == 0) {
           B[j] = 0;
           S[j] = new uint32_t[numVertices];
        }
     }
  }

  timeval start;
  timeval end;
  gettimeofday(&start, 0);

  for (int s = 0; s < numVertices; s++) {
     #pragma omp parallel for private(j) schedule(static) collapse(2)
     for (int k = 0; k < K; k++) {
        for (int j = 0; j < numVertices; j++) {
           child_count[k][j] = 0;
           sigma[k][j] = delta[k][j] = 0;

           if (k == 0) {
              dist[j] = MAX_VALUE;
              sigmaSums[j] = 0;
              S_count[j] = 0;
           }
        }
     }
     uint32_t phase = 0;
     sigma[0][s] = 1;
     dist[s] = 0;
     S[phase][0] = s;
     S_count[phase] = 1;
     uint32_t count = 1;

     while (count > 0) {
        S_count[phase+1] = count = 0;
        #pragma omp parallel for private (v, w, distW, deltaW, p, out_i, out_end) schedule(dynamic)
        for (int i = 0; i < S_count[phase]; i++) { // parallel
           v = S[phase][i];
           for (tie(out_i, out_end) = boost::out_edges(v, g); out_i != out_end; ++out_i) { // parallel
              w = boost::target(*out_i, g);
              distW = BOOST_ATOMIC_CAS(&dist[w], phase+1, MAX_VALUE);
              deltaW = dist[v] - distW + 1;
              if (distW == MAX_VALUE) {
                 p = BOOST_ATOMIC_ADD(&S_count[phase+1], 1);
                 BOOST_ATOMIC_ADD(&count, 1);
                 assert(p < numVertices);
                 assert(phase < numVertices - 1);
                 S[phase+1][p] = w;
                 distW = phase + 1;
                 deltaW = dist[v] - distW + 1;
              }
              if (deltaW  < K) {
                 p = BOOST_ATOMIC_ADD(&child_count[deltaW][v], 1);
                 assert(p < numEdges);
                 assert(v < numVertices);
                 assert(deltaW < K);
                 child[deltaW][v][p] = w;
              }
              if (deltaW <= min(K-1, 1)) {
                 BOOST_ATOMIC_ADD(&sigma[deltaW][w], sigma[0][v]);
              }
           }
        }
        phase = phase + 1;
     }

     for (int k = 1; k < K; k++) {
        for (int i = 0; i < phase; i++) {
           #pragma omp parallel for private(v, w, j, d, dj) schedule(dynamic)
           for (j = 0; j < S_count[i]; j++) { //parallel
              v = S[i][j];
              for (d = 0; d < child_count[0][v]; d++) { //parallel
                 w = child[0][v][d];
                 BOOST_ATOMIC_ADD(&sigma[k][w], sigma[k][v]);
              }
              if (k < (K-1)) {
                 for (dj = 1; dj <= k+1; dj++) { //parallel
                    for (d = 0; d < child_count[dj][v]; d++) { //parallel
                       w = child[dj][v][d];
                       BOOST_ATOMIC_ADD(&sigma[k+1][w], sigma[k+1-dj][v]);
                    }
                 }
              }
           }
        }
     }

     #pragma omp parallel for private(j) schedule(static) collapse(2)
     for (int k = 0; k < K; k++) {
        for (j = 0; j < numVertices; j++) {
           BOOST_ATOMIC_ADD(&sigmaSums[j], sigma[k][j]);
        }
     }

     phase = phase - 1;
     for (int k = 0; k < K; k++) {
        p = phase;
        for ( ; p > 0; p--) {
           #pragma omp parallel for private(v, w, d, j, di, dj) schedule(dynamic)
           for (int i = 0; i < S_count[p]; i++) { // parallel
              v = S[p][i];
              for (d = 0; d <= k; d++) { // parallel
                 for (j = 0; j < child_count[d][v]; j++) { ///parallel
                    w = child[d][v][j];
                    for (di = 0; di <= (k-d); di++) {
                       uint32_t sum = 0;
                       uint32_t e = k - d - di;
                       for (dj = 0; dj <= e; dj++) {
                          sum += W(e - dj, e, w, sigma) * sigma[dj][v];
                       }
                       delta[k][v] += (sigma[0][w] == 0 ? 0 : (sum * (delta[di][w] / pow(sigma[0][w], e+1))));
                    }
                    delta[k][v] += (sigmaSums[w] == 0 ? 0 : (double)sigma[k - d][v] / ((double)sigmaSums[w]));
                 }
              }
              B[v] += delta[k][v];
           }
        }
     }
  }

  gettimeofday(&end, 0);
  double totaltime = (((double)(end.tv_sec-start.tv_sec)) + ((double)(end.tv_usec-start.tv_usec))*1.0e-6);
  cerr << "Elapsed time threaded is " << totaltime << endl;

  delete[] sigmaSums;
  delete[] dist;
  delete[] S_count;
  #pragma opm parallel for private(j) schedule(static) collapse(2)
  for (int k = 0; k < K; k++) {
     for (j = 0; j < numVertices; j++) {
        if (k == 0) delete[] S[j];
        delete[] child[k][j];
     }
     delete[] child[k];
     delete[] sigma[k];
     delete[] delta[k];
     delete[] child_count[k];
  }
  delete[] S;
  delete[] child;
  delete[] child_count;
  delete[] sigma;
  delete[] delta;

  return B;
}

double* findShortestPathRepeatsParallel(Graph &g) {
  return findShortestPathRepeatsParallel(g, 0);
}

void visitAllPaths(
   double *numTimesOnPath,
   Graph const&g,
   VertexName &vertexNames,
   const Vertex* predMap,
   Vertex const i,
   Vertex const j) {

   if (i==j) {
      return;
   } else if (predMap[j] == boost::graph_traits<Graph>::null_vertex()) {
      return;
   } else {
      visitAllPaths(numTimesOnPath, g, vertexNames, predMap, i, predMap[j]);
      numTimesOnPath[vertexNames[j]]++;
   }
}

void testShortestPaths() {
  typedef boost::rand48 RandomGenerator;
  RandomGenerator gen;

  size_t N = 100;
  size_t M = 100;
  double p = 0.05;
  double* B = NULL;
  Graph g;

  timeval start;
  timeval end;

  for (int i = 0; i < 100; i++) {
    if (i % 2 == 0) {
       g = Graph(boost::erdos_renyi_iterator<RandomGenerator, Graph>(gen, N, p), boost::erdos_renyi_iterator<RandomGenerator, Graph>(), 100);
       gettimeofday(&start, 0);
       B = findShortestPathRepeatsParallel(g, 0);
    }
    else {
       g = Graph(boost::small_world_iterator<RandomGenerator, Graph>(gen, N, M, p), boost::small_world_iterator<RandomGenerator, Graph>(), 100);
       gettimeofday(&start, 0);
       B = findShortestPathRepeatsParallel(g, 0);
    }
    gettimeofday(&end, 0);
    double totaltime = (((double)(end.tv_sec-start.tv_sec)) + ((double)(end.tv_usec-start.tv_usec))*1.0e-6);
    cerr << "Parallel program done in " << totaltime << endl;

    int32_t      numVertices = boost::num_vertices(g);
    std::vector<double> centrality(numVertices);
    // compute centrality using known algorithm
    gettimeofday(&start, 0);
    brandes_betweenness_centrality(g,
      boost::centrality_map(boost::make_iterator_property_map(centrality.begin(), get(boost::vertex_index, g), double())));
    gettimeofday(&end, 0);
    totaltime = (((double)(end.tv_sec-start.tv_sec)) + ((double)(end.tv_usec-start.tv_usec))*1.0e-6);
    cerr << "Elapsed time linear is " << totaltime << endl;

    for (int i = 0; i < numVertices; i++) {
       if (abs(centrality[i] - B[i]) > 0.001) {
          cerr << "Error: incorrectly computed centrality for node " << i << " should be: " << centrality[i] << " and instead it is: " << B[i] << endl;
          assert(0);
       }
    }
    delete[] B;
  }
}

double* computeShortestPaths(Graph &g) {
   VertexName vertexNames = get(boost::vertex_name, g);
   EdgeWeight  edgeWeights = get(boost::edge_weight, g);

   int32_t      numVertices = boost::num_vertices(g);
   double_t*    distMap     = new double_t[numVertices];
   Vertex*      predMap     = new Vertex[numVertices];
   vector<int32_t> qarray;

   double *numTimesOnPath = new double[numVertices];
   pair<VertexIterator, VertexIterator> i;
   for (i = boost::vertices(g); i.first != i.second; ++i.first) {
      numTimesOnPath[vertexNames[*i.first]] = 0;
   }

  timeval start;
  timeval end;
  gettimeofday(&start, 0);

   for (i = boost::vertices(g); i.first != i.second; ++i.first) {
      Vertex source = *i.first;
      // initialize arrays
      qarray.clear();
      pair<VertexIterator, VertexIterator> j;
      for (j = boost::vertices(g); j.first != j.second; ++j.first) {
         Vertex vertex = *j.first;

         qarray.push_back(vertex);
         distMap[vertex] = std::numeric_limits<int>::max();
         predMap[vertex] = boost::graph_traits<Graph>::null_vertex();
      }

      // initialize distance to the source to be 0
      distMap[source] = 0;
      while (qarray.size() != 0) {
         double_t min = std::numeric_limits<double>::max();
         Vertex minNode = boost::graph_traits<Graph>::null_vertex();
         vector<int32_t>::iterator index;

         for (vector<int32_t>::iterator it = qarray.begin(); it < qarray.end(); it++) {
            // for all nodes we haven't processed yet, check if they have shortest next distance
            if (distMap[*it] < min) {
               min = distMap[*it];
               minNode = *it;
               index = it;
            }
         }

         if (minNode == boost::graph_traits<Graph>::null_vertex() || min == std::numeric_limits<int32_t>::max()) {
            break;
         }

         qarray.erase(index);
         EdgeIterator out_i, out_end;
         for (tie(out_i, out_end) = boost::out_edges(minNode, g); out_i != out_end; ++out_i) {
            Vertex t = boost::target(*out_i, g);
               if (distMap[minNode] + edgeWeights[*out_i] < distMap[t]) {
                  distMap[t] = distMap[minNode] + edgeWeights[*out_i];
                  predMap[t] = minNode;
               }
         }
      }
      for (j = boost::vertices(g); j.first != j.second; ++j.first) {
         visitAllPaths(numTimesOnPath, g, vertexNames, predMap, source, *j.first);
      }
   }
   gettimeofday(&end, 0);
   double totaltime = (((double)(end.tv_sec-start.tv_sec)) + ((double)(end.tv_usec-start.tv_usec))*1.0e-6);
   cerr << "Elapsed time is " << totaltime << endl;

   delete[] distMap;
   delete[] predMap;

   return numTimesOnPath;
}

// compute shortest paths using one of the specified methods
void findShortestPathRepeats(Graph &g, Bank_t &contig_bank, set<ID_t> &repeats) {
   // find the all-pairs shortest paths
   timeval start;
   timeval end;
   if (globals.debug > 2) cerr << "FINDING SHORTEST PATHS" << endl;
   gettimeofday(&start, 0);

   double* numTimesOnPath;
   int32_t      numVertices = boost::num_vertices(g);
   int32_t      numEdges = boost::num_edges(g);

   if (globals.debug > 3) {
      double minNeighbors = numVertices;
      double maxNeighbors = 0;
      double meanOnPath = 0, varianceOnPath = 0, stdevOnPath = 0, N = 0;
      for(pair<VertexIterator, VertexIterator> i = boost::vertices(g); i.first != i.second; ++i.first) {
         uint32_t out = boost::out_degree(*i.first, g);
         if (out < minNeighbors) { minNeighbors = out; }
         if (out > maxNeighbors) { maxNeighbors = out; }
         N++;
         double delta = (out - meanOnPath);
         meanOnPath += (delta/N);
         varianceOnPath += delta * (out - meanOnPath);
      }
      varianceOnPath /= N;
      stdevOnPath = sqrt(varianceOnPath);
      cerr << "The graph with " << numVertices << " nodes and " << numEdges << " edges has min " << minNeighbors << " max neighbors " << maxNeighbors << " mean " << meanOnPath << "+-" << stdevOnPath << endl;
   }

   if (globals.runOriginal) {
      if (globals.debug > 1) { cerr << "Using linear repeat detection" << endl; }
      numTimesOnPath = computeShortestPaths(g);
   } else {
      if ((numVertices * numEdges) <= MIN_PARALLEL_SIZE) {
         if (globals.debug > 1) { cerr << "Using 1 thread" << endl; }
         globals.numThreads = 1;
#ifdef AMOS_HAVE_OPENMP
         omp_set_num_threads(globals.numThreads);
#endif
      }
      else {
         // compute in parallel
         if (globals.debug > 1) { cerr << "Using parallel repeat detection" << endl; }
      }
      numTimesOnPath = findShortestPathRepeatsParallel(g, globals.kPath);
   }

   // finally adjust our path counts by node sizes, we expect larger nodes to have more connections by chance
   VertexLength vertexLength = get(boost::vertex_index1, g);
   VertexName vertexNames = get(boost::vertex_name, g);
   pair<VertexIterator, VertexIterator> i;
   for(i = boost::vertices(g); i.first != i.second; ++i.first) {
      numTimesOnPath[vertexNames[*i.first]] /= vertexLength[*i.first];
   }

   gettimeofday(&end, 0);
   double totaltime = (((double)(end.tv_sec-start.tv_sec)) + ((double)(end.tv_usec-start.tv_usec))*1.0e-6);
   if (globals.debug > 2) cerr << "DONE SHORTEST PATHS in " << totaltime << " seconds" << endl;

   double meanOnPath = 0, varianceOnPath = 0, stdevOnPath = 0, N = 0;
   for (int j = 1; j < numVertices; j++) {
      N++;
      double delta = (numTimesOnPath[j] - meanOnPath);
      meanOnPath += (delta/N);
      varianceOnPath += delta * (numTimesOnPath[j] - meanOnPath);
   }
   varianceOnPath /= N;
   stdevOnPath = sqrt(varianceOnPath);

   for (i = boost::vertices(g); i.first != i.second; ++i.first) {
      ID_t iid = vertexNames[*i.first];
      if (iid == 0) continue;

      if (globals.debug > 1) cerr << "CONTIG " << iid << " IS NO PATH=" << numTimesOnPath[iid] << " MEAN IS " << meanOnPath << " STDEV: " << stdevOnPath << " DELTA: " << (numTimesOnPath[iid] - (meanOnPath+(MAX_REPEAT_STDEV*stdevOnPath))) << endl;
      if ((numTimesOnPath[iid] - (meanOnPath+(MAX_REPEAT_STDEV*stdevOnPath))) > 0) {
         // remove the vertices from our assembly and from the graph that is used for connected components
         repeats.insert(iid);
         boost::clear_vertex(*i.first, g);

         if (globals.debug > 1 ) cerr << "CONTIG " << iid << " AND ONPATH=" << numTimesOnPath[iid] << " AND MEAN=" << meanOnPath << " AND STDEV=" << stdevOnPath << " IS A CLASS 3 REPEAT\n";
      }
   }

   delete[] numTimesOnPath;
}

void findConnectedComponentRepeats(Graph &g, Bank_t &contig_bank, set<ID_t> &repeats) {
   if (globals.debug > 2) cerr << "FINDING CONNECTED COMPONENTS PATHS" << endl;
   typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, VertexProperty, EdgeProperty> UndirectedGraph;
   hash_map<ID_t, vector<Contig_t *>, hash<ID_t>, equal_to<ID_t> > ctgsByComponent;
   vector<Contig_t *> allContigs;
   UndirectedGraph ug;
   boost::copy_graph(g, ug);

   VertexName vertexNames = get(boost::vertex_name, g);
   int32_t* component = new int32_t[boost::num_vertices(ug)];

   // this section is very memory heavy, we load all the contigs from the bank into memory for the ctgsByComponent code
   // if memory becomes an issue, we can instead read contigs from the bank instead
   boost::connected_components(ug, &component[0]);
   pair<VertexIterator, VertexIterator> i;
   Contig_t *ctg;
   for (i = boost::vertices(g); i.first != i.second; ++i.first) {
      if (vertexNames[*i.first] == 0) continue;

      ctg = new Contig_t();
      contig_bank.fetch(vertexNames[*i.first], (*ctg));
      allContigs.push_back(ctg);
      ctgsByComponent[component[*i.first]].push_back(ctg);
   }

   if (globals.debug > 2) cerr << "COMPUTING CONNECTED COMPONENTS COVERAGE " << endl;
   double globalArrivalRate = computeArrivalRate(allContigs);
   allContigs.clear();

   double globalMean = 0, globalVariance = 0, globalStdev = 0, globalN = 0;
   for (hash_map<ID_t, vector<Contig_t *>, hash<ID_t>, equal_to<ID_t> >::iterator i = ctgsByComponent.begin(); i != ctgsByComponent.end(); i++) {
      double arrivalRate = computeArrivalRate(i->second);
      for (vector<Contig_t *>::iterator j = i->second.begin(); j < i->second.end(); j++) {
         double cov = (*j)->getCovStat(arrivalRate);

         globalN++;
         double globalDelta = cov-globalMean;
         globalMean+=(globalDelta/globalN);
         globalVariance += globalDelta * (cov - globalMean);
      }
   }
   globalVariance /= globalN;
   globalStdev = sqrt(globalVariance);

cerr << "The number of components is " << ctgsByComponent.size() << endl;

   globals.numThreads = (globals.numThreads < (int)ctgsByComponent.size() / PARALLEL_SCALING ? globals.numThreads : (int)ctgsByComponent.size() / PARALLEL_SCALING);
   if (globals.numThreads < 1) { globals.numThreads = 1; }
#ifdef AMOS_HAVE_OPENMP
   omp_set_num_threads(globals.numThreads);
#endif

   #pragma opm parallel for private(j, arrivalRate, mean, variance, stdev, N) schedule(dynamic)
   for (hash_map<ID_t, vector<Contig_t *>, hash<ID_t>, equal_to<ID_t> >::iterator i = ctgsByComponent.begin(); i != ctgsByComponent.end(); i++) {
      double arrivalRate = computeArrivalRate(i->second);
      double mean = 0, variance = 0, stdev = 0, N = 0;
      for (vector<Contig_t *>::iterator j = i->second.begin(); j < i->second.end(); j++) {
         double cov = (*j)->getCovStat(arrivalRate);

         N++;
         double delta = cov - mean;
         mean += (delta/N);
         variance += delta * (cov - mean);
      }
      variance /= N;
      stdev = sqrt(variance);

      if (globals.debug > 2) cerr << "Processing Connected component " << i->first << " with " << i->second.size() << " elements with coverage " << mean << " and stdev " << stdev << " GLOBAL ARRIVAL: " << globalArrivalRate << " LOCAL: " << arrivalRate << endl;
      for (vector<Contig_t *>::iterator j = i->second.begin(); j < i->second.end(); j++) {
         double cov = (*j)->getCovStat(arrivalRate);
         double globalCov = (*j)->getCovStat(globalArrivalRate);

         if (globals.debug > 1) cerr << "CONTIG " << (*j)->getIID() << " HAS COVERAGE " << cov << " GLOBAL COV: " << globalCov << " MEAN: " << mean << " STDEV:" << stdev << " SIZE " << (*j)->getUngappedLength() << " DELTA IS " << (cov + (mean - MAX_REPEAT_STDEV*stdev)) << endl;
         if ((cov < MAX_REPEAT_COV) && (*j)->getUngappedLength() < MAX_REPEAT_SIZE) {
            if (globals.debug > 1) cerr << "CONTIG " << (*j)->getEID() << " OF SIZE " << (*j)->getUngappedLength() << " WITH " << (*j)->getReadTiling().size() << " READS AND COVERAGE " << cov << " IS TOO LOW" << endl;
            #pragma omp critical
            {
               repeats.insert((*j)->getIID());
            }
         } else if (i->second.size() == 1 && globalCov < MAX_REPEAT_COV) {
             if (globals.debug > 1) cerr << "CONTIG " << (*j)->getEID() << " OF SIZE " << (*j)->getUngappedLength() << " GLOBAL COVERAGE " << globalCov << " IS TOO LOW" << endl;
            #pragma omp critical
            {
               repeats.insert((*j)->getIID());
            }
         } else if (globals.doAgressiveRepeatFinding == true && globalCov <= MAX_REPEAT_COV && (*j)->getUngappedLength() < MAX_REPEAT_SIZE) {
             if (globals.debug > 1) cerr << "AGRESSIVE REPEAT" << cov << " IS TOO LOW" << endl;
            #pragma omp critical
            {
               repeats.insert((*j)->getIID());
            }
         }
         // free memory the contig was using as soon as we're done with it
         delete (*j);
      }
   }

   delete[] component;
}
#endif //AMOS_HAVE_BOOST

int main(int argc, char *argv[]) {
   if (!GetOptions(argc, argv)){
      cerr << "Command line parsing failed" << endl;
      printHelpText();
      exit(1);
   }

#ifdef AMOS_HAVE_OPENMP
   omp_set_dynamic(false);
   omp_set_nested(true);
   omp_set_num_threads(globals.numThreads);
#endif

   if (globals.test == true) {
#ifdef AMOS_HAVE_BOOST
#ifdef AMOS_HAVE_OPENMP
      testShortestPaths();
#endif
#endif
      return 0;
   }

   if (globals.bank == ""){ // no bank was specified
      cerr << "A bank must be specified" << endl;
      exit(1);
   }

   BankStream_t edge_stream (ContigEdge_t::NCODE);
   if (! edge_stream.exists(globals.bank)){
      cerr << "No edge account found in bank " << globals.bank << endl;
      exit(1);
   }
   try {
      edge_stream.open(globals.bank, B_READ);
   } catch (Exception_t & e) {
      cerr << "Failed to open edge account in bank " << globals.bank << ": " << endl << e << endl;
      edge_stream.close();
      exit(1);
   }

   Bank_t contig_bank (Contig_t::NCODE);
   BankStream_t contig_stream (Contig_t::NCODE);
   if (! contig_stream.exists(globals.bank)){
     cerr << "No contig account found in bank " << globals.bank << endl;
     exit(1);
   }
   try {
     contig_bank.open(globals.bank, B_READ);
     contig_stream.open(globals.bank, B_READ);
   } catch (Exception_t & e) {
       cerr << "Failed to open contig account in bank " << globals.bank << ": " << endl << e << endl;
       contig_bank.close();
       contig_stream.close();
       exit(1);
   }

   Graph g;
   Contig_t ctg;
   ContigEdge_t cte;
   buildGraph(g, contig_stream, edge_stream, &ctg, &cte, globals.redundancy, globals.weighted);
   edge_stream.close();
   contig_stream.close();

   set<ID_t> repeats;
#ifdef AMOS_HAVE_BOOST
   if (globals.noPathRepeats == false) {
      findShortestPathRepeats(g, contig_bank, repeats);
   }
   if (globals.noCoverageRepeats == false) {
      findConnectedComponentRepeats(g, contig_bank, repeats);
   }
#endif //AMOS_HAVE_BOOST

   for (set<ID_t>::iterator i = repeats.begin(); i != repeats.end(); i++) {
      cout << *i << endl;
   }
   contig_bank.close();

   return 0;
}


/*
    Copyright 2005-2007 Adobe Systems Incorporated and others
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

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

#ifndef ADOBE_MD5_HPP
#define ADOBE_MD5_HPP

#include <adobe/config.hpp>

#include <boost/array.hpp>
#include <boost/cstdint.hpp>

#include <cstddef>

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

/*
    Relevant copyright information is provided below and may not be removed from this file.

    Derived from the RSA Data Security, Inc. MD5 Message-Digest Algorithm.
*/

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

/*
    MD5C.C - RSA Data Security, Inc., MD5 message-digest algorithm

    Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All rights
    reserved.

    License to copy and use this software is granted provided that it is
    identified as the "RSA Data Security, Inc. MD5 Message-Digest Algorithm" in
    all material mentioning or referencing this software or this function.

    License is also granted to make and use derivative works provided that such
    works are identified as "derived from the RSA Data Security, Inc. MD5
    Message-Digest Algorithm" in all material mentioning or referencing the
    derived work.

    RSA Data Security, Inc. makes no representations concerning either the
    merchantability of this software or the suitability of this software for
    any particular purpose. It is provided "as is" without express or implied
    warranty of any kind.

    These notices must be retained in any copies of any part of this
    documentation and/or software.
*/

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

namespace adobe {

/*!
\defgroup secure_algorithm Security Related
\ingroup algorithm
*/

/*!
\class adobe::md5_t
\ingroup secure_algorithm

\brief MD5 hash generator
*/

/*!
\typedef adobe::md5_t::digest_t

Type used to store the digest result of an MD5 hash
*/

/*!
\fn void adobe::md5_t::update(void* input_block, std::size_t input_length)

Includes a block of data in the MD5 hash being performed.

\param input_block  The block of data to be hashed
\param input_length Length in bytes of the input data
*/

/*!
\fn adobe::md5_t::digest_t adobe::md5_t::final()

Finalizes the input hash and clears the MD5 hash state information

\return An MD5 digest of the input blocks specified
*/

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

class md5_t {
public:
    typedef boost::array<boost::uint8_t, 16> digest_t;

    md5_t();

    void update(void* input_block, std::size_t input_length);

    digest_t final();

private:
    void reset();

    boost::uint32_t state_m[4];  /* state (ABCD) */
    boost::uint32_t count_m[2];  /* number of bits, modulo 2^64 (lsb first) */
    boost::uint8_t buffer_m[64]; /* input buffer */
};

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

/*!
\relates md5_t

Quick and dirty MD5 hash function for a single input block

\param input_block  The block of data to be hashed
\param input_length Length in bytes of the input data

\return An MD5 digest of the input block for the length specified
*/
inline md5_t::digest_t md5(void* input_block, std::size_t input_length) {
    md5_t m;

    m.update(input_block, input_length);

    return m.final();
}

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

} // namespace adobe

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

#endif

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


/*
 * Copyright (C) 2021 Open Source Robotics Foundation
 *
 * 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 RMF_FREESPACE_PLANNER__KINODYNAMIC_RRT_STAR_HPP
#define RMF_FREESPACE_PLANNER__KINODYNAMIC_RRT_STAR_HPP

#include "rmf_freespace_planner/rmf_freespace_planner.hpp"

#include <rmf_traffic/agv/RouteValidator.hpp>

#include <Eigen/Core>

#include <deque>
#include <random>

namespace rmf_freespace_planner {
namespace kinodynamic_rrt_star {
class KinodynamicRRTStar : public FreespacePlanner::Planner
{
public:
  struct ComputedTrajectory
  {
    rmf_traffic::Trajectory trajectory;
    double cost;
  };

  struct State
  {
    State(
      Eigen::Vector3d position,
      Eigen::Vector3d velocity,
      Eigen::Vector2d distance);

    double x() const
    {
      return position.x();
    }

    double y() const
    {
      return position.y();
    }

    double yaw() const
    {
      return position.z();
    }

    double vx() const
    {
      return velocity.x();
    }

    double vy() const
    {
      return velocity.y();
    }

    double vyaw() const
    {
      return velocity.z();
    }

    Eigen::Vector3d position;
    Eigen::Vector3d velocity;
    Eigen::Vector2d distance;
  };

  struct Vertex
  {
    Vertex(
      State state,
      std::shared_ptr<Vertex> parent,
      double cost_to_root,
      double cost_to_parent,
      rmf_traffic::Trajectory trajectory);

    State state;
    std::shared_ptr<Vertex> parent;
    double cost_to_root;
    double cost_to_parent;
    rmf_traffic::Trajectory trajectory;
  };

  class LocalPlanner
  {
  public:
    virtual std::optional<ComputedTrajectory> compute_trajectory(
      const std::shared_ptr<Vertex>& start,
      const std::shared_ptr<Vertex>& end) const = 0;
  };

  KinodynamicRRTStar(
    rmf_utils::clone_ptr<rmf_traffic::agv::RouteValidator> validator,
    std::shared_ptr<rmf_traffic::schedule::ItineraryViewer> itinerary_viewer,
    std::optional<std::unordered_set<rmf_traffic::schedule::ParticipantId>> excluded_participants,
    std::unique_ptr<LocalPlanner> local_planner);

  std::vector<rmf_traffic::Route> plan(
    const FreespacePlanner::Start& start,
    const FreespacePlanner::Goal& goal,
    const rmf_traffic::agv::VehicleTraits& traits,
    const std::optional<std::vector<FreespacePlanner::Obstacle>>& obstacles,
    const std::string& map) override;

  struct Neighborhood
  {
    std::shared_ptr<Vertex> closest_vertex;
    std::vector<std::shared_ptr<Vertex>> close_vertices;
    rmf_traffic::Trajectory trajectory;
    double cost;
  };

  double get_estimated_total_cost() const
  {
    return estimated_total_cost;
  }

private:
  class InternalState
  {
  public:
    InternalState(
      std::string map,
      std::shared_ptr<Vertex> start_vertex,
      std::shared_ptr<Vertex> goal_vertex,
      rmf_traffic::Time start_time,
      rmf_traffic::agv::VehicleTraits traits,
      KinodynamicRRTStar* kinodynamic_rrt_star
    );

    std::shared_ptr<Vertex> generate_random_vertex();

    std::deque<std::shared_ptr<Vertex>> get_seed_vertices() const;

    Eigen::Vector2d transform_point(const Eigen::Vector2d& point) const;

    Neighborhood find_close_vertices(const std::shared_ptr<Vertex>& new_vertex)
    const;

    void rewire(
      const std::shared_ptr<Vertex>& new_vertex,
      const std::shared_ptr<Vertex>& vertex_to_rewire);

    void propagate_cost(const std::shared_ptr<Vertex>& initial_vertex);

    rmf_traffic::Trajectory construct_trajectory() const;

    void update_estimated_total_cost(const std::shared_ptr<Vertex>& vertex);

    Eigen::Matrix<double, 6, 2> state_limits;

    const std::string map;

    const rmf_traffic::Time start_time;

    const std::shared_ptr<Vertex> start_vertex;

    const std::shared_ptr<Vertex> goal_vertex;

    std::vector<std::shared_ptr<Vertex>> vertex_list;

    std::random_device rd;

    std::mt19937 gen;

    double min_goal_distance;

    const rmf_traffic::agv::VehicleTraits traits;

    KinodynamicRRTStar* kinodynamic_rrt_star;
  };

  static bool compare_vertices(
    const std::shared_ptr<const Vertex>& vertex1,
    const std::shared_ptr<const Vertex>& vertex2);

  rmf_utils::clone_ptr<rmf_traffic::agv::RouteValidator> validator;

  std::shared_ptr<rmf_traffic::schedule::ItineraryViewer> itinerary_viewer;

  std::optional<std::unordered_set<rmf_traffic::schedule::ParticipantId>>
  excluded_participants;

  std::unique_ptr<LocalPlanner> local_planner;

  double estimated_total_cost;
};
}
}

#endif //RMF_FREESPACE_PLANNER__KINODYNAMIC_RRT_STAR_HPP