/*
    Copyright 2013 Christian Henning and Juan V. Puertos
    Use, modification and distribution are subject to the Boost Software License,
    Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt).
*/

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

#ifndef BOOST_GIL_EXTENSION_TOOLBOX_IMAGE_TYPES_SUBCHROMA_IMAGE_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_IMAGE_TYPES_SUBCHROMA_IMAGE_HPP

////////////////////////////////////////////////////////////////////////////////////////
/// \file subchroma_image.hpp
/// \brief Subchroma Image extension
/// \author Christian Henning and Juan V. Puertos\n
///
/// \date 2013 \n
///
////////////////////////////////////////////////////////////////////////////////////////

#include <boost/mpl/divides.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/vector_c.hpp>

#include <boost/gil/image.hpp>

namespace boost{ namespace gil {

typedef boost::gil::point2< std::ptrdiff_t > point_t;

////////////////////////////////////////////////////////////////////////////////////////
/// \class subchroma_image_deref_fn
/// \ingroup PixelLocatorModel PixelBasedModel
/// \brief Used for virtual_2D_locator
///
////////////////////////////////////////////////////////////////////////////////////////
template< typename Locator
        , typename Factors
        >
struct subchroma_image_deref_fn
{
    typedef gray8_view_t::locator plane_locator_t;

    typedef subchroma_image_deref_fn     const_t;
    typedef typename Locator::value_type value_type;
    typedef value_type                   reference;
    typedef value_type                   const_reference;
    typedef point_t                      argument_type;
    typedef reference                    result_type;

    static const bool is_mutable = false;

    /// default constructor
    subchroma_image_deref_fn() {}

    /// constructor
    subchroma_image_deref_fn( const plane_locator_t& y_locator
                            , const plane_locator_t& v_locator
                            , const plane_locator_t& u_locator
                             )
    : _y_locator( y_locator )
    , _v_locator( v_locator )
    , _u_locator( u_locator )
    {}

    /// operator()
    typename result_type operator()( const point_t& p ) const
    {
        typedef Scaling_Factors< mpl::at_c< Factors, 0 >::type::value
                               , mpl::at_c< Factors, 1 >::type::value
                               , mpl::at_c< Factors, 2 >::type::value
                               > scaling_factors_t;

        plane_locator_t y = _y_locator.xy_at( p );
        plane_locator_t v = _v_locator.xy_at( p.x / scaling_factors_t::ss_X, p.y / scaling_factors_t::ss_X );
        plane_locator_t u = _u_locator.xy_at( p.x / scaling_factors_t::ss_X, p.y / scaling_factors_t::ss_X );

        return value_type( at_c< 0 >( *y )
                         , at_c< 0 >( *v )
                         , at_c< 0 >( *u )
                         );
    }

    ///
    const plane_locator_t& y_locator() const { return _y_locator; }
    const plane_locator_t& v_locator() const { return _v_locator; }
    const plane_locator_t& u_locator() const { return _u_locator; }

private:

    plane_locator_t _y_locator;
    plane_locator_t _v_locator;
    plane_locator_t _u_locator;
};

////////////////////////////////////////////////////////////////////////////////////////
/// \class subchroma_image_locator_type
/// \ingroup PixelLocatorModel PixelBasedModel
/// \brief
///
////////////////////////////////////////////////////////////////////////////////////////
template< typename Locator
        , typename Factors
        >
struct subchroma_image_locator
{
    typedef virtual_2d_locator< subchroma_image_deref_fn< Locator
                                                        , Factors
                                                        > // Deref
                              , false // IsTransposed
                              > type;
};

/////////////////////////////
//  PixelBasedConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct channel_type< subchroma_image_locator< Locator, Factors > >
    : public channel_type< typename subchroma_image_locator< Locator, Factors >::type > {};

template < typename Locator, typename Factors >
struct color_space_type< subchroma_image_locator< Locator, Factors > >
    : public color_space_type< typename subchroma_image_locator< Locator, Factors >::type > {};

template < typename Locator, typename Factors >
struct channel_mapping_type< subchroma_image_locator< Locator, Factors > >
    : public channel_mapping_type< typename subchroma_image_locator< Locator, Factors >::type > {};

template < typename Locator, typename Factors >
struct is_planar< subchroma_image_locator< Locator, Factors > >
    : public is_planar< typename subchroma_image_locator< Locator, Factors >::type > {};

/////////////////////////////
//  HasDynamicXStepTypeConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct dynamic_x_step_type< subchroma_image_locator< Locator, Factors > >
{
    typedef typename subchroma_image_locator< Locator, Factors >::type type;
};

/////////////////////////////
//  HasDynamicYStepTypeConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct dynamic_y_step_type< subchroma_image_locator< Locator, Factors > >
{
    typedef typename subchroma_image_locator< Locator, Factors >::type type;
};

/////////////////////////////
//  HasTransposedTypeConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct transposed_type< subchroma_image_locator< Locator, Factors > >
{
    typedef typename subchroma_image_locator< Locator, Factors >::type type;
};

//////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////////
/// \class subchroma_image_view
/// \ingroup ImageViewModel PixelBasedModel
/// \brief A lightweight object that interprets a subchroma image.
///
////////////////////////////////////////////////////////////////////////////////////////
template< typename Locator
        , typename Factors = mpl::vector_c< int, 4, 4, 4 >
        >
class subchroma_image_view : public image_view< Locator >
{
public:

    typedef typename Locator                     locator;
    typedef typename locator::deref_fn_t         deref_fn_t;
    typedef typename deref_fn_t::plane_locator_t plane_locator_t;

    typedef subchroma_image_view const_t;

    typedef image_view< plane_locator_t > plane_view_t;

    /// default constructor
    subchroma_image_view()
    : image_view< Locator >()
    {}

    /// constructor
    subchroma_image_view( const point_t& y_dimensions
                        , const point_t& v_dimensions
                        , const point_t& u_dimensions
                        , const Locator& locator
                        )
    : image_view< Locator >( y_dimensions, locator )
    , _y_dimensions( y_dimensions )
    , _v_dimensions( v_dimensions )
    , _u_dimensions( u_dimensions )
    {}

    /// copy constructor
    template< typename Subchroma_View >
    subchroma_image_view( const Subchroma_View& v )
    : image_view< locator >( v )
    {}

    const point_t& v_ssfactors() const { return point_t( get_deref_fn().vx_ssfactor(), get_deref_fn().vx_ssfactor() ); }
    const point_t& u_ssfactors() const { return point_t( get_deref_fn().ux_ssfactor(), get_deref_fn().ux_ssfactor() ); }

    const point_t& y_dimension() const { return _y_dimensions; }
    const point_t& v_dimension() const { return _v_dimensions; }
    const point_t& u_dimension() const { return _u_dimensions; }

    const plane_locator_t& y_plane() const { return get_deref_fn().y_locator(); }
    const plane_locator_t& v_plane() const { return get_deref_fn().v_locator(); }
    const plane_locator_t& u_plane() const { return get_deref_fn().u_locator(); }

    const plane_view_t y_plane_view() const { return plane_view_t( _y_dimensions, y_plane() ); }
    const plane_view_t v_plane_view() const { return plane_view_t( _v_dimensions, v_plane() ); }
    const plane_view_t u_plane_view() const { return plane_view_t( _u_dimensions, u_plane() ); }

private:

    const deref_fn_t& get_deref_fn() const { return this->pixels().deref_fn(); }

private:

    template< typename Locator, typename Factors > friend class subchroma_image_view;

    point_t _y_dimensions;
    point_t _v_dimensions;
    point_t _u_dimensions;
};

/////////////////////////////
//  PixelBasedConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct channel_type< subchroma_image_view< Locator, Factors > >
    : public channel_type< Locator > {};

template < typename Locator, typename Factors >
struct color_space_type< subchroma_image_view< Locator, Factors > >
    : public color_space_type< Locator > {};

template < typename Locator, typename Factors >
struct channel_mapping_type< subchroma_image_view< Locator, Factors > >
     : public channel_mapping_type< Locator > {};

template < typename Locator, typename Factors >
struct is_planar< subchroma_image_view< Locator, Factors > >
    : public is_planar< Locator > {};

/////////////////////////////
//  HasDynamicXStepTypeConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct dynamic_x_step_type< subchroma_image_view< Locator, Factors > >
{
    typedef image_view< typename dynamic_x_step_type< Locator >::type > type;
};

/////////////////////////////
//  HasDynamicYStepTypeConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct dynamic_y_step_type< subchroma_image_view< Locator, Factors > >
{
    typedef image_view< typename dynamic_y_step_type< Locator >::type > type;
};

/////////////////////////////
//  HasTransposedTypeConcept
/////////////////////////////

template < typename Locator, typename Factors >
struct transposed_type< subchroma_image_view< Locator, Factors > >
{
    typedef image_view< typename transposed_type< Locator >::type > type;
};

/////////////////////////////////////////////////////////////
template< int J
        , int a
        , int b
        >
struct Scaling_Factors
{
    BOOST_STATIC_ASSERT(( mpl::equal_to< mpl::int_< J >, mpl::int_< 4 > >::type::value ));

    BOOST_STATIC_ASSERT(( mpl::or_< mpl::equal_to< mpl::int_< a >, mpl::int_< 4 > >
                                  , mpl::or_< mpl::equal_to< mpl::int_< a >, mpl::int_< 2 > >
                                            , mpl::equal_to< mpl::int_< a >, mpl::int_< 1 > >
                                            >
                                  >::type::value
                       ));

    BOOST_STATIC_ASSERT(( mpl::or_< mpl::equal_to< mpl::int_< b >, mpl::int_< 4 > >
                                  , mpl::or_< mpl::equal_to< mpl::int_< b >, mpl::int_< 2 > >
                                            , mpl::or_< mpl::equal_to< mpl::int_< b >, mpl::int_< 1 > >
                                                      , mpl::equal_to< mpl::int_< b >, mpl::int_< 0 > >
                                                      >
                                            >
                                  >::type::value
                       ));

    BOOST_STATIC_CONSTANT( int, ss_X = ( mpl::divides< mpl::int_< J >
                                                     , mpl::int_< a >
                                                     >::type::value )
                         );

    BOOST_STATIC_CONSTANT( int, ss_Y = ( mpl::if_< mpl::equal_to< mpl::int_< b >, mpl::int_< 0 > >
                                                 , mpl::int_< 2 >
                                                 , mpl::if_< mpl::equal_to< mpl::int_< a >, mpl::int_< b > >
                                                           , mpl::int_< 1 >
                                                           , mpl::int_< 4 >
                                                           >::type
                                                 >::type::value )
                         );
};

////////////////////////////////////////////////////////////////////////////////////////
/// \ingroup ImageModel PixelBasedModel
/// \brief container interface over image view. Models ImageConcept, PixelBasedConcept
///
/// A subchroma image holds a bunch of planes which don't need to have the same resolution.
///
////////////////////////////////////////////////////////////////////////////////////////
template< typename Pixel
        , typename Factors   = mpl::vector_c< int, 4, 4, 4 >
        , typename Allocator = std::allocator< unsigned char >
        >
class subchroma_image : public Scaling_Factors< mpl::at_c< Factors, 0 >::type::value
                                              , mpl::at_c< Factors, 1 >::type::value
                                              , mpl::at_c< Factors, 2 >::type::value
                                              >
{

public:

    typedef typename channel_type< Pixel >::type channel_t;
    typedef pixel< channel_t, gray_layout_t> pixel_t;

    typedef image< pixel_t, false, Allocator > plane_image_t;

    typedef typename plane_image_t::view_t plane_view_t;
    typedef typename plane_image_t::const_view_t plane_const_view_t;
    typedef typename plane_view_t::locator plane_locator_t;

    typedef typename view_type_from_pixel< Pixel >::type pixel_view_t;
    typedef typename pixel_view_t::locator pixel_locator_t;

    typedef typename subchroma_image_locator< pixel_locator_t
                                            , Factors
                                            >::type locator_t;

    typedef typename plane_image_t::coord_t x_coord_t;
    typedef typename plane_image_t::coord_t y_coord_t;

    typedef subchroma_image_view< locator_t, Factors > view_t;
    typedef typename view_t::const_t                   const_view_t;

    /// constructor
    subchroma_image( const x_coord_t y_width
                   , const y_coord_t y_height
                   )
    : _y_plane(        y_width,        y_height, 0, Allocator() )
    , _v_plane( y_width / ss_X, y_height / ss_Y, 0, Allocator() )
    , _u_plane( y_width / ss_X, y_height / ss_Y, 0, Allocator() )
    {
        init();
    }

public:

    view_t _view;

private:

    void init()
    {
        typedef subchroma_image_deref_fn< pixel_locator_t
                                        , Factors
                                        > defer_fn_t;

        defer_fn_t deref_fn( view( _y_plane ).xy_at( 0, 0 )
                           , view( _v_plane ).xy_at( 0, 0 )
                           , view( _u_plane ).xy_at( 0, 0 )
                           );

        // init a virtual_2d_locator
        locator_t locator( point_t( 0, 0 ) // p
                         , point_t( 1, 1 ) // step
                         , deref_fn
                         );

        _view = view_t( _y_plane.dimensions()
                      , _v_plane.dimensions()
                      , _u_plane.dimensions()
                      , locator
                      );
    }

private:

    plane_image_t _y_plane;
    plane_image_t _v_plane;
    plane_image_t _u_plane;
};

/////////////////////////////
//  PixelBasedConcept
/////////////////////////////

template < typename Pixel, typename Factors, typename Alloc >
struct channel_type< subchroma_image< Pixel, Factors, Alloc > >
    : public channel_type< Pixel > {};

template < typename Pixel, typename Factors, typename Alloc >
struct color_space_type< subchroma_image< Pixel, Factors, Alloc > >
    : public color_space_type< Pixel > {};

template < typename Pixel, typename Factors, typename Alloc >
struct channel_mapping_type<  subchroma_image< Pixel, Factors, Alloc > >
    : public channel_mapping_type< Pixel > {};

template < typename Pixel, typename Factors, typename Alloc >
struct is_planar< subchroma_image< Pixel, Factors, Alloc > >
    : public mpl::bool_< false > {};

/////////////////////////////////////////////////////////////////////////////////////////
/// \name view, const_view
/// \brief Get an image view from an subchroma_image
/// \ingroup ImageModel
/// \brief Returns the non-constant-pixel view of an image
/////////////////////////////////////////////////////////////////////////////////////////
template< typename Pixel
        , typename Factors
        >
inline
const typename subchroma_image< Pixel, Factors >::view_t& view( subchroma_image< Pixel, Factors >& img )
{
    return img._view;
}

template< typename Pixel
        , typename Factors
        >
inline
const typename subchroma_image< Pixel, Factors >::const_view_t const_view( subchroma_image< Pixel, Factors >& img )
{
    return static_cast< const typename subchroma_image< Pixel, Factors >::const_view_t>( img._view );
}

/////////////////////////////////////////////////////////////////////////////////////////
/// \ingroup ImageViewSTLAlgorithmsFillPixels
/// \brief std::fill for subchroma_image views
/////////////////////////////////////////////////////////////////////////////////////////
template< typename Locator
        , typename Factors
        , typename Pixel
        >
void fill_pixels( const subchroma_image_view< Locator, Factors >& view
                , const Pixel&                                    value
                )
{
    typedef typename subchroma_image< Pixel, Factors >::plane_view_t::value_type channel_t;

    fill_pixels( view.y_plane_view(), channel_t( at_c< 0 >( value )));
    fill_pixels( view.v_plane_view(), channel_t( at_c< 1 >( value )));
    fill_pixels( view.u_plane_view(), channel_t( at_c< 2 >( value )));
}

/////////////////////////////////////////////////////////////////////////////////////////
/// \ingroup ImageViewConstructors
/// \brief Creates a subchroma view from a raw memory
/////////////////////////////////////////////////////////////////////////////////////////
template< typename Pixel
        , typename Factors
        >
typename subchroma_image< Pixel
                        , Factors
                        >::view_t subchroma_view( std::size_t    y_width
                                                , std::size_t    y_height
                                                , unsigned char* y_base
                                                )
{
    typedef Scaling_Factors< mpl::at_c< Factors, 0 >::type::value
                           , mpl::at_c< Factors, 1 >::type::value
                           , mpl::at_c< Factors, 2 >::type::value
                           > scaling_factors_t;

    std::size_t y_channel_size = 1;
    std::size_t u_channel_size = 1;

    unsigned char* u_base = y_base + ( y_width  * y_height * y_channel_size );
    unsigned char* v_base = u_base + ( y_width  / scaling_factors_t::ss_X )
                                   * u_channel_size;

    typedef subchroma_image< Pixel, Factors >::plane_view_t plane_view_t;

    plane_view_t y_plane = interleaved_view( y_width
                                           , y_height
                                           , (plane_view_t::value_type*) y_base // pixels
                                           , y_width                            // rowsize_in_bytes
                                           );

    plane_view_t v_plane = interleaved_view( y_width  / scaling_factors_t::ss_X
                                           , y_height / scaling_factors_t::ss_Y
                                           , (plane_view_t::value_type*) v_base // pixels
                                           , y_width                            // rowsize_in_bytes
                                           );

    plane_view_t u_plane = interleaved_view( y_width  / scaling_factors_t::ss_X
                                           , y_height / scaling_factors_t::ss_Y
                                           , (plane_view_t::value_type*) u_base // pixels
                                           , y_width                            // rowsize_in_bytes
                                           );

    typedef subchroma_image_deref_fn< typename subchroma_image< Pixel
                                                              , Factors
                                                              >::pixel_locator_t
                                    , Factors
                                    > defer_fn_t;

    defer_fn_t deref_fn( y_plane.xy_at( 0, 0 )
                       , v_plane.xy_at( 0, 0 )
                       , u_plane.xy_at( 0, 0 )
                       );

    typedef subchroma_image< Pixel
                           , Factors
                           >::locator_t locator_t;

    locator_t locator( point_t( 0, 0 ) // p
                     , point_t( 1, 1 ) // step
                     , deref_fn
                     );

    typedef subchroma_image< Pixel
                           , Factors
                           >::view_t view_t;

    return view_t( point_t(                           y_width,                           y_height )
                 , point_t( y_width / scaling_factors_t::ss_X, y_height / scaling_factors_t::ss_Y )
                 , point_t( y_width / scaling_factors_t::ss_X, y_height / scaling_factors_t::ss_Y )
                 , locator
                 );
}

} // namespace gil
} // namespace boost

#endif // BOOST_GIL_EXTENSION_TOOLBOX_IMAGE_TYPES_SUBCHROMA_IMAGE_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 TP: C++ for Finance

 Illustration of
 - Quote objects (Derived from test suite quotes.cpp)
 - Observer pattern and lazy evaluation
*/

// the only header you need to use QuantLib
#include <ql/quantlib.hpp>
#include "CsvHelper.hpp"
#include "HestonTools.hpp"

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
   exceptions. Warning: unpredictable results can arise...

   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
   Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() {
        return 0;
    }

}
#endif

int main(int, char* []) {

    Handle<Quote> s0(boost::shared_ptr<Quote>(new SimpleQuote(1290.59)));

    try {

// interest rates and dividend yield

        std::fstream file1("/home/phn/dev/R/ValidationModeles/rate_div.csv", ios::in);
        if (!file1.is_open()) {
            std::cout << "Rate/Yield File not found!\n";
            return 1;
        }

        std::vector<RD> RDData;
        readCSV_RD(file1, RDData);
        file1.close();

        std::fstream file2("/home/phn/dev/ModelRisk/Heston/HestonSearch.csv", ios::in);
        if (!file2.is_open()) {
            std::cout << "Heston Parameters File not found!\n";
            return 1;
        }

        std::vector<HESTONPARAMS> HestonPar;
        readCSV_HestonParams(file2, HestonPar);
        file2.close();

        std::vector<double> res(5, 0.0);

        std::cout << "Size of search file: " << HestonPar.size() << std::endl;

        // loop through the parameter set
        // set iTest to run through a few iterations, for testing purpose

        Size iTest = 3;
        for (Size i=0; i<std::min(iTest,HestonPar.size()); ++i) {

            res[0] = HestonPar[i].theta;
            res[1] = HestonPar[i].kappa;
            res[2] = HestonPar[i].sigma;
            res[3] = HestonPar[i].rho;
            res[4] = HestonPar[i].v0;

            int nbPaths = 20000;
            int nbSteps = 100;
            Time horizon = 5;
            BigNatural seed = 0;

            // nbPaths+1 to make room for time steps column
            std::vector< std::vector<double> > paths(nbPaths+1, vector<double>(nbSteps+1, 0));

            HestonSimulate(paths, s0, RDData, res,
                           nbPaths, nbSteps, horizon, seed);

            // write to csv: rows are steps, cols are scenarios
            // first col is time line

            std::ostringstream iCase;
            iCase << i;

            std::cout << "set: " << i << std::endl;

            string fname = "../Heston/Paths/Paths_" + iCase.str() + ".csv";
            write2DtoCsv(paths, fname.c_str(), false);
        }

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}


/* #include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/regex.hpp>
#include <idmlib/tdt/similb.h>
#include <idmlib/tdt/ema.h>

BOOST_AUTO_TEST_SUITE( t_similb )

using namespace idmlib::tdt;

template <class T>
void PrintVec(const std::string& info, const std::vector<T>& vec)
{
    std::cout<<info;
    for(std::size_t i=0;i<vec.size();i++)
    {
        if(i!=0)
        {
            std::cout<<",";
        }
        std::cout<<vec[i];
    }
    std::cout<<std::endl;
}

template <class T>
void PrintVec2(const std::string& info, const std::vector<T>& vec)
{
    std::cout<<info<<std::endl;
    for(std::size_t i=0;i<vec.size();i++)
    {
        std::cout<<i<<"\t"<<vec[i]<<std::endl;
    }
}

BOOST_AUTO_TEST_CASE(shape_test )
{
    std::vector<double> s1(10), s2(10), s3(10);
    s1[0] = 3.5;
    s1[1] = 3;
    s1[2] = 4;
    for (int i = 3; i < 10; i++)
        s1[i] = s1[i-1] + 1./14.;
    s2[0] = 2;
    s2[1] = 2;
    for (int i = 2; i < 10; i++)
        s2[i] = 3;

    s3[0] = 0.5;
    s3[1] = 1;
    s3[2] = 2;
    for (int i = 3; i < 10; i++)
        s3[i] = s3[i-1] - 1./14.;

    std::vector<double> psi1;
    SimilB::GetPsi(s1, s1, psi1);
    PrintVec("psi1 : ", psi1);
    std::cout << "The Similarity Value for s1 and s1: " << SimilB::Sim(s1, s1) << std::endl;
    std::cout << "The Similarity Value for s2 and s2: " << SimilB::Sim(s2, s2) << std::endl;
    std::cout << "The Similarity Value for s3 and s3: " << SimilB::Sim(s3, s3) << std::endl;
    std::cout << "The Similarity Value for s1 and s2: " << SimilB::Sim(s1, s2) << std::endl;
    std::cout << "The Similarity Value for s2 and s1: " << SimilB::Sim(s2, s1) << std::endl;
    std::cout << "The Similarity Value for s2 and s3: " << SimilB::Sim(s2, s3) << std::endl;
    std::cout << "The Similarity Value for s3 and s2: " << SimilB::Sim(s3, s2) << std::endl;
}

BOOST_AUTO_TEST_CASE(perturb_test )
{
    std::vector<double> s1(500), s2(500);
    for (int i = 0; i < 500; i++) {
        s1[i] = sin((double)i);
        s2[i] = sin((double)i);
        if (i == 200)
            s2[i] = 4;
    }
    std::cout << "perturb_test The Similarity Value for s1 and s2: " << SimilB::Sim(s1, s2) << std::endl;
    int SPAN = 25;
    for (int i = 0; i < 475; i++)
    {
        std::cout << "perturb_test The Similarity Value for s1 and s2 on [" << i << ", " << i + SPAN << "] -> " << SimilB::Sim(s1, s2, i, i+SPAN) << std::endl;
    }
//     std::vector<double> psi;
//     SimilB::GetPsi(s1, s2, psi);
//     PrintVec2("perturb_test psi : ", psi);
}

BOOST_AUTO_TEST_CASE(chirp_test )
{
  std::vector<double> s1(250), s2(250);

  int freq = 1;
  for (int i = 0; i < 250; i++, freq++) {
    s1[i] = sin((double)freq * (double)i);
    s2[i] = sin((double)(250 - freq) * (double)i);
  }
  std::vector<double> psi;
  SimilB::GetPsi(s1, s2, psi);
//   PrintVec2("chirp_test psi : ", psi);
//   std::cout << "s1: " << s1 << std::endl;
//   std::cout << "s2: " << s2 << std::endl;

  std::cout << "chirp_test The Similarity Value for s1 and s2: " << SimilB::Sim(s1, s2) << std::endl;

  int SPAN = 25;
  for (int i = 0; i < 225; i++)
    std::cout << "chirp_test The Similarity Value for s1 and s2 on [" << i << ", " << i + SPAN << "] -> " << SimilB::Sim(s1, s2, i, i+SPAN) << std::endl;
}

// BOOST_AUTO_TEST_CASE(chirp_test2 )
// {
//   std::vector<double> s1(250), s2(250);
//
//   double step = 0.1/150;
//   for (int i = 0; i < 250; i++) {
//     s1[i] = 0.05 + step*i;
//   }
//
//   for (int i = 0; i < 250; i++) {
//     s2[i] = 0.05 + step* (250-i);
//   }
// //   PrintVec("chirp_test2 s1 : ", s1);
// //   PrintVec("chirp_test2 s2 : ", s2);
//
//   std::vector<double> psi1;
//   SimilB::GetPsi(s1, s1, psi1);
// //   std::cout<<psi1.size()<<std::endl;
//   PrintVec("chirp_test2 psi1 : ", psi1);
//   std::vector<double> psi2;
//   SimilB::GetPsi(s2, s2, psi2);
//   PrintVec("chirp_test2 psi2 : ", psi2);
//
//   std::vector<double> psi;
//   SimilB::GetPsi(s1, s2, psi);
//   PrintVec("chirp_test2 psi : ", psi);
//   std::cout << "chirp_test2 The Similarity Value for s1 and s1: " << SimilB::Sim(s1, s1) << std::endl;
//
//   std::cout << "chirp_test2 The Similarity Value for s2 and s2: " << SimilB::Sim(s2, s2) << std::endl;
//
//   std::cout << "chirp_test2 The Similarity Value for s1 and s2: " << SimilB::Sim(s1, s2) << std::endl;
//
// }

BOOST_AUTO_TEST_SUITE_END()
	*/


/* Copyright 2020 Oinam Romesh Meitei

   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 <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>
#include <cmath>
#include <vector>

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>

#include "getham.h"
#include "agradc.h"
#include "grad_ana.h"

namespace py = pybind11;

/* Analytical gradients for pulse amplitude at every trotter step */

gradc grad_trotter(
                               std::vector<double> &tlist,
                               std::vector<std::complex<double> > &ini_vec,
                               pulsec pobj,
                               std::vector< std::vector< Eigen::SparseMatrix
			       <double,0,ptrdiff_t> > > hdrive,
                               std::vector< std::complex<double> > dsham,
			       std::vector< int> &states,
			       Eigen::MatrixXcd &cHam){

  gradc gradd= grad_ana(tlist, ini_vec, pobj, hdrive, dsham, states, cHam);

  return gradd;
}

gradc grad_trotter_normalized(
                               std::vector<double> &tlist,
                               std::vector<std::complex<double> > &ini_vec,
                               pulsec pobj,
                               std::vector< std::vector< Eigen::SparseMatrix
			       <double,0,ptrdiff_t> > > hdrive,
                               std::vector< std::complex<double> > dsham,
			       std::vector< int> &states,
			       Eigen::MatrixXcd &cHam){

  gradc gradd= grad_ana_normalized(tlist, ini_vec, pobj, hdrive, dsham, states, cHam);

  return gradd;
}
PYBIND11_MODULE(agradc,m){
  m.def("grad_trotter", &grad_trotter, "gtrotter");
  m.def("grad_trotter_normalized", &grad_trotter_normalized, "gtrotterN");
  py::class_<gradc>(m,"gradc")
    .def(py::init<
	 double &, double &,
	 std::vector<std::vector< double > > & > ())
    .def_readonly("energy", &gradc::energy)
    .def_readonly("norm", &gradc::norm)
    .def_readonly("gradient",&gradc::gradient);
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_BUFFER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_BUFFER_HPP

#include <cstddef>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/algorithms/detail/disjoint.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

template <typename BoxIn, typename BoxOut, typename T, std::size_t C, std::size_t D, std::size_t N>
struct box_loop
{
    typedef typename coordinate_type<BoxOut>::type coordinate_type;

    static inline void apply(BoxIn const& box_in, T const& distance, BoxOut& box_out)
    {
        coordinate_type d = distance;
        set<C, D>(box_out, get<C, D>(box_in) + d);
        box_loop<BoxIn, BoxOut, T, C, D + 1, N>::apply(box_in, distance, box_out);
    }
};

template <typename BoxIn, typename BoxOut, typename T, std::size_t C, std::size_t N>
struct box_loop<BoxIn, BoxOut, T, C, N, N>
{
    static inline void apply(BoxIn const&, T const&, BoxOut&) {}
};

// Extends a box with the same amount in all directions
template<typename BoxIn, typename BoxOut, typename T>
inline void buffer_box(BoxIn const& box_in, T const& distance, BoxOut& box_out)
{
    assert_dimension_equal<BoxIn, BoxOut>();

    static const std::size_t N = dimension<BoxIn>::value;

    box_loop<BoxIn, BoxOut, T, min_corner, 0, N>::apply(box_in, -distance, box_out);
    box_loop<BoxIn, BoxOut, T, max_corner, 0, N>::apply(box_in, distance, box_out);
}

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Input,
    typename Output,
    typename TagIn = typename tag<Input>::type,
    typename TagOut = typename tag<Output>::type
>
struct buffer: not_implemented<TagIn, TagOut>
{};

template <typename BoxIn, typename BoxOut>
struct buffer<BoxIn, BoxOut, box_tag, box_tag>
{
    template <typename Distance>
    static inline void apply(BoxIn const& box_in, Distance const& distance,
                Distance const& , BoxIn& box_out)
    {
        detail::buffer::buffer_box(box_in, distance, box_out);
    }
};

// Many things to do. Point is easy, other geometries require self intersections
// For point, note that it should output as a polygon (like the rest). Buffers
// of a set of geometries are often lateron combined using a "dissolve" operation.
// Two points close to each other get a combined kidney shaped buffer then.

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc{buffer}
\ingroup buffer
\details \details_calc{buffer, \det_buffer}.
\tparam Input \tparam_geometry
\tparam Output \tparam_geometry
\tparam Distance \tparam_numeric
\param geometry_in \param_geometry
\param geometry_out \param_geometry
\param distance The distance to be used for the buffer
\param chord_length (optional) The length of the chord's in the generated arcs around points or bends
\note Currently only implemented for box, the trivial case, but still useful

\qbk{[include reference/algorithms/buffer.qbk]}
 */
template <typename Input, typename Output, typename Distance>
inline void buffer(Input const& geometry_in, Output& geometry_out,
            Distance const& distance, Distance const& chord_length = -1)
{
    concept::check<Input const>();
    concept::check<Output>();

    dispatch::buffer
        <
            Input,
            Output
        >::apply(geometry_in, distance, chord_length, geometry_out);
}

/*!
\brief \brief_calc{buffer}
\ingroup buffer
\details \details_calc{return_buffer, \det_buffer}. \details_return{buffer}.
\tparam Input \tparam_geometry
\tparam Output \tparam_geometry
\tparam Distance \tparam_numeric
\param geometry \param_geometry
\param distance The distance to be used for the buffer
\param chord_length (optional) The length of the chord's in the generated arcs
    around points or bends
\return \return_calc{buffer}
 */
template <typename Output, typename Input, typename Distance>
Output return_buffer(Input const& geometry, Distance const& distance, Distance const& chord_length = -1)
{
    concept::check<Input const>();
    concept::check<Output>();

    Output geometry_out;

    dispatch::buffer
        <
            Input,
            Output
        >::apply(geometry, distance, chord_length, geometry_out);

    return geometry_out;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_BUFFER_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

/* meridional distance for ellipsoid and inverse
**	8th degree - accurate to < 1e-5 meters when used in conjunction
**		with typical major axis values.
**	Inverse determines phi to EPS (1e-11) radians, about 1e-6 seconds.
*/

#ifndef BOOST_GEOMETRY_PROJECTIONS_PJ_MLFN_HPP
#define BOOST_GEOMETRY_PROJECTIONS_PJ_MLFN_HPP

#include <cstdlib>

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

namespace boost { namespace geometry { namespace projections {

namespace detail {

template <typename T>
struct en
{
    static const std::size_t size = 5;

    T const& operator[](size_t i) const { return data[i]; }
    T & operator[](size_t i) { return data[i]; }

private:
    T data[5];
};

template <typename T>
inline en<T> pj_enfn(T const& es)
{
    static const T C00 = 1.;
    static const T C02 = .25;
    static const T C04 = .046875;
    static const T C06 = .01953125;
    static const T C08 = .01068115234375;
    static const T C22 = .75;
    static const T C44 = .46875;
    static const T C46 = .01302083333333333333;
    static const T C48 = .00712076822916666666;
    static const T C66 = .36458333333333333333;
    static const T C68 = .00569661458333333333;
    static const T C88 = .3076171875;

    T t;
    detail::en<T> en;

    {
        en[0] = C00 - es * (C02 + es * (C04 + es * (C06 + es * C08)));
        en[1] = es * (C22 - es * (C04 + es * (C06 + es * C08)));
        en[2] = (t = es * es) * (C44 - es * (C46 + es * C48));
        en[3] = (t *= es) * (C66 - es * C68);
        en[4] = t * es * C88;
    }

    return en;
}

template <typename T>
inline T pj_mlfn(T const& phi, T sphi, T cphi, detail::en<T> const& en)
{
    cphi *= sphi;
    sphi *= sphi;
    return(en[0] * phi - cphi * (en[1] + sphi*(en[2]
        + sphi*(en[3] + sphi*en[4]))));
}

template <typename T>
inline T pj_inv_mlfn(T const& arg, T const& es, detail::en<T> const& en)
{
    static const T EPS = 1e-11;
    static const int MAX_ITER = 10;

    T s, t, phi, k = 1./(1.-es);
    int i;

    phi = arg;
    for (i = MAX_ITER; i ; --i) { /* rarely goes over 2 iterations */
        s = sin(phi);
        t = 1. - es * s * s;
        phi -= t = (pj_mlfn(phi, s, cos(phi), en) - arg) * (t * sqrt(t)) * k;
        if (geometry::math::abs(t) < EPS)
            return phi;
    }
    BOOST_THROW_EXCEPTION( projection_exception(error_non_conv_inv_meri_dist) );
    return phi;
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif


/*
 *  Copyright (c) 2016-2021 Jeremy HU <jeremy-at-dust3d dot org>. All rights reserved.
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:

 *  The above copyright notice and this permission notice shall be included in all
 *  copies or substantial portions of the Software.

 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *  SOFTWARE.
 */

#include <Eigen/Dense>
#include <cmath>
#include <dust3d/uv/triangulate.h>

namespace dust3d
{
namespace uv
{

static Eigen::Vector3d norm(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, const Eigen::Vector3d &p3)
{
    auto side1 = p2 - p1;
    auto side2 = p3 - p1;
    auto perp = side1.cross(side2);
    return perp.normalized();
}

static float angle360(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &direct)
{
    auto angle = atan2((a.cross(b)).norm(), a.dot(b)) * 180.0 / 3.1415926;
    auto c = a.cross(b);
    if (c.dot(direct) < 0) {
        angle = 360 - angle;
    }
    return angle;
}

static Eigen::Vector3d vertexToEigenVector3d(const Vertex &vertex)
{
    return Eigen::Vector3d(vertex.xyz[0], vertex.xyz[1], vertex.xyz[2]);
}

static bool pointInTriangle(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &c, const Eigen::Vector3d &p)
{
    auto u = b - a;
    auto v = c - a;
    auto w = p - a;
    auto vXw = v.cross(w);
    auto vXu = v.cross(u);
    if (vXw.dot(vXu) < 0.0) {
        return false;
    }
    auto uXw = u.cross(w);
    auto uXv = u.cross(v);
    if (uXw.dot(uXv) < 0.0) {
        return false;
    }
    auto denom = uXv.norm();
    auto r = vXw.norm() / denom;
    auto t = uXw.norm() / denom;
    return r + t <= 1.0;
}

static Eigen::Vector3d ringNorm(const std::vector<Vertex> &vertices, const std::vector<size_t> &ring)
{
    Eigen::Vector3d normal(0.0, 0.0, 0.0);
    for (size_t i = 0; i < ring.size(); ++i) {
        auto j = (i + 1) % ring.size();
        auto k = (i + 2) % ring.size();
        const auto &enter = vertexToEigenVector3d(vertices[ring[i]]);
        const auto &cone = vertexToEigenVector3d(vertices[ring[j]]);
        const auto &leave = vertexToEigenVector3d(vertices[ring[k]]);
        normal += norm(enter, cone, leave);
    }
    return normal.normalized();
}

void triangulate(const std::vector<Vertex> &vertices, std::vector<Face> &faces, const std::vector<size_t> &ring)
{
    if (ring.size() < 3)
        return;
    std::vector<size_t> fillRing = ring;
    Eigen::Vector3d direct = ringNorm(vertices, fillRing);
    while (fillRing.size() > 3) {
        bool newFaceGenerated = false;
        for (decltype(fillRing.size()) i = 0; i < fillRing.size(); ++i) {
            auto j = (i + 1) % fillRing.size();
            auto k = (i + 2) % fillRing.size();
            const auto &enter = vertexToEigenVector3d(vertices[fillRing[i]]);
            const auto &cone = vertexToEigenVector3d(vertices[fillRing[j]]);
            const auto &leave = vertexToEigenVector3d(vertices[fillRing[k]]);
            auto angle = angle360(cone - enter, leave - cone, direct);
            if (angle >= 1.0 && angle <= 179.0) {
                bool isEar = true;
                for (size_t x = 0; x < fillRing.size() - 3; ++x) {
                    auto h = (i + 3 + x) % fillRing.size();
                    auto fourth = vertexToEigenVector3d(vertices[fillRing[h]]);
                    if (pointInTriangle(enter, cone, leave, fourth)) {
                        isEar = false;
                        break;
                    }
                }
                if (isEar) {
                    Face newFace;
                    newFace.indices[0] = fillRing[i];
                    newFace.indices[1] = fillRing[j];
                    newFace.indices[2] = fillRing[k];
                    faces.push_back(newFace);
                    fillRing.erase(fillRing.begin() + j);
                    newFaceGenerated = true;
                    break;
                }
            }
        }
        if (!newFaceGenerated)
            break;
    }
    if (fillRing.size() == 3) {
        Face newFace;
        newFace.indices[0] = fillRing[0];
        newFace.indices[1] = fillRing[1];
        newFace.indices[2] = fillRing[2];
        faces.push_back(newFace);
    }
}

}
}


#include <armadillo>
#include <chrono>
#include <stdlib.h>
#include "apply_pauli.cpp"

using namespace std;
using namespace arma;

int main()
{
  int n = 32768;
  cx_vec psi1, psi2;

  psi1.randn(n);
  psi2.randn(n);

  psi2 = psi1;
  //cout << psi1 << endl;

  std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
  apply_pauli_fast(0, true, true, psi1);
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
  std::cout << "Fast algorithm" << endl;
  std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() << "[µs]" << std::endl;
  std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end - begin).count() << "[ns]" << std::endl;
  //cout << psi1 << endl;

  //  cout << psi2 << endl;
  /*
  std::chrono::steady_clock::time_point begin2 = std::chrono::steady_clock::now();
  apply_pauli_slow(0, true, true, psi2);
  std::chrono::steady_clock::time_point end2 = std::chrono::steady_clock::now();
  std::cout << "Slow algorithm" << endl;
  std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::microseconds>(end2 - begin2).count() << "[µs]" << std::endl;
  std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end2 - begin2).count() << "[ns]" << std::endl;
  */
  cout << sum(abs(psi1-psi2)) << endl;
}


/*  Sirikata
 *  TransformFilter.cpp
 *
 *  Copyright (c) 2010, Ewen Cheslack-Postava
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are
 *  met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *  * Neither the name of Sirikata nor the names of its contributors may
 *    be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "TransformFilter.hpp"
#include <sirikata/mesh/Meshdata.hpp>
#include <boost/lexical_cast.hpp>

namespace Sirikata {
namespace Mesh {

Filter* TransformFilter::createTranslate(const String& args) {
    Vector3f offset = boost::lexical_cast<Vector3f>(args);
    return new TransformFilter( Matrix4x4f::translate(offset) );
}

Filter* TransformFilter::createScale(const String& args) {
    float sc = boost::lexical_cast<float>(args);
    return new TransformFilter( Matrix4x4f::scale(sc) );
}

Filter* TransformFilter::createRotate(const String& args) {
    Quaternion rot = boost::lexical_cast<Quaternion>(args);
    return new TransformFilter( Matrix4x4f::rotate(rot) );
}

TransformFilter::TransformFilter(const Matrix4x4f& xform)
 : mTransform(xform)
{
}

FilterDataPtr TransformFilter::apply(FilterDataPtr input) {
    // To apply the transform, we just add a new node and put all root nodes as
    // children of it.

    for(FilterData::const_iterator md_it = input->begin(); md_it != input->end(); md_it++) {
        VisualPtr vis = *md_it;
        MeshdataPtr md( std::tr1::dynamic_pointer_cast<Meshdata>(vis) );

        if (!md) {
            SILOG(transform-filter, warning, "Can't transform this type of visual: " << vis->type());
            continue;
        }

        Node new_root(NullNodeIndex, mTransform);
        new_root.children = md->rootNodes;

        NodeIndex new_root_index = md->nodes.size();
        md->nodes.push_back(new_root);

        md->rootNodes.clear();
        md->rootNodes.push_back(new_root_index);
    }

    return input;
}

} // namespace Mesh
} // namespace Sirikata


#include "Classes.h"
#include <Eigen/Dense>
#include <algorithm>
#include <fstream>
#include <cstdlib>
using namespace std;
using namespace Eigen;

Wireframe Projection2D::create3D(){
    ///
    /// Function that creates the wireframe model from the 2d orthographic views
    ///
    cout << "lets start"<<endl;
    allpoints = determineAllPoints();
    cout <<"adjacencyMatrix point" <<endl;
    // printmatrix(adjacencyMatrix);

    determineIntersectedEdges();
    cout << "determineintersected edges complete "<< endl;
    // printmatrix(adjacencyMatrix);
    cout << "execute corollary 1" <<endl;
    executeCorollary1();
    // printmatrix(adjacencyMatrix);

    cout << "corollary 1 finished chek3edge ==================================================="<< endl;
    chkif3edgesanddefthem();
    // printmatrix(adjacencyMatrix);

    cout<< "chek3edge finished loop starting==================================================="<< endl;
    int numcountwhile = 0;
    while(numofpossibleedge()!=0){
        //function below checks if for a point a possible and definite point are collinear and removes the possible point
        cout << "chekcollinearpossanddef started ================================================================" << endl;
        bool ifcollinearpossanddef = chkcollinearpossanddef();
        // printmatrix(adjacencyMatrix);
        cout << "chkcollinearpossandposs started ================================================================" <<endl;
        bool ifcollinearpossandposs = chkcollinearpossandposs();
        // printmatrix(adjacencyMatrix);
        cout << "chkposshasdefinanother started ================================================================" <<endl;

        bool ifposshasdefinother = chkposshasdefinother();
        // printmatrix(adjacencyMatrix);
        cout << "chk3dgesanddefthem started ================================================================" <<endl;

        chkif3edgesanddefthem();
        // printmatrix(adjacencyMatrix);
        numcountwhile++;
        if(numcountwhile==50){
            cout << "wrong answer"<<endl;
            break;
        }
        // break;
    }
    cout << "numcount is "<< numcountwhile << endl;
    Wireframe answer;
    answer.vertices = allpoints;
    for(auto verticesitr = answer.vertices.begin();verticesitr!=answer.vertices.end();verticesitr++){
        cout << "final points are "<< *verticesitr<<endl;
    }
    int numpoints = allpoints.size();
    for(int i=0;i<numpoints;i++){
        for(int j=0;j<numpoints;j++){
            if(adjacencyMatrix[i][j]==2){
                Edge thisedge;
                thisedge.p1 = * (indextopointmap[i]);
                thisedge.p2 = * (indextopointmap[j]);
                answer.edges.push_back(thisedge);
            }
        }
    }
    int numedges = answer.edges.size();
    for(int i=0;i<numedges;i++){
        cout << "thisedge is " << answer.edges[i].p1 << " is connected to " << answer.edges[i].p2 << endl;
    }
    answer.adjacencyMatrix = adjacencyMatrix;
    cout << "final print of adjacencyMatrix"<<endl;
    printmatrix(answer.adjacencyMatrix);
    numpoints = allpoints.size();
    for(int i=0;i<numpoints;i++){
        vector<int> neighvec = findneighbours(i);
        // cout << "i is "<<i <<endl;
        // cout<< "found neighbours"<<endl;
        int numneigh = neighvec.size();
        for(int j=0;j<numneigh;j++){
            vector<vector<int> > allpaths = giveallpaths(i,neighvec[j]);
            // cout << "given all paths for j = "<<j<<"or neighbour "<<neighvec[j]<<endl;
            // for(int k=0;k<allpaths.size();k++){
            //     for(int l=0;l<allpaths[k].size();l++){
            //         cout << allpaths[k][l] << " ";
            //     }
            //     cout<<endl;
            // }
            // cout<< "printed allpaths"<<endl;
            vector<vector<int> > goodpaths = giveplanarpaths(allpaths);
            // cout << "given all goodpaths for j = "<<j<<endl;
            // for(int k=0;k<goodpaths.size();k++){
            //     for(int l=0;l<goodpaths[k].size();l++){
            //         cout << goodpaths[k][l] << " ";
            //     }
            //     cout<<endl;
            // }
            // cout<< "printed goodpaths"<<endl;
            addsuitablepaths(goodpaths);
            // cout << "added suitable paths for j = "<<j<<endl;
        }
    }
    cout << "printing final faces"<<endl;
    for(int k=0;k<deducedfaces.size();k++){
                for(int l=0;l<deducedfaces[k].size();l++){
                    cout << indextopointmap[deducedfaces[k][l]]->label << " ";
                }
                cout<<endl;
    }
    cout << "printed final faces"<<endl;
    makescadfile();
    return answer;
}

//doublecalculaedpossibleedges has all the edges calculated twice now

vector<Point> Projection2D::determineAllPoints(){
    ///
    /// Function to determine points by intersection of the corresponding points from topview and frontview
    ///
    vector<Point> answer;
    for(auto it=(frontview.vertices.begin());it!=frontview.vertices.end();it++){
        for(auto nestit=(it->points.begin());nestit!=it->points.end();nestit++){
            string thislabel = nestit->label;
            //cout << "iteration on front label "<<thislabel<<endl;
            for(auto itfind=topview.vertices.begin();itfind!=topview.vertices.end();itfind++){
                bool found = false;
                for(auto nestitfind=itfind->points.begin();nestitfind!= itfind->points.end();nestitfind++){
                    string findlabel = nestitfind->label;
                    //cout << "   corresponding iteration on top label " << findlabel <<endl;
                    if(thislabel==findlabel){
                        //cout << "   aloha found the same label"<<endl;
                        found=true;
                        //Point determinedpoint = new Point;
                        Point determinedpoint = determinePoint(*nestit, *nestitfind);
                        //cout << "   determined point is "<< determinedpoint << endl;
                        int sizeanswer = answer.size();
                        determinedpoint.adjacencyIndex=sizeanswer;
                        answer.push_back(determinedpoint);
                        sizeanswer = answer.size();
                        //determinedpoint = answer[sizeanswer-1];

                        //Point * pointerdeterpoint = new Point;
                        //Point * pointerdeterpoint = &(determinedpoint);
                        //cout << "   determined point is "<< * pointerdeterpoint << endl;
                        //cout << "sizeanswer is "<<sizeanswer <<endl;
                        //indextopointmap.insert(make_pair(sizeanswer-1,&(determinedpoint)));
                        // indextopointmap[sizeanswer-1] = & (answer[sizeanswer-1]);
                        //labeltopointmap.insert(make_pair(determinedpoint.label,&(determinedpoint)));
                        // labeltopointmap[determinedpoint.label]=& (answer[sizeanswer-1]);
                        //knownpoints.push_back(&determinedpoint);
                        //cout << "sizeanswer - 1 is" << (sizeanswer-1) <<endl;
                        //cout << "label is " << determinedpoint.label<<endl;
                        //cout << "   isindextopoint working "<< * indextopointmap[sizeanswer-1]<<endl;
                        //cout << "   islabeltopoint working "<< * labeltopointmap[determinedpoint.label]<<endl;
                        break;
                    }

                }
                if(found==true){
                    break;
                }
            }

        }

    }
    // cout << "lets make the indextopointmap ============================================================="<< endl;
    int numpoints = answer.size();
    for(vector<Point>::iterator iteranswer = answer.begin();iteranswer!=answer.end();iteranswer++){
        // cout<< "adding " << * iteranswer << endl;
        indextopointmap.insert(make_pair(iteranswer->adjacencyIndex,&(* iteranswer)));
    }
    for(vector<Point>::iterator iteranswer = answer.begin();iteranswer!=answer.end();iteranswer++){
        // cout<< "adding " << * iteranswer << endl;
        labeltopointmap.insert(make_pair(iteranswer->label,&(* iteranswer)));
    }
    //adjacencyMatrix(numpoints,temp);
    // cout << "iterating over labeltopointmap========================================================================================" <<endl;
    // for(map<string,Point *>::iterator it = labeltopointmap.begin();it!=labeltopointmap.end();it++){
    //     cout << it->first << " " << * (it->second) << endl;
    // }
    // cout << "iterating over indextopointmap========================================================================================" <<endl;

    // for(map<int,Point *>::iterator it = indextopointmap.begin();it!=indextopointmap.end();it++){
    //     cout << it->first << " " << * (it->second) << endl;
    // }
    for(int i=0;i<numpoints;i++){
        // cout << "answer[i] is " << answer[i]<<endl;
        // cout << "check for label to point" << * labeltopointmap.at(answer[i].label)<< endl;
        // cout << "check for int to point" << * indextopointmap.at(answer[i].adjacencyIndex)<< endl;

        vector<int> temp(numpoints,0);
        adjacencyMatrix.push_back(temp);
    }

    return answer;
}

Point Projection2D::determinePoint(Point pfront, Point ptop) {
    ///
    /// This function extends the points from the top view and front view to give the 3D projection of the Point
    ///
    Point coincidingpoint;
    if(!(pfront.x == ptop.x)){
        //throw exception that not possible
        cout << "not possible projection" << endl;
    }
    coincidingpoint.setCoordinatesAndLabel(ptop.x,ptop.y,pfront.z,ptop.label);
    return coincidingpoint;
}

pair <vector<Edge>, vector<vector<int> > > Projection2D::determinePossibleEdges(Point vertex, vector<Point> *topneighbourptr, vector<Point> *frontneighbourptr) {
    ///
    /// This function determines the edges containing the Point vertex as one of the endpoints
    /// By interpreting its neighbours from the top view and front view
    ///
    vector<Edge> answer;
    vector<vector<int> > thismatrix;
    int numpoints = allpoints.size();
    for(int i=0;i<numpoints;i++){
        vector<int> temp(numpoints,0);
        thismatrix.push_back(temp);
    }

    vector<Point> intersection;
    vector<Point> topneighbour = * (topneighbourptr);

    vector<Point> frontneighbour = * (frontneighbourptr);
    // cout<< "secondneighbours of consideration are --------------------------------------------"<< endl;
    // for(int i=0;i<frontneighbour.size();i++){
    //     cout << frontneighbour[i] << endl;
    // }
    for(auto topneighbouritr=topneighbourptr->begin();topneighbouritr!=topneighbourptr->end();topneighbouritr++){
        auto correspondingpointinfrontasptr = find_if(frontneighbourptr->begin(),frontneighbourptr->end(),[topneighbouritr](Point p)->bool{
            return(p.label==topneighbouritr->label);
        });
        if(correspondingpointinfrontasptr!=frontneighbourptr->end()){
            // cout<< "the collided point will be "<< correspondingpointinfrontasptr->label<<endl;
            Point collidedpoint = * labeltopointmap[correspondingpointinfrontasptr->label];
            intersection.push_back(collidedpoint);
        }
    }
    int numintersectionp = intersection.size();
    // for(int i=0;i<numintersectionp;i++){
    //     cout << "intersection points are "<< intersection[i]<<endl;
    // }
    int numpointsaa = allpoints.size();

    for(auto intersectionitr = intersection.begin();intersectionitr!=intersection.end();intersectionitr++){
        Edge tempedge;
        tempedge.p1 = vertex;
        tempedge.p2 = *(labeltopointmap[intersectionitr->label]);
        //int i = vertex.adjacencyIndex;
        int i = labeltopointmap[vertex.label]->adjacencyIndex;
        int j = labeltopointmap[intersectionitr->label]->adjacencyIndex;
        //adjacencyMatrix[i][j]=adjacencyMatrix[i][j]+1;
        //adjacencyMatrix[j][i]=adjacencyMatrix[j][i]+1;
        thismatrix[i][j] = 1;
        thismatrix[j][i] = 1;
        answer.push_back(tempedge);
    }

    return make_pair(answer,thismatrix);

}

void Projection2D::determineIntersectedEdges(){
    ///
    ///Funtion to determine all the  possible edges for all the points
    ///
    for(auto pointitr=allpoints.begin();pointitr!=allpoints.end();pointitr++){
        // cout << "-----------------------------------------------------------------------" <<endl;
        // cout << "iterating on point "<< pointitr->label <<endl;
        // cout << "-----------------------------------------------------------------------" <<endl;
        vector<Point> topneighbours = topview.possibleNeighbours(* pointitr);
        // cout << "-----------------------------------------------------------------------" <<endl;
        // cout << "topneighbours are "<<endl;
        // for(int i=0;i<topneighbours.size();i++){
        //     cout << topneighbours[i]<< endl;
        // }
        // cout << "-----------------------------------------------------------------------" <<endl;
        vector<Point> frontneighbours = frontview.possibleNeighbours(* pointitr);
        // cout << "-----------------------------------------------------------------------" <<endl;
        // cout << "frontneighbours are "<<endl;
        // for(int i=0;i<frontneighbours.size();i++){
        //     cout << frontneighbours[i]<< endl;
        // }
        // cout << "-----------------------------------------------------------------------" <<endl;
        vector<Point> sideneighbours = sideview.possibleNeighbours(* pointitr);
        // cout << "-----------------------------------------------------------------------" <<endl;
        // cout << "sideneighbours are "<<endl;
        // for(int i=0;i<sideneighbours.size();i++){
        //     cout << sideneighbours[i]<< endl;
        // }
        // cout << "-----------------------------------------------------------------------" <<endl;
        // cout<< "we have to intersect topandfront ------------------------------"<<endl;
        // vector<Edge> topAndFront = determinePossibleEdges(*pointitr,&topneighbours,&frontneighbours).first;
        auto topandfrontmatrix = determinePossibleEdges(*pointitr,&topneighbours,&frontneighbours).second;
        // cout<< "topandfronthasbeenintersected -------------------"<<endl;

        // cout<< "we have to intersect sideandfront ------------------------------"<<endl;

        // vector<Edge> sideAndFront = determinePossibleEdges(*pointitr,&sideneighbours,&frontneighbours).first;
        auto sideandfrontmatrix = determinePossibleEdges(*pointitr,&sideneighbours,&frontneighbours).second;
        // cout << "sideandfronthasbeenintersected -----------------------"<<endl;

        // cout<< "we have to intersect topandside ------------------------------"<<endl;

        // vector<Edge> sideAndFront = determinePossibleEdges(*pointitr,&sideneighbours,&frontneighbours).first;
        auto topandsidematrix = determinePossibleEdges(*pointitr,&topneighbours,&sideneighbours).second;
        // cout << "topandfronthasbeenintersected -----------------------"<<endl;

        int numpoints = allpoints.size();
        for(int i = 0; i<numpoints;i++){
            for(int j=0;j<numpoints;j++){
                if(topandfrontmatrix[i][j]==1 && sideandfrontmatrix[i][j]==1 && topandsidematrix[i][j]==1){
                    adjacencyMatrix[i][j] =1;
                }else{
                    //adjacencyMatrix[i][j]=0;
                }
            }
        }

    }
}

void Projection2D::executeCorollary1onebyone(OrthoProjection* view1, OrthoProjection* view2, OrthoProjection* view3){
    ///
    /// Function to execute corollary one of superimposed points in one view and possible edge in other view for each view
    ///
    for(auto clusteritr = view1->vertices.begin();clusteritr!=view1->vertices.end();clusteritr++){
        if(clusteritr->points.size()==2){
            bool isconnectedinotherview2 = view2->isConnected(clusteritr->points[0].label,clusteritr->points[1].label);
            bool isconnectedinotherview3 = view3->isConnected(clusteritr->points[0].label,clusteritr->points[1].label);
            if(isconnectedinotherview2 && isconnectedinotherview3){
                int i = labeltopointmap[clusteritr->points[0].label]->adjacencyIndex;
                int j = labeltopointmap[clusteritr->points[1].label]->adjacencyIndex;
                if(adjacencyMatrix[i][j]==1 || adjacencyMatrix[j][i]==1){
                    adjacencyMatrix[i][j] = 2;
                    adjacencyMatrix[j][i] = 2;
                }
            }
        }
    }
}
void Projection2D::executeCorollary1(){
    ///
    /// Function to execute corollary one for all views
    ///
    executeCorollary1onebyone(&topview,&frontview,&sideview);

    executeCorollary1onebyone(&frontview,&topview,&sideview);
    // executeCorollary1onebyone(&frontview,&sideview);
    executeCorollary1onebyone(&sideview,&frontview,&topview);
    // executeCorollary1onebyone(&topview,&sideview);
    // executeCorollary1onebyone(&sideview,&topview);

}
void Projection2D::chkif3edgesanddefthem(){
    ///
    /// Function to check if there are only three possible edges and then make them the final edge
    ///
    int numpointsall = allpoints.size();
    for(int i=0; i<numpointsall;i++){
        int numedge = count_if(adjacencyMatrix[i].begin(),adjacencyMatrix[i].end(),[](int v)->bool{
            return(v>0);
        });
        // cout << "for int i " << i << " " << numedge << endl;
        if(numedge==3){
            for(int j=0;j<numpointsall;j++){
                if(adjacencyMatrix[i][j]==1){
                    adjacencyMatrix[i][j] = 2;
                    adjacencyMatrix[j][i] = 2;
                }
            }
        }
    }
}
int Projection2D::numofpossibleedge(){
    ///
    /// Function to determine possible edges
    ///
    int num1=0;
    int numpointsall = allpoints.size();
    for(int i=0;i<numpointsall;i++){
        num1 = num1 + count(adjacencyMatrix[i].begin(),adjacencyMatrix[i].end(),1);
    }
    return num1;
}
bool Projection2D::chkcollinearpossanddef(){
    ///
    /// check if a point has two collinear points as its edge, one being definite and other being possible, if so, remove the possible edge
    ///
    int numpoints = allpoints.size();
    bool ret =false;
    for(int i=0;i<numpoints;i++){
        Point * thispoint = & (allpoints[i]);

        vector<int> thisvec = adjacencyMatrix[i];
        int numposs = count(thisvec.begin(),thisvec.end(),1);
        int numdef = count(thisvec.begin(),thisvec.end(),2);
        if(numposs>0 && numdef>0){
            vector<int> defindexvec;
            for(int j=0;j<numpoints;j++){
                if(thisvec[j]==2){
                    defindexvec.push_back(j);
                }
            }
            for(int j=0;j<numpoints;j++){
                if(thisvec[j]==1){
                    Point * posspoint = (indextopointmap[j]);
                    for(auto defindexvecitr = defindexvec.begin();defindexvecitr!=defindexvec.end();defindexvecitr++){
                        Point * defpoint = indextopointmap[(* defindexvecitr)];
                        bool iscollinear = thispoint->checkcollinear(posspoint, defpoint);
                        if(iscollinear){
                            adjacencyMatrix[i][j] = 0;
                            adjacencyMatrix[j][i] = 0;
                            ret = true;
                            break;
                        }
                    }
                }
            }
        }
    }
    return ret;
}
bool Projection2D::chkcollinearpossandposs(){
    ///
    /// if two points are collinear and possible edges only one of them could be definite, definite being the closest point,
    /// Given that the point whose neighbours are being calculated lies on one end
    ///
    // cout << "poss and poss started "<< endl;
    int numpoints = allpoints.size();
    bool ret =false;
    for(int i=0;i<numpoints;i++){
        Point * thispoint = new Point;
        thispoint = & (allpoints[i]);
        // cout << "point of consideration is " << *thispoint << endl;
        vector<int> thisvec = adjacencyMatrix[i];
        int numposs = count(thisvec.begin(),thisvec.end(),1);
        int numdef = count(thisvec.begin(),thisvec.end(),2);
        // cout << "numpos and numdef are " << numposs << " " << numdef << endl;
        if(numposs>=2){
            for(int j=0;j<numpoints;j++){
                if(adjacencyMatrix[i][j]==1){
                    for(int k=j+1;k<numpoints;k++){
                        if(adjacencyMatrix[i][k]==1){
                            // cout << "j and k are " << j << " " << k << endl;
                            if(thispoint->checkcollinear(indextopointmap[j],indextopointmap[k])){
                                // cout << "j and k are collinear "<< endl;
                                Point * jpointptr = new Point;
                                Point * kpointptr = new Point;
                                jpointptr = indextopointmap[j];
                                kpointptr = indextopointmap[k];
                                // cout << "jpoint is " << *jpointptr << endl;
                                // cout << "kpoint is " << *kpointptr << endl;
                                for(int it=0;it<3;it++){
                                    // cout << "it is " <<it << endl;
                                    vector<Point> topcluster;
                                    if(it==0){
                                        topcluster =topview.sameclusterpointsincluded(*thispoint);
                                    }else if(it==1){
                                        topcluster = frontview.sameclusterpointsincluded(*thispoint);
                                    }else{
                                        topcluster = sideview.sameclusterpointsincluded(*thispoint);
                                    }
                                    // cout << "topcluster computed" <<endl;
                                    if(topcluster.size()==0){
                                        continue;
                                    }
                                    // cout << "topcluster size isnt 0" << endl;
                                    auto jiterator = find_if(topcluster.begin(),topcluster.end(),[jpointptr](Point p)->bool{
                                        return(jpointptr->label==p.label);
                                    });
                                    if(jiterator!=topcluster.end()){
                                        // cout << "it must enter in this j"<< endl;
                                        auto kiterator = find_if(topcluster.begin(),topcluster.end(),[kpointptr](Point p)->bool{
                                            return(kpointptr->label==p.label);
                                        });
                                        if(kiterator!=topcluster.end()){
                                            // cout << "it must enter in this k"<< endl;
                                            int topclustersize = topcluster.size();
                                            // cout << "end and start of this cluster are "<< topcluster[0].label << " and "<< topcluster[topclustersize-1].label << endl;
                                            if((topcluster[0].label)==(thispoint->label) || (topcluster[topclustersize-1].label)==(thispoint->label)){
                                                // cout << "do not change the end part "<< endl;
                                                Vector3d a1(thispoint->x,thispoint->y,thispoint->z);
                                                Vector3d a2(jpointptr->x,jpointptr->y,jpointptr->z);
                                                Vector3d a3(kpointptr->x,kpointptr->y,kpointptr->z);
                                                double dis1 = (a2-a1).norm();
                                                double dis2 = (a3-a1).norm();
                                                if(dis1<dis2){
                                                    adjacencyMatrix[i][j]=2;
                                                    adjacencyMatrix[j][i]=2;
                                                    adjacencyMatrix[i][k]=0;
                                                    adjacencyMatrix[k][i]=0;
                                                }else{
                                                    adjacencyMatrix[i][j]=0;
                                                    adjacencyMatrix[j][i]=0;
                                                    adjacencyMatrix[i][k]=2;
                                                    adjacencyMatrix[k][i]=2;
                                                }
                                                ret = true;
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    return ret;
}
bool Projection2D::chkposshasdefinother(){
    ///
    /// Function to check if a possible edge point has definite neighbours with other edges, if so remove them
    ///
    int numpoints = allpoints.size();
    bool ret =false;
    for(int i=0;i<numpoints;i++){
        Point * thispoint = & (allpoints[i]);
        //vector<int> thisvec = adjacencyMatrix[i];
        vector<int> posspointsvec,defpointsvec;
        for(int j=0;j<numpoints;j++){
            if(adjacencyMatrix[i][j]==1){
                posspointsvec.push_back(j);
            }else if(adjacencyMatrix[i][j]==2){
                defpointsvec.push_back(j);
            }
        }
        if(defpointsvec.size()<2){
            continue;
        }
        for(int j=0;j<posspointsvec.size();j++){
            int thisposspointindex = posspointsvec[j];
            int numberofintersections = 0;
            for(int k=0;k<defpointsvec.size();k++){
                if(adjacencyMatrix[thisposspointindex][defpointsvec[k]]==2){
                    numberofintersections++;
                }
            }
            if(numberofintersections>=2){
                adjacencyMatrix[i][thisposspointindex]=0;
                adjacencyMatrix[thisposspointindex][i]=0;
                ret =true;
            }
        }
    }
    return ret;
}

void Projection2D::printmatrix(vector<vector<int> > thisone){
    ///
    /// Function to print the matrix
    ///
    int tempint = allpoints.size();
        for(int i=-1;i<tempint;i++){
        for(int j=-1;j<tempint;j++){
            if(i==-1){
                if(j!=-1){
                    cout << allpoints[j].label << " ";
                    if(j==allpoints.size()-1){
                        cout << endl;
                    }
                }else{
                    cout << "  ";
                }
            }else{
                if(j!=-1){
                    cout << thisone[i][j]<< " ";
                    if(j == allpoints.size()-1){
                        cout << endl;
                    }
                }else{
                    cout << allpoints[i].label << " ";
                }

            }
        }
    }
}

vector<int> Projection2D::findneighbours(int thispoint){
    ///
    /// function to find the neighbours of a point
    ///

    vector<int> neighvec;
    int numpoints = allpoints.size();
    for(int i = 0 ;i<numpoints;i++){
        if(adjacencyMatrix[thispoint][i]==2){
            neighvec.push_back(i);
        }
    }
    return neighvec;
}
vector<vector<int> > Projection2D::giveallpaths(int start, int dest){
    ///
    /// function to give all the possible paths from start to destination
    ///
    int numpoints = allpoints.size();
    vector<vector<int> > allpaths;
    bool *visited = new bool[numpoints];
    int *path = new int[numpoints];
    int path_index = 0;
    for (int i = 0; i < numpoints; i++)
        visited[i] = false;
    giveAllPathsUtil(start, dest, visited, path, path_index, allpaths);
    return allpaths;
}

void Projection2D::giveAllPathsUtil(int u, int d, bool visited[], int path[], int &path_index, vector<vector<int> > &allpaths){
    ///
    /// function to give all possible paths from u to d
    ///
    visited[u] = true;
    path[path_index] = u;
    path_index++;
    if (u == d)
    {
        // vector<int> thispath(path, path + path_index);
        vector<int> thispath;
        for(int i = 0;i<path_index;i++){
            thispath.push_back(path[i]);
        }
        allpaths.push_back(thispath);
    }
    else // If current vertex is not destination
    {
        // Recur for all the vertices adjacent to current vertex
        vector<int> neighvec = findneighbours(u);
        for(auto iter= neighvec.begin();iter!=neighvec.end();iter++){
            if(!visited[*iter]){
                giveAllPathsUtil(*iter, d, visited, path, path_index,allpaths);
            }
        }
    }
    path_index--;
    visited[u] = false;
}

vector<vector<int> > Projection2D::giveplanarpaths(vector<vector<int> > allpaths){
    ///
    /// function to filter out non-planar paths
    ///
    vector<vector<int> > filteredpaths;
    int numallpaths = allpaths.size();
    for(int i=0;i<numallpaths;i++){
        vector<int> thispath = allpaths[i];
        if(thispath.size()==3){
            filteredpaths.push_back(thispath);
            // cout<<"adding path of lenght 3"<<endl;
        }else if(thispath.size()>3){
            bool isplanarbool= isplanar(thispath);
            if(isplanarbool){
                filteredpaths.push_back(thispath);
            }
        }

    }
    return filteredpaths;
}

bool Projection2D::isplanar(vector<int> thispath){
    ///
    /// function to check if a path is planar or not
    ///
    bool isplanar=true;
    int thispathsize = thispath.size();
    // cout<<"vector of consideration is "<<endl;
    // for(int i=0;i<thispathsize;i++){
    //     cout << thispath[i]<<" ";
    // }
    // cout <<endl;
    Point firstpoint = * (indextopointmap[thispath[0]]);
    Point secondpoint = * (indextopointmap[thispath[1]]);
    Point thirdpoint = * (indextopointmap[thispath[2]]);
    Vector3d firstpointvec(firstpoint.x,firstpoint.y,firstpoint.z);
    Vector3d secondpointvec(secondpoint.x,secondpoint.y,secondpoint.z);
    Vector3d thirdpointvec(thirdpoint.x,thirdpoint.y,thirdpoint.z);
    Vector3d firstedge = secondpointvec-firstpointvec;
    Vector3d secondedge = thirdpointvec-secondpointvec;
    Vector3d normal = firstedge.cross(secondedge);
    for(int i=3;i<thispathsize;i++){
        Point thispoint = * (indextopointmap[thispath[i]]);
        Point prevpoint = * (indextopointmap[thispath[i-1]]);
        Vector3d thispointvec(thispoint.x,thispoint.y,thispoint.z);
        Vector3d prevpointvec(prevpoint.x,prevpoint.y,prevpoint.z);
        Vector3d thisedge = thispointvec-prevpointvec;
        double dotproduct = normal.dot(thisedge);
        if(dotproduct==0.0){
            continue;
        }else{
            isplanar=false;
            break;
        }
    }
    // cout <<"was this planar "<<isplanar<<endl;
    return isplanar;
}

void Projection2D::addsuitablepaths(vector<vector<int> > goodpaths){
    ///
    /// function to add planar paths to faces
    ///
    for(auto iter = goodpaths.begin();iter!= goodpaths.end();iter++){
        if(!ispresent(*iter)){
            deducedfaces.push_back(*iter);
        }
    }
}

bool Projection2D::ispresent(vector<int> thispath){
    ///
    /// function to check if the given face has already been added or not
    ///
    bool ispresent = false;
    for(auto iter = deducedfaces.begin();iter!=deducedfaces.end();iter++){
        if(isequavalent(*iter,thispath)){
            ispresent=true;
            break;
        }else{
            continue;
        }
    }
    return ispresent;
}

bool Projection2D::isequavalent(vector<int> vec1,vector<int> vec2){
    ///
    /// function to check whether two representation of vectors are equal
    ///
    bool isequavalent = false;
    if(vec1.size()==vec2.size()){
        sort(vec1.begin(),vec1.end());
        sort(vec2.begin(),vec2.end());
        for(int i=0;i<vec1.size();i++){
            if(vec1[i]!=vec2[i]){
                return false;
            }
        }
        isequavalent=true;
        return true;
    }
    return isequavalent;
}
void Projection2D::makescadfile(){
    ///
    /// function to make scad file, stl, render scad file and then run it
    ///
    ofstream scadfile ("object.scad");
    if(scadfile.is_open()){
        // scadfile<< "cube([2,3,9]);";
        scadfile <<"ObjectPoints = [\n";
        int numpoints = allpoints.size();
        for(int i=0;i<numpoints;i++){
            Point temp = allpoints[i];
            scadfile<<"    [ "<<temp.x<<", "<<temp.y<<", "<<temp.z<<" ]";
            if(i!=numpoints-1){
                scadfile<<",";
            }else{
                scadfile<<"];";
            }
            scadfile<<  "  //"<<i<<"\n";
        }
        scadfile <<"\n";
        scadfile << "ObjectFaces = [\n";
        int numfaces = deducedfaces.size();
        for(int i=0;i<numfaces;i++){
            scadfile<<"[";
            for(int j=0;j<deducedfaces[i].size();j++){
                scadfile<<deducedfaces[i][j];
                if(j!=deducedfaces[i].size()-1){
                    scadfile<<",";
                }
            }
            scadfile<< "]";
            if(i!=numfaces-1){
                scadfile<<",\n";
            }else{
                scadfile<<"];\n";
            }
        }
        scadfile<< "polyhedron( ObjectPoints, ObjectFaces );";
        scadfile.close();
        system("openscad -o object.stl object.scad");
        ofstream renderfile ("render.scad");
        if(renderfile.is_open()){
            renderfile<< "render(){import(\"object.stl\");}";
            renderfile.close();
            system("openscad render.scad");
        }else{
            cout<< "unable to render the stl";
        }
    }else{
        cout<< "unable to open file"<<endl;
    }

}


#pragma once

#include <boost/circular_buffer.hpp>

#include <Geometry2d/Point.hpp>
#include <Utils.hpp>
#include <Configuration.hpp>

#include "vision/robot/CameraRobot.hpp"
#include "vision/filter/KalmanFilter3D.hpp"

class WorldRobot;

/**
 * Filtered robot estimation for a single camera
 */
class KalmanRobot {
public:
    /**
     * Checks previousWorldRobot to see if it's valid
     *
     * @param cameraID ID of the camera this filter is applied to
     * @param creationTime Time this filter was created
     * @param initMeasurement Initial robot measurement
     * @param previousWorldRobot World robot from last frame (or invalid world robot)
     */
    KalmanRobot(unsigned int cameraID, RJ::Time creationTime,
                CameraRobot initMeasurement, const WorldRobot& previousWorldRobot);

    /**
     * Predicts one time step forward
     *
     * @param currentTime Current time of the prediction step
     */
    void predict(RJ::Time currentTime);

    /**
     * Predicts one time step forward then triangulates toward the measurement
     *
     * @param currentTime Current time of the prediction/update step
     * @param updateRobot Robot measurement that we are using as feedback
     */
    void predictAndUpdate(RJ::Time currentTime, CameraRobot updateRobot);

    /**
     * Returns true when the filter hasn't been updated in a while and should be deleted
     */
    bool isUnhealthy() const;

    /**
     * @return The camera id this belongs to
     */
    unsigned int getCameraID() const;

    /**
     * @return This robot's id
     */
    int getRobotID() const;

    /**
     * @return How healthy this filter is. AKA How often it's been updated
     */
    int getHealth() const;

    /**
     * @return Best estimate of the linear position of the robot
     */
    Geometry2d::Point getPos() const;

    /**
     * @return Best estimate of the heading. Not bounded
     */
    double getTheta() const;

    /**
     * @return Best estimate of the linear velocity of the robot
     */
    Geometry2d::Point getVel() const;

    /**
     * @return Best estimate of the angular velocity
     */
    double getOmega() const;

    /**
     * @return Covariance in X and Y linear direction of the position of the robot
     */
    Geometry2d::Point getPosCov() const;

    /**
     * @return Covariance of theta of the robot
     */
    double getThetaCov() const;

    /**
     * @return Covariance in X and Y linear direction of the velocity of the robot
     */
    Geometry2d::Point getVelCov() const;

    /**
     * @return Covariance of omega of the robot
     */
    double getOmegaCov() const;

    /**
     * @return List of previous camera robot measurements for kick detection
     */
    const boost::circular_buffer<CameraRobot>& getPrevMeasurements() const;

    static void createConfiguration(Configuration* cfg);

private:
    RJ::Time lastUpdateTime;
    RJ::Time lastPredictTime;

    boost::circular_buffer<CameraRobot> previousMeasurements;

    KalmanFilter3D filter;

    double previousTheta;
    int unwrapThetaCtr;
    int health;

    int robotID;

    unsigned int cameraID;

    // Max number of seconds without a measurement before the object
    // is deleted
    static ConfigDouble* max_time_outside_vision;
};