/*
 * DIPlib 3.0
 * This file contains definitions of functions for adaptive Gaussian filtering.
 *
 * (c)2018, Erik Schuitema.
 * Based on original DIPlib code: (c)1995-2014, Delft University of Technology.
 *
 * 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.
 */

#include "diplib/nonlinear.h"
#include "diplib/framework.h"
#include "diplib/generation.h"
#include "diplib/overload.h"
#include "diplib/pixel_table.h"
#include "diplib/private/constfor.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#if __GNUC__ >= 7 || __clang_major__ >= 12
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#if __GNUC__ >= 9
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <Eigen/Geometry>

namespace dip {

namespace {

// KernelTransform: transforms kernel pixel coordinates according to one or more parameter images
// The base class performs no specific transformation,
// other than simply adding kernel coordinates to the current image coordinates
class KernelTransform {
   public:
      // Virtual destructor
      virtual ~KernelTransform() = default;

      // Clone the kernel transform. This is done when creating a copy for each thread
      // to avoid unwanted sharing between threads of the members altered inside SetImageCoords().
      virtual KernelTransform* Clone() const { return new KernelTransform( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) {
         // Cast-copy
         imgCoords_.resize( imgCoords.size() );
         for( UnsignedArray::size_type ii = 0; ii < imgCoords.size(); ++ii ) {
            imgCoords_[ ii ] = static_cast< dfloat >( imgCoords[ ii ] );
         }
         // Note for writing derived classes: perform any parameter computation or other preparation here,
         // so it is done only once per input pixel.
      }

      // Transforms kernel coordinates to input image coordinates
      virtual void Transform( IntegerArray const& kernelCoords, dip::uint /*tensorIndex*/, FloatArray& transformedCoords ) const {
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] );
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 1 ] );
      }

   protected:
      FloatArray imgCoords_;
      // Makes a quick copy of a parameter image and expands its tensor if necessary
      static void ParamImageQuickCopyAndExpandTensor( Image const& paramImage, Image& expandedParamImage, dip::uint inputTensorElements ) {
         expandedParamImage = paramImage.QuickCopy();
         if( expandedParamImage.TensorElements() != inputTensorElements ) {
            expandedParamImage.ExpandSingletonTensor( inputTensorElements );
         }
      }
};

// Scaling helper class
template< dip::uint nDims >
class KernelTransformScale {
   public:
      // The `kernelScale` image tensor need not be expanded beforehand
      KernelTransformScale( Image const& kernelScale, dip::uint inputTensorElements )
            : scaleAtImgCoords_( inputTensorElements ), inputTensorElements_( inputTensorElements ) {
         // The kernel scale image must be a COL_MAJOR_MATRIX tensor image with the following tensor size:
         // Rows == input tensor size (the input image has a column vector tensor)
         // Cols == input dimensionality == kernelScaleDimensionality
         kernelScale_ = kernelScale.QuickCopy();
         dip::uint tensorRows = inputTensorElements;
         dip::uint tensorCols = kernelScale.Dimensionality();
         // See if we need to reshape the tensor
         if( kernelScale_.TensorElements() == 1 ) {
            // Singleton expansion to COL_MAJOR_MATRIX tensor
            kernelScale_.ExpandSingletonTensor( tensorRows * tensorCols );
            kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( kernelScale_.TensorColumns() == tensorCols && kernelScale_.TensorRows() == tensorRows ) {
            // Dimensions are ok, only create LUT
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( inputTensorElements == 1 && kernelScale_.TensorShape() == Tensor::Shape::COL_VECTOR && kernelScale_.TensorRows() == tensorCols ) {
            // The scale tensor is a col vector but must be a row vector
            kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( kernelScale_.TensorColumns() == 1 && kernelScale_.TensorRows() == tensorRows ) {
            // Only the row size matches: tensor contains a scalar per input tensor element -> create special LUT but leave the tensor itself unchanged
            // LUT is indexed as: [col * NUM_ROWS + row]
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable(); // LUT for column vector
            //kernelScale_.ExpandSingletonTensor(tensorRows * tensorCols);
            //kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            // Complete LUT by adding the same indices for each column
            dip::sint LUTColSize = static_cast< dip::sint >( scaleTensorLUT_.size() );
            for( dip::uint iDim = 1; iDim < nDims; ++iDim ) {
               scaleTensorLUT_.insert( scaleTensorLUT_.end(), scaleTensorLUT_.begin(), scaleTensorLUT_.begin() + LUTColSize );
            }
         } else {
            DIP_THROW( "Scale parameter image tensor has wrong size, must have " + std::to_string( inputTensorElements ) + " rows and " + std::to_string( nDims ) + " columns" );
         }
      }

      // Computes scaleAtImgCoords_.
      // Prerequisite: SetImageCoords() must have been called to populate the image coords
      void SetScaleAtImgCoords( UnsignedArray const& imgCoords ) {
         // Given a tensor with `M` rows and `N` columns, tensor element `(m,n)` has the linear
         // index given by `scaleTensorLUT_[n*M+m] = Tensor::LookUpTable()[n*M+m]`.
         Image::Pixel scalePixel = kernelScale_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < inputTensorElements_; ++iTE ) {
            // scaleTensorRowIndex = iTE : The scale tensor has a row for each input tensor element.
            for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
               // scaleTensorColIndex = iDim : The scale tensor has a column for each kernel/input dimension ( 0 for X, 1 for Y ).
               scaleAtImgCoords_[ iTE ][ iDim ] = scalePixel[ scaleTensorLUT_[ iDim * inputTensorElements_ + iTE ]].template As< dfloat >();
            }
         }
      }

   protected:
      std::vector< std::array< dfloat, nDims >> scaleAtImgCoords_; // The vector is over the input tensor elements; the array is over the kernel dimensions
      Image kernelScale_;
      dip::uint inputTensorElements_;
      std::vector< dip::sint > scaleTensorLUT_;
};

// Kernel transformation: 2D rotation
class KernelTransform2DRotation : public KernelTransform {
   public:
      // The `orientation` image tensor need not be expanded beforehand
      KernelTransform2DRotation( Image const& orientation, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( orientation, orientation_, inputTensorElements );
         csn_.resize( inputTensorElements );
         sn_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DRotation( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel dirPixel = orientation_.At( imgCoords );
         // Iterate over tensor elements
         for( dip::uint iTE = 0; iTE < orientation_.TensorElements(); ++iTE ) {
            csn_[ iTE ] = std::cos( dip::pi * 0.5 - dirPixel[ iTE ].As< dfloat >() );
            sn_[ iTE ] = std::sin( dip::pi * 0.5 - dirPixel[ iTE ].As< dfloat >() );
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * csn_[ tensorIndex ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - static_cast< dfloat >( kernelCoords[ 0 ] ) * sn_[ tensorIndex ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * csn_[ tensorIndex ];
      }

   protected:
      std::vector< dfloat > csn_, sn_;   // Length is equal to number of input tensor elements
      Image orientation_;
};

class KernelTransform2DScaledRotation : public KernelTransform2DRotation, public KernelTransformScale< 2 > {
   public:
      // The `orientation` and `kernelScale` image tensors need not be expanded beforehand
      KernelTransform2DScaledRotation( Image const& orientation, Image const& kernelScale, dip::uint inputTensorElements )
            : KernelTransform2DRotation( orientation, inputTensorElements ), KernelTransformScale< 2 >( kernelScale, inputTensorElements ) {}

      virtual KernelTransform* Clone() const override { return new KernelTransform2DScaledRotation( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DRotation::SetImageCoords( imgCoords );
         SetScaleAtImgCoords( imgCoords );
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         // First scale, then rotate
         dfloat scaledKernelCoords[ 2 ] = {
               scaleAtImgCoords_[ tensorIndex ][ 0 ] * static_cast< dfloat >( kernelCoords[ 0 ] ),
               scaleAtImgCoords_[ tensorIndex ][ 1 ] * static_cast< dfloat >( kernelCoords[ 1 ] )
         };
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + scaledKernelCoords[ 0 ] * csn_[ tensorIndex ]
                                  + scaledKernelCoords[ 1 ] * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - scaledKernelCoords[ 0 ] * sn_[ tensorIndex ]
                                  + scaledKernelCoords[ 1 ] * csn_[ tensorIndex ];
      }
};

// Kernel transformation: 3D rotation using phi3 and theta3
class KernelTransform3DRotationZ : public KernelTransform {
   public:
      // The `phi3` and `theta3` image tensors need not be expanded beforehand
      KernelTransform3DRotationZ( Image const& phi3, Image const& theta3, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( phi3, phi3_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta3, theta3_, inputTensorElements );
         R_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform3DRotationZ( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel phiPixel = phi3_.At( imgCoords );
         Image::Pixel thetaPixel = theta3_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < phi3_.TensorElements(); ++iTE ) {
            dfloat phi = phiPixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta = thetaPixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat cs_p = std::cos( phi );
            dfloat sn_p = std::sin( phi );
            dfloat cs_t = std::cos( theta );
            dfloat sn_t = std::sin( theta );

            RotMatrix& R = R_[ iTE ];
            R[ 0 ] = cs_p * cs_t;   R[ 1 ] = -sn_p;   R[ 2 ] = cs_p * sn_t;
            R[ 3 ] = sn_p * cs_t;   R[ 4 ] = cs_p;    R[ 5 ] = sn_p * sn_t;
            R[ 6 ] = -sn_t;         R[ 7 ] = 0;       R[ 8 ] = cs_t;
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         RotMatrix const& R = R_[ tensorIndex ];
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 0 ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * R[ 1 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 2 ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 3 ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * R[ 4 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 5 ];
         transformedCoords[ 2 ] = imgCoords_[ 2 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 6 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 8 ];
      }

   protected:
      Image phi3_;
      Image theta3_;
      using RotMatrix = std::array< dfloat, 9 >;
      std::vector< RotMatrix > R_;  // Rotation matrix. The vector is over the input tensor elements. // TODO: use Matrix class?
};

// Kernel transformation: 3D rotation using phi2, theta2, phi3 and theta3
class KernelTransform3DRotationXY : public KernelTransform {
   public:
      // The `phi2`, `theta2`, `phi3` and `theta3` image tensors need not be expanded beforehand
      KernelTransform3DRotationXY( Image const& phi2, Image const& theta2, Image const& phi3, Image const& theta3, dip::uint inputTensorElements ) {
         // Expand parameter images if necessary
         ParamImageQuickCopyAndExpandTensor( phi2, phi2_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta2, theta2_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( phi3, phi3_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta3, theta3_, inputTensorElements );
         T_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform3DRotationXY( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         Image::Pixel phi2Pixel = phi2_.At( imgCoords );
         Image::Pixel theta2Pixel = theta2_.At( imgCoords );
         Image::Pixel phi3Pixel = phi3_.At( imgCoords );
         Image::Pixel theta3Pixel = theta3_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < phi2_.TensorElements(); ++iTE ) {
            dfloat phi2 = phi2Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta2 = theta2Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat phi3 = phi3Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta3 = theta3Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements

            // Create transformation matrix from basis vectors
            Eigen::Vector3d xAxis( GetAxis( phi2, theta2 ));
            Eigen::Vector3d yAxis( GetAxis( phi3, theta3 ));
            Eigen::Vector3d zAxis( xAxis.cross( yAxis ).normalized() ); // TODO: do we need to normalize? Only needed if xAxis and zAxis are not perpendicular..
            // Set rotation part
            T_[ iTE ].linear() << xAxis, yAxis, zAxis;
            // Set translation part
            T_[ iTE ].translation() << Eigen::Vector3d( imgCoords_[ 0 ], imgCoords_[ 1 ], imgCoords_[ 2 ] );
         }
      }

      virtual void Transform( IntegerArray const& /*kernelCoords*/, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         Eigen::Map< Eigen::Vector3d > output( transformedCoords.data() );   // Create an Eigen map that directly accesses transformedCoords
         output = T_[ tensorIndex ] * output;
      }

   protected:
      Eigen::Vector3d GetAxis( dfloat phi, dfloat theta ) {
         dfloat sin_phi = std::sin( phi );
         dfloat cos_phi = std::cos( phi );
         dfloat sin_theta = std::sin( theta );
         dfloat cos_theta = std::cos( theta );
         return Eigen::Vector3d( sin_phi * cos_theta, sin_phi * sin_theta, cos_phi );
      }

      Image phi2_;
      Image theta2_;
      Image phi3_;
      Image theta3_;
      using CompactTransform = Eigen::Transform< dfloat, 3, Eigen::AffineCompact >;
      std::vector< CompactTransform, Eigen::aligned_allocator< CompactTransform >> T_;  // Transformation matrix. The vector is over the input tensor elements.
};

// Kernel transformation: 2D skew
class KernelTransform2DSkew : public KernelTransform {
   public:
      // The `skew` image tensor need not be expanded beforehand
      KernelTransform2DSkew( Image const& skew, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( skew, skew_, inputTensorElements );
         s_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DSkew( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel skewPixel = skew_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < skew_.TensorElements(); ++iTE ) {
            s_[ iTE ] = skewPixel[ iTE ].As< dfloat >();
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         dfloat kernelCoordX = static_cast< dfloat >( kernelCoords[ 0 ] );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX;
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 1 ] ) + s_[ tensorIndex ] * kernelCoordX;
      }

   protected:
      Image skew_;
      std::vector< dfloat > s_;  // The vector is over the input tensor elements
};

// Kernel transformation: 2D banana
class KernelTransform2DBanana : public KernelTransform2DRotation {
   public:
      // The `orientation` and `hcurvature` image tensors need not be expanded beforehand
      KernelTransform2DBanana( Image const& orientation, Image const& hcurvature, dip::uint inputTensorElements )
            : KernelTransform2DRotation( orientation, inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( hcurvature, hcurvature_, inputTensorElements );
         hcurv_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DBanana( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DRotation::SetImageCoords( imgCoords );
         Image::Pixel cPixel = hcurvature_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < orientation_.TensorElements(); ++iTE ) {
            hcurv_[ iTE ] = -0.5 * cPixel[ iTE ].As< dfloat >();
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         dfloat kernelCoordX = static_cast< dfloat> ( kernelCoords[ 0 ] );
         dfloat kernelCoordY = static_cast< dfloat >( kernelCoords[ 1 ] ) + ( hcurv_[ tensorIndex ] * kernelCoordX * kernelCoordX );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX * csn_[ tensorIndex ] + kernelCoordY * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - kernelCoordX * sn_[ tensorIndex ] + kernelCoordY * csn_[ tensorIndex ];
      }

   protected:
      Image hcurvature_;
      std::vector< dfloat > hcurv_; // The vector is over the input tensor elements
};

class KernelTransform2DScaledBanana : public KernelTransform2DBanana, public KernelTransformScale< 2 > {
   public:
      // The `orientation`, `hcurvature` and `kernelScale` image tensors need not be expanded beforehand
      KernelTransform2DScaledBanana( Image const& orientation, Image const& hcurvature, Image const& kernelScale, dip::uint inputTensorElements )
            : KernelTransform2DBanana( orientation, hcurvature, inputTensorElements ), KernelTransformScale< 2 >( kernelScale, inputTensorElements ) {}

      virtual KernelTransform* Clone() const override { return new KernelTransform2DScaledBanana( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DBanana::SetImageCoords( imgCoords );
         SetScaleAtImgCoords( imgCoords );
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         // First scale, then curve, then rotate
         dfloat kernelCoordX = scaleAtImgCoords_[ tensorIndex ][ 0 ] * static_cast< dfloat >( kernelCoords[ 0 ] );
         dfloat kernelCoordY = scaleAtImgCoords_[ tensorIndex ][ 1 ] * static_cast< dfloat >( kernelCoords[ 1 ] ) + ( hcurv_[ tensorIndex ] * kernelCoordX * kernelCoordX );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX * csn_[ tensorIndex ] + kernelCoordY * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - kernelCoordX * sn_[ tensorIndex ] + kernelCoordY * csn_[ tensorIndex ];
      }
};

// Input interpolation class
template< typename TPI, typename TPO >
class InputInterpolator {
   public:
      explicit InputInterpolator( Image const& in )
            : in_( in ), inOrigin_( static_cast< TPI* >( in_.Origin() )), inTensorStride_( in_.TensorStride() ) {}

      virtual ~InputInterpolator() = default;

      virtual TPO GetInputValue( FloatArray& /*coords*/, dip::uint /*tensorIndex*/, bool /*mirrorAtImageBoundaries*/ ) const { return 0; }

   protected:
      // Maps coords to a location inside the image using mirroring at the image boundaries.
      // Returns false if coordinates don't fall inside the image even after mirroring.
      template< dip::uint nDims >
      bool MapCoords_Mirror( dfloat* coords ) const {
         // Make sure all coordinates are within the image. Mirror at the boundaries.
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            const dfloat cMax = static_cast< dfloat >( in_.Size( iDim ) - 1 );
            dfloat& c = coords[ iDim ];
            if( c < 0 ) {
               c = -c;
               if( c > cMax ) {
                  return false; // Mirroring exceeds border
               }
            } else if( c > cMax ) {
               c = cMax - ( c - cMax );
               if( c < 0 ) {
                  return false; // Mirroring exceeds border
               }
            }
         }
         return true;
      }

      Image const& in_; // Input image
      TPI* inOrigin_;   // Input image origin
      dip::sint inTensorStride_;   // Input image tensor stride
};

// Input interpolation class, templated in the number of input dimensions
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorFixedDims : public InputInterpolator< TPI, TPO > {
   public:
      explicit InputInterpolatorFixedDims( Image const& in ) : InputInterpolator< TPI, TPO >( in ) {
         // Verify input dimensionality
         DIP_THROW_IF( in_.Dimensionality() != nDims, "Interpolation dimensionality incorrect" );
         // Cache strides and sizes
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            inStrides_[ iDim ] = in_.Stride( iDim );
            inSizes_[ iDim ] = in_.Size( iDim );
         }
      }

   protected:
      using InputInterpolator< TPI, TPO >::in_;
      std::array< dip::sint, nDims > inStrides_;
      std::array< dip::uint, nDims > inSizes_;
};

// Zero order hold input interpolator
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorZOH : public InputInterpolatorFixedDims< nDims, TPI, TPO > {
   public:
      explicit InputInterpolatorZOH( Image const& in ) : InputInterpolatorFixedDims< nDims, TPI, TPO >( in ) {}

      // Get zero-order-hold input value at floating point coordinates, arbitrary input dimensionality
      TPO GetInputValue( FloatArray& coords, dip::uint tensorIndex, bool mirrorAtImageBoundaries ) const override {
         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords )) {
               return 0;
            }
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< nDims >( &coords[ 0 ] )) {
               return 0;
            }
         }

         // Compute pixel offset
         dip::sint pixelOffset = 0;
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            dip::sint integerCoord = floor_cast( coords[ iDim ] );
            pixelOffset += integerCoord * inStrides_[ iDim ];
         }

         // Add tensor offset
         pixelOffset += static_cast< dip::sint >( tensorIndex ) * inTensorStride_;

         // Output single input value
         TPI inValue = *( inOrigin_ + pixelOffset );
         return static_cast< TPO >(inValue);
      }

      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inStrides_;
      using InputInterpolator< TPI, TPO >::inTensorStride_;
      using InputInterpolator< TPI, TPO >::inOrigin_;
      using InputInterpolator< TPI, TPO >::in_;
};

// First order hold input interpolator
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorFOH : public InputInterpolatorFixedDims< nDims, TPI, TPO > {
   public:
      explicit InputInterpolatorFOH( Image const& in ) : InputInterpolatorFixedDims< nDims, TPI, TPO >( in ) {}

      // Get linearly interpolated input value at floating point coordinates, arbitrary input dimensionality
      TPO GetInputValue( FloatArray& coords, dip::uint tensorIndex, bool mirrorAtImageBoundaries ) const override {
         using InputAsFloat = FloatType< TPI >;

         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords )) {
               return 0;
            }
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< nDims >( &coords[ 0 ] )) {
               return 0;
            }
         }

         // Start linear interpolation
         // Compute lower bound index and interpolation factor for all coordinates
         //  E.g.: if coords[i] == 1.1, then loBoundIndices[i] == 1.0 and factors[i] == 0.1
         std::array< dip::sint, nDims > loBoundIndices;
         std::array< InputAsFloat, nDims > factors; // Linear interpolation factors
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            loBoundIndices[ iDim ] = floor_cast( coords[ iDim ] );   // Take lower bound using floor_cast
            if( loBoundIndices[ iDim ] == static_cast< dip::sint >( inSizes_[ iDim ] - 1 )) { // Because we interpolate between loBound and loBound+1, make sure we don't go beyond the image borders
               loBoundIndices[ iDim ]--;
            }
            factors[ iDim ] = static_cast< InputAsFloat >( coords[ iDim ] ) - static_cast< InputAsFloat >( loBoundIndices[ iDim ] );
         }

         // Compute pixel offset for first pixel of the interpolation input window, including tensor offset
         dip::sint interpOriginOffset = 0;
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            interpOriginOffset += loBoundIndices[ iDim ] * inStrides_[ iDim ];
         }

         // Compute pixel offsets and composite interpolation factors for a total of 2 ^ nDims pixels
         // We can avoid an ND-loop by exploiting the fact that only 2 coordinates are processed per dimension.
         constexpr dip::uint numInterpPixels = dip::uint( 1 ) << nDims;

         TPO result = 0.0;
         //for( dip::uint iOffset = 0; iOffset < numInterpPixels; ++iOffset ) {
         const_for< numInterpPixels >(
               [ & ]( dip::uint iOffset ) {
                  dip::sint pixelOffset = interpOriginOffset;
                  InputAsFloat pixelFactor = 1.0;
                  //for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
                  const_for< nDims >(
                        [ & ]( dip::uint iDim ) {
                           // To process all permutations of lowerbound and upperbound (= lowerbound + 1) in each dimension,
                           // decide whether to process the upperbound in this dimension by testing the bits in iOffset
                           bool useUpperBound = ( iOffset & ( dip::uint( 1 ) << iDim )) != 0;
                           if( useUpperBound ) {
                              pixelOffset += inStrides_[ iDim ];
                              pixelFactor *= factors[ iDim ];
                           } else {
                              pixelFactor *= 1 - factors[ iDim ];
                           }
                        } );
                  // Add tensor offset
                  pixelOffset += static_cast< dip::sint >( tensorIndex ) * inTensorStride_;
                  // Multiply pixel input values with composite interpolation factors
                  TPI inValue = *( inOrigin_ + pixelOffset );
                  result += static_cast< TPO >(inValue) * pixelFactor;
               } );

         return result;
      }

      /*
      // GetInputValue_LinearInterpolation() 2D reference implementation for speed comparison
      TPO GetInputValue_LinearInterpolation_2D( FloatArray& coords, bool mirrorAtImageBoundaries ) const {
         using InputAsFloat = FloatType< TPI >;
         assert( in_.Dimensionality() == nDims );

         // Cache strides
         std::array< dip::sint, 2 > strides = { in_.Stride( 0 ), in_.Stride( 1 ) };

         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords ) )
               return 0;
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< 2 >( &coords[ 0 ] ) )
               return 0;
         }

         // Start linear interpolation
         // Compute lower bound index and interpolation factor for all coordinates
         //  E.g.: if coords[i] == 1.1, then loBoundIndices[i] == 1.0 and factors[i] == 0.1
         std::array< dip::uint, 2> loBoundIndices;
         for( dip::uint iDim = 0; iDim < 2; ++iDim ) {
            loBoundIndices[ iDim ] = (floor_cast( coords[ iDim ] ));   // Take lower bound using floor_cast
            if( coords[ iDim ] == in_.Size( iDim ) - 1 )  // Because we interpolate between loBound and loBound+1, make sure we don't go beyond the image borders
               loBoundIndices[ iDim ]--;
         }

         // Compute pixel offset of p00
         dip::sint offset = loBoundIndices[ 0 ] * strides[ 0 ] + loBoundIndices[ 1 ] * strides[ 1 ];

         TPI* origin = static_cast<TPI*>(in_.Origin());
         TPO p00 = static_cast<TPO>(*(origin + offset));
         TPO p01 = static_cast<TPO>(*(origin + offset + strides[ 1 ]));
         TPO p10 = static_cast<TPO>(*(origin + offset + strides[ 0 ]));
         TPO p11 = static_cast<TPO>(*(origin + offset + strides[ 0 ] + strides[ 1 ]));
         InputAsFloat xo = coords[ 0 ] - loBoundIndices[ 0 ];
         InputAsFloat yo = coords[ 1 ] - loBoundIndices[ 1 ];
         InputAsFloat xs = 1 - xo;
         InputAsFloat ys = 1 - yo;

         return (TPO)((p00 * xs * ys)
            + (p10 * xo * ys)
            + (p01 * xs * yo)
            + (p11 * xo * yo));
      }
      */

      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inSizes_;
      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inStrides_;
      using InputInterpolator< TPI, TPO >::inTensorStride_;
      using InputInterpolator< TPI, TPO >::inOrigin_;
      using InputInterpolator< TPI, TPO >::in_;
};

// The adaptive window convolution filter for adaptive gauss and its variants
template< typename TPI, typename TPO = FlexType< TPI >>
class AdaptiveWindowConvolutionLineFilter : public Framework::FullLineFilter {
   public:
      AdaptiveWindowConvolutionLineFilter( Image const& in, Kernel const& kernel, ImageArray const& params, String const& interpolation, BoundaryCondition bc, String const& transform )
            : in_( in ), kernel_( kernel ) {
         // Determine kernel transformation
         if( in_.Dimensionality() == 2 ) {
            // === 2D ===
            // Construct input interpolator
            ConstructInputInterpolator< 2 >( in, interpolation );

            // Construct kernel transformation
            ConstructKernelTransform2D( transform, params, in.TensorElements() );

         } else if( in_.Dimensionality() == 3 ) {
            // === 3D ===
            // Construct input interpolator
            ConstructInputInterpolator< 3 >( in, interpolation );

            // Determine kernel transformation
            ConstructKernelTransform3D( transform, params, in.TensorElements() );
         } else {
            DIP_THROW( "No transform \"" + transform + "\" known for input dimensionality " + std::to_string( in_.Dimensionality() ));
         }

         // Store boundary condition. We only support mirroring or zeros for now.
         DIP_THROW_IF( bc != BoundaryCondition::SYMMETRIC_MIRROR && bc != BoundaryCondition::ADD_ZEROS, "Unsupported boundary condition" );
         mirrorAtInputBoundaries_ = ( bc == BoundaryCondition::SYMMETRIC_MIRROR );
      }

      virtual void SetNumberOfThreads( dip::uint numThreads, PixelTableOffsets const& pixelTable ) override {
         offsets_ = pixelTable.Offsets();
         kernelTransforms_.resize( numThreads - 1 );  // kernelTransform_ is used for thread 0
      }

      virtual void Filter( Framework::FullLineFilterParameters const& params ) override {
         TPI* in = static_cast< TPI* >( params.inBuffer.buffer );
         dip::sint inStride = params.inBuffer.stride;
         TPO* out = static_cast< TPO* >( params.outBuffer.buffer );
         dip::sint outStride = params.outBuffer.stride;
         dip::sint outTensorStride = params.outBuffer.tensorStride;
         dip::uint length = params.bufferLength;
         PixelTableOffsets const& pixelTableOffsets = params.pixelTable;
         std::vector< dfloat > const& weights = pixelTableOffsets.Weights();
         UnsignedArray inCoords( params.position );
         PixelTable pixelTable = kernel_.PixelTable( in_.Dimensionality(), params.dimension );  // TODO: move to constructor
         FloatArray transformedKernelCoords( in_.Dimensionality() );

         // Obtain kernel transform for this thread
         std::unique_ptr< KernelTransform >& kernelTransform = params.thread == 0 ? kernelTransform_ : kernelTransforms_[ params.thread - 1 ];
         // Clone kernelTransform_ for this thread if not done already.
         // Needed because the KernelTransform computes and stores values per input pixel and uses those repeatedly for each kernel element.
         if( !kernelTransform ) {
            kernelTransform.reset( kernelTransform_->Clone() );
         }

         for( dip::uint ii = 0; ii < length; ++ii ) {
            for( dip::uint iTE = 0; iTE < in_.TensorElements(); ++iTE ) {
               *( out + static_cast< dip::sint >( iTE ) * outTensorStride ) = 0;
            }
            auto itWeight = weights.begin();
            // Prepare kernel transform for current input coordinates
            kernelTransform->SetImageCoords( inCoords );
            // Apply kernel
            for( PixelTable::iterator itPT = pixelTable.begin(); itPT != pixelTable.end(); ++itPT ) {
               for( dip::uint iTE = 0; iTE < in_.TensorElements(); ++iTE ) {
                  // Apply kernel transformation
                  kernelTransform->Transform( *itPT, iTE, transformedKernelCoords );
                  // Obtain input value at the transformed kernel coords
                  *( out + static_cast< dip::sint >( iTE ) * outTensorStride ) +=
                        inputInterpolator_->GetInputValue( transformedKernelCoords, iTE, mirrorAtInputBoundaries_ ) * static_cast< FloatType< TPO >>( *itWeight );
               }
               ++itWeight;
            }
            // Prepare next buffer element
            inCoords[ params.dimension ]++;
            in += inStride;
            out += outStride;
         }
      }

   private:
      template< dip::uint nDims >
      void ConstructInputInterpolator( Image const& in, String const& interpolation ) {
         // Determine input interpolator
         if( interpolation == S::ZERO_ORDER ) {
            inputInterpolator_ = std::make_unique< InputInterpolatorZOH< nDims, TPI, TPO >>( in );
         } else if( interpolation == S::LINEAR ) {
            inputInterpolator_ = std::make_unique< InputInterpolatorFOH< nDims, TPI, TPO >>( in );
         } else {
            DIP_THROW( "Unknown interpolation \"" + interpolation + "\"" );
         }
      }

      void ConstructKernelTransform2D( String const& transform, ImageArray const& params, dip::uint inputTensorElements ) {
         // Determine kernel transformation
         if( transform == "none" ) {
            kernelTransform_ = std::make_unique< KernelTransform >();
         } else if( transform == "ellipse" ) {
            if( params.size() == 1 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DRotation >( params[ 0 ], inputTensorElements );
            } else if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DScaledRotation >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else {
               DIP_THROW( E::ARRAY_PARAMETER_WRONG_LENGTH );
            }
         } else if( transform == "banana" ) {
            if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DBanana >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else if( params.size() == 3 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DScaledBanana >( params[ 0 ], params[ 1 ], params[ 2 ], inputTensorElements );
            } else {
               DIP_THROW( E::ARRAY_PARAMETER_WRONG_LENGTH );
            }
         } else if( transform == "skew" ) {
            DIP_THROW_IF( params.size() != 1, E::ARRAY_PARAMETER_WRONG_LENGTH );
            kernelTransform_ = std::make_unique< KernelTransform2DSkew >( params[ 0 ], inputTensorElements );
         } else {
            DIP_THROW( "Unknown 2D transform \"" + transform + "\"" );
         }
      }

      void ConstructKernelTransform3D( String const& transform, ImageArray const& params, dip::uint inputTensorElements ) {
         if( transform == "none" ) {
            kernelTransform_ = std::make_unique< KernelTransform >();
         } else if( transform == "ellipse" ) {
            if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform3DRotationZ >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else if( params.size() == 4 ) {
               kernelTransform_ = std::make_unique< KernelTransform3DRotationXY >( params[ 0 ], params[ 1 ], params[ 2 ], params[ 3 ], inputTensorElements );
            }
         } else {
            DIP_THROW( "Unknown 3D transform \"" + transform + "\"" );
         }
      }

      std::vector< dip::sint > offsets_;  // Pixel table offsets
      Image const& in_; // Input image
      Kernel const& kernel_;  // Kernel
      std::unique_ptr< KernelTransform > kernelTransform_;  // Kernel transform for thread 0
      std::vector< std::unique_ptr< KernelTransform >> kernelTransforms_; // Kernel transforms for remaining threads
      std::unique_ptr< InputInterpolator< TPI, TPO >> inputInterpolator_; // Input interpolator
      bool mirrorAtInputBoundaries_;   // Boundary condition: either mirror or zeros
};

} // namespace

void AdaptiveFilter(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition,
      String const& transform
) {
   DIP_THROW_IF( !in.IsForged(), E::IMAGE_NOT_FORGED );

   // Prepare parameter images: expand singleton dimensions, including the tensor
   ImageArray paramImages( params.size() );
   for( dip::uint iP = 0; iP < params.size(); ++iP ) {
      paramImages[ iP ] = params[ iP ].get().QuickCopy();
      paramImages[ iP ].ExpandSingletonDimensions( in.Sizes() );
      // The param image's tensor is expanded while constructing the kernel transformation
      // TODO: ExpandSingletonDimensions() could also be done while constructing the kernel transformation, so this loop is removed altogether
   }
   DIP_STACK_TRACE_THIS( ArrayUseParameter( sigmas, in.Dimensionality(), 1.0 ));

   DIP_START_STACK_TRACE
      // Create gaussian kernel
      Kernel kernel{ CreateGauss( sigmas, orders, truncation, exponents ) };

      BoundaryCondition bc = StringToBoundaryCondition( boundaryCondition );
      DataType outputType = DataType::SuggestFlex( in.DataType() );
      std::unique_ptr< Framework::FullLineFilter > lineFilter;
      DIP_OVL_NEW_ALL( lineFilter, AdaptiveWindowConvolutionLineFilter, ( in, kernel, paramImages, interpolationMethod, bc, transform ), in.DataType() );
      // We use the full framework to allow multi-threading. Its parameters prevent input or output buffering to minimize overhead. Border expansion is not used either.
      Framework::Full( in, out, in.DataType(), outputType, outputType, in.TensorElements(), { bc }, kernel, *lineFilter, Framework::FullOption::BorderAlreadyExpanded );// for performance comparisons: +Framework::FullOption::NoMultiThreading );

   DIP_END_STACK_TRACE
}

void AdaptiveGauss(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray const& sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition
) {
   AdaptiveFilter( in, params, out, sigmas, orders, truncation, exponents, interpolationMethod, boundaryCondition, "ellipse" );
}

void AdaptiveBanana(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray const& sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition
) {
   AdaptiveFilter( in, params, out, sigmas, orders, truncation, exponents, interpolationMethod, boundaryCondition, "banana" );
}

} // namespace dip

#if defined(__GNUG__) || defined(__clang__)
#pragma GCC diagnostic pop
#endif


#ifdef MEX

#include <igl/copyleft/cgal/mesh_boolean.h>
#include <igl/copyleft/cgal/remesh_self_intersections.h>
#include <igl/copyleft/cgal/mesh_boolean_type_to_funcs.h>
#include <igl/copyleft/cgal/BinaryWindingNumberOperations.h>
#include <igl/remove_unreferenced.h>
#ifndef IGL_NO_CORK
#  include <igl/copyleft/cork/mesh_boolean.h>
#endif
#include <igl/copyleft/cgal/string_to_mesh_boolean_type.h>

#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/validate_arg.h>
#include <igl/C_STR.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

#include <cstring>

enum BooleanLibType
{
  BOOLEAN_LIB_TYPE_LIBIGL = 0,
  BOOLEAN_LIB_TYPE_CORK = 1,
  NUM_BOOLEAN_LIB_TYPES = 2
};

void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  std::vector<Eigen::MatrixXd> & Vlist,
  std::vector<Eigen::MatrixXi> & Flist,
  igl::MeshBooleanType & type,
  std::function<int(const Eigen::Matrix<int,1,Eigen::Dynamic>) > & wind_func,
  std::function<int(const int, const int)> & keep_func,
  BooleanLibType & boolean_lib,
  bool & debug
  )
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace igl::copyleft::cgal;
  using namespace Eigen;
  mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");

  const auto & parse_mesh = [](
    const mxArray *pV,
    const mxArray *pF,
    Eigen::MatrixXd & V,
    Eigen::MatrixXi & F)
  {
    const int dim = mxGetN(pV);
    mexErrMsgTxt(dim==0 || dim == 3,
      "Mesh vertex list must be #V by 3 list of vertex positions");
    mexErrMsgTxt(mxGetN(pF)==0 || dim == mxGetN(pF),
      "Mesh \"face\" simplex size must equal dimension");
    parse_rhs_double(&pV,V);
    parse_rhs_index(&pF,F);
  };

  int i = 0;
  while(!mxIsChar(prhs[i]))
  {
    if(mxIsCell(prhs[i]))
    {
      mexErrMsgTxt(
        mxIsCell(prhs[i+1]),
        "Cell input for vertices requires cell input for faces, too");
      const int k = mxGetNumberOfElements(prhs[i]);
      mexErrMsgTxt(
        k == mxGetNumberOfElements(prhs[i+1]),
        "Face cell input must be same length");
      for(int j = 0;j<k;j++)
      {
        Vlist.emplace_back();
        Flist.emplace_back();
        parse_mesh(
          mxGetCell(prhs[i],j),
          mxGetCell(prhs[i+1],j),
          Vlist[Vlist.size()-1],Flist[Flist.size()-1]);
      }
    }else if(mxIsNumeric(prhs[i]))
    {
      Vlist.emplace_back();
      Flist.emplace_back();
      parse_mesh(prhs[i],prhs[i+1],Vlist[Vlist.size()-1],Flist[Flist.size()-1]);
    }else
    {
      mexErrMsgTxt(false,"Mesh inputs must be numeric");
    }
    i+=2;
  }

  mexErrMsgTxt(mxIsChar(prhs[i]), C_STR("Type should be char"));
  std::string type_str = std::string(mxArrayToString(prhs[i]));
  if(type_str != "")
  {
    mexErrMsgTxt(string_to_mesh_boolean_type(type_str,type),
      C_STR(type_str << " is not a known boolean operation"));
    igl::copyleft::cgal::mesh_boolean_type_to_funcs(type,wind_func,keep_func);
  }
  i++;

  {
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("BooleanLib",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        const char * type_name = mxArrayToString(prhs[++i]);
        if(strcmp("libigl",type_name)==0)
        {
          boolean_lib = BOOLEAN_LIB_TYPE_LIBIGL;
#ifndef IGL_NO_CORK
        }else if(strcmp("cork",type_name)==0)
        {
          mexErrMsgTxt( type_str != "",
            "Cork does not support arbitrary extraction functions");
          boolean_lib = BOOLEAN_LIB_TYPE_CORK;
#endif
        }else
        {
          mexErrMsgTxt(false,C_STR("Unknown BooleanLib: "<<type_name));
        }
      }else if(strcmp("WindingNumberFilter",name) == 0)
      {
        validate_arg_function_handle(i,nrhs,prhs,name);
        mxArray * handle = const_cast<mxArray*>(prhs[++i]);
        wind_func = [handle](const Eigen::RowVectorXi & w)->int
        {
          int fnlhs = 1;
          mxArray *fplhs[1];
          int fnrhs = 2;
          mxArray *fprhs[2];
          // same as preparing lhs
          fprhs[0] = handle;
          igl::matlab::prepare_lhs_double(w,fprhs+1);
          mexCallMATLAB(fnlhs, fplhs, fnrhs, fprhs,"feval");
          // This might be slow
          int res = 0;
          if( mxIsNumeric(fplhs[0]) )
          {
            res = (int)(mxGetScalar(fplhs[0]));
          }else if( mxIsLogical(fplhs[0]))
          {
            res = (bool)*mxGetLogicals(fplhs[0]);
          }else{
            mexErrMsgTxt(
              false, "WindingNumberFilter should return number or logical");
          }
          // clean up
          //mxDestroyArray(fprhs[0]);
          mxDestroyArray(fprhs[1]);
          mxDestroyArray(fplhs[0]);
          return res;
        };
      }else if(strcmp("Debug",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        debug = (bool)*mxGetLogicals(prhs[++i]);
      }else
      {
        mexErrMsgTxt(false,"Unknown parameter");
      }
      i++;
    }
  }
  mexErrMsgTxt(type_str != "" || wind_func,
    "If no type provided, then must provided extraction function");
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  std::vector<MatrixXd> Vlist;
  std::vector<MatrixXi> Flist;
  MatrixXd VC;
  MatrixXi FC;
  VectorXi J;
  MeshBooleanType type;
  std::function<int(const Eigen::Matrix<int,1,Eigen::Dynamic>) > wind_func;
  std::function<int(const int, const int)> keep_func =
    igl::copyleft::cgal::KeepInside();

  BooleanLibType boolean_lib = BOOLEAN_LIB_TYPE_LIBIGL;
  bool debug = false;
  parse_rhs(
    nrhs,
    prhs,
    Vlist,
    Flist,
    type,
    wind_func,
    keep_func,
    boolean_lib,debug);
  if(debug)
  {
    cout<<"parsed input."<<endl;
  }
  switch(boolean_lib)
  {
    case BOOLEAN_LIB_TYPE_LIBIGL:
      igl::copyleft::cgal::mesh_boolean(
        Vlist,Flist,wind_func,keep_func,VC,FC,J);
      break;
#ifndef IGL_NO_CORK
    case BOOLEAN_LIB_TYPE_CORK:
    {
      mexErrMsgTxt(Vlist.size() == 2 && Flist.size() == 2,
        "Must provide exactly two meshes for cork.");
      assert(Vlist.size() == 2);
      assert(Flist.size() == 2);
      igl::copyleft::cork::mesh_boolean(
        Vlist[0],Flist[0],
        Vlist[1],Flist[1],
        type,VC,FC);
      break;
    }
#endif
    default:
      mexErrMsgTxt(false,"Unknown boolean lib.");
      break;
  }
  if(debug)
  {
    cout<<"Computed boolean."<<endl;
  }
  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 3:
    {
      prepare_lhs_index(J,plhs+2);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_index(FC,plhs+1);
      // Fall through
    }
    case 1:
    {
      prepare_lhs_double(VC,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}

#endif


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is 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.
// __END_LICENSE__

/// \file point2las.cc
///

#include <fstream>
#include <iostream>
#include <string>
#include <boost/program_options.hpp>
#include <liblas/liblas.hpp>

#include <asp/Core/Macros.h>
#include <asp/Core/Common.h>
#include <asp/Core/PointUtils.h>

#include <vw/Cartography/PointImageManipulation.h>
#include <vw/Core/Stopwatch.h>
#include <vw/Math/Statistics.h>

using namespace vw;
namespace po = boost::program_options;

// A class to collect some positive errors, and return the error at
// given percentile multiplied by given factor.
class PercentileErrorAccum : public ReturnFixedType<void> {
  typedef double accum_type;
  std::vector<accum_type> m_vals;
public:
  typedef accum_type value_type;

  PercentileErrorAccum() { m_vals.clear(); }

  void operator()( accum_type const& value ) {
    // Don't add zero errors, those most likely came from invalid points
    if (value > 0)
      m_vals.push_back(value);
  }

  int size(){
    return m_vals.size();
  }

  value_type value(Vector2 const& outlier_removal_params, bool use_tukey_outlier_removal){

    // Care here with empty sets
    if (m_vals.empty()) {
      vw_out() << "Found no positive triangulation errors in the sample.\n";
      return 0.0;
    }

    std::sort(m_vals.begin(), m_vals.end());

    int len = m_vals.size();

    vw_out() << "Collected a sample of " << len << " positive triangulation errors.\n";

    double mean = vw::math::mean(m_vals);

    vw_out() << "For this sample: "
             << "min = "     << m_vals.front()
             << ", mean = "  << mean
             << ", stdev = " << vw::math::standard_deviation(m_vals, mean)
             << ", max = " << m_vals.back() << "." << std::endl;

    int i25 = round((len - 1) * 0.25);
    int i50 = round((len - 1) * 0.50);
    int i75 = round((len - 1) * 0.75);

    double Q1 = m_vals[i25];
    double Q2 = m_vals[i50];
    double Q3 = m_vals[i75];
    vw_out() << "Error percentiles: "
             << "Q1 (25%): " << Q1 << ", "
             << "Q2 (50%): " << Q2 << ", "
             << "Q3 (75%): " << Q3 << "."
             << std::endl;

    if (use_tukey_outlier_removal) {
      vw_out() << "Using as outlier cutoff the Tukey formula Q3 + 1.5*(Q3 - Q1)." << std::endl;
      return Q3 + 1.5*(Q3 - Q1);
    }

    double pct    = outlier_removal_params[0]/100.0; // e.g., 0.75
    double factor = outlier_removal_params[1];
    int k         = (int)round((len - 1) * pct);

    vw_out() << "Using as outlier cutoff the " << outlier_removal_params[0] << " percentile times "
             << factor << "." << std::endl;

    return m_vals[k] * factor;
  }

};

struct Options : vw::cartography::GdalWriteOptions {
  // Input
  std::string reference_spheroid, datum;
  std::string pointcloud_file;
  std::string target_srs_string;
  bool        compressed, use_tukey_outlier_removal;
  Vector2     outlier_removal_params;
  double      max_valid_triangulation_error;
  int         num_samples;

  // Output
  std::string out_prefix;
  Options() : compressed(false), max_valid_triangulation_error(0.0), num_samples(0) {}
};

void handle_arguments( int argc, char *argv[], Options& opt ) {

  po::options_description general_options("General Options");
  general_options.add_options()
    ("compressed,c", po::bool_switch(&opt.compressed)->default_value(false)->implicit_value(true),
     "Compress using laszip.")
    ("output-prefix,o", po::value(&opt.out_prefix), "Specify the output prefix.")
    ("datum", po::value(&opt.datum),
          "Create a geo-referenced LAS file in respect to this datum. Options: WGS_1984, D_MOON (1,737,400 meters), D_MARS (3,396,190 meters), MOLA (3,396,000 meters), NAD83, WGS72, and NAD27. Also accepted: Earth (=WGS_1984), Mars (=D_MARS), Moon (=D_MOON).")
    ("reference-spheroid,r", po::value(&opt.reference_spheroid),
     "This is identical to the datum option.")
    ("t_srs", po::value(&opt.target_srs_string)->default_value(""),
     "Specify a custom projection (PROJ.4 string).")
    ("remove-outliers-params", po::value(&opt.outlier_removal_params)->default_value(Vector2(75.0, 3.0), "pct factor"),
     "Outlier removal based on percentage. Points with triangulation error larger than pct-th percentile times factor will be removed as outliers. [default: pct=75.0, factor=3.0]")
    ("use-tukey-outlier-removal", po::bool_switch(&opt.use_tukey_outlier_removal)->default_value(false)->implicit_value(true),
     "Remove outliers above Q3 + 1.5*(Q3 - Q1). Takes precedence over the above approach.")

    ("max-valid-triangulation-error", po::value(&opt.max_valid_triangulation_error)->default_value(0.0),
     "Outlier removal based on threshold. Points with triangulation error larger than this, if positive (measured in meters) will be removed from the cloud. Takes precedence over the above methods.")
    ("num-samples-for-outlier-estimation", po::value(&opt.num_samples)->default_value(1000000),
     "Approximate number of samples to pick from the input cloud to find the outlier cutoff based on triangulation error.");

  general_options.add( vw::cartography::GdalWriteOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("input-file", po::value(&opt.pointcloud_file), "Input Point Cloud");

  po::positional_options_description positional_desc;
  positional_desc.add("input-file", 1);

  std::string usage("[options] <point-cloud>");
  bool allow_unregistered = false;
  std::vector<std::string> unregistered;
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options, general_options,
                             positional, positional_desc, usage,
                             allow_unregistered, unregistered );

  if ( opt.pointcloud_file.empty() )
    vw_throw( ArgumentErr() << "Missing point cloud.\n"
              << usage << general_options );

  if ( opt.out_prefix.empty() )
    opt.out_prefix =
      vw::prefix_from_filename( opt.pointcloud_file );

  // reference_spheroid and datum are aliases.
  boost::to_lower(opt.reference_spheroid);
  boost::to_lower(opt.datum);
  if (opt.datum != "" && opt.reference_spheroid != "")
    vw_throw( ArgumentErr() << "Both --datum and --reference-spheroid were specified.\n");
  if (opt.datum == "")
    opt.datum = opt.reference_spheroid;

  double pct = opt.outlier_removal_params[0], factor = opt.outlier_removal_params[1];
  if (pct <= 0.0 || pct > 100.0 || factor <= 0.0){
    vw_throw( ArgumentErr() << "Invalid values were provided for outlier removal parameters.\n");
  }

  if (opt.max_valid_triangulation_error < 0.0)
    vw_throw( ArgumentErr() << "The maximum valid triangulation error must be non-negative.\n");

  if (opt.num_samples <= 0)
    vw_throw( ArgumentErr() << "Must pick a positive number of samples.\n");

  // Create the output directory
  vw::create_out_dir(opt.out_prefix);

  // Turn on logging to file
  asp::log_to_file(argc, argv, "", opt.out_prefix);
}

void find_error_image_and_do_stats(Options& opt, ImageViewRef<double> & error_image) {

  std::vector<std::string> pointcloud_files;
  pointcloud_files.push_back(opt.pointcloud_file);
  error_image = asp::point_cloud_error_image(pointcloud_files);

  if (error_image.rows() == 0 || error_image.cols() == 0) {
    vw_out() << "The point cloud files must have an equal number of channels which "
             << "must be 4 or 6 to be able to remove outliers.\n";
    opt.max_valid_triangulation_error = 0.0;
    return;
  }

  if (opt.max_valid_triangulation_error > 0.0) {
    vw_out() << "Using the set maximum valid triangulation error as outlier cutoff: "
             << opt.max_valid_triangulation_error << "." << std::endl;
    return;
  }

  vw_out() << "Estimating the maximum valid triangulation error (outlier cutoff).\n";

  int num_err_cols = error_image.cols();
  int num_err_rows = error_image.rows();

  double area = std::max(num_err_cols * num_err_rows, 1);
  int sample_rate = round(sqrt(double(area) / double(opt.num_samples)));
  if (sample_rate < 1)
    sample_rate = 1;

  Stopwatch sw;
  sw.start();
  PixelAccumulator<PercentileErrorAccum> error_accum;
  for_each_pixel(subsample(error_image, sample_rate),
                 error_accum,
                 TerminalProgressCallback
                 ("asp","Error estim : ") );

  opt.max_valid_triangulation_error = error_accum.value(opt.outlier_removal_params,
                                                        opt.use_tukey_outlier_removal);

  sw.stop();
  vw_out(DebugMessage, "asp") << "Elapsed time: " << sw.elapsed_seconds() << std::endl;
  vw_out() << "Found the maximum valid triangulation error (outlier cutoff): "
           << opt.max_valid_triangulation_error << "." << std::endl;
}

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

  // TODO(oalexan1): need to understand what is the optimal strategy
  // for traversing the input point cloud file to minimize the reading
  // time.

  Options opt;
  try {
    handle_arguments(argc, argv, opt);

    ImageViewRef<double> error_image;
    if (opt.outlier_removal_params[0] < 100.0 || opt.max_valid_triangulation_error > 0.0)
      find_error_image_and_do_stats(opt, error_image);

    // Save the las file in respect to a reference spheroid if provided
    // by the user.
    liblas::Header header;
    cartography::Datum datum;
    bool has_user_datum = asp::read_user_datum(0, 0, opt.datum, datum);

    cartography::GeoReference georef;
    bool has_georef = vw::cartography::read_georeference(georef, opt.pointcloud_file);
    if (has_georef && opt.target_srs_string.empty()) {
      opt.target_srs_string = georef.overall_proj4_str();
    }

    bool is_geodetic = false;
    if (has_user_datum || !opt.target_srs_string.empty()){

      // Set the srs string into georef.
      asp::set_srs_string(opt.target_srs_string,
                          has_user_datum, datum, georef);
      liblas::SpatialReference ref;
      std::string target_srs = georef.overall_proj4_str();

      ref.SetFromUserInput(target_srs);
      vw_out() << "Using projection string: '" << target_srs << "'"<< std::endl;
      header.SetSRS(ref);

      is_geodetic = true;
      datum = georef.datum();
    }

    // Save the las file with given georeference, if present
    ImageViewRef<Vector3> point_image = asp::read_asp_point_cloud<3>(opt.pointcloud_file);
    if (is_geodetic) {
      point_image = cartesian_to_geodetic(point_image, datum);
      double avg_lon = asp::find_avg_lon(point_image); // see if to use [-180, 180] or [0, 360]
      point_image = geodetic_to_point(asp::recenter_longitude(point_image, avg_lon), georef);
    }

    BBox3 cloud_bbox = asp::pointcloud_bbox(point_image, is_geodetic);

    // The las format stores the values as 32 bit integers. So, for a
    // given point, we store round((point-offset)/scale), as well as
    // the offset and scale values. Here we decide the values for
    // offset and scale to lose minimum amount of precision. We make
    // the scale almost as large as it can be without causing integer overflow.
    Vector3 offset = (cloud_bbox.min() + cloud_bbox.max())/2.0;
    double  maxInt = std::numeric_limits<int32>::max();
            maxInt *= 0.95; // Just in case stay a bit away
    Vector3 scale  = cloud_bbox.size()/(2.0*maxInt);
    for (size_t i = 0; i < scale.size(); i++){
      if (scale[i] <= 0.0) scale[i] = 1.0e-16; // avoid degeneracy
    }

    // The line below causes trouble with compression in libLAS-1.7.0.
    //header.SetDataFormatId(liblas::ePointFormat1);
    header.SetScale (scale [0], scale [1], scale [2]);
    header.SetOffset(offset[0], offset[1], offset[2]);

    // Populate the min and max fields of the LAS header
    header.SetMax(cloud_bbox.max().x(),cloud_bbox.max().y(),cloud_bbox.max().z());
    header.SetMin(cloud_bbox.min().x(),cloud_bbox.min().y(),cloud_bbox.min().z());

    std::string lasFile;
    header.SetCompressed(opt.compressed);
    if (opt.compressed)
      lasFile = opt.out_prefix + ".laz";
    else
      lasFile = opt.out_prefix + ".las";

    vw_out() << "Writing LAS file: " << lasFile + "\n";
    std::ofstream ofs;
    ofs.open(lasFile.c_str(), std::ios::out | std::ios::binary);
    liblas::Writer writer(ofs, header);

    TerminalProgressCallback tpc("asp", "\t--> ");
    long long int num_total_points = 0;
    long long int num_kept_points = 0;

    for (int row = 0; row < point_image.rows(); row++){
      tpc.report_fractional_progress(row, point_image.rows());
      for (int col = 0; col < point_image.cols(); col++){

        Vector3 point = point_image(col, row);

        // Skip no-data points
        bool is_good = ( (!is_geodetic && point != vw::Vector3()) ||
                         (is_geodetic  && !boost::math::isnan(point.z())) );
        if (!is_good) continue;

        num_total_points++;

        if (opt.max_valid_triangulation_error > 0.0 &&
            error_image(col, row) > opt.max_valid_triangulation_error)
          continue;

        num_kept_points++;
#if 0
        // For comparison later with las2txt.
        std::cout.precision(16);
        std::cout << "\npoint " << point[0] << ' ' << point[1] << ' '
                  << point[2] << std::endl;
#endif

        liblas::Point las_point(&header);
        las_point.SetCoordinates(point[0], point[1], point[2]);
        writer.WritePoint(las_point);

      }
    }
    tpc.report_finished();

    vw_out () << "Saved: " << num_kept_points << " points." << std::endl;

    if (opt.max_valid_triangulation_error > 0.0) {
      long long int num_excluded = num_total_points - num_kept_points;
      double percent = 100.0 * double(num_excluded)/num_total_points;
      percent = round(percent * 100.0)/100.0; // don't keep too many digits
      vw_out() << "Excluded based on triangulation error " << num_excluded << " points ("
               << percent << "%)." << std::endl;
    }

  } ASP_STANDARD_CATCHES;

  return 0;
}


#include <tiny.h>
#include <geometry.h>   // needed for geometry::Box, geometry::Sphere
#include <convex_shapes.h>

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

BOOST_AUTO_TEST_SUITE(convex_shapes);

BOOST_AUTO_TEST_CASE(case_by_case_testing)
{
  typedef tiny::MathTypes<double> M;
  typedef M::vector3_type         V;

  geometry::Box<V>        const box;
  convex::Capsule<M>      const capsule;
  convex::Cone<M>         const cone;
  convex::ConvexHull<M>         hull;
  convex::Cylinder<M>     const cylinder;
  convex::Ellipsoid<M>    const ellipsoid;
  geometry::Sphere<V>     const sphere;

  V const zero     = V::make( 0.0, 0.0, 0.0);
  V const up       = V::make( 0.0, 0.0, 1.0);
  V const down     = V::make( 0.0, 0.0,-1.0);
  V const right    = V::make( 1.0, 0.0, 0.0);
  V const left     = V::make(-1.0, 0.0, 0.0);
  V const forward  = V::make( 0.0, 1.0, 0.0);
  V const backward = V::make( 0.0,-1.0, 0.0);

  // Test what happens if zero vector is given as search direction
  {
    V p = box.get_support_point( zero );
    BOOST_CHECK( (p(0)!=0.0) || (p(1)!=0) || (p(2)!=0) );

    p = capsule.get_support_point( zero );
    BOOST_CHECK( (p(0)!=0.0) || (p(1)!=0) || (p(2)!=0) );

    p = cone.get_support_point( zero );
    BOOST_CHECK( (p(0)!=0.0) || (p(1)!=0) || (p(2)!=0) );

    p = cylinder.get_support_point( zero );
    BOOST_CHECK( (p(0)!=0.0) || (p(1)!=0) || (p(2)!=0) );

    p = ellipsoid.get_support_point( zero );
    BOOST_CHECK( (p(0)!=0.0) || (p(1)!=0) || (p(2)!=0) );

    p = sphere.get_support_point( zero );
    BOOST_CHECK( (p(0)!=0.0) || (p(1)!=0) || (p(2)!=0) );
  }

  // Test what happens if up vector is given
  {
    V p = box.get_support_point( up );

    BOOST_CHECK( p(0) <=  box.half_extent()(0) );
    BOOST_CHECK( p(1) <=  box.half_extent()(1) );
    BOOST_CHECK( p(0) >= -box.half_extent()(0) );
    BOOST_CHECK( p(1) >= -box.half_extent()(1) );
    BOOST_CHECK( p(2) ==  box.half_extent()(2) );
  }
  {
    V p = capsule.get_support_point( up );
    BOOST_CHECK( p(0) == 0.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == 2.0 );
  }
  {
    V p = cone.get_support_point( up );
    BOOST_CHECK( p(0) == 0.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == 1.0 );
  }
  {
    V p = cylinder.get_support_point( up );
    BOOST_CHECK( p(0) == 0.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == 1.0 );
  }
  {
    V p = ellipsoid.get_support_point( up );
    BOOST_CHECK( p(0) == 0.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == 1.0 );
  }
  {
    V p = sphere.get_support_point( up );
    BOOST_CHECK( p(0) == 0.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == 1.0 );
  }

  // Test what happen if right vector is given
  {
    V p = box.get_support_point( right );

    BOOST_CHECK( p(0) ==  box.half_extent()(0) );
    BOOST_CHECK( p(1) <=  box.half_extent()(1) );
    BOOST_CHECK( p(2) <=  box.half_extent()(2) );
    BOOST_CHECK( p(1) >= -box.half_extent()(1) );
    BOOST_CHECK( p(2) >= -box.half_extent()(2) );
  }
  {
    V p = capsule.get_support_point( right );
    BOOST_CHECK( p(0) == 1.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == 0.0 );
  }
  {
    V p = cone.get_support_point( right );
    BOOST_CHECK( p(0) ==  1.0 );
    BOOST_CHECK( p(1) ==  0.0 );
    BOOST_CHECK( p(2) == -1.0 );
  }
  {
    V p = cylinder.get_support_point( right );
    BOOST_CHECK( p(0) == cylinder.radius() );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) <=  cylinder.half_height() );
    BOOST_CHECK( p(2) >= -cylinder.half_height() );
  }
  {
    V p = ellipsoid.get_support_point( right );
    BOOST_CHECK( p(0) ==  1.0 );
    BOOST_CHECK( p(1) ==  0.0 );
    BOOST_CHECK( p(2) ==  0.0 );
  }
  {
    V p = sphere.get_support_point( right );
    BOOST_CHECK( p(0) ==  1.0 );
    BOOST_CHECK( p(1) ==  0.0 );
    BOOST_CHECK( p(2) ==  0.0 );
  }

  // Test what happen if down vector is given
  {
    V p = box.get_support_point( down );

    BOOST_CHECK( p(2) == -box.half_extent()(2) );
    BOOST_CHECK( p(0) <=  box.half_extent()(0) );
    BOOST_CHECK( p(1) <=  box.half_extent()(1) );
    BOOST_CHECK( p(0) >= -box.half_extent()(0) );
    BOOST_CHECK( p(1) >= -box.half_extent()(1) );
  }
  {
    V p = capsule.get_support_point( down );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==  -2.0 );
  }
  {
    V p = cone.get_support_point( down );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==  -1.0 );
  }
  {
    V p = cylinder.get_support_point( down );
    BOOST_CHECK( p(0) == 0.0 );
    BOOST_CHECK( p(1) == 0.0 );
    BOOST_CHECK( p(2) == -cylinder.half_height() );
  }
  {
    V p = ellipsoid.get_support_point( down );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==  -1.0 );
  }
  {
    V p = sphere.get_support_point( down );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==  -1.0 );
  }
  // Test the convex hull
  {
    hull.add_point( left     );
    hull.add_point( right    );
    hull.add_point( backward );
    hull.add_point( forward  );
    hull.add_point( down     );
    hull.add_point( up       );

    V p = hull.get_support_point(right );
    BOOST_CHECK( p(0) ==   1.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==   0.0 );

    p = hull.get_support_point( left );
    BOOST_CHECK( p(0) ==  -1.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==   0.0 );

    p = hull.get_support_point( forward );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   1.0 );
    BOOST_CHECK( p(2) ==   0.0 );

    p = hull.get_support_point( backward );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==  -1.0 );
    BOOST_CHECK( p(2) ==   0.0 );

    p = hull.get_support_point( up );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==   1.0 );

    p = hull.get_support_point( down );
    BOOST_CHECK( p(0) ==   0.0 );
    BOOST_CHECK( p(1) ==   0.0 );
    BOOST_CHECK( p(2) ==  -1.0 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef VARIATIONAL_PARAMETERS_HPP
#define VARIATIONAL_PARAMETERS_HPP

#include "genotype_matrix.hpp"
#include "parameters.hpp"
#include "typedefs.hpp"
#include "hyps.hpp"

#include "tools/eigen3.3/Dense"
#include "file_utils.hpp"

#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/filesystem.hpp>

#include <iostream>
#include <limits>

namespace boost_io = boost::iostreams;

long dXtEEX_col_ind(long kk, long jj, long n_env);

class VariationalParamsBase {
public:
	// This stores parameters used in VB and some summary quantities that
	// depend on those parameters.
	parameters p;

	// Variational parameters for slab (params 1)
	// P x (E+1)
	Eigen::ArrayXd alpha_beta;
	Eigen::ArrayXd mu1_beta;
	Eigen::ArrayXd s1_beta_sq;
	Eigen::ArrayXd mu2_beta;
	Eigen::ArrayXd s2_beta_sq;

	// Variational parameters for spike (MoG prior mode; params 2)
	// P x (E+1)
	Eigen::ArrayXd alpha_gam;
	Eigen::ArrayXd mu1_gam;
	Eigen::ArrayXd s1_gam_sq;
	Eigen::ArrayXd mu2_gam;
	Eigen::ArrayXd s2_gam_sq;

	// Variational parameters for covariate main effects
	// n_covar x 1
	Eigen::ArrayXd muc;
	Eigen::ArrayXd sc_sq;

	// Variational params for weights
	// n_env x 1
	Eigen::ArrayXd muw;
	Eigen::ArrayXd sw_sq;

	VariationalParamsBase(const parameters& my_params) : p(my_params){
	};

	/*** utility functions ***/
	void resize(std::int32_t n_samples, std::int32_t n_var, long n_covar, long n_env);
	void run_default_init(long n_var, long n_covar, long n_env);

	/*** mean of latent variables ***/
	Eigen::VectorXd mean_covars() const;
	Eigen::VectorXd mean_beta() const;
	Eigen::VectorXd mean_gam() const;
	Eigen::VectorXd mean_weights() const;

	double mean_beta(std::uint32_t jj) const;
	double mean_gam(std::uint32_t jj) const;

	/*** variance of latent variables ***/
	Eigen::ArrayXd var_beta() const;
	Eigen::ArrayXd var_gam() const;
	Eigen::ArrayXd mean_beta_sq(int u0) const;
	Eigen::ArrayXd mean_gam_sq(int u0) const;

	/*** IO ***/
	void env_to_file(const std::string& path, const std::vector<std::string>& env_names) const;
	void covar_to_file(const std::string& path, const std::vector<std::string>& covar_names) const;
	void snps_to_file(const std::string& path, const GenotypeMatrix &X, long n_env) const;
	void dump_to_prefix(const std::string& prefix,
	                    const GenotypeMatrix &X,
	                    const std::vector<std::string>& env_names,
	                    const std::vector<std::string>& covar_names) const;
};

// Store subset of Variational Parameters to be RAM efficient
class VariationalParametersLite : public VariationalParamsBase {
public:
// Other quantities to track
	EigenDataVector yx;
	EigenDataVector ym;
	EigenDataVector eta;
	EigenDataVector eta_sq;

	VariationalParametersLite(const parameters& my_params) : VariationalParamsBase(my_params) {
	};
};

class VariationalParameters : public VariationalParamsBase {
public:
// This stores parameters used in VB and some summary quantities that
// depend on those parameters.

// Summary quantities
	EigenRefDataVector yx;
	EigenRefDataVector ym;
	EigenRefDataVector eta;
	EigenRefDataVector eta_sq;

	Eigen::ArrayXd EdZtZ;

	VariationalParameters(const parameters my_params,
	                      EigenRefDataVector my_ym,
	                      EigenRefDataVector my_yx,
	                      EigenRefDataVector my_eta,
	                      EigenRefDataVector my_eta_sq) : VariationalParamsBase(my_params), yx(my_yx), ym(my_ym),
		eta(my_eta), eta_sq(my_eta_sq){
	};

	~VariationalParameters(){
	};

	void init_from_lite(const VariationalParametersLite& init);

	VariationalParametersLite convert_to_lite() const;

	void calcEdZtZ(const Eigen::Ref<const Eigen::ArrayXXd>& dXtEEX, const long& n_env);
};

#endif


#include <fstream>
#include <iostream>
#include <limits>

#define CATCH_CONFIG_RUNNER
#include <Catch/catch.hpp>

#include <BigNumber/BigNumber.hxx>

#ifdef ENABLE_GMP
#include <boost/multiprecision/gmp.hpp>
#endif

/** @test This test checks the string representation functions of the BigNumber
 *  class to make sure created BigNumber objects correctly represent their
 *  numerical value, despite the fact the data is internally stored as a string,
 *  rather than a numerical value.
 *
 */

TEST_CASE("BigNumber String Representation", "[String Representation]")
{
    REQUIRE(BigNumber { }.numberString() == "0");
    REQUIRE(BigNumber { "127" }.numberString() == "127");
    REQUIRE(BigNumber { -17 }.numberString() == "-17");
}

/** @test This test checks to make sure the numeric value of the BigNumber
 *  object matches up with the textual representation stored internally.
 *
 */

TEST_CASE("BigNumber Numeric Value", "[Numeric Value]")
{
    REQUIRE(BigNumber { }.value() == 0);
    REQUIRE(BigNumber { "127" }.value() == 127);
    REQUIRE(BigNumber { INT_MAX }.value() == INT_MAX);
    REQUIRE(BigNumber { LLONG_MAX }.value() == LLONG_MAX);

    /** @test This particular test case aims to find out if the BigNum numeric
     *  value is greater than long long max. If it is, it will return -1. Otherwise,
     *  it will return the numeric value as a long long.
     *
     *  @todo Implement REQUIRE(BigNumber { LLONG_MAX + 1 }.value() == -1) test case
     *
     */

    // TODO: Implement this test case to ensure BigNum can transform to long long
    //REQUIRE(BigNumber { LLONG_MAX + 1 }.value() == -1);
}

/** @fn int main(int argc, char *argv[])
 *
 *  @description This is the main function for the Catch test framework. It will
 *  run all the defined test cases and return successfully if all tests passes,
 *  or it will abort on the first test failed.
 *
 *  @returns int
 *
 */

int main(int argc, char *argv[])
{
    return Catch::Session().run(argc, argv);
}


//
//  cvx_vpgl_perspective_camera.cpp
//  calib
//
//  Created by jimmy on 2017-07-26.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "pgl_perspective_camera.h"
#include <Eigen/Geometry>

namespace cvx_pgl {
    perspective_camera::perspective_camera()
    {
        R_.set_identity();
        camera_center_.setConstant(0);
    }

    perspective_camera::~perspective_camera()
    {

    }

    perspective_camera::perspective_camera( const perspective_camera& other )
    {
        if (&other == this) {
            return;
        }
        K_ = other.K_;
        camera_center_ = other.camera_center_;
        R_ = other.R_;
        recompute_matrix();
    }

    void perspective_camera::set_calibration( const calibration_matrix& K )
    {
        K_ = calibration_matrix(K);
        recompute_matrix();
    }
    void perspective_camera::set_camera_center( const Eigen::Vector3d& camera_center )
    {
        camera_center_ = camera_center;
        recompute_matrix();
    }
    void perspective_camera::set_translation(const Eigen::Vector3d& t)
    {
        Eigen::Matrix3d Rt = R_.as_matrix().transpose();
        Eigen::Vector3d cv = -(Rt * t);
        camera_center_ = cv;
        recompute_matrix();
    }

    void perspective_camera::set_rotation(const Eigen::Vector3d& rvector)
    {
        R_ = rotation3d(rvector);
        recompute_matrix();
    }

    void perspective_camera::set_rotation( const Eigen::Matrix3d& R )
    {
        R_ = rotation3d(R);
        recompute_matrix();
    }

     void perspective_camera::recompute_matrix()
     {
         // Set new projection matrix to [ I | -C ].
         Matrix34d Pnew;
         Pnew.setConstant(0);

         for ( int i = 0; i < 3; i++ ){
             Pnew(i,i) = 1.0;
         }
         Pnew(0,3) = -camera_center_.x();
         Pnew(1,3) = -camera_center_.y();
         Pnew(2,3) = -camera_center_.z();

         this->set_matrix(K_.get_matrix() * R_.as_matrix() * Pnew);
     }

}


#ifndef QIRPARSER
#define QIRPARSER

#include <vector>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/fusion/include/std_tuple.hpp>
#include <boost/variant.hpp>
#include "ast.hpp"

namespace parser {

    template<typename Iterator>
    struct grammar : public qi::grammar<Iterator, std::vector<ast::value>(), qi::space_type> {
        template<typename T>
        using rule = qi::rule<Iterator, T, qi::space_type>;

        rule<ast::value()> expr, expr2, expr3, expr4, constant, variant, vdec, vid, id, assign, ifstat;
        rule<std::vector<ast::value>()> stat;
        std::string id_save, vdec_id;

        grammar() : grammar::base_type(stat) {
            stat %= *(qi::lit('{') >> stat
                | ifstat[phx::bind(&grammar::id_save, this) = phx::bind(&grammar::vdec_id, this) = ""]
                >> (qi::lit('}') | qi::lit(';'))
                | (
                    vdec[phx::bind(&grammar::id_save, this) = phx::bind(&grammar::vdec_id, this) = ""]
                    | assign[phx::bind(&grammar::id_save, this) = phx::bind(&grammar::vdec_id, this) = ""]
                ) >> qi::lit(';'));
            ifstat %= qi::lit("if") >> expr[_val = _1] >> stat[_val = phx::construct<ast::ifstat>(_val, _1, std::vector<ast::value>())];
            vdec = qi::lit("let") >> vid >> qi::lit(':')
                >> (
                    qi::lit("int") >> qi::lit('=') >> expr[_val = phx::construct<ast::vdec<int>>(phx::bind(&grammar::vdec_id, this), _1)]
                    | qi::lit("bool") >> qi::lit('=') >> expr[_val = phx::construct<ast::vdec<bool>>(phx::bind(&grammar::vdec_id, this), _1)]
                )[phx::bind(&grammar::vdec_id, this) = ""];
            assign = vid >> qi::lit('=') >> expr[_val = phx::construct<ast::assign>(phx::bind(&grammar::vdec_id, this), _1)][phx::bind(&grammar::vdec_id, this) = ""];
            vid = *(qi::char_ - qi::char_("=:;{}"))[phx::push_back(phx::bind(&grammar::vdec_id, this), _1)];
            expr %= expr2[_val = _1]
                >> *(
                    qi::lit('<') >> expr2[_val = phx::construct<ast::binary_op<ast::lt>>(_val, _1)]
                    | qi::lit("<=") >> expr2[_val = phx::construct<ast::binary_op<ast::lte>>(_val, _1)]
                    | qi::lit('>') >> expr2[_val = phx::construct<ast::binary_op<ast::gt>>(_val, _1)]
                    | qi::lit(">=") >> expr2[_val = phx::construct<ast::binary_op<ast::gte>>(_val, _1)]
                    | qi::lit("==") >> expr2[_val = phx::construct<ast::binary_op<ast::eql>>(_val, _1)]
                    | qi::lit("!=") >> expr2[_val = phx::construct<ast::binary_op<ast::neq>>(_val, _1)]
                );
            expr2 %= (
                    qi::lit('+') >> expr3[_val = _1]
                    | qi::lit('-') >> expr3[_val = phx::construct<ast::binary_op<ast::sub>>(0, _1)]
                    | expr3[_val = _1]
                )
                >> *(
                    qi::lit('+') >> expr3[_val = phx::construct<ast::binary_op<ast::add>>(_val, _1)]
                    | qi::lit('-') >> expr3[_val = phx::construct<ast::binary_op<ast::sub>>(_val, _1)]
                );
            expr3 %= expr4[_val = _1]
                >> *(
                    (qi::lit('*') >> expr4[_val = phx::construct<ast::binary_op<ast::mul>>(_val, _1)])
                    | (qi::lit('/') >> expr4[_val = phx::construct<ast::binary_op<ast::div>>(_val, _1)])
                    | (qi::lit('%') >> expr4[_val = phx::construct<ast::binary_op<ast::mod>>(_val, _1)])
                );
            expr4 %= constant
                | (qi::lit('(') >> expr >> qi::lit(')'))[_val = _1]
                | id[_val = phx::construct<ast::var_ref>(phx::bind(&grammar::id_save, this))][phx::bind(&grammar::id_save, this) = ""];
            constant = qi::int_ | qi::bool_;
            id = *(qi::char_ - qi::char_("+-*/%") - qi::char_("<>!=:;{}"))[phx::push_back(phx::bind(&grammar::id_save, this), _1)];
        }
    };
}

#endif


///////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::meta::is_distribution.hpp                           //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_META_IS_SCALAR_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_META_IS_SCALAR_DISTRIBUTION_HPP_ER_2009
#include <boost/statistics/detail/distribution_toolkit/meta/is_math_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{
namespace meta{

    template<typename D>
    struct is_scalar_distribution : is_math_distribution<D> {};

    // Specialize to bool_<true> for those distribution defined within
    // distribution_toolkit, such as location_scale

}// meta
}// distribution
}// toolkit
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <sstream>

#include <boost/date_time/posix_time/posix_time.hpp>

#include <elle/Duration.hh>
#include <elle/compiler.hh>
#include <elle/date/date.h>
#include <elle/test.hh>

using namespace std::literals;

namespace
{
  elle::Duration id(elle::Duration d = 30s)
  {
    return d;
  }

  elle::Duration optid(elle::DurationOpt d = 30s)
  {
    return *d;
  }

  void
  optional()
  {
    auto s = elle::Duration{1s};
    BOOST_TEST(s == 1000ms);

    BOOST_TEST(elle::Duration{1ms} == 1ms);
    BOOST_TEST(*elle::DurationOpt{1ms} == 1ms);

    BOOST_TEST(elle::Duration{30s} == 30s);
    BOOST_TEST(*elle::DurationOpt{30s} == 30s);
    BOOST_TEST(id() == 30s);
    BOOST_TEST(optid() == 30s);
  }

  void
  io()
  {
    auto const ref = std::string{"20131009T233600"};
    // Check Boost's format.  We use it to forge dates for S3.
    {
      auto const time
        = boost::posix_time::ptime
        (boost::gregorian::date(2013, boost::gregorian::Oct, 9),
         boost::posix_time::hours(23) + boost::posix_time::minutes(36));
      BOOST_TEST(to_iso_string(time) == ref);
    }
    // Likewise, but using Hinnant's Date.
    {
      using namespace date;
      auto const time = sys_days{2013_y/oct/9} + 23h + 36min;
      BOOST_TEST(format("%Y%m%dT%H%M%S", time) == ref);
      BOOST_TEST(format("%Y%m%d", time) == ref.substr(0, 8));
    }
    // Likewise, using the date parser.
#if ! GCC_VERSION_LTE(4, 9)
    {
      using namespace date;
      auto&& s = std::istringstream{ref};
      auto time = elle::Time{};
      from_stream(s, "%Y%m%dT%H%M%S", time);
      auto const ref = sys_days{2013_y/oct/9} + 23h + 36min;
      BOOST_CHECK_EQUAL(time, ref);
    }
#endif
  }
}

ELLE_TEST_SUITE()
{
  auto& master = boost::unit_test::framework::master_test_suite();
  master.add(BOOST_TEST_CASE(optional), 0, 1);
  master.add(BOOST_TEST_CASE(io), 0, 1);
}


#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <ros/publisher.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float32.h>
#include <stdio.h>
#include <stdlib.h>

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/asio.hpp>
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>

#include "std_msgs/Float64MultiArray.h"

using namespace std;

// Image Processing
void blockEnvironment(const cv::Mat&);
cv::Mat find_orange_in_ROI(const cv::Mat&);
cv::Mat cutSmall(const cv::Mat&, int);
cv::Mat canny_cut_bodies(const cv::Mat&, const cv::Mat&);
cv::Mat find_unique_centers(const cv::Mat&, double);
cv::Mat perform_watershed(const cv::Mat&, const cv::Mat&, const cv::Mat&);
cv::Mat draw_box_and_calc_dist(const cv::Mat&, const cv::Mat&);
cv::Mat kernel(int, int);
void publishImage(ros::Publisher, cv::Mat, std::string);

ros::Publisher pub_debug_pos, pub_debug_mask, pub_pointcloud, pub_closest_point;
int blockSky_height, blockWheels_height, blockBumper_height;
typedef cv::Mat Mat;
typedef cv::Point Point;
int low_H, high_H, low_S, low_V;
int canny_cut_min_threshold, minimum_area_cut;
double percent_max_distance_transform;

// PointCloud Processing
pcl::PointCloud<pcl::PointXYZ> draw_cone_circles();
void drawCircle(pcl::PointCloud<pcl::PointXYZ>&, pcl::PointXYZ, double, int);
void fovCallback(const sensor_msgs::CameraInfoConstPtr& msg);
void loadCameraFOV(ros::NodeHandle& nh);
void publish_closest_point();

cv_bridge::CvImagePtr cv_ptr;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
double fy, real_cone_height, real_cone_radius;
double camera_fov_horizontal;
cv::Size imageSize;
bool fov_callback_called;
double angle_constant;
vector<pcl::PointXYZ> cone_points;

void publish_closest_point() {
    if (cone_points.size() != 0) {
        double min = cone_points[0].x;
        for (pcl::PointXYZ cone_point : cone_points) {
            if (cone_point.x < min) {
                min = cone_point.x;
            }
        }
        std_msgs::Float32 msg;
        msg.data = static_cast<float>(min);
        pub_closest_point.publish(msg);
    }
}

void fovCallback(const sensor_msgs::CameraInfoConstPtr& msg) {
    double fx = msg->P[0];  // horizontal focal length of rectified image, in px
    //    fy = msg->P[5]; //vertical focal length
    imageSize = cv::Size(msg->width, msg->height);
    camera_fov_horizontal = 2 * atan2(imageSize.width, 2 * fx);
    fov_callback_called = true;
    angle_constant = camera_fov_horizontal / 2.0 / imageSize.width;
}

void loadCameraFOV(ros::NodeHandle& nh) {
    auto infoSub = nh.subscribe("/camera/camera_info", 1, fovCallback);

    fov_callback_called = false;
    while (!fov_callback_called) {
        ros::spinOnce();
        ros::Duration(1).sleep();
    }
    ROS_INFO("Using horizontal FOV %f ", camera_fov_horizontal);
}

void publishPointCloud(pcl::PointCloud<pcl::PointXYZ>& cloud) {
    sensor_msgs::PointCloud2 outmsg;
    pcl::toROSMsg(cloud, outmsg);
    outmsg.header.frame_id = "base_footprint";
    pub_pointcloud.publish(outmsg);
}

void drawCircle(pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::PointXYZ point, double radius, int numPoints) {
    const double pi = boost::math::constants::pi<double>();
    double angleStep = (2.0 * pi) / numPoints;

    double angle = 0;  // start angle

    for (int i = 0; i < numPoints; i++) {
        // draw a circle
        double x = radius * cos(angle);
        double y = radius * sin(angle);

        cloud.push_back(pcl::PointXYZ(point.x + x, point.y + y,
                                      0));  // point.x + radius is center of circle
        angle = angle + angleStep;
    }
}

pcl::PointCloud<pcl::PointXYZ> draw_cone_circles() {
    pcl::PointCloud<pcl::PointXYZ> cone_cloud;
    for (pcl::PointXYZ cone_point : cone_points) {
        drawCircle(cone_cloud, cone_point, real_cone_radius, 20);
    }
    return cone_cloud;
}

void publishImage(ros::Publisher pub, Mat img, std::string img_type) {
    if (pub.getNumSubscribers() > 0) {
        sensor_msgs::Image outmsg;
        cv_ptr->image = img;
        cv_ptr->encoding = img_type;
        cv_ptr->toImageMsg(outmsg);
        pub.publish(outmsg);
    }
}

cv::Mat draw_box_and_calc_dist(const cv::Mat& frame, const cv::Mat& sure_bodies) {
    vector<vector<Point>> contours;
    findContours(sure_bodies, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
    vector<cv::Rect> rect(contours.size());
    cone_points.clear();

    for (int i = 0; i < contours.size(); i++) {
        rect[i] = boundingRect(contours[i]);
        if (rect[i].height < 10 || rect[i].width < 5 || rect[i].width > 4 * rect[i].height) {
            continue;
        }

        cv::Moments m = moments(contours[i], true);
        Point center = Point(m.m10 / m.m00, m.m01 / m.m00);

        int offset = 0;
        double distance = (real_cone_height * fy) / rect[i].height;
        int px_horz_dist = frame.cols / 2 - center.x;
        double horz_dist = distance * px_horz_dist / fy;

        if (distance > 8) {
            continue;
        }

        cone_points.push_back(pcl::PointXYZ(distance, horz_dist, 0));

        // Black Line Down Middle
        line(frame, Point(frame.cols / 2, 0), Point(frame.cols / 2, frame.rows), (0, 255, 0), 2);

        // Debugging Printing
        // printf("Pixels: (%d, %d) Real: (%.2f, %.2f)\n", rect[i].height,
        // px_horz_dist, distance, horz_dist);

        stringstream stream_dist, stream_horz_dist;
        stream_dist << fixed << setprecision(1) << distance;
        stream_horz_dist << fixed << setprecision(1) << horz_dist;
        string position = "(" + stream_dist.str() + ", " + stream_horz_dist.str() + ")";

        drawContours(frame, contours, i, cv::Scalar(0, 255), 1, 8, vector<cv::Vec4i>(), 0, Point());
        rectangle(frame, rect[i].tl(), rect[i].br(), cv::Scalar(0, 255), 3, 8, 0);
        circle(frame, center, 5, cv::Scalar(255, 0, 255), cv::FILLED, 8, 0);
        putText(frame, position, Point(center.x - 80, center.y - 30), cv::FONT_HERSHEY_SIMPLEX, 1,
                cv::Scalar(255, 255, 255), 2, true);
    }

    return frame;
}

cv::Mat kernel(int x, int y) {
    return cv::getStructuringElement(cv::MORPH_RECT, cv::Size(x, y));
}

cv::Mat find_unique_centers(const cv::Mat& img, double thres_value) {
    Mat dist_transform, thres, dist_mask, mask;
    Mat sure_front = Mat::zeros(img.size(), CV_8UC1);
    vector<vector<Point>> contours1;
    double min, max;

    distanceTransform(img, dist_transform, CV_DIST_L2, 3);
    findContours(img, contours1, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);

    for (int i = 0; i < contours1.size(); i++) {
        mask = Mat::zeros(img.size(), CV_8UC1);
        dist_mask = cv::Scalar::all(0);

        drawContours(mask, contours1, i, 255, cv::FILLED);

        dist_transform.copyTo(dist_mask, mask);

        cv::minMaxLoc(dist_mask, &min, &max);

        threshold(dist_mask, thres, max * thres_value, 255, CV_THRESH_BINARY);
        thres.convertTo(thres, CV_8UC1);
        bitwise_or(thres, sure_front, sure_front);
    }

    return sure_front;
}

cv::Mat perform_watershed(const cv::Mat& frame, const cv::Mat& orange_found, const cv::Mat& detected_bodies) {
    Mat sure_back, dist_transform, unknown, markers;

    Mat sure_front = find_unique_centers(detected_bodies, percent_max_distance_transform);
    sure_front.convertTo(sure_front, CV_8UC1);

    dilate(orange_found, sure_back, kernel(3, 3));

    subtract(sure_back, sure_front, unknown);

    connectedComponents(sure_front, markers);
    markers += 1;
    markers.setTo(0, unknown == 255);

    watershed(frame, markers);

    Mat lines = Mat::zeros(orange_found.size(), CV_8UC1);
    lines.setTo(cv::Scalar(255), markers == -1);
    rectangle(lines, Point(0, 0), Point(frame.cols, frame.rows), cv::Scalar(0), 2);

    dilate(lines, lines, kernel(2, 1));
    floodFill(lines, Point(0, 0), cv::Scalar(255));

    Mat sure_body;
    bitwise_not(lines, sure_body);

    sure_body = cutSmall(sure_body, minimum_area_cut);

    return sure_body;
}

cv::Mat canny_cut_bodies(const cv::Mat& frame, const cv::Mat& orange_found) {
    Mat frame_gray, detected_edges, detected_bodies;

    cv::Mat frame_copy = frame.clone();
    GaussianBlur(frame_copy, frame_copy, cv::Size(3, 3), 0, 0, cv::BORDER_DEFAULT);
    cv::cvtColor(frame_copy, frame_gray, cv::COLOR_BGR2GRAY);
    bitwise_and(frame_gray, orange_found, frame_gray);

    cv::Canny(frame_gray, detected_edges, canny_cut_min_threshold, canny_cut_min_threshold * 3, 3);
    dilate(detected_edges, detected_edges, kernel(2, 2));

    floodFill(detected_edges, Point(0, 0), cv::Scalar(255));

    bitwise_not(detected_edges, detected_bodies);
    morphologyEx(detected_bodies, detected_bodies, cv::MORPH_OPEN, kernel(3, 3));
    detected_bodies = cutSmall(detected_bodies, minimum_area_cut);

    return detected_bodies;
}

cv::Mat cutSmall(const cv::Mat& color_edges, int size_min) {
    cv::Mat contours_color(color_edges.rows, color_edges.cols, CV_8UC1, cv::Scalar::all(0));
    std::vector<std::vector<cv::Point>> contours;

    cv::findContours(color_edges, contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
    for (int i = 0; i < contours.size(); i++) {
        if (size_min < cv::arcLength(contours[i], false)) {
            cv::drawContours(contours_color, contours, i, cv::Scalar(255), cv::FILLED, 8);
        }
    }
    return contours_color;
}

cv::Mat find_orange_in_ROI(const cv::Mat& img) {
    cv::Mat hsv_frame, orange_found, frame_cut;
    frame_cut = img.clone();
    blockEnvironment(frame_cut);
    cv::cvtColor(frame_cut, hsv_frame, cv::COLOR_BGR2HSV);
    cv::inRange(hsv_frame, cv::Scalar(low_H, low_S, low_V), cv::Scalar(high_H, 255, 255), orange_found);
    orange_found = cutSmall(orange_found, minimum_area_cut);
    return orange_found;
}

void blockEnvironment(const cv::Mat& img) {
    cv::rectangle(img, cv::Point(0, 0), cv::Point(img.cols, blockSky_height), cv::Scalar(0, 0, 0), cv::FILLED);

    cv::rectangle(img, cv::Point(0, img.rows), cv::Point(img.cols, blockWheels_height), cv::Scalar(0, 0, 0),
                  cv::FILLED);

    cv::rectangle(img, cv::Point(img.cols / 3, img.rows), cv::Point(2 * img.cols / 3, blockBumper_height),
                  cv::Scalar(0, 0, 0), cv::FILLED);
}

void img_callback(const sensor_msgs::ImageConstPtr& msg) {
    // Convert msg to Mat image
    cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
    cv::Mat frame = cv_ptr->image;

    // Find Orange HSV in Region of interest
    cv::Mat orange_found = find_orange_in_ROI(frame);

    // Cut HSV to find individual cones
    //    cv::Mat detected_bodies = canny_cut_bodies(frame, orange_found);
    //    cv::Mat sure_bodies = perform_watershed(frame, orange_found,
    //    detected_bodies);

    // Find Distance
    frame = draw_box_and_calc_dist(frame, orange_found);

    // publish Images
    publishImage(pub_debug_pos, frame, "bgr8");
    publishImage(pub_debug_mask, orange_found, "mono8");

    // Add Circles for cones to PointCloud and Publish
    pcl::PointCloud<pcl::PointXYZ> cone_cloud = draw_cone_circles();
    publishPointCloud(cone_cloud);

    // Publish Closest Point
    publish_closest_point();
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "coneDetectionHeight");

    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");
    std::string subscription_node;
    nhp.param("orange_low_H", low_H, 5);
    nhp.param("orange_high_H", high_H, 25);
    nhp.param("orange_low_S", low_S, 140);
    nhp.param("orange_low_V", low_V, 140);

    nhp.param("canny_min_threshold", canny_cut_min_threshold, 40);
    nhp.param("percent_max_distance_transform", percent_max_distance_transform, 0.7);
    nhp.param("minimum_area_cut", minimum_area_cut, 40);

    nhp.param("camera_focal_length_y", fy, 611.46);
    nhp.param("real_cone_height", real_cone_height, .2286);
    nhp.param("real_cone_radius", real_cone_radius, .07);

    nhp.param("blockSky_height", blockSky_height, 220);
    nhp.param("blockWheels_height", blockWheels_height, 200);
    nhp.param("blockBumper_height", blockBumper_height, 200);

    nhp.param("subscription_node", subscription_node, std::string("/camera/image_color_rect"));

    loadCameraFOV(nh);
    pub_debug_pos = nh.advertise<sensor_msgs::Image>("/cones/height/debug_coordinates", 1);
    pub_debug_mask = nh.advertise<sensor_msgs::Image>("/cones/height/debug_bodies_mask", 1);
    pub_pointcloud = nh.advertise<sensor_msgs::PointCloud2>("/cones/height/pointcloud", 1);
    pub_closest_point = nh.advertise<std_msgs::Float32>("/cones/height/closest_point", 1);
    auto img_real = nh.subscribe(subscription_node, 1, img_callback);

    ros::spin();
    return 0;
}