//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_LU_
#define _BOOST_UBLAS_LU_

#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>

// LU factorizations in the spirit of LAPACK and Golub & van Loan

namespace boost { namespace numeric { namespace ublas {

    template<class T = std::size_t, class A = unbounded_array<T> >
    class permutation_matrix:
        public vector<T, A> {
    public:
        typedef vector<T, A> vector_type;
        typedef typename vector_type::size_type size_type;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        permutation_matrix (size_type size):
            vector<T, A> (size) {
            for (size_type i = 0; i < size; ++ i)
                (*this) (i) = i;
        }
        BOOST_UBLAS_INLINE
        ~permutation_matrix () {}

        // Assignment
        BOOST_UBLAS_INLINE
        permutation_matrix &operator = (const permutation_matrix &m) {
            vector_type::operator = (m);
            return *this;
        }
    };

    template<class PM, class MV>
    BOOST_UBLAS_INLINE
    void swap_rows (const PM &pm, MV &mv, vector_tag) {
        typedef typename PM::size_type size_type;
        typedef typename MV::value_type value_type;

        size_type size = pm.size ();
        for (size_type i = 0; i < size; ++ i) {
            if (i != pm (i))
                std::swap (mv (i), mv (pm (i)));
        }
    }
    template<class PM, class MV>
    BOOST_UBLAS_INLINE
    void swap_rows (const PM &pm, MV &mv, matrix_tag) {
        typedef typename PM::size_type size_type;
        typedef typename MV::value_type value_type;

        size_type size = pm.size ();
        for (size_type i = 0; i < size; ++ i) {
            if (i != pm (i))
                row (mv, i).swap (row (mv, pm (i)));
        }
    }
    // Dispatcher
    template<class PM, class MV>
    BOOST_UBLAS_INLINE
    void swap_rows (const PM &pm, MV &mv) {
        swap_rows (pm, mv, typename MV::type_category ());
    }

    // LU factorization without pivoting
    template<class M>
    typename M::size_type lu_factorize (M &m) {
        typedef M matrix_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix_type cm (m);
#endif
        int singular = 0;
        size_type size1 = m.size1 ();
        size_type size2 = m.size2 ();
        size_type size = (std::min) (size1, size2);
        for (size_type i = 0; i < size; ++ i) {
            matrix_column<M> mci (column (m, i));
            matrix_row<M> mri (row (m, i));
            if (m (i, i) != value_type/*zero*/()) {
                project (mci, range (i + 1, size1)) *= value_type (1) / m (i, i);
            } else if (singular == 0) {
                singular = i + 1;
            }
            project (m, range (i + 1, size1), range (i + 1, size2)).minus_assign (
                outer_prod (project (mci, range (i + 1, size1)),
                            project (mri, range (i + 1, size2))));
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (singular != 0 ||
                           detail::expression_type_check (prod (triangular_adaptor<matrix_type, unit_lower> (m),
                                                                triangular_adaptor<matrix_type, upper> (m)),
                                                          cm), internal_logic ());
#endif
        return singular;
    }

    // LU factorization with partial pivoting
    template<class M, class PM>
    typename M::size_type lu_factorize (M &m, PM &pm) {
        typedef M matrix_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix_type cm (m);
#endif
        int singular = 0;
        size_type size1 = m.size1 ();
        size_type size2 = m.size2 ();
        size_type size = (std::min) (size1, size2);
        for (size_type i = 0; i < size; ++ i) {
            matrix_column<M> mci (column (m, i));
            matrix_row<M> mri (row (m, i));
            size_type i_norm_inf = i + index_norm_inf (project (mci, range (i, size1)));
            BOOST_UBLAS_CHECK (i_norm_inf < size1, external_logic ());
            if (m (i_norm_inf, i) != value_type/*zero*/()) {
                if (i_norm_inf != i) {
                    pm (i) = i_norm_inf;
                    row (m, i_norm_inf).swap (mri);
                } else {
                    BOOST_UBLAS_CHECK (pm (i) == i_norm_inf, external_logic ());
                }
                project (mci, range (i + 1, size1)) *= value_type (1) / m (i, i);
            } else if (singular == 0) {
                singular = i + 1;
            }
            project (m, range (i + 1, size1), range (i + 1, size2)).minus_assign (
                outer_prod (project (mci, range (i + 1, size1)),
                            project (mri, range (i + 1, size2))));
        }
#if BOOST_UBLAS_TYPE_CHECK
        swap_rows (pm, cm);
        BOOST_UBLAS_CHECK (singular != 0 ||
                           detail::expression_type_check (prod (triangular_adaptor<matrix_type, unit_lower> (m),
                                                                triangular_adaptor<matrix_type, upper> (m)), cm), internal_logic ());
#endif
        return singular;
    }

    template<class M, class PM>
    typename M::size_type axpy_lu_factorize (M &m, PM &pm) {
        typedef M matrix_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;
        typedef vector<value_type> vector_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix_type cm (m);
#endif
        int singular = 0;
        size_type size1 = m.size1 ();
        size_type size2 = m.size2 ();
        size_type size = (std::min) (size1, size2);
#ifndef BOOST_UBLAS_LU_WITH_INPLACE_SOLVE
        matrix_type mr (m);
        mr.assign (zero_matrix<value_type> (size1, size2));
        vector_type v (size1);
        for (size_type i = 0; i < size; ++ i) {
            matrix_range<matrix_type> lrr (project (mr, range (0, i), range (0, i)));
            vector_range<matrix_column<matrix_type> > urr (project (column (mr, i), range (0, i)));
            urr.assign (solve (lrr, project (column (m, i), range (0, i)), unit_lower_tag ()));
            project (v, range (i, size1)).assign (
                project (column (m, i), range (i, size1)) -
                axpy_prod<vector_type> (project (mr, range (i, size1), range (0, i)), urr));
            size_type i_norm_inf = i + index_norm_inf (project (v, range (i, size1)));
            BOOST_UBLAS_CHECK (i_norm_inf < size1, external_logic ());
            if (v (i_norm_inf) != value_type/*zero*/()) {
                if (i_norm_inf != i) {
                    pm (i) = i_norm_inf;
                    std::swap (v (i_norm_inf), v (i));
                    project (row (m, i_norm_inf), range (i + 1, size2)).swap (project (row (m, i), range (i + 1, size2)));
                } else {
                    BOOST_UBLAS_CHECK (pm (i) == i_norm_inf, external_logic ());
                }
                project (column (mr, i), range (i + 1, size1)).assign (
                    project (v, range (i + 1, size1)) / v (i));
                if (i_norm_inf != i) {
                    project (row (mr, i_norm_inf), range (0, i)).swap (project (row (mr, i), range (0, i)));
                }
            } else if (singular == 0) {
                singular = i + 1;
            }
            mr (i, i) = v (i);
        }
        m.assign (mr);
#else
        matrix_type lr (m);
        matrix_type ur (m);
        lr.assign (identity_matrix<value_type> (size1, size2));
        ur.assign (zero_matrix<value_type> (size1, size2));
        vector_type v (size1);
        for (size_type i = 0; i < size; ++ i) {
            matrix_range<matrix_type> lrr (project (lr, range (0, i), range (0, i)));
            vector_range<matrix_column<matrix_type> > urr (project (column (ur, i), range (0, i)));
            urr.assign (project (column (m, i), range (0, i)));
            inplace_solve (lrr, urr, unit_lower_tag ());
            project (v, range (i, size1)).assign (
                project (column (m, i), range (i, size1)) -
                axpy_prod<vector_type> (project (lr, range (i, size1), range (0, i)), urr));
            size_type i_norm_inf = i + index_norm_inf (project (v, range (i, size1)));
            BOOST_UBLAS_CHECK (i_norm_inf < size1, external_logic ());
            if (v (i_norm_inf) != value_type/*zero*/()) {
                if (i_norm_inf != i) {
                    pm (i) = i_norm_inf;
                    std::swap (v (i_norm_inf), v (i));
                    project (row (m, i_norm_inf), range (i + 1, size2)).swap (project (row (m, i), range (i + 1, size2)));
                } else {
                    BOOST_UBLAS_CHECK (pm (i) == i_norm_inf, external_logic ());
                }
                project (column (lr, i), range (i + 1, size1)).assign (
                    project (v, range (i + 1, size1)) / v (i));
                if (i_norm_inf != i) {
                    project (row (lr, i_norm_inf), range (0, i)).swap (project (row (lr, i), range (0, i)));
                }
            } else if (singular == 0) {
                singular = i + 1;
            }
            ur (i, i) = v (i);
        }
        m.assign (triangular_adaptor<matrix_type, strict_lower> (lr) +
                  triangular_adaptor<matrix_type, upper> (ur));
#endif
#if BOOST_UBLAS_TYPE_CHECK
        swap_rows (pm, cm);
        BOOST_UBLAS_CHECK (singular != 0 ||
                           detail::expression_type_check (prod (triangular_adaptor<matrix_type, unit_lower> (m),
                                                                triangular_adaptor<matrix_type, upper> (m)), cm), internal_logic ());
#endif
        return singular;
    }

    // LU substitution
    template<class M, class E>
    void lu_substitute (const M &m, vector_expression<E> &e) {
        typedef const M const_matrix_type;
        typedef vector<typename E::value_type> vector_type;

#if BOOST_UBLAS_TYPE_CHECK
        vector_type cv1 (e);
#endif
        inplace_solve (m, e, unit_lower_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (triangular_adaptor<const_matrix_type, unit_lower> (m), e), cv1), internal_logic ());
        vector_type cv2 (e);
#endif
        inplace_solve (m, e, upper_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (triangular_adaptor<const_matrix_type, upper> (m), e), cv2), internal_logic ());
#endif
    }
    template<class M, class E>
    void lu_substitute (const M &m, matrix_expression<E> &e) {
        typedef const M const_matrix_type;
        typedef matrix<typename E::value_type> matrix_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix_type cm1 (e);
#endif
        inplace_solve (m, e, unit_lower_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (triangular_adaptor<const_matrix_type, unit_lower> (m), e), cm1), internal_logic ());
        matrix_type cm2 (e);
#endif
        inplace_solve (m, e, upper_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (triangular_adaptor<const_matrix_type, upper> (m), e), cm2), internal_logic ());
#endif
    }
    template<class M, class PMT, class PMA, class MV>
    void lu_substitute (const M &m, const permutation_matrix<PMT, PMA> &pm, MV &mv) {
        swap_rows (pm, mv);
        lu_substitute (m, mv);
    }
    template<class E, class M>
    void lu_substitute (vector_expression<E> &e, const M &m) {
        typedef const M const_matrix_type;
        typedef vector<typename E::value_type> vector_type;

#if BOOST_UBLAS_TYPE_CHECK
        vector_type cv1 (e);
#endif
        inplace_solve (e, m, upper_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (e, triangular_adaptor<const_matrix_type, upper> (m)), cv1), internal_logic ());
        vector_type cv2 (e);
#endif
        inplace_solve (e, m, unit_lower_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (e, triangular_adaptor<const_matrix_type, unit_lower> (m)), cv2), internal_logic ());
#endif
    }
    template<class E, class M>
    void lu_substitute (matrix_expression<E> &e, const M &m) {
        typedef const M const_matrix_type;
        typedef matrix<typename E::value_type> matrix_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix_type cm1 (e);
#endif
        inplace_solve (e, m, upper_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (e, triangular_adaptor<const_matrix_type, upper> (m)), cm1), internal_logic ());
        matrix_type cm2 (e);
#endif
        inplace_solve (e, m, unit_lower_tag ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (detail::expression_type_check (prod (e, triangular_adaptor<const_matrix_type, unit_lower> (m)), cm2), internal_logic ());
#endif
    }
    template<class MV, class M, class PMT, class PMA>
    void lu_substitute (MV &mv, const M &m, const permutation_matrix<PMT, PMA> &pm) {
        swap_rows (pm, mv);
        lu_substitute (mv, m);
    }

}}}

#endif


//
// $Id: peakpickerqtof.hpp 6337 2014-06-06 20:04:10Z witek96 $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// 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 PEAKPICKER_H
#define PEAKPICKER_H

#include <boost/math/special_functions.hpp>
#include "pwiz/utility/findmf/base/resample/convert2dense.hpp"
#include "pwiz/utility/findmf/base/filter/filter.hpp"
#include "pwiz/utility/findmf/base/ms/simplepicker.hpp"
#include "pwiz/utility/findmf/base/filter/gaussfilter.hpp"
#include "pwiz/utility/findmf/base/base/interpolate.hpp"
#include "pwiz/utility/findmf/base/resample/determinebinwidth.hpp"
#include "pwiz/utility/findmf/base/base/copyif.hpp"

namespace ralab{
  namespace base{
    namespace ms{

      /// resamples spectrum, apply smoothing,
      /// determines zero crossings,
      /// integrates peaks.

      template<typename TReal>
      struct SimplePeakArea{
        TReal integwith_;

        SimplePeakArea(TReal integwith):integwith_(integwith){}

        /// intagrates the peak intesnities
        template<typename Tzerocross, typename Tintensity, typename Tout>
        void operator()( Tzerocross beginZ,
                         Tzerocross endZ,
                         Tintensity intensity,
                         Tintensity resmpled,
                         Tout area)const
        {
          typedef typename std::iterator_traits<Tout>::value_type AreaType;
          for( ; beginZ != endZ ; ++beginZ , ++area )
          {
            size_t idx = static_cast<size_t>( *beginZ );
            size_t start = static_cast<size_t>( boost::math::round( idx - integwith_ ) );
            size_t end = static_cast<size_t>( boost::math::round( idx + integwith_ + 2.) );
            AreaType aread = 0.;
            for( ; start != end ; ++start )
            {
              aread += *(resmpled + start);
            }
            *area = aread;
          }
        }
      };

      /// extends peak to the left and to the right to the next local minimum or a predefined threshol
      /// or a maximum allowed extension.
      template<typename TReal>
      struct LocalMinPeakArea{
        typedef TReal value_type;
        TReal integwith_;
        TReal threshold_;

        LocalMinPeakArea(TReal integwith,//!<maximal allowed peak width +- in pixel
                         TReal threshold = .1// minimum intensity
            ):integwith_(integwith),threshold_(threshold){}

        /// intagrates the peak intesnities
        template< typename Tzerocross, typename Tintensity, typename Tout >
        void operator()( Tzerocross beginZ,
                         Tzerocross endZ,
                         Tintensity intensity,
                         Tintensity resampled,
                         Tout area) const
        {
          typedef typename std::iterator_traits<Tout>::value_type AreaType;
          for( ; beginZ != endZ ; ++beginZ , ++area )
          {
            size_t idx = static_cast<size_t>( *beginZ );
            size_t start = static_cast<size_t>( boost::math::round( idx - integwith_ ) );
            size_t end = static_cast<size_t>( boost::math::round( idx + integwith_ + 2) );

            Tintensity st = intensity + start;
            Tintensity en = intensity + end;
            Tintensity center = intensity + idx;
            std::ptrdiff_t x1 = std::distance(st, center);
            std::ptrdiff_t y1 = std::distance(center,en);
            mextend(st , en , center);
            std::ptrdiff_t x2 = std::distance(intensity,st);
            std::ptrdiff_t y2 = std::distance(intensity,en);
            std::ptrdiff_t pp = std::distance(st,en);
            AreaType areav = std::accumulate(resampled+x2,resampled+y2,0.);
            *area = areav;
          }
        }

      private:
        ///exend peak to left and rigth
        template<typename TInt >
        void mextend( TInt &start, TInt &end, TInt idx) const
        {
          typedef typename std::iterator_traits<TInt>::value_type Intensitytype;
          //
          for(TInt intens = idx ; intens >= start;  --intens){
            Intensitytype val1 = *intens;
            Intensitytype val2 = *(intens-1);
            if(val1 > threshold_){
              if(val1 < val2 ){
                start = intens;
                break;
              }
            }
            else{
              start = intens;
              break;
            }
          }

          for(TInt intens = idx ; intens <= end;  ++intens){
            Intensitytype val1 = *intens;
            Intensitytype val2 = *(intens+1);
            if(val1 > threshold_){
              if(val1 < val2 ){
                end = intens;
                break;
              }
            }
            else{
              end = intens;
              break;
            }
          }
        }
      };

      /// resamples spectrum, apply smoothing,
      /// determines zero crossings,
      /// integrates peaks.
      template<typename TReal, template <typename B> class TIntegrator >
      struct PeakPicker{
        typedef TReal value_type;
        typedef TIntegrator<value_type> PeakIntegrator;

        TReal resolution_;
        ralab::base::resample::Convert2Dense c2d_; // resamples spectrum
        std::vector<TReal> resampledmz_, resampledintensity_; // keeps result of convert to dense
        std::vector<TReal> filter_, zerocross_, smoothedintensity_; // working variables
        std::vector<TReal> peakmass_, peakarea_; //results
        TReal smoothwith_;
        TReal integrationWidth_;
        ralab::base::ms::SimplePicker<TReal> simplepicker_;
        ralab::base::resample::SamplingWith sw_;
        PeakIntegrator integrator_;
        TReal intensitythreshold_;
        bool area_;
        uint32_t maxnumbersofpeaks_;

        PeakPicker(TReal resolution, //!< instrument resolution
                   std::pair<TReal, TReal> & massrange, //!< mass range of spectrum
                   TReal width = 2., //!< smooth width
                   TReal intwidth = 2., //!< integration width used for area compuation
                   TReal intensitythreshold = 10., // intensity threshold
                   bool area = true,//!< compute area or height? default - height.
                   uint32_t maxnumberofpeaks = 0, //!< maximum of peaks returned by picker
                   double c2d = 1e-5  //!< instrument resampling with small default dissables automatic determination
            ): resolution_(resolution),c2d_( c2d ) ,smoothwith_(width),
          integrationWidth_(intwidth),sw_(),integrator_(integrationWidth_),
          intensitythreshold_(intensitythreshold),area_(area),maxnumbersofpeaks_(maxnumberofpeaks)
        {
          c2d_.defBreak(massrange,ralab::base::resample::resolution2ppm(resolution));
          c2d_.getMids(resampledmz_);
          ralab::base::filter::getGaussianFilterQuantile(filter_,width);
        }

        template<typename Tmass, typename Tintensity>
        void operator()(Tmass begmz, Tmass endmz, Tintensity begint )
        {
          typename std::iterator_traits<Tintensity>::value_type minint = *std::upper_bound(begint,begint+std::distance(begmz,endmz),0.1);

          //determine sampling with
          double a = sw_(begmz,endmz);
          //resmpale the spectrum
          c2d_.am_ = a;
          c2d_.convert2dense(begmz,endmz, begint, resampledintensity_);

          //smooth the resampled spectrum
          ralab::base::filter::filter(resampledintensity_ , filter_ , smoothedintensity_ , true);
          //determine zero crossings
          zerocross_.resize( smoothedintensity_.size()/2 );
          size_t nrzerocross = simplepicker_( smoothedintensity_.begin( ) , smoothedintensity_.end() , zerocross_.begin(), zerocross_.size());

          peakmass_.resize(nrzerocross);
          //determine mass of zerocrossing
          ralab::base::base::interpolate_linear( resampledmz_.begin() , resampledmz_.end() ,
                                                 zerocross_.begin(),  zerocross_.begin()+nrzerocross ,
                                                 peakmass_.begin());

          //determine peak area
          if(area_){
            peakarea_.resize(nrzerocross);
            integrator_( zerocross_.begin(), zerocross_.begin() + nrzerocross ,
                         smoothedintensity_.begin(),resampledintensity_.begin(), peakarea_.begin() );
          }else{
            //determine intensity
            peakarea_.resize(nrzerocross);
            ralab::base::base::interpolate_cubic( smoothedintensity_.begin() , smoothedintensity_.end() ,
                                                  zerocross_.begin(),  zerocross_.begin()+nrzerocross ,
                                                  peakarea_.begin());
          }

          TReal threshold = static_cast<TReal>(minint) * intensitythreshold_;

          if(maxnumbersofpeaks_ > 0){
            double threshmax = getNToppeaks();
            if(threshmax > threshold)
              threshold = threshmax;
          }

          if(threshold > 0.01){
            filter(threshold);
          }
        }

        /// get min instensity of peak to qualify for max-intensity;
        TReal getNToppeaks(){
          TReal intthres  = 0.;
          if(maxnumbersofpeaks_ < peakarea_.size())
          {
            std::vector<TReal> tmparea( peakarea_.begin() , peakarea_.end() );
            std::nth_element(tmparea.begin(),tmparea.end() - maxnumbersofpeaks_ , tmparea.end());
            intthres = *(tmparea.end() - maxnumbersofpeaks_);
          }
          return intthres;
        }

        /// clean the masses using the threshold
        void filter(TReal threshold){
          typename std::vector<TReal>::iterator a = ralab::base::utils::copy_if(peakarea_.begin(),peakarea_.end(),peakmass_.begin(),
                                                                                peakmass_.begin(),boost::bind(std::greater<TReal>(),_1,threshold));
          peakmass_.resize(std::distance(peakmass_.begin(),a));
          typename std::vector<TReal>::iterator b = ralab::base::utils::copy_if(peakarea_.begin(),peakarea_.end(),
                                                                                peakarea_.begin(),boost::bind(std::greater<TReal>(),_1,threshold));
          peakarea_.resize(std::distance(peakarea_.begin(),b));
          //int x = 1;
        }

        const std::vector<TReal> & getPeakMass(){
          return peakmass_;
        }

        const std::vector<TReal> & getPeakArea(){
          return peakarea_;
        }

        const std::vector<TReal> & getResampledMZ(){
          return resampledmz_;
        }

        const std::vector<TReal> & getResampledIntensity(){
          return resampledintensity_;
        }

        const std::vector<TReal> & getSmoothedIntensity(){
          return smoothedintensity_;
        }
      };
    }//ms
  }//base
}//ralab

#endif // PEAKPICKER_H


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

The Medical Imaging Interaction Toolkit (MITK)

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

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

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

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

// std includes
#include <string>
#include <sstream>
#include <fstream>
#include <vector>
#include <map>
#include <utility>

// boost includes
#include <boost/algorithm/string.hpp>

// ITK includes
#include <itkImageFileWriter.h>

// CTK includes
#include "mitkCommandLineParser.h"

// MITK includes
#include <mitkConnectomicsStatisticsCalculator.h>
#include <mitkConnectomicsNetworkThresholder.h>
#include <itkConnectomicsNetworkToConnectivityMatrixImageFilter.h>
#include <mitkIOUtil.h>

int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;

  parser.setTitle("Network Creation");
  parser.setCategory("Connectomics");
  parser.setDescription("");
  parser.setContributor("MBI");

  parser.setArgumentPrefix("--", "-");

  parser.addArgument("inputNetwork", "i", mitkCommandLineParser::InputFile, "Input network", "input connectomics network (.cnf)", us::Any(), false);
  parser.addArgument("outputFile", "o", mitkCommandLineParser::OutputFile, "Output file", "name of output file", us::Any(), false);

  parser.addArgument("noGlobalStatistics", "g", mitkCommandLineParser::Bool, "No global statistics", "Do not calculate global statistics");
  parser.addArgument("createConnectivityMatriximage", "I", mitkCommandLineParser::Bool, "Write connectivity matrix image", "Write connectivity matrix image");
  parser.addArgument("binaryConnectivity", "b", mitkCommandLineParser::Bool, "Binary connectivity", "Whether to create a binary connectivity matrix");
  parser.addArgument("rescaleConnectivity", "r", mitkCommandLineParser::Bool, "Rescale connectivity", "Whether to rescale the connectivity matrix");
  parser.addArgument("localStatistics", "L", mitkCommandLineParser::StringList, "Local statistics", "Provide a list of node labels for local statistics", us::Any());
  parser.addArgument("regionList", "R", mitkCommandLineParser::StringList, "Region list", "A space separated list of regions. Each region has the format\n regionname;label1;label2;...;labelN", us::Any());
  parser.addArgument("granularity", "gr", mitkCommandLineParser::Int, "Granularity", "How finely to test the density range and how many thresholds to consider",1);
  parser.addArgument("startDensity", "d", mitkCommandLineParser::Float, "Start Density", "Largest density for the range",1.0);
  parser.addArgument("thresholdStepSize", "t", mitkCommandLineParser::Int, "Step size threshold", "Distance of two adjacent thresholds",3);

  parser.setCategory("Connectomics");
  parser.setTitle("Network Statistics");
  parser.setDescription("");
  parser.setContributor("MBI");

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  //default values
  bool noGlobalStatistics( false );
  bool binaryConnectivity( false );
  bool rescaleConnectivity( false );
  bool createConnectivityMatriximage( false );
  int granularity( 1 );
  double startDensity( 1.0 );
  int thresholdStepSize( 3 );

  // parse command line arguments
  std::string networkName = us::any_cast<std::string>(parsedArgs["inputNetwork"]);
  std::string outName = us::any_cast<std::string>(parsedArgs["outputFile"]);

  mitkCommandLineParser::StringContainerType localLabels;

  if(parsedArgs.count("localStatistics"))
  {
    localLabels = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["localStatistics"]);
  }

  mitkCommandLineParser::StringContainerType unparsedRegions;
  std::map< std::string, std::vector<std::string> > parsedRegions;
  std::map< std::string, std::vector<std::string> >::iterator parsedRegionsIterator;

  if(parsedArgs.count("regionList"))
  {
    unparsedRegions = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["regionList"]);

    for(unsigned int index(0); index < unparsedRegions.size(); index++ )
    {
      std::vector< std::string > tempRegionVector;

      boost::split(tempRegionVector, unparsedRegions.at(index), boost::is_any_of(";"));

      std::vector< std::string >::const_iterator begin = tempRegionVector.begin();
      std::vector< std::string >::const_iterator last = tempRegionVector.begin() + tempRegionVector.size();
      std::vector< std::string > insertRegionVector(begin + 1, last);

      if( parsedRegions.count( tempRegionVector.at(0) ) == 0 )
      {
        parsedRegions.insert( std::pair< std::string, std::vector<std::string>  >( tempRegionVector.at(0), insertRegionVector) );
      }
      else
      {
        MITK_ERROR << "Region already exists. Skipping second occurrence.";
      }
    }
  }

  if (parsedArgs.count("noGlobalStatistics"))
    noGlobalStatistics = us::any_cast<bool>(parsedArgs["noGlobalStatistics"]);
  if (parsedArgs.count("binaryConnectivity"))
    binaryConnectivity = us::any_cast<bool>(parsedArgs["binaryConnectivity"]);
  if (parsedArgs.count("rescaleConnectivity"))
    rescaleConnectivity = us::any_cast<bool>(parsedArgs["rescaleConnectivity"]);
  if (parsedArgs.count("createConnectivityMatriximage"))
    createConnectivityMatriximage = us::any_cast<bool>(parsedArgs["createConnectivityMatriximage"]);
  if (parsedArgs.count("granularity"))
    granularity = us::any_cast<int>(parsedArgs["granularity"]);
  if (parsedArgs.count("startDensity"))
    startDensity = us::any_cast<float>(parsedArgs["startDensity"]);
  if (parsedArgs.count("thresholdStepSize"))
    thresholdStepSize = us::any_cast<int>(parsedArgs["thresholdStepSize"]);

  try
  {
    // load network
    std::vector<mitk::BaseData::Pointer> networkFile =
      mitk::IOUtil::Load( networkName);
    if( networkFile.empty() )
    {
      std::string errorMessage = "File at " + networkName + " could not be read. Aborting.";
      MITK_ERROR << errorMessage;
      return EXIT_FAILURE;
    }
    mitk::BaseData* networkBaseData = networkFile.at(0);
    mitk::ConnectomicsNetwork* network = dynamic_cast<mitk::ConnectomicsNetwork*>( networkBaseData );

    if( !network )
    {
      std::string errorMessage = "Read file at " + networkName + " could not be recognized as network. Aborting.";
      MITK_ERROR << errorMessage;
      return EXIT_FAILURE;
    }

    // streams
    std::stringstream globalHeaderStream;
    globalHeaderStream  << "NumberOfVertices "
      << "NumberOfEdges "
      << "AverageDegree "
      << "ConnectionDensity "
      << "NumberOfConnectedComponents "
      << "AverageComponentSize "
      << "LargestComponentSize "
      << "RatioOfNodesInLargestComponent "
      << "HopPlotExponent "
      << "EffectiveHopDiameter "
      << "AverageClusteringCoefficientsC "
      << "AverageClusteringCoefficientsD "
      << "AverageClusteringCoefficientsE "
      << "AverageVertexBetweennessCentrality "
      << "AverageEdgeBetweennessCentrality "
      << "NumberOfIsolatedPoints "
      << "RatioOfIsolatedPoints "
      << "NumberOfEndPoints "
      << "RatioOfEndPoints "
      << "Diameter "
      << "Diameter90 "
      << "Radius "
      << "Radius90 "
      << "AverageEccentricity "
      << "AverageEccentricity90 "
      << "AveragePathLength "
      << "NumberOfCentralPoints "
      << "RatioOfCentralPoints "
      << "SpectralRadius "
      << "SecondLargestEigenValue "
      << "AdjacencyTrace "
      << "AdjacencyEnergy "
      << "LaplacianTrace "
      << "LaplacianEnergy "
      << "LaplacianSpectralGap "
      << "NormalizedLaplacianTrace "
      << "NormalizedLaplacianEnergy "
      << "NormalizedLaplacianNumberOf2s "
      << "NormalizedLaplacianNumberOf1s "
      << "NormalizedLaplacianNumberOf0s "
      << "NormalizedLaplacianLowerSlope "
      << "NormalizedLaplacianUpperSlope "
      << "SmallWorldness"
      << std::endl;

    std::stringstream localHeaderStream;
    std::stringstream regionalHeaderStream;

    std::stringstream globalDataStream;
    std::stringstream localDataStream;
    std::stringstream regionalDataStream;

    std::string globalOutName = outName + "_global.txt";
    std::string localOutName = outName + "_local.txt";
    std::string regionalOutName = outName + "_regional.txt";

    bool firstRun( true );
    // iterate over all three possible methods
    for(unsigned int method( 0 ); method < 3; method++)
    {
      // 0 - Random removal threshold
      // 1 - Largest density below threshold
      // 2 - Threshold based

      // iterate over possible targets
      for( unsigned int step( 0 ); step < granularity; step++ )
      {
        double targetValue( 0.0 );
        bool newStep( true );

        switch ( method )
        {
        case mitk::ConnectomicsNetworkThresholder::RandomRemovalOfWeakest :
        case mitk::ConnectomicsNetworkThresholder::LargestLowerThanDensity :
          targetValue = startDensity * (1 - static_cast<double>( step ) / ( granularity + 0.5 ) );
          break;
        case mitk::ConnectomicsNetworkThresholder::ThresholdBased :
          targetValue = static_cast<double>( thresholdStepSize * step );
          break;
        default:
          MITK_ERROR << "Invalid thresholding method called, aborting.";
          return EXIT_FAILURE;
          break;
        }

        mitk::ConnectomicsNetworkThresholder::Pointer thresholder = mitk::ConnectomicsNetworkThresholder::New();
        thresholder->SetNetwork( network );
        thresholder->SetTargetThreshold( targetValue );
        thresholder->SetTargetDensity( targetValue );
        thresholder->SetThresholdingScheme( static_cast<mitk::ConnectomicsNetworkThresholder::ThresholdingSchemes>(method) );
        mitk::ConnectomicsNetwork::Pointer thresholdedNetwork = thresholder->GetThresholdedNetwork();

        mitk::ConnectomicsStatisticsCalculator::Pointer statisticsCalculator = mitk::ConnectomicsStatisticsCalculator::New();
        statisticsCalculator->SetNetwork( thresholdedNetwork );
        statisticsCalculator->Update();

        // global statistics
        if( !noGlobalStatistics )
        {
          globalDataStream << statisticsCalculator->GetNumberOfVertices() << " "
            << statisticsCalculator->GetNumberOfEdges() << " "
            << statisticsCalculator->GetAverageDegree() << " "
            << statisticsCalculator->GetConnectionDensity() << " "
            << statisticsCalculator->GetNumberOfConnectedComponents() << " "
            << statisticsCalculator->GetAverageComponentSize() << " "
            << statisticsCalculator->GetLargestComponentSize() << " "
            << statisticsCalculator->GetRatioOfNodesInLargestComponent() << " "
            << statisticsCalculator->GetHopPlotExponent() << " "
            << statisticsCalculator->GetEffectiveHopDiameter() << " "
            << statisticsCalculator->GetAverageClusteringCoefficientsC() << " "
            << statisticsCalculator->GetAverageClusteringCoefficientsD() << " "
            << statisticsCalculator->GetAverageClusteringCoefficientsE() << " "
            << statisticsCalculator->GetAverageVertexBetweennessCentrality() << " "
            << statisticsCalculator->GetAverageEdgeBetweennessCentrality() << " "
            << statisticsCalculator->GetNumberOfIsolatedPoints() << " "
            << statisticsCalculator->GetRatioOfIsolatedPoints() << " "
            << statisticsCalculator->GetNumberOfEndPoints() << " "
            << statisticsCalculator->GetRatioOfEndPoints() << " "
            << statisticsCalculator->GetDiameter() << " "
            << statisticsCalculator->GetDiameter90() << " "
            << statisticsCalculator->GetRadius() << " "
            << statisticsCalculator->GetRadius90() << " "
            << statisticsCalculator->GetAverageEccentricity() << " "
            << statisticsCalculator->GetAverageEccentricity90() << " "
            << statisticsCalculator->GetAveragePathLength() << " "
            << statisticsCalculator->GetNumberOfCentralPoints() << " "
            << statisticsCalculator->GetRatioOfCentralPoints() << " "
            << statisticsCalculator->GetSpectralRadius() << " "
            << statisticsCalculator->GetSecondLargestEigenValue() << " "
            << statisticsCalculator->GetAdjacencyTrace() << " "
            << statisticsCalculator->GetAdjacencyEnergy() << " "
            << statisticsCalculator->GetLaplacianTrace() << " "
            << statisticsCalculator->GetLaplacianEnergy() << " "
            << statisticsCalculator->GetLaplacianSpectralGap() << " "
            << statisticsCalculator->GetNormalizedLaplacianTrace() << " "
            << statisticsCalculator->GetNormalizedLaplacianEnergy() << " "
            << statisticsCalculator->GetNormalizedLaplacianNumberOf2s() << " "
            << statisticsCalculator->GetNormalizedLaplacianNumberOf1s() << " "
            << statisticsCalculator->GetNormalizedLaplacianNumberOf0s() << " "
            << statisticsCalculator->GetNormalizedLaplacianLowerSlope() << " "
            << statisticsCalculator->GetNormalizedLaplacianUpperSlope() << " "
            << statisticsCalculator->GetSmallWorldness()
            << std::endl;

        } // end global statistics

        //create connectivity matrix png
        if( createConnectivityMatriximage )
        {
          std::string connectivity_png_postfix = "_connectivity";
          if( binaryConnectivity )
          {
            connectivity_png_postfix += "_binary";
          }
          else if( rescaleConnectivity )
          {
            connectivity_png_postfix += "_rescaled";
          }
          connectivity_png_postfix += ".png";

          /* File format
          * A png file depicting the binary connectivity matrix
          */
          itk::ConnectomicsNetworkToConnectivityMatrixImageFilter::Pointer filter = itk::ConnectomicsNetworkToConnectivityMatrixImageFilter::New();
          filter->SetInputNetwork( network );
          filter->SetBinaryConnectivity( binaryConnectivity );
          filter->SetRescaleConnectivity( rescaleConnectivity );
          filter->Update();

          typedef itk::ConnectomicsNetworkToConnectivityMatrixImageFilter::OutputImageType connectivityMatrixImageType;

          itk::ImageFileWriter< connectivityMatrixImageType >::Pointer connectivityWriter = itk::ImageFileWriter< connectivityMatrixImageType >::New();

          connectivityWriter->SetInput( filter->GetOutput() );
          connectivityWriter->SetFileName( outName + connectivity_png_postfix);
          connectivityWriter->Update();

          std::cout << "Connectivity matrix image written.";
        } // end create connectivity matrix png

        /*
         * We can either calculate local indices for specific nodes, or specific regions
         */

        // Create LabelToIndex translation
        std::map< std::string, int > labelToIdMap;
        std::vector< mitk::ConnectomicsNetwork::NetworkNode > nodeVector = thresholdedNetwork->GetVectorOfAllNodes();
        for(int loop(0); loop < nodeVector.size(); loop++)
        {
          labelToIdMap.insert( std::pair< std::string, int>(nodeVector.at(loop).label, nodeVector.at(loop).id) );
        }

        std::vector< int > degreeVector = thresholdedNetwork->GetDegreeOfNodes();
        std::vector< double > ccVector = thresholdedNetwork->GetLocalClusteringCoefficients( );
        std::vector< double > bcVector = thresholdedNetwork->GetNodeBetweennessVector( );

        // calculate local indices

        {
          // only add to header for the first step of the first method
          if( firstRun )
          {
            localHeaderStream << "Th_method " << "Th_target " << "density";
          }

          double density = statisticsCalculator->GetConnectionDensity();

          localDataStream << "\n"
            << method << " "
            << targetValue << " "
            << density;

          for(unsigned int loop(0); loop < localLabels.size(); loop++ )
          {
            if( network->CheckForLabel(localLabels.at( loop )) )
            {
              if( firstRun )
              {
                localHeaderStream  << " "
                  << localLabels.at( loop ) << "_Degree "
                  << localLabels.at( loop ) << "_CC "
                  << localLabels.at( loop ) << "_BC";
              }

          localDataStream  << " " << degreeVector.at( labelToIdMap.find( localLabels.at( loop ) )->second )
            << " " << ccVector.at( labelToIdMap.find( localLabels.at( loop ) )->second )
            << " " << bcVector.at( labelToIdMap.find( localLabels.at( loop ) )->second );
            }
            else
            {
              MITK_ERROR << "Illegal label. Label: \"" << localLabels.at( loop ) << "\" not found.";
            }
          }
        }

        // calculate regional indices

        {
          // only add to header for the first step of the first method
          if( firstRun )
          {
            regionalHeaderStream << "Th_method " << "Th_target " << "density";
          }

          double density = statisticsCalculator->GetConnectionDensity();

          regionalDataStream << "\n"
            << method << " "
            << targetValue << " "
            << density;

          for( parsedRegionsIterator = parsedRegions.begin(); parsedRegionsIterator != parsedRegions.end(); parsedRegionsIterator++ )
          {
            std::vector<std::string> regionLabelsVector = parsedRegionsIterator->second;
            std::string regionName = parsedRegionsIterator->first;

            double sumDegree( 0 );
            double sumCC( 0 );
            double sumBC( 0 );
            double count( 0 );

            for( int loop(0); loop < regionLabelsVector.size(); loop++ )
            {
              if( thresholdedNetwork->CheckForLabel(regionLabelsVector.at( loop )) )
              {
                sumDegree = sumDegree + degreeVector.at( labelToIdMap.find( regionLabelsVector.at( loop ) )->second );
                sumCC = sumCC + ccVector.at( labelToIdMap.find( regionLabelsVector.at( loop ) )->second );
                sumBC = sumBC + bcVector.at( labelToIdMap.find( regionLabelsVector.at( loop ) )->second );
                count = count + 1;
              }
              else
              {
                MITK_ERROR << "Illegal label. Label: \"" << regionLabelsVector.at( loop ) << "\" not found.";
              }
            }

            // only add to header for the first step of the first method
            if( firstRun )
            {
              regionalHeaderStream  << " " << regionName << "_LocalAverageDegree "
                << regionName << "_LocalAverageCC "
                << regionName << "_LocalAverageBC "
                << regionName << "_NumberOfNodes";
            }

            regionalDataStream  << " " << sumDegree / count
              << " " << sumCC / count
              << " " << sumBC / count
              << " " << count;

            // count number of connections and fibers between regions
            std::map< std::string, std::vector<std::string> >::iterator loopRegionsIterator;
            for (loopRegionsIterator = parsedRegionsIterator; loopRegionsIterator != parsedRegions.end(); loopRegionsIterator++)
            {
              int numberConnections(0), possibleConnections(0);
              double summedFiberCount(0.0);
              std::vector<std::string> loopLabelsVector = loopRegionsIterator->second;
              std::string loopName = loopRegionsIterator->first;

              for (int loop(0); loop < regionLabelsVector.size(); loop++)
              {
                if (thresholdedNetwork->CheckForLabel(regionLabelsVector.at(loop)))
                {
                  for (int innerLoop(0); innerLoop < loopLabelsVector.size(); innerLoop++)
                  {
                    if (thresholdedNetwork->CheckForLabel(loopLabelsVector.at(loop)))
                    {
                      bool exists = thresholdedNetwork->EdgeExists(
                        labelToIdMap.find(regionLabelsVector.at(loop))->second,
                        labelToIdMap.find(loopLabelsVector.at(innerLoop))->second);
                      possibleConnections++;
                      if (exists)
                      {
                        numberConnections++;
                        summedFiberCount += thresholdedNetwork->GetEdge(
                          labelToIdMap.find(regionLabelsVector.at(loop))->second,
                          labelToIdMap.find(loopLabelsVector.at(innerLoop))->second).weight;
                      }
                    }
                    else
                    {
                      MITK_ERROR << "Illegal label. Label: \"" << loopLabelsVector.at(loop) << "\" not found.";
                    }
                  }
                }
                else
                {
                  MITK_ERROR << "Illegal label. Label: \"" << regionLabelsVector.at(loop) << "\" not found.";
                }
              }

              if (firstRun)
              {
                regionalHeaderStream << " " << regionName << "_" << loopName << "_Connections "
                  << " " << regionName << "_" << loopName << "_possibleConnections "
                  << " " << regionName << "_" << loopName << "_ConnectingFibers";
              }

              regionalDataStream << " " << numberConnections
                << " " << possibleConnections
                << " " << summedFiberCount;

            }
          }
        }

        firstRun = false;
      }

    }// end calculate local averages

    if( !noGlobalStatistics )
    {
      std::cout << "Writing to " << globalOutName;
      std::ofstream glocalOutFile( globalOutName.c_str(), ios::out );
      if( ! glocalOutFile.is_open() )
      {
        std::string errorMessage = "Could not open " + globalOutName + " for writing.";
        MITK_ERROR << errorMessage;
        return EXIT_FAILURE;
      }
      glocalOutFile << globalHeaderStream.str() << globalDataStream.str();
      glocalOutFile.close();
    }

    if( localLabels.size() > 0 )
    {
      std::cout << "Writing to " << localOutName;
      std::ofstream localOutFile( localOutName.c_str(), ios::out );
      if( ! localOutFile.is_open() )
      {
        std::string errorMessage = "Could not open " + localOutName + " for writing.";
        MITK_ERROR << errorMessage;
        return EXIT_FAILURE;
      }
      localOutFile << localHeaderStream.str() << localDataStream.str();
      localOutFile.close();
    }

    if( parsedRegions.size() > 0 )
    {
      std::cout << "Writing to " << regionalOutName;
      std::ofstream regionalOutFile( regionalOutName.c_str(), ios::out );
      if( ! regionalOutFile.is_open() )
      {
        std::string errorMessage = "Could not open " + regionalOutName + " for writing.";
        MITK_ERROR << errorMessage;
        return EXIT_FAILURE;
      }
      regionalOutFile << regionalHeaderStream.str() << regionalDataStream.str();
      regionalOutFile.close();
    }

    return EXIT_SUCCESS;
  }
  catch (itk::ExceptionObject e)
  {
    std::cout << e;
    return EXIT_FAILURE;
  }
  catch (std::exception e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  std::cout << "DONE";
  return EXIT_SUCCESS;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Francisca Gil Ureta <gilureta@cs.nyu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "edge_topology.h"
#include "sort_vectors_ccw.h"
#include "streamlines.h"
#include "per_face_normals.h"
#include "polyvector_field_matchings.h"
#include "segment_segment_intersect.h"
#include "triangle_triangle_adjacency.h"
#include "barycenter.h"
#include "slice.h"

#include <Eigen/Geometry>

IGL_INLINE void igl::streamlines_init(
                                      const Eigen::MatrixXd V,
                                      const Eigen::MatrixXi F,
                                      const Eigen::MatrixXd &temp_field,
                                      const bool treat_as_symmetric,
                                      StreamlineData &data,
                                      StreamlineState &state,
                                      double percentage
                                      ){
  using namespace Eigen;
  using namespace std;

  igl::edge_topology(V, F, data.E, data.F2E, data.E2F);
  igl::triangle_triangle_adjacency(F, data.TT);

  // prepare vector field
  // --------------------------
  int half_degree = temp_field.cols() / 3;
  int degree = treat_as_symmetric ? half_degree * 2 : half_degree;
  data.degree = degree;

  Eigen::MatrixXd FN;
  Eigen::VectorXi order;
  Eigen::RowVectorXd sorted;

  igl::per_face_normals(V, F, FN);
  data.field.setZero(F.rows(), degree * 3);
  for (unsigned i = 0; i < F.rows(); ++i){
    const Eigen::RowVectorXd &n = FN.row(i);
    Eigen::RowVectorXd temp(1, degree * 3);
    if (treat_as_symmetric)
      temp << temp_field.row(i), -temp_field.row(i);
    else
      temp = temp_field.row(i);
    igl::sort_vectors_ccw(temp, n, order, sorted);

    // project vectors to tangent plane
    for (int j = 0; j < degree; ++j)
    {
      Eigen::RowVector3d pd = sorted.segment(j * 3, 3);
      pd = (pd - (n.dot(pd)) * n).normalized();
      data.field.block(i, j * 3, 1, 3) = pd;
    }
  }
  Eigen::VectorXd curl;
  igl::polyvector_field_matchings(data.field, V, F, false, treat_as_symmetric, data.match_ab, data.match_ba, curl);

  // create seeds for tracing
  // --------------------------
  Eigen::VectorXi samples;
  int nsamples;

  nsamples = percentage * F.rows();
  Eigen::VectorXd r;
  r.setRandom(nsamples, 1);
  r = (1 + r.array()) / 2.;
  samples = (r.array() * F.rows()).cast<int>();
  data.nsample = nsamples;

  Eigen::MatrixXd BC, BC_sample;
  igl::barycenter(V, F, BC);
  igl::slice(BC, samples, 1, BC_sample);

  // initialize state for tracing vector field

  state.start_point = BC_sample.replicate(degree,1);
  state.end_point = state.start_point;

  state.current_face = samples.replicate(1, degree);

  state.current_direction.setZero(nsamples, degree);
  for (int i = 0; i < nsamples; ++i)
    for (int j = 0; j < degree; ++j)
      state.current_direction(i, j) = j;

}

IGL_INLINE void igl::streamlines_next(
                                      const Eigen::MatrixXd V,
                                      const Eigen::MatrixXi F,
                                      const StreamlineData & data,
                                      StreamlineState & state
                                      ){
  using namespace Eigen;
  using namespace std;

  int degree = data.degree;
  int nsample = data.nsample;

  state.start_point = state.end_point;

  for (int i = 0; i < degree; ++i)
  {
    for (int j = 0; j < nsample; ++j)
    {
      int f0 = state.current_face(j,i);
      if (f0 == -1) // reach boundary
        continue;
      int m0 = state.current_direction(j, i);

      // the starting point of the vector
      const Eigen::RowVector3d &p = state.start_point.row(j + nsample * i);
      // the direction where we are trying to go
      const Eigen::RowVector3d &r = data.field.block(f0, 3 * m0, 1, 3);

      // new state,
      int f1, m1;

      for (int k = 0; k < 3; ++k)
      {
        f1 = data.TT(f0, k);

        // edge vertices
        const Eigen::RowVector3d &q = V.row(F(f0, k));
        const Eigen::RowVector3d &qs = V.row(F(f0, (k + 1) % 3));
        // edge direction
        Eigen::RowVector3d s = qs - q;

        double u;
        double t;
        if (igl::segments_intersect(p, r, q, s, t, u))
        {
          // point on next face
          state.end_point.row(j + nsample * i) = p + t * r;
          state.current_face(j,i) = f1;

          // matching direction on next face
          int e1 = data.F2E(f0, k);
          if (data.E2F(e1, 0) == f0)
            m1 = data.match_ab(e1, m0);
          else
            m1 = data.match_ba(e1, m0);

          state.current_direction(j, i) = m1;
          break;
        }

      }

    }
  }
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *
 *    Notes
 *      This file doesn't test any specific Runge-Kutta-type integrators, but rather some general
 *      functionality adopted in the the RungeKuttaVariableStepSizeIntegrator class, applicable to
 *      all Runge-Kutta-type integrators.
 *
 *      It should be noted that the getCurrentStateDerivatives() member function is not tested in a
 *      fully generic manner at the moment; the test is setup specifically based on the
 *      Runge-Kutta-Fehlberg 4(5) (RKF45) integrator. A more comprehensive test should be designed
 *      to ensure that the member function performs as desired regardless of the coefficient set
 *      chosen.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "tudat/math/integrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "tudat/math/integrators/rungeKutta4Integrator.h"
#include "tudat/math/integrators/rungeKuttaCoefficients.h"
#include "tudat/basics/testMacros.h"
#include "tudat/math/integrators/numericalIntegratorTestFunctions.h"

#include <limits>
#include <string>
#include <typeinfo>

namespace tudat
{
namespace unit_tests
{

using numerical_integrators::RungeKuttaCoefficients;
using numerical_integrators::RungeKuttaVariableStepSizeIntegrator;
using numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd;
using numerical_integrator_test_functions::computeZeroStateDerivative;

BOOST_AUTO_TEST_SUITE( test_runge_kutta_variable_step_size_integrator )

//! Test different types of states and state derivatives.
BOOST_AUTO_TEST_CASE( testCompilerErrors )
{
    // Case 1: test the VectorXd typdef. There is no need to explicitly test anything, since we're
    //         looking for compiler errors.
    {
        RungeKuttaVariableStepSizeIntegratorXd integrator(
                    RungeKuttaCoefficients( ),
                    &computeZeroStateDerivative,
                    0.0,
                    Eigen::Vector3d::Zero( ),
                    0.01,
                    std::numeric_limits< double >::infinity( ),
                    10.0 * std::numeric_limits< double >::epsilon( ),
                    10.0 * std::numeric_limits< double >::epsilon( ) );

        // Test integrateTo() function.
        integrator.integrateTo( 10.0, 0.1 );

        // Test performIntegrationStep() function.
        integrator.performIntegrationStep( 0.1 );

        // Test rollbackToPreviousState() function.
        integrator.rollbackToPreviousState( );

        // Check if the default types are correct.
        BOOST_CHECK_EQUAL(
                    std::string( typeid( integrator.getCurrentIndependentVariable( ) ).name( ) ),
                    std::string( typeid( double ).name( ) ) );
        BOOST_CHECK_EQUAL( std::string( typeid( integrator.getCurrentState( ) ).name( ) ),
                           std::string( typeid( Eigen::VectorXd ).name( ) ) );
    }

    // Case 2: test different types of states and state derivatives. There is no need to explicitly
    //         test anything, since we're looking for compiler errors.
    {
        RungeKuttaVariableStepSizeIntegrator < double, Eigen::Vector3d, Eigen::VectorXd >
                integrator( RungeKuttaCoefficients( ),
                            &computeZeroStateDerivative,
                            0.0,
                            Eigen::Vector3d::Zero( ),
                            0.01,
                            std::numeric_limits< double >::infinity( ),
                            10.0 * std::numeric_limits< double >::epsilon( ),
                            10.0 * std::numeric_limits< double >::epsilon( ) );

        // Test integrateTo() function.
        integrator.integrateTo( 10.0, 0.1 );

        // Test performIntegrationStep() function.
        integrator.performIntegrationStep( 0.1 );

        // Test rollbackToPreviousState() function.
        integrator.rollbackToPreviousState( );
    }

    // Case 3: test same types of states and state derivatives (MatrixXd). There is no need to
    //         explicitly test anything, since we're looking for compiler errors.
    {
        RungeKuttaVariableStepSizeIntegrator < double, Eigen::MatrixXd, Eigen::MatrixXd >
                integrator( RungeKuttaCoefficients( ),
                            &computeZeroStateDerivative,
                            0.0,
                            Eigen::Vector3d::Zero( ),
                            0.01,
                            std::numeric_limits< double >::infinity( ),
                            10.0 * std::numeric_limits< double >::epsilon( ),
                            10.0 * std::numeric_limits< double >::epsilon( ) );

        // Test integrateTo() function.
        integrator.integrateTo( 10.0, 0.1 );

        // Test performIntegrationStep() function.
        integrator.performIntegrationStep( 0.1 );

        // Test rollbackToPreviousState() function.
        integrator.rollbackToPreviousState( );
    }
}

//! Test that exceeding minimum step size throws a runtime error.
BOOST_AUTO_TEST_CASE( testMinimumStepSizeRuntimeError )
{
    RungeKuttaVariableStepSizeIntegratorXd integrator(
                RungeKuttaCoefficients( ),
                &computeZeroStateDerivative,
                0.0,
                Eigen::Vector3d::Zero( ),
                100.0,
                std::numeric_limits< double >::infinity( ),
                std::numeric_limits< double >::epsilon( ),
                std::numeric_limits< double >::epsilon( ) );

    // Case 1: test that minimum step size is exceeded when using integrateTo().
    {
        // Declare boolean flag to test if minimum step size is exceed for integrateTo().
        bool isMinimumStepSizeExceededForIntegrateTo = false;

        // Try integrateTo(), which should result in a runtime error.
        try
        {
            // Test integrateTo() function.
            integrator.integrateTo( 10.0, 0.1 );
        }

        // Catch the expected runtime error, and set the boolean flag to true.
        catch ( RungeKuttaVariableStepSizeIntegratorXd::MinimumStepSizeExceededError
                minimumStepSizeExceededError )
        {
            isMinimumStepSizeExceededForIntegrateTo = true;
            BOOST_CHECK_EQUAL( minimumStepSizeExceededError.minimumStepSize, 100.0 );
        }

        // Check that the minimum step size was indeed exceeded.
        BOOST_CHECK( isMinimumStepSizeExceededForIntegrateTo );
    }

    // Case 2: test that minimum step size is exceeded when using performIntegrationStep().
    {
        // Declare boolean flag to test if minimum step size is exceed for
        // performIntegrationStep().
        bool isMinimumStepSizeExceededForPerformIntegrationStep = false;

        // Try performIntegrationStep(), which should result in a runtime error.
        try
        {
            // Test performIntegrationStep() function.
            integrator.performIntegrationStep( 0.1 );
        }

        // Catch the expected runtime error, and set the boolean flag to true.
        catch ( RungeKuttaVariableStepSizeIntegratorXd::MinimumStepSizeExceededError
                minimumStepSizeExceededError )
        {
            isMinimumStepSizeExceededForPerformIntegrationStep = true;
            BOOST_CHECK_EQUAL( minimumStepSizeExceededError.minimumStepSize, 100.0 );
        }

        // Check that the minimum step size was indeed exceeded.
        BOOST_CHECK( isMinimumStepSizeExceededForPerformIntegrationStep );
    }
}

//! Test if the state derivative evaliations are properly returned.
BOOST_AUTO_TEST_CASE( testStateDerivativeRetrievalFunction )
{
    using namespace numerical_integrators;
    using namespace unit_tests::numerical_integrator_test_functions;

    // This test is based on the Runge-Kutta-Fehlberg 4(5) coefficient set, hence the test does not
    // robustly ensure that the getCurrentStateDerivatives() works correctly for any given
    // coefficient set currently. This test is more of an preliminary check that the function
    // performs as required, with the extrapolation that it is likely to perform consistently in
    // this manner for any Runge-Kutta-type coefficient set.

    // Create test integrator.
    RungeKuttaVariableStepSizeIntegratorXd integrator(
                RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                &computeVanDerPolStateDerivative,
                0.0,
                ( Eigen::VectorXd( 2 ) << 1.0, 2.0 ).finished( ),
                0.0, 10.0, 1.0E-8, 1.0E-8 );

    // Perform first integration step.
    integrator.performIntegrationStep( 1.0 );

    // Retrieve time and state after first integration step.
    const double previousTime = integrator.getCurrentIndependentVariable( );
    const Eigen::VectorXd previousState = integrator.getCurrentState( );

    // Perform additional integration step to verify that there are no problems with
    // getCurrentStateDerivatives after >1 iterations (i.e not being reset etc.).
    integrator.performIntegrationStep( integrator.getNextStepSize( ) );

    // Get current time and previous time step.
    const double currentTime = integrator.getCurrentIndependentVariable( );
    const double stepSize = currentTime - previousTime;

    // Retrieve state derivative values used in previous time step.
    std::vector< Eigen::VectorXd > stateDerivatives = integrator.getCurrentStateDerivatives( );

    // Check size of state derivative vector. This test is specifically set up to test the for the
    // number of stages in the RKF45 integrator.
    BOOST_CHECK_EQUAL( stateDerivatives.size( ), 6 );

    // Perform manual state derivative evaluations at previous time step using RKF45 method
    RungeKuttaCoefficients rkf45Coefficients = RungeKuttaCoefficients::get(
                RungeKuttaCoefficients::rungeKuttaFehlberg45 );
    std::vector< Eigen::VectorXd > directStateDerivativeValues;

    for ( int stage = 0; stage < rkf45Coefficients.cCoefficients.rows( ); stage++ )
    {
        // Compute the intermediate state to pass to the state derivative for this stage.
        Eigen::VectorXd intermediateState( previousState );

        // Compute the intermediate state.
        for ( int column = 0; column < stage; column++ )
        {
            intermediateState += stepSize * rkf45Coefficients.aCoefficients( stage, column )
                    * directStateDerivativeValues[ column ];
        }

        // Compute state derivative.
        directStateDerivativeValues.push_back(
                    computeVanDerPolStateDerivative(
                        previousTime +
                        rkf45Coefficients.cCoefficients( stage ) * stepSize,
                        intermediateState ) );

        // Check if manual result matched result from NumericalIntegrator.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( directStateDerivativeValues.at( stage ),
                                           stateDerivatives.at( stage ),
                                           1.0E-15 );
    }
}

//! Test if integtrateTo function works for variable step size where last step is modified.
BOOST_AUTO_TEST_CASE( testVariableStepIntegrateToFunction )
{
    using namespace numerical_integrators;
    using namespace unit_tests::numerical_integrator_test_functions;

    // In this test, the integrateTo function is used with variable step size integrator, where the
    // step size is modified by the last step size, to ensure the correct operation of the
    // integrator in this case.

    // Create variable step size integrator.
    RungeKuttaVariableStepSizeIntegratorXd integrator(
                RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                &computeSecondNonAutonomousModelStateDerivative,
                0.0,
                ( Eigen::VectorXd( 1 ) << 1.0 ).finished( ),
                0.0, 10.0, 1.0E-10, 1.0E-10 );

    // Use integrateTo function
    Eigen::VectorXd integratedValue = integrator.integrateTo( 0.5, 1.0 );
    double currentTime = integrator.getCurrentIndependentVariable( );

    // Check if current time is correct.
    BOOST_CHECK_CLOSE_FRACTION( currentTime, 0.5, std::numeric_limits< double >::epsilon( ) );

    // Create integrator to see if performing a single step with the given settings will indeed
    // result in the step size being adapted.
    RungeKuttaVariableStepSizeIntegratorXd verificationIntegrator(
                RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                &computeSecondNonAutonomousModelStateDerivative,
                0.0,
                ( Eigen::VectorXd( 1 ) << 1.0 ).finished( ),
                0.0, 10.0, 1.0E-10, 1.0E-10 );

    // Check if single step size of 0.5 will be adapted.
    verificationIntegrator.performIntegrationStep( 0.5 );
    BOOST_CHECK_EQUAL( ( verificationIntegrator.getCurrentIndependentVariable( ) < 0.5 *
                         ( 1.0 - 10.0 * std::numeric_limits< double >::epsilon( ) ) ), true );

    // Use a fixed step size integrator to check the original result of integrateTo
    RungeKutta4IntegratorXd fixedStepSizeIntegrator(
                &computeSecondNonAutonomousModelStateDerivative,0.0,
                ( Eigen::VectorXd( 1 ) << 1.0 ).finished( ) );
    Eigen::VectorXd fixedStepIntegratedValue = integrator.integrateTo( 0.5, 0.01 );
    BOOST_CHECK_CLOSE_FRACTION( fixedStepIntegratedValue.x( ), integratedValue.x( ), 1.0E-10 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <ros/ros.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_broadcaster.h>

#include <local_tool/filters.hpp>
#include <local_tool/mathematics.hpp>
#include <local_tool/registration.hpp>

using namespace std;

class Regi
{
	private:
		ros::NodeHandle n;
		ros::Rate r;

		ros::Publisher trg_pub;
		ros::Publisher ipt_pub;
		ros::Publisher out_pub;

		ros::Publisher tf_pub;

		ros::Subscriber lidar_sub;
		ros::Subscriber camera_sub;

		nav_msgs::Odometry odo;

		pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud;
		pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud;
		pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud;

		pcl::PointCloud<pcl::PointXYZ>::Ptr lidar_cloud;
		pcl::PointCloud<pcl::PointXYZ>::Ptr camera_cloud;

		string LIDAR_TOPIC;
		string CAMERA_TOPIC;

		string PARENT_FRAME;

	public:
		Regi(ros::NodeHandle& n);

		void alignment(void);
		void map(void);

		void lidarCallback(const sensor_msgs::PointCloud2ConstPtr msgs);
		void cameraCallback(const sensor_msgs::PointCloud2ConstPtr msgs);
		bool spin()
		{
			ros::Rate loop_rate(r);

			while(ros::ok()){
				alignment();
				// map();	//mapの確認
				ros::spinOnce();
				loop_rate.sleep();

			}
			return true;
		}

};

Regi::Regi(ros::NodeHandle &n) :
	r(7)
{

	n.getParam("lidar_topic",LIDAR_TOPIC);
	n.getParam("camera_topic",CAMERA_TOPIC);
	n.getParam("parent_frame",PARENT_FRAME);

	trg_pub  = n.advertise<sensor_msgs::PointCloud2>("/target_cloud", 1);
	ipt_pub  = n.advertise<sensor_msgs::PointCloud2>("/input_cloud", 1);
	out_pub  = n.advertise<sensor_msgs::PointCloud2>("/lidar2zed", 1);

	tf_pub  = n.advertise<nav_msgs::Odometry>("/lidar2camera", 100);

	lidar_sub = n.subscribe(LIDAR_TOPIC,1000,&Regi::lidarCallback,this);
	camera_sub = n.subscribe(CAMERA_TOPIC,1000,&Regi::cameraCallback,this);

	target_cloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
	input_cloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
	output_cloud.reset (new pcl::PointCloud<pcl::PointXYZ>);

	lidar_cloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
	camera_cloud.reset (new pcl::PointCloud<pcl::PointXYZ>);

}

void
Regi::lidarCallback(const sensor_msgs::PointCloud2ConstPtr msgs){

	pcl::fromROSMsg(*msgs,*lidar_cloud);

}

void
Regi::cameraCallback(const sensor_msgs::PointCloud2ConstPtr msgs){

	pcl::fromROSMsg(*msgs,*camera_cloud);
}

void
Regi::alignment(void){

	Eigen::Matrix4f a;
	double l_roll, l_pitch, l_yaw;//角度etc

	//icpはinputをtargetに持っていく input = output
	// a = registration_icp_vis(target_cloud,input_cloud,output_cloud);
	// a = registration_ndt_vis(target_cloud,input_cloud,output_cloud);
	a = registration_icp_vis(lidar_cloud,camera_cloud,output_cloud);

	calc_rpy(a,l_roll,l_pitch,l_yaw);

	//////////////////変換行列から得た答え
	//
	cout<<"t(0,3) : "<<a(0, 3)<<endl;
	cout<<"t(1,3) : "<<a(1, 3)<<endl;
	cout<<"t(2,3) : "<<a(2, 3)<<endl;
	cout<<"l_roll : "<<l_roll<<endl;
	cout<<"l_pitch : "<<l_pitch<<endl;
	cout<<"l_yaw : "<<l_yaw<<endl;

	sensor_msgs::PointCloud2 pc, pc2, pc3;
	pcl::toROSMsg(*camera_cloud, pc);
	pcl::toROSMsg(*lidar_cloud, pc2);
	pcl::toROSMsg(*output_cloud, pc3);

	pc.header.frame_id  = "odom";
	pc2.header.frame_id = "odom";
	pc3.header.frame_id = "centerlaser";

	pc.header.stamp  = ros::Time::now();
	pc2.header.stamp = ros::Time::now();
	pc3.header.stamp = ros::Time::now();

	odo.header.stamp = ros::Time::now();
	odo.pose.pose.position.x = a(0,3);
	odo.pose.pose.position.y = a(1,3);
	odo.pose.pose.position.z = a(2,3);
	odo.pose.pose.orientation.x = l_roll;
	odo.pose.pose.orientation.y = l_pitch;
	odo.pose.pose.orientation.z = l_yaw;

	trg_pub.publish(pc);
	ipt_pub.publish(pc2);
	out_pub.publish(pc3);

	tf_pub.publish(odo);

}

int main (int argc, char** argv)
{
	ros::init(argc, argv, "lidar2zed");
	ros::NodeHandle n;

	cout<<"-------lidar2zed start---------"<<endl;

	Regi regi(n);

	regi.spin();

	return (0);
}


#ifndef STAN_MATH_PRIM_FUN_LDLT_FACTOR_HPP
#define STAN_MATH_PRIM_FUN_LDLT_FACTOR_HPP

#include <stan/math/prim/err/check_square.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <stan/math/prim/fun/log.hpp>
#include <stan/math/prim/fun/sum.hpp>
#include <stan/math/prim/fun/is_nan.hpp>
#include <boost/shared_ptr.hpp>

namespace stan {
namespace math {

/**
 * LDLT_factor is a thin wrapper on Eigen::LDLT to allow for
 * reusing factorizations and efficient autodiff of things like
 * log determinants and solutions to linear systems.
 *
 * Memory is allocated in the constructor and stored in a
 * boost::shared_ptr, which ensures that is freed
 * when the object is released.
 *
 * After the constructor and/or compute() is called, users of
 * LDLT_factor are responsible for calling success() to
 * check whether the factorization has succeeded.  Use of an LDLT_factor
 * object (e.g., in mdivide_left_ldlt) is undefined if success() is false.
 *
 * Its usage pattern is:
 *
 * ~~~
 * Eigen::Matrix<T, R, C> A1, A2;
 *
 * LDLT_factor<T, R, C> ldlt_A1(A1);
 * LDLT_factor<T, R, C> ldlt_A2;
 * ldlt_A2.compute(A2);
 * ~~~
 *
 * The caller should check that ldlt_A1.success() and ldlt_A2.success()
 * are true or abort accordingly.  Alternatively, call check_ldlt_factor().
 *
 * Note that ldlt_A1 and ldlt_A2 are completely equivalent.  They simply
 * demonstrate two different ways to construct the factorization.
 *
 * The caller can use the LDLT_factor objects as needed.  For
 * instance
 *
 * ~~~
 * x1 = mdivide_left_ldlt(ldlt_A1, b1);
 * x2 = mdivide_right_ldlt(b2, ldlt_A2);
 *
 * d1 = log_determinant_ldlt(ldlt_A1);
 * d2 = log_determinant_ldlt(ldlt_A2);
 * ~~~
 *
 * This class is conceptually similar to the corresponding Eigen
 * class.  Any symmetric, positive-definite matrix A can be
 * decomposed as LDL' where L is unit lower-triangular and D is
 * diagonal with positive diagonal elements.
 *
 * @tparam T type of elements in the matrix
 * @tparam R number of rows, can be Eigen::Dynamic
 * @tparam C number of columns, can be Eigen::Dynamic
 */
template <typename T, int R, int C>
class LDLT_factor {
 public:
  using vector_t = Eigen::Matrix<T, Eigen::Dynamic, 1>;
  using matrix_t = Eigen::Matrix<T, R, C>;
  using ldlt_t = Eigen::LDLT<matrix_t>;
  using size_type = size_t;
  using value_type = double;

  LDLT_factor() : N_(0), ldltP_(new ldlt_t()) {}

  explicit LDLT_factor(const matrix_t& A) : N_(0), ldltP_(new ldlt_t()) {
    compute(A);
  }

  inline void compute(const matrix_t& A) {
    check_square("LDLT_factor", "A", A);
    N_ = A.rows();
    ldltP_->compute(A);
  }

  inline bool success() const {
    if (ldltP_->info() != Eigen::Success) {
      return false;
    }
    if (!(ldltP_->isPositive())) {
      return false;
    }
    vector_t ldltP_diag(ldltP_->vectorD());
    for (int i = 0; i < ldltP_diag.size(); ++i) {
      if (ldltP_diag(i) <= 0 || is_nan(ldltP_diag(i))) {
        return false;
      }
    }
    return true;
  }

  inline T log_abs_det() const { return sum(log(ldltP_->vectorD())); }

  inline void inverse(matrix_t& invA) const {
    invA.setIdentity(N_);
    ldltP_->solveInPlace(invA);
  }

  template <typename Rhs>
  inline const Eigen::Solve<ldlt_t, Rhs> solve(
      const Eigen::MatrixBase<Rhs>& b) const {
    return ldltP_->solve(b);
  }

  inline matrix_t solveRight(const matrix_t& B) const {
    return ldltP_->solve(B.transpose()).transpose();
  }

  inline vector_t vectorD() const { return ldltP_->vectorD(); }

  inline ldlt_t matrixLDLT() const { return ldltP_->matrixLDLT(); }

  inline size_t rows() const { return N_; }
  inline size_t cols() const { return N_; }

  size_t N_;
  boost::shared_ptr<ldlt_t> ldltP_;
};

}  // namespace math
}  // namespace stan

#endif


#pragma once

#include <navigator_vision_lib/image_acquisition/ros_camera_stream.hpp>
#include <navigator_vision_lib/colorizer/common.hpp>
#include <boost/shared_ptr.hpp>
#include <Eigen/Core>

namespace nav{

struct alignas(16) ColorObservation
{
  using Vec = std::vector<ColorObservation>;
  using VecImg = cv::Mat_<Vec>;
  // float tstamp;   // seconds
  float xyz[3];
  uint8_t bgr[3];
};

class UnoccludedPointsImg
{
/* This class stores the indices of points on a point cloud that are not occluded
   from a given camera view. It quantizes the projection of the point cloud points
   into the image plane into pixel bins and selects the point closest to the center
   of projection to be the only visible one for a given image pixel.
 */

  ColorObservation::VecImg unouccluded_pt_idxs;
  cv::Mat_<float> distance_image;
  PCDPtr<pcl::PointXYZ> cloud_ptr;

public:
  UnoccludedPointsImg();
};

struct PointColorStats
{
  float xyz[3];
  uint8_t bgr[3];
  float var[3];
  int n;
};

}  //namespace nav


#ifndef CLIENT_HPP
#define CLIENT_HPP

#include "connection.hpp"
#include "module.hpp"
#include "sim.hpp"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

#include <random>
#include <utility>

namespace ba = boost::accumulators;

class stats;
class traffic;

/**
 * The client class.  This class takes care of reporting the stats.
 */
class client: public module<sim>
{
  // The client ID.
  int id;

  // The tear down time.
  double tdt;

  // The holding time distribution.
  std::exponential_distribution<> m_htd;

  // The number of units distribution.
  std::poisson_distribution<> m_nud;

  // The connection.
  connection conn;

  // The traffic object the client belongs to.
  traffic &tra;

  // The statistics object to which we report.
  stats &st;

public:
  client(double mht, double mnu, traffic &tra);

  // Processes the event and changes the state of the client.
  void operator()(double t);

  const connection &
  get_connection() const;

private:
  bool set_up();
  void destroy();
};

#endif /* CLIENT_HPP */


#ifndef CUAUV_CONF_VEHICLE_H
#define CUAUV_CONF_VEHICLE_H

#include <string>
#include <utility>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

namespace cuauv {
namespace conf {

struct drag_plane {
    Eigen::Vector3d x;
    Eigen::Vector3d n;
    double c;
    double a;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct thruster {
    std::string name;
    Eigen::Vector3d x;
    double yaw;
    double pitch;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct camera {
    std::string tag;
    std::string type;
    int id;
    std::string camera_name;
    int width;
    int height;
    Eigen::Vector3d position;
    Eigen::Vector3d orientation_hpr;

    // Sensor sizes and focal length are in millimeters.
    double sensor_width;
    double sensor_height;
    double focal_length;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

typedef std::vector<drag_plane, Eigen::aligned_allocator<drag_plane>> drag_plane_vector;
typedef std::vector<thruster, Eigen::aligned_allocator<thruster>> thruster_vector;
typedef std::vector<camera, Eigen::aligned_allocator<camera>> camera_vector;

// Eigen only goes up to Vector4d. Lower-cased because the rest of this code
// uses C++ standard library capitalization.
typedef Eigen::Matrix<double, 6, 1> vector6d;

struct vehicle {
    Eigen::Vector3d center_of_buoyancy;
    double buoyancy_force;
    double gravity_force;
    double sub_height;
    Eigen::Matrix3d I;
    Eigen::Vector3d Ib;
    Eigen::Quaterniond btom_rq;
    drag_plane_vector drag_planes;
    vector6d cwhe_axes;
    vector6d thruster_minimums;
    vector6d thruster_maximums;
    drag_plane_vector uncompensated_drag_planes;
    thruster_vector thrusters;
    camera_vector cameras;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * Loads a vehicle object from the TOML file at
 * ${CUAUV_SOFTWARE}/conf/${CUAUV_VEHICLE}.toml
 * @throws std::runtime_error Thrown if the file cannot be opened or parsed.
 */
vehicle load_vehicle(void);

} // namespace conf
} // namespace cuauv

#endif // CUAUV_CONF_VEHICLE_H


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_PREDECESSOR_HPP_INCLUDED

#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/bitfloating.hpp>
#include <boost/simd/function/bitinteger.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( predecessor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(is_gez(a1), "predecessor rank must be non negative");
      if (Valmin<A0>()+a1 > a0) return Valmin<A0>();
      return a0-a1;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( predecessor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return saturated_(dec)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( predecessor_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      BOOST_ASSERT_MSG(is_gez(a1), "predecessor rank must be non negative");
      if (is_nan(a0)) return a0;
      return bitfloating(saturated_(minus)(bitinteger(a0), i_t(a1)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( predecessor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (is_nan(a0)) return a0;
      return bitfloating(saturated_(dec)(bitinteger(a0)));
    }
  };
} } }

#endif


// SXC superblocks list
// Superblocks - Version 0.4.3
//
// explorer: http://sexcoin.lavajumper.com:9950/

#include <iostream>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
using namespace std;
using namespace boost;

int list_length = 100000;

int main() {
	cout << "[SXC] Sexcoin - Superblock list" << endl << endl;
	cout << "BLOCK, REWARD" << endl;
	for (int nHeight = 0; nHeight <= list_length; nHeight++) {
		random::mt19937 gen(nHeight);
		random::uniform_int_distribution<> dist(1, 100000);
		int rand = dist(gen);
		if(rand > 99990)
			cout << nHeight << ", 5000" << endl;
		else if (rand < 2001)
			cout << nHeight << ", 500" << endl;
	}
	return 0;
}