/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 the Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100903    K. Kumar          File created.
 *      100916    L. Abdulkadir     File checked.
 *      100929    K. Kumar          Checked code by D. Dirkx added.
 *      101110    K. Kumar          Added raiseToExponentPower() function.
 *      102410    D. Dirkx          Minor comment changes as code check.
 *      101213    K. Kumar          Bugfix raiseToIntegerExponent(); renamed raiseToIntegerPower().
 *                                  Added computeAbsoluteValue() functions.
 *      110202    K. Kumar          Added overload for State* for computeLinearInterpolation().
 *      110111    J. Melman         Added computeModulo() function.
 *      110411    K. Kumar          Added convertCartesianToSpherical() function.
 *      110606    J. Melman         Removed possible singularity from
 *                                  convertCartesianToSpherical.
 *      110707    K. Kumar          Added computeSampleMean(), computeSampleVariance() functions.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120127    D. Dirkx          First version branched from basic mathematics in Tudat Core.
 *      120127    K. Kumar          Minor comment edits.
 *      120118    D. Gondelach      Added new convertCylindricalToCartesian functions.
 *      120214    K. Kumar          Branched from old Tudat trunk for new coordinate conversions.
 *      120217    K. Kumar          Updated computeModuloForSignedValues() to computeModulo()
 *                                  from Tudat Core.
 *      120926    E. Dekens         Added spherical gradient to Cartesian conversion.
 *      131022    T. Roegiers       Added conversion from spherical state to Cartesian state.
 *                                  Added conversion from Cartesian state to spherical state.
 *      140114    E. Brandon        Reorganized includes.
 *                                  Minor changes during code check.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *      Torok, J.S. Analytical Mechanics: with an Introduction to Dynamical Systems, John Wiley and
 *          Sons, Inc., 2000.
 *      Vallado, D.A. Fundamentals of Astrodynamics and Applications. Microcosm Press, 2001.
 *
 *    Notes
 *
 */

#include <cmath>
#include <iostream>
#include <limits>

#include <boost/math/special_functions/sign.hpp>

#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"

namespace tudat
{

namespace coordinate_conversions
{

//! Convert spherical (radius_, zenith, azimuth) to Cartesian (x,y,z) coordinates.
Eigen::Vector3d convertSphericalToCartesian( const Eigen::Vector3d& sphericalCoordinates )
{
    // Create local variables.
    double radius_ = sphericalCoordinates( 0 );
    double zenithAngle_ = sphericalCoordinates( 1 );
    double azimuthAngle_ = sphericalCoordinates( 2 );

    // Declaring sine which has multiple usages to save computation time.
    double sineOfZenithAngle_ = std::sin( sphericalCoordinates( 1 ) );

    // Create output Vector3d.
    Eigen::Vector3d convertedCartesianCoordinates_ = Eigen::Vector3d::Zero( 3 );

    // Perform transformation.
    convertedCartesianCoordinates_( 0 ) = radius_ * std::cos( azimuthAngle_ ) * sineOfZenithAngle_;
    convertedCartesianCoordinates_( 1 ) = radius_ * std::sin( azimuthAngle_ ) * sineOfZenithAngle_;
    convertedCartesianCoordinates_( 2 ) = radius_ * std::cos( zenithAngle_ );

    return convertedCartesianCoordinates_;
}

//! Convert Cartesian (x,y,z) to spherical (radius, zenith, azimuth) coordinates.
Eigen::Vector3d convertCartesianToSpherical( const Eigen::Vector3d& cartesianCoordinates )
{
    // Create output Vector3d.
    Eigen::Vector3d convertedSphericalCoordinates_ = Eigen::Vector3d::Zero( 3 );

    // Compute transformation of Cartesian coordinates to spherical coordinates.
    convertedSphericalCoordinates_( 0 ) = cartesianCoordinates.norm( );

    // Check if coordinates are at origin.
    if ( convertedSphericalCoordinates_( 0 ) < std::numeric_limits< double >::epsilon( ) )
    {
        convertedSphericalCoordinates_( 1 ) = 0.0;
        convertedSphericalCoordinates_( 2 ) = 0.0;
    }
    // Else compute coordinates using trigonometric relationships.
    else
    {
        convertedSphericalCoordinates_( 1 ) = std::acos( cartesianCoordinates( 2 )
                                                         / convertedSphericalCoordinates_( 0 ) );
        convertedSphericalCoordinates_( 2 ) = std::atan2( cartesianCoordinates( 1 ),
                                                          cartesianCoordinates( 0 ) );
    }

    return convertedSphericalCoordinates_;
}

//! Convert cylindrical to Cartesian coordinates.
Eigen::Vector3d convertCylindricalToCartesian( const double radius,
                                               const double azimuthAngle, const double z )
{
    // Create Cartesian coordinates vector.
    Eigen::Vector3d cartesianCoordinates;

    // If radius < 0, then give warning.
    if ( radius < 0.0 )
    {
        std::cerr << "Warning: cylindrical radial coordinate is negative!\n"
                  << "This could give incorrect results!\n";
    }

    // Compute and set Cartesian coordinates.
    cartesianCoordinates << radius * std::cos( azimuthAngle ),   // x-coordinate
                            radius * std::sin( azimuthAngle ),   // y-coordinate
                            z;                                   // z-coordinate

    return cartesianCoordinates;
}

//! Convert cylindrical to cartesian coordinates.
Eigen::Vector3d convertCylindricalToCartesian( const Eigen::Vector3d& cylindricalCoordinates )
{
    // Create Cartesian coordinates vector.
    Eigen::Vector3d cartesianCoordinates;

    // If radius < 0, then give warning.
    if ( cylindricalCoordinates( 0 ) < 0.0 )
    {
        std::cerr << "Warning: cylindrical radial coordinate is negative! "
                  << "This could give incorrect results!\n";
    }

    // Compute and set Cartesian coordinates.
    cartesianCoordinates
            << cylindricalCoordinates( 0 )
               * std::cos( cylindricalCoordinates( 1 ) ),    // x-coordinate
               cylindricalCoordinates( 0 )
               * std::sin( cylindricalCoordinates( 1 ) ),    // y-coordinate
               cylindricalCoordinates( 2 );                  // z-coordinate

    return cartesianCoordinates;
}

//! Convert cylindrical to Cartesian state.
basic_mathematics::Vector6d convertCylindricalToCartesianState(
        const basic_mathematics::Vector6d& cylindricalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    basic_mathematics::Vector6d cartesianState = basic_mathematics::Vector6d::Zero( );

    // Get azimuth angle, theta.
    double azimuthAngle = cylindricalState( 1 );

    // Compute and set Cartesian coordinates.
    cartesianState.head( 3 ) = convertCylindricalToCartesian(
                Eigen::Vector3d( cylindricalState.head( 3 ) ) );

    // If r = 0 AND Vtheta > 0, then give warning and assume Vtheta=0.
    if ( std::fabs(cylindricalState( 0 )) <= std::numeric_limits< double >::epsilon( )
         && std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) )
    {
        std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while "
                  << "the radius (r) is zero! Vtheta is taken equal to zero!\n";

        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                              // zdot
    }

    else
    {
        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle )
                   - cylindricalState( 4 ) * std::sin( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle )
                   + cylindricalState( 4 ) * std::cos( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                                // zdot
    }

    return cartesianState;
}

//! Convert Cartesian to cylindrical coordinates.
Eigen::Vector3d convertCartesianToCylindrical( const Eigen::Vector3d& cartesianCoordinates )
{
    // Create cylindrical coordinates vector.
    Eigen::Vector3d cylindricalCoordinates;

    // Declare new variable, the azimuth angle.
    double azimuthAngle;

    // Compute azimuth angle, theta.
    /* If x = 0, then azimuthAngle = pi/2 (y>0) or 3*pi/2 (y<0) or 0 (y=0),
       else azimuthAngle = arctan(y/x).
    */
    using mathematical_constants::PI;
    if ( std::fabs(cartesianCoordinates( 0 ) ) <= std::numeric_limits< double >::epsilon( ) )
    {
        azimuthAngle = basic_mathematics::computeModulo(
                    static_cast< double >( boost::math::sign( cartesianCoordinates( 1 ) ) )
                    * 0.5 * PI, 2.0 * PI );
    }

    else
    {
        azimuthAngle = basic_mathematics::computeModulo(
                    std::atan2( cartesianCoordinates( 1 ),
                                cartesianCoordinates( 0 ) ), 2.0 * PI );
    }

    // Compute and set cylindrical coordinates.
    cylindricalCoordinates <<
        std::sqrt( pow( cartesianCoordinates( 0 ), 2 )
                   + pow( cartesianCoordinates( 1 ), 2 ) ), // Radius
        azimuthAngle,                                       // Azimuth angle, theta
        cartesianCoordinates( 2 );                          // z-coordinate

    return cylindricalCoordinates;
}

//! Convert Cartesian to cylindrical state.
basic_mathematics::Vector6d convertCartesianToCylindricalState(
        const basic_mathematics::Vector6d& cartesianState )
{
    // Create cylindrical state vector, initialized with zero entries.
    basic_mathematics::Vector6d cylindricalState = basic_mathematics::Vector6d::Zero( );

    // Compute and set cylindrical coordinates.
    cylindricalState.head( 3 ) = convertCartesianToCylindrical(
                Eigen::Vector3d( cartesianState.head( 3 ) ) );

    // Compute and set cylindrical velocities.
    /* If radius = 0, then Vr = sqrt(xdot^2+ydot^2) and Vtheta = 0,
       else Vr = (x*xdot+y*ydot)/radius and Vtheta = (x*ydot-y*xdot)/radius.
    */
    if ( cylindricalState( 0 ) <= std::numeric_limits< double >::epsilon( ) )
    {
        cylindricalState.tail( 3 ) <<
            std::sqrt( pow( cartesianState( 3 ), 2 ) + pow( cartesianState( 4 ), 2 ) ), // Vr
            0.0,                                                                        // Vtheta
            cartesianState( 5 );                                                        // Vz
    }

    else
    {
        cylindricalState.tail( 3 ) <<
            ( cartesianState( 0 ) * cartesianState( 3 )
              + cartesianState( 1 ) * cartesianState( 4 ) ) / cylindricalState( 0 ),    // Vr
            ( cartesianState( 0 ) * cartesianState( 4 )
              - cartesianState( 1 ) * cartesianState( 3 ) ) / cylindricalState( 0 ),    // Vtheta
                cartesianState( 5 );                                                    // Vz
    }

    return cylindricalState;
}

//! Convert spherical to Cartesian gradient.
Eigen::Vector3d convertSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient,
                                                     const Eigen::Vector3d& cartesianCoordinates )
{
    // Compute radius.
    const double radius = std::sqrt( cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
                                     + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 )
                                     + cartesianCoordinates( 2 ) * cartesianCoordinates( 2 ) );

    // Compute square of distance within xy-plane.
    const double xyDistanceSquared = cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
            + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 );

    // Compute distance within xy-plane.
    const double xyDistance = std::sqrt( xyDistanceSquared );

    // Compute transformation matrix.
    const Eigen::Matrix3d transformationMatrix = (
                Eigen::Matrix3d( 3, 3 ) <<
                cartesianCoordinates( 0 ) / radius,
                - cartesianCoordinates( 0 ) * cartesianCoordinates( 2 )
                / ( radius * radius * xyDistance ),
                - cartesianCoordinates( 1 ) / xyDistanceSquared,
                cartesianCoordinates( 1 ) / radius,
                - cartesianCoordinates( 1 ) * cartesianCoordinates( 2 )
                / ( radius * radius * xyDistance ),
                + cartesianCoordinates( 0 ) / xyDistanceSquared,
                cartesianCoordinates( 2 ) / radius,
                xyDistance / ( radius * radius ),
                0.0
                ).finished( );

    // Return Cartesian gradient.
    return transformationMatrix * sphericalGradient;
}

//! Convert spherical to Cartesian state.
basic_mathematics::Vector6d convertSphericalToCartesianState(
        const basic_mathematics::Vector6d& sphericalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    basic_mathematics::Vector6d convertedCartesianState = basic_mathematics::Vector6d::Zero( );

    // Create local variables.
    const double radius = sphericalState( 0 );
    const double azimuthAngle = sphericalState( 1 );
    const double elevationAngle = sphericalState( 2 );

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( elevationAngle );
    const double sineOfElevationAngle = std::sin( elevationAngle );
    const double cosineOfAzimuthAngle = std::cos( azimuthAngle );
    const double sineOfAzimuthAngle = std::sin( azimuthAngle );

    // Set up transformation matrix for spherical to cylindrical conversion.
    Eigen::Matrix3d transformationMatrixSphericalToCylindrical = Eigen::Matrix3d::Zero( );
    transformationMatrixSphericalToCylindrical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 0, 2 ) = -sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 1, 1 ) = 1.0;
    transformationMatrixSphericalToCylindrical( 2, 0 ) = sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for cylindrical to Cartesian conversion.
    Eigen::Matrix3d transformationMatrixCylindricalToCartesian = Eigen::Matrix3d::Zero( );
    transformationMatrixCylindricalToCartesian( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 0, 1 ) = -sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 0 ) = sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 2, 2 ) = 1.0;

    // Compute transformation matrix for spherical to Cartesian conversion.
    const Eigen::Matrix3d transformationMatrixSphericalToCartesian
            = transformationMatrixCylindricalToCartesian
            * transformationMatrixSphericalToCylindrical;

    // Perform transformation of position coordinates.
    convertedCartesianState( 0 ) = radius * cosineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 1 ) = radius * sineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 2 ) = radius * sineOfElevationAngle;

    // Perform transformation of velocity vector.
    convertedCartesianState.segment( 3, 3 ) =
        transformationMatrixSphericalToCartesian * sphericalState.segment( 3, 3 );

    // Return Cartesian state vector.
    return convertedCartesianState;
}

//! Convert Cartesian to spherical state.
basic_mathematics::Vector6d convertCartesianToSphericalState(
        const basic_mathematics::Vector6d& cartesianState )
{
    // Create spherical state vector, initialized with zero entries.
    basic_mathematics::Vector6d convertedSphericalState = basic_mathematics::Vector6d::Zero( );

    // Compute radius.
    convertedSphericalState( 0 ) = cartesianState.segment( 0, 3 ).norm( );

    // Check if radius is nonzero.
    /*
     * If r > 0, the elevation and azimuth angles are computed using trigonometric relationships.
     * If r = 0, the coordinates are at the origin, the elevation and azimuth angles equal to zero.
     * Since the state vector was initialized with zeroes, this is already the case.
     */
    if ( convertedSphericalState( 0 ) > std::numeric_limits< double >::epsilon( ) )
    {
        // Compute elevation and azimuth angles using trigonometric relationships.
        // Azimuth angle.
        convertedSphericalState( 1 ) = std::atan2( cartesianState( 1 ), cartesianState( 0 ) );
        // Elevation angle.
        convertedSphericalState( 2 ) = std::asin( cartesianState( 2 )
                                                   / convertedSphericalState( 0 ) );
    }

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( convertedSphericalState( 2 ) );
    const double sineOfElevationAngle = std::sin( convertedSphericalState( 2 ) );
    const double cosineOfAzimuthAngle = std::cos( convertedSphericalState( 1 ) );
    const double sineOfAzimuthAngle = std::sin( convertedSphericalState( 1 ) );

    // Set up transformation matrix for cylindrical to spherical conversion.
    Eigen::Matrix3d transformationMatrixCylindricalToSpherical = Eigen::Matrix3d::Zero( );
    transformationMatrixCylindricalToSpherical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 0, 2 ) = sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 1, 1 ) = 1.0;
    transformationMatrixCylindricalToSpherical( 2, 0 ) = -sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for Cartesian to cylindrical conversion.
    Eigen::Matrix3d transformationMatrixCartesianToCylindrical = Eigen::Matrix3d::Zero( );
    transformationMatrixCartesianToCylindrical( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 0, 1 ) = sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 0 ) = -sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 2, 2 ) = 1.0;

    // Compute transformation matrix for Cartesian to spherical conversion.
    const Eigen::Matrix3d transformationMatrixCartesianToSpherical
            = transformationMatrixCylindricalToSpherical
            * transformationMatrixCartesianToCylindrical;

    // Perform transformation of velocity vector.
    convertedSphericalState.segment( 3, 3 )
            = transformationMatrixCartesianToSpherical * cartesianState.segment( 3, 3 );

    // Return spherical state vector.
    return convertedSphericalState;
}

} // namespace coordinate_conversions

} // namespace tudat


#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <chrono>
using namespace std;
using namespace cv;
// define VecVec2d and VecVec3d
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

// Functions Get the matching points
void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches);

// Functions to Change the pixel points into camera normalization points
Point2d pixel2cam(const Point2d &p, const Mat &K);

// Bundle Adjustment  By Gauss-Newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose    // need to be optimazied
);

// Bundle Adjustment By G2O
void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

int main(int argc, char const *argv[]){
    if(argc!=5){
        cout<<"Usage (bash): pose_estimation3d2d.cpp  <image01> <image02> <rgbd-image01-path>  <rgbd-image02-path> "<<endl;
       return 1;
    }
 //  Load Images
  Mat img_1 = imread(argv[1]);
  Mat img_2 = imread(argv[2]);
  assert(img_1.data && img_2.data && "Can not load images!");

 // Find keypoints matches using orb-brief
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
cout << "Totally found " << matches.size() << " matches" << endl;

//  Build 3d -2d  reflections

//  depth image is 16-bit unchar mono-channel picture
Mat rgbd_image01 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
Mat camera_K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for(DMatch m :matches){
 // reference : Mat.ptr<unchar>(int a)[int b] : means the pixel in row a, col b.
 // get the keypoint's deep
ushort d = rgbd_image01.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if(d==0) continue;
float normalized_d = d /1000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt,camera_K);
pts_3d.push_back(Point3f(p1.x*normalized_d,p1.y*normalized_d,normalized_d));  // keypint one's 3d , 相机坐标
pts_2d.push_back(keypoints_2[m.trainIdx].pt);      // keypoint2 :only 2d
}

// Solve PNP using OpenCV
Mat r, t;  // r is rotation vector , t is translation vector
solvePnP(pts_3d,pts_2d,camera_K,Mat(),r,t, false, cv::SOLVEPNP_EPNP);
Mat inliners;
bool result  = solvePnPRansac(pts_3d,pts_2d,camera_K,Mat(),r,t,false,100,4.0,0.99,inliners);
cout<<result<<endl;
Mat R;
cv::Rodrigues(r,R);
cout<<" Rotation Matrix R (3,3 ): "<<"\n"<<R<<endl;
cout<<"  translation vector  (3,1): "<<"\n"<<t<<endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }
  // 使用 G-N  BA
  cout << "Calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, camera_K, pose_gn);

 Sophus::SE3d pose_g2o;
bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, camera_K, pose_g2o);
return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  //BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
 typedef Eigen::Matrix<double, 6, 1> Vector6d;   // because Se(3) is a 6 dim vector, First 3 P is translation, Second 3 Fi is rotatiom
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  /// camera model//
  // X' = f(X/Z) Y'= f(Y/Z)
  // u = aX' +cx   v= bY' + cy

  // Camera inner parameter
  //         [ fx, 0 , cx ]
 //  K =  [ 0, fx,  cy ]
 //          [ 0,  0,   1  ]
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();  //这里之所以是 6 x 6 是因为J(x)TJ(X) = H, J(X)是2 x 6
    Vector6d b = Vector6d::Zero(); // b = J(x)Tf(x), J(x)T是 6 x 2, f(x)是 x,y 2个维度的误差，所以是2 x 1.
    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2]; // 1/Z'
      double inv_z2 = inv_z * inv_z;  // 1/Z^2
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;  // 重投影误差

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;  // 可以直接套结论的矩阵
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;  // n点累加
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b); // 更新 se3

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation // 更新SE(3)
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {  // 当需要更新的左乘se(3)足够小。
      // converge
      break;
    }
  }
  cout << "pose by g-n: \n" << pose.matrix() << endl;

}

// 定义 Vertex 和  Edge

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // Eigen 内部处理动态分配变量的对齐问题
    // setToOriginImpl()用来重置优化变量， 这里初始化一个空 的 SE3
    // 这里使用虚函数的原因是由C++11新特性，重写了父类的虚函数
    virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }
   /// left multiplication on SE3， setToOriginImpl()用来Upfate优化变量,这里使用左乘来更新
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }
    virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

/// 定义边， 也就是重新投影误差， 我们知道重投影误差为2位， 类型是vector2d, 优化对象的类型是一个VertexPose
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 // 构造函数
  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
// 计算重新投影误差
  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();  // 获得顶点的观测value
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d); // // K TP = s[u,v]
    pos_pixel /= pos_pixel[2]; // 齐次到非齐次
    _error = _measurement - pos_pixel.head<2>(); //_measurement：存储观测值
  }
   // 更新操作, 定义Jaco矩阵即可，这里使用就是G-N法
  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  };

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
){
  // 构建图优化，先设定g2o
  // pose is 6 ， 3 for rotation, 3 for translation. landmark is 3 : X,Y,Z,观测点的维度
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器, 去优化Pose Matrix
    // 梯度下降方法，可以从GN, LM, DogLeg 中选， 接受一个BlockSolverType,用Linear
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

    g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 下面开始往图里面添加 Vertex 和 Edge
  // vertex， 只有一个R,t
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

    //  Camera K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose); // 对应成员函数_vertex
    edge->setMeasurement(p2d); // 对应成员函数_mesurement
    edge->setInformation(Eigen::Matrix2d::Identity()); // 图中的Q就是信息矩阵，为了表示我们对误差各分量重视程度的不一样。
    // 一般情况下，我们都设置这个矩阵为单位矩阵，表示我们对所有的误差分量的重视程度都一样。
    optimizer.addEdge(edge);
    index++;
  }
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
  pose = vertex_pose->estimate();
}


#ifndef SEGWAY_SIM_COMMON_H
#define SEGWAY_SIM_COMMON_H

#include "segway_sim/CyberTimer.hpp"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <math.h>
#include <vector>
#include <map>
#include <functional>
#include <signal.h>

static const uint32_t STATE_LENGTH = 7;
static const uint32_t INPUT_LENGTH = 2;
static const uint32_t CMD_LENGTH = 2;

inline void quat2eulZYX(const Eigen::Quaterniond &q,
                              Eigen::Vector3d &eul)
{
	double eul_tmp = 2.0 * (q.y() * q.y());
	eul(0) = atan2(2.0 * q.w() * q.x() + 2.0 * q.y() * q.z(), (1.0 - 2.0 * (q.x() * q.x())) - eul_tmp);

	double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
	if (abs(sinp) >= 1)
		eul(1) = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
	else
		eul(1) = asin(sinp);

	eul(2) = atan2(2.0 * q.w() * q.z() + 2.0 * q.x() * q.y(), (1.0 - eul_tmp) - 2.0 * (q.z() * q.z()));
}

inline void quat2rotm(const Eigen::Quaterniond &q,
                            Eigen::Matrix3d &rotm)
{
	rotm(0,0) = 1-2*q.y()*q.y()-2*q.z()*q.z();
	rotm(0,1) = 2*q.x()*q.y()-2*q.z()*q.w();
	rotm(0,2) = 2*q.x()*q.z()+2*q.y()*q.w();
	rotm(1,0) = 2*q.x()*q.y()+2*q.z()*q.w();
	rotm(1,1) = 1-2*q.x()*q.x()-2*q.z()*q.z();
	rotm(1,2) = 2*q.y()*q.z()-2*q.x()*q.w();
	rotm(2,0) = 2*q.x()*q.z()-2*q.y()*q.w();
	rotm(2,1) = 2*q.y()*q.z()+2*q.x()*q.w();
	rotm(2,2) = 1-2*q.x()*q.x()-2*q.y()*q.y();
}

inline void eul2quatZYX(const Eigen::Vector3d &eul,
                              Eigen::Quaterniond &q)
{
	double cy = cos(eul(2) * 0.5);
	double sy = sin(eul(2) * 0.5);
	double cp = cos(eul(1) * 0.5);
	double sp = sin(eul(1) * 0.5);
	double cr = cos(eul(0) * 0.5);
	double sr = sin(eul(0) * 0.5);

	q.w() = cy * cp * cr + sy * sp * sr;
	q.x() = cy * cp * sr - sy * sp * cr;
	q.y() = sy * cp * sr + cy * sp * cr;
	q.z() = sy * cp * cr - cy * sp * sr;
}

inline void removeYaw(Eigen::Quaterniond &q)
{
	Eigen::Vector3d eul;
	quat2eulZYX(q,eul);
	eul(2) = 0;
	eul2quatZYX(eul,q);
}

inline Eigen::Matrix3d rotx(double angle)
{
	Eigen::Matrix3d rotm;
	rotm(0,0) =  1; rotm(0,1) =           0; rotm(0,2) =           0;
	rotm(1,0) =  0; rotm(1,1) =  cos(angle); rotm(1,2) = -sin(angle);
	rotm(2,0) =  0; rotm(2,1) =  sin(angle); rotm(2,2) =  cos(angle);
	return rotm;
}

inline Eigen::Matrix3d roty(double angle)
{
	Eigen::Matrix3d rotm;
	rotm(0,0) =  cos(angle); rotm(0,1) =  0; rotm(0,2) = sin(angle);
	rotm(1,0) =           0; rotm(1,1) =  1; rotm(1,2) = 0;
	rotm(2,0) = -sin(angle); rotm(2,1) =  0; rotm(2,2) = cos(angle);
	return rotm;
}

inline Eigen::Matrix3d rotz(double angle)
{
	Eigen::Matrix3d rotm;
	rotm(0,0) =  cos(angle); rotm(0,1) = -sin(angle); rotm(0,2) = 0;
	rotm(1,0) =  sin(angle); rotm(1,1) =  cos(angle); rotm(1,2) = 0;
	rotm(2,0) =           0; rotm(2,1) =           0; rotm(2,2) = 1;
	return rotm;
}

inline double deg2rad(double angle)
{
	return angle*M_PI/180.0;
}

inline double rad2deg(double angle)
{
	return angle*180.0/M_PI;
}

inline double saturate(double in, double min, double max)
{
	double out;
	if(min>max)
	{
		double tmp = max;
		max = min;
		min = tmp;
	}

	if(in>max)
		out = max;
	else if(in<min)
		out = min;
	else
		out = in;

	return out;
}

inline void saturateInPlace(double &in, double min, double max)
{
	if(min>max)
	{
		double tmp = max;
		max = min;
		min = tmp;
	}

	if(in>max)
		in = max;
	else if(in<min)
		in = min;
}

inline void saturateInPlace(double in[], double min, double max, uint32_t len)
{
	if(min>max)
	{
		double tmp = max;
		max = min;
		min = tmp;
	}

	for(uint32_t i = 0; i<len; i++)
	{
		if(in[i]>max)
			in[i] = max;
		else if(in[i]<min)
			in[i] = min;
	}

}

inline double constrainAngle(double x){
    x = fmod(x + M_PI,2*M_PI);
    if (x < 0)
        x += M_PI;
    return x - M_PI;
}

inline void omega2eulRatesZYX(const Eigen::Vector3d &omega,
                              const Eigen::Quaterniond &q,
                                    Eigen::Vector3d &eulDot)
{
	Eigen::Vector3d eul;
	quat2eulZYX(q,eul);
	const double x = eul(0);
	const double y = eul(1);

	Eigen::Matrix3d M;
	M(0,0) = 1; M(0,1) = sin(x)*tan(y); M(0,2) = cos(x)*tan(y);
	M(1,0) = 0; M(1,1) =        cos(x); M(1,2) =       -sin(x);
	M(2,0) = 0; M(2,1) = sin(x)/cos(y); M(2,2) = cos(x)/cos(y);

	eulDot = M*omega;
}

inline void eulRates2omegaZYX(const Eigen::Vector3d &eulDot,
                              const Eigen::Quaterniond &q,
                                    Eigen::Vector3d &omega)
{
	Eigen::Vector3d eul;
	quat2eulZYX(q,eul);
	const double x = eul(0);
	const double y = eul(1);

	Eigen::Matrix3d M;
	M(0,0) = 1; M(0,1) =       0; M(0,2) =       -sin(y);
	M(1,0) = 0; M(1,1) =  cos(x); M(1,2) = sin(x)*cos(y);
	M(2,0) = 0; M(2,1) = -sin(x); M(2,2) = cos(x)*cos(y);

	omega = M*eulDot;
}
#endif


/**
 * @brief 2D配准 闭式解算法
 * @author hqy
 * @date 2021.3.15
 */

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

template<typename T>
void printMat(const T& mat, int size){
    for (int i = 0; i < size; i++){
        for (int j = 0; j < size - 1; j++){
            std::cout << mat(i, j) << ", ";
        }
        std::cout << mat(i, size - 1) << std::endl;
    }
}

void hMatrixSolver(
    const std::vector<cv::DMatch>& matches,
    const std::vector<cv::KeyPoint>& pts1,
    const std::vector<cv::KeyPoint>& pts2,
    Eigen::Matrix3d& H
) {
    int size = matches.size();
    Eigen::Matrix3Xd P(3, size);
    Eigen::Matrix3Xd Q(3, size);
    P.setZero();
    Q.setZero();
    #pragma omp parallel for num_threads(8)
    for (size_t i = 0; i < matches.size(); i++) {
        const cv::Point2f& _p1 = pts1[matches[i].queryIdx].pt;
        const cv::Point2f& _p2 = pts2[matches[i].trainIdx].pt;
        P.block<3, 1>(0, i) = Eigen::Vector3d(_p1.x, _p1.y, 1);
        Q.block<3, 1>(0, i) = Eigen::Vector3d(_p2.x, _p2.y, 1);
    }
    Eigen::Matrix3d PPT = P * P.transpose();
    Eigen::Matrix3d inv = (PPT).ldlt().solve(Eigen::Matrix3d::Identity());    // LDLT分解求逆矩阵 (PP^T)
    Eigen::Matrix3d res = PPT * inv;
    H = Q * P.transpose() * inv;
}

void featureExtract(
    const cv::Mat& src1,
    const cv::Mat& src2,
    cv::Mat& dst,
    Eigen::Matrix3d& H
){
    std::vector<cv::DMatch> matches;
    std::vector<cv::KeyPoint> pts1, pts2;
    #define POINT_NUM 64
    cv::Mat dscp1, dscp2;
    #pragma omp parallel sections
    {
        #pragma omp section
        {
            cv::Ptr<cv::FeatureDetector> det1 = cv::ORB::create(POINT_NUM);
            cv::Ptr<cv::DescriptorExtractor> des1 = cv::ORB::create(POINT_NUM);
            det1->detectAndCompute(src1, cv::noArray(), pts1, dscp1);
        }
        #pragma omp section
        {
            cv::Ptr<cv::FeatureDetector> det2 =  cv::ORB::create(POINT_NUM);
            cv::Ptr<cv::DescriptorExtractor> des2 =  cv::ORB::create(POINT_NUM);
            det2->detectAndCompute(src2, cv::noArray(), pts2, dscp2);
        }
    }
    std::cout << "Parallel ORB descriptors found.\n";
    cv::Ptr<cv::DescriptorMatcher> match = cv::DescriptorMatcher::create(cv::DescriptorMatcher::BRUTEFORCE_HAMMINGLUT);
    match->match(dscp1, dscp2, matches);
    std::cout << "ORB descriptors matched.\n";
    std::vector<cv::Point2f> crn1, crn2;
    for (const cv::DMatch& mt: matches){
        crn1.push_back(pts1[mt.queryIdx].pt);
        crn2.push_back(pts2[mt.trainIdx].pt);
    }
    cv::Mat mask;

    cv::Mat cvH = cv::findHomography(crn1, crn2, mask, cv::RANSAC, 16);
    printf("Homography found. mask is %d, %d, %d, %d\n", mask.cols, mask.rows, mask.type(), CV_8UC1);
    printf("The original match num is %lu.\n", matches.size());
    std::vector<cv::DMatch> truth;
    for (size_t i = 0; i < matches.size(); i++){
        if (mask.at<uchar>(i) == 1){
            truth.push_back(matches[i]);
        }
    }
    cv::drawMatches(src1, pts1, src2, pts2, truth, dst);
    hMatrixSolver(truth, pts1, pts2, H);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            printf("%lf, ", cvH.at<double>(i, j));
        }
        printf("\n");
    }
}

/// 可视化
void alignmentVisualize(const cv::Mat& src, cv::Mat&dst, Eigen::Matrix3d H) {
    dst.create(src.rows, src.cols, CV_8UC3);
    #pragma for num_threads(8);
    for (int i = 0; i < src.rows; i++) {
        for (int j = 0; j < src.cols; j++) {
            Eigen::Vector3d now(j, i, 1.0);
            Eigen::Vector3d trans = H * now;
            int px = trans[0], py = trans[1];
            if (px >= 0 && px < dst.cols && py >= 0 && py < dst.rows) {
                dst.at<cv::Vec3b>(py, px) = src.at<cv::Vec3b>(i, j);
            }
        }
    }
}

int main() {
    cv::Mat src1 = cv::imread("../data/ImageA.jpg");
    cv::Mat src2 = cv::imread("../data/ImageB.jpg");
    cv::Mat gray1, gray2;
    cv::cvtColor(src1, gray1, cv::COLOR_BGR2GRAY);
    cv::cvtColor(src2, gray2, cv::COLOR_BGR2GRAY);
    cv::equalizeHist(gray1, gray1);
    cv::equalizeHist(gray2, gray2);
    cv::Mat aligned;
    cv::Mat feats;

    Eigen::Matrix3d H;

    featureExtract(gray1, gray2, feats, H);
    std::cout << "H matrix is:\n";
    printMat<Eigen::Matrix3d>(H, 3);
    cv::imwrite("../data/features.jpg", feats);

    alignmentVisualize(src1, aligned, H);
    cv::imwrite("../data/result.jpg", aligned);
    return 0;
}


/**
 * Kalman Filter
 * Skeleton code for teaching
 * A3M33MKR
 * Czech Technical University
 * Faculty of Electrical Engineering
 * Intelligent and Mobile Robotics group
 *
 * Authors: Zdeněk Kasl, Karel Košnar kosnar@labe.felk.cvut.cz
 *
 * this code is inspired by tutorial on Kalman Filter by Greg Czerniak
 *
 * Licence: MIT (see LICENSE file)
 **/
/*TODO set up the convarience matrices better and re-measure the noice constants (process noise/measurenment noice)*/

#include<cmath>
#include<cassert>
#include<cstdlib>
#include<fstream>
#include<iostream>
#include <Eigen/Dense>

#include "gui/gui.h"
#include "systemSimulator/system.h"

using namespace imr;
using namespace gui;
using namespace Eigen;
using namespace std;

#define PRINT(name) Print(#name, (name))

void Print(const char *name, Matrix4f m);

Point KalmanFilter(Point measuredPosition);

IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

Matrix4f A, B, Sigma, R, Q, E, C;
//Matrix2f Q;
//Matrix<float, 2, 4> C;
Vector4f mu, u;
float dt = 0.1;

//noice constants
double q = 100;
double r = 0.1;

//step constant
int step = 10;

void initMatrices() {
    A << 1, 0, dt, 0,
            0, 1, 0, dt,
            0, 0, 1, 0,
            0, 0, 0, 1;

    B << 0, 0, 0, 0,
            0, -0.5 * dt * dt, 0, 0,
            0, 0, 0, 0,
            0, 0, 0, -dt;

    u << 9.8, 9.8, 9.8, 9.8;

    mu << -300, 100, 0, 0;

    Sigma << 0.1, 0, 0, 0,
            0, 0.1, 0, 0,
            0, 0, 0.1, 0,
            0, 0, 0, 0.1;

// /Noice consts.
//-------------------------------------------------------------------
    C <<    1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1;

    Q <<    q, 0, 0, 0,
            0, q, 0, 0,
            0, 0, q, 0,
            0, 0, 0, q;

    R <<    r, 0, 0, 0,
            0, r, 0, 0,
            0, 0, r, 0,
            0, 0, 0, r;
//-------------------------------------------------------------------
    E <<    1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1;
}

void help(char **argv) {
    std::cout << "\nUsage of the program " << argv[0] + 2 << std::endl
              << "Parameter [-h or -H] displays this message." << std::endl
              << " Parametr [-n or -N] number of simulation steps."
              << std::endl;
}

int main(int argc, char **argv) {
    int nSteps = 1000;
    char *dataFile;

    // Parse all console parameters
    for (int i = 0; i < argc; i++) {
        if (argv[i][0] == '-') {
            switch (argv[i][1]) {
                //> HELP
                case 'H' :
                case 'h' :
                    help(argv);
                    break;
                case 'N':
                case 'n':
                    assert(i + 1 < argc);
                    assert(atoi(argv[i + 1]) > 1);
                    step = atoi(argv[i + 1]);
                    break;
                default :
                    std::cout << "Parameter \033[1;31m" << argv[i] << "\033[0m is not valid!\n"
                              << "Use parameter -h or -H for help." << std::endl;
                    break;
            }
        }
    }
    // All parameters parsed

    Gui gui;
    System system;

    //> comment line below in order to let the program
    //> continue right away
    gui.startInteractor();

    initMatrices();

    Point measurement;
    Point truth;
    Point kfPosition;

    for (int i = 1; i < nSteps; i++) {
        system.makeStep();
        truth = system.getTruthPosition();
        measurement = system.getMeasurement();
        kfPosition = KalmanFilter(measurement);
        gui.setPoints(truth, measurement, kfPosition);

        //> comment line below in order to let the program
        //> continue right away
        //if (i % step == 0) gui.startInteractor();

    }
    gui.startInteractor();
    return EXIT_SUCCESS;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//                               implement Kalman Filter here
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

Point KalmanFilter(const Point measuredPosition) {
    Point ret;
    Vector4f mu_actual, z, y;
    Matrix4f Sigma_actual, K, S, S_inv;

    z << measuredPosition.x,
            measuredPosition.y,
            0,
            0;

    PRINT(A);
    PRINT(C);
    //-------------------------------------------------------------------------------
    mu_actual = A * mu + B * u;

    //Error estimation
    //-------------------------------------------------------------------------------
    Sigma_actual = A * Sigma * A.transpose() + R;
    PRINT(Sigma_actual);

    //K gain
    //-------------------------------------------------------------------------------
    S = C * Sigma_actual * C.transpose() + Q;
    PRINT(S);

    K = Sigma_actual * C.transpose() * S.inverse();
    PRINT(K);

    //improvement
    //-------------------------------------------------------------------------------
    y = z - C * mu_actual;
    mu = mu_actual + K * y;
    Sigma = (E - K * C) * Sigma_actual;
    PRINT(Sigma);

    ret.x = mu(0);
    ret.y = mu(1);

    //Simulation of system
    //ret.x = mu_actual(0);
    //ret.y = mu_actual(1);
    //mu = mu_actual;

    cout << "X: " << ret.x << "    Y: " << ret.y <<
             "    Vx: " << mu(2) << "    Vy: " << mu(3) << endl;
    cout << "-------------------------------------------------------------------" << endl;

    return ret;
}

void Print(const char *name, Matrix4f m){
    cout << name << ": " << endl;
    cout << m.format(CleanFmt) << endl;
    cout << "-------------------------------------------------------------------" << endl;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Shikhar Vashistha
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/utility.hpp>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

double f(double) { /* cout << "double\n"; */ return 1.0; }
complex<double> f(complex<double>)
{
    //cout << "complex\n";
    return complex<double>(1.0, -1.0);
}

int main(int, char**)
{
    using namespace mtl; using mtl::io::tout;
    unsigned size = 4, row = size + 1, col = size;

    double tol(0.00001);
    dense_vector<double>                vec(size), vec1(size);
    dense2D<double>                     A(row, col), Q(row, row), R(row, col), A_test(row, col),
        A_t(col, row), Q_t(col, col), R_t(col, row), A_t_test(col, row);
    dense2D<complex<double> >                            dz(row, col), Qz(row, row), Rz(row, col);
    dense2D<double, mat::parameters<col_major> >         dc(size, size);
    compressed2D<double>                                 Ac(size, size), Qc(size, size), Rc(size, size), A_testc(size, size);
    A = 0;

    A[0][0] = 1;    A[0][1] = 1;    A[0][2] = 1;
    A[1][0] = 3;    A[1][1] = -1;   A[1][2] = -2;
    A[2][0] = 1;    A[2][1] = 7;    A[2][2] = 1;
    A[3][3] = -10;  A[4][0] = 4;    A[4][2] = 3;
    tout << "A=\n" << A << "\n";
    laplacian_setup(Ac, 2, 2);

    tout << "START-----dense2d---------row > col\n";

    dense2D<double> A1(A[iall][iall]), A2(A);
    boost::tie(Q, R) = lq(A1);
    tout << "R=\n" << R << "\n";
    tout << "Q=\n" << Q << "\n";
    A_test = Q * R - A2;
    tout << "Q*R=\n" << Q * R << "\n";

    tout << "one_norm(Rest A)=" << one_norm(A_test) << "\n";
    MTL_THROW_IF(one_norm(A_test) > tol, mtl::logic_error("wrong LQ decomposition of matrix A"));

    tout << "START------dense2d-------row < col\n";

    A_t = trans(A);
    boost::tie(Q_t, R_t) = lq(A_t);
    tout << "R_t=\n" << R_t << "\n";
    tout << "Q_t=\n" << Q_t << "\n";
    A_t_test = Q_t * R_t - A_t;
    tout << "Q_t*R_t=\n" << Q_t * R_t << "\n";

    tout << "one_norm(Rest A')=" << one_norm(A_t_test) << "\n";
    MTL_THROW_IF(one_norm(A_t_test) > tol, mtl::logic_error("wrong LQ decomposition of matrix trans(A)"));

    tout << "START-------compressed2d-------row > col\n";
#if 1
    boost::tie(Qc, Rc) = lq(Ac);
    tout << "R=\n" << Rc << "\n";
    tout << "Q=\n" << Qc << "\n";
    A_testc = Qc * Rc - Ac;
    tout << "Q*R=\n" << Qc * Rc << "\n";
    tout << "A=\n" << Ac << "\n";

    tout << "one_norm(Rest A)=" << one_norm(A_testc) << "\n";
    MTL_THROW_IF(one_norm(A_testc) > tol, mtl::logic_error("wrong LQ decomposition of matrix A"));
#endif

#if 0
    dz[0][0] = complex<double>(1.0, 0.0);
    dz[0][1] = complex<double>(1.0, 0.0);
    dz[0][2] = complex<double>(1, 0);
    dz[1][0] = complex<double>(1, 0);
    dz[1][1] = complex<double>(-1, 0);
    dz[1][2] = complex<double>(-2, 0);
    dz[2][0] = complex<double>(1, 0);
    dz[2][1] = complex<double>(-2, 0);
    dz[2][2] = complex<double>(1, 0);
    dz[3][3] = complex<double>(-10, 0);
    tout << "MAtrix complex=\n" << dz << "\n";

    tout << "START-----complex---------" << dz[0][0] << "\n";
    //AxB==BxA(For square matrix) LQ==QR(Transpose(A)
    boost::tie(Qz, Rz) = lq(dz);
    // Rz= lq_zerl(dz).second;
    // tout<<"MAtrix  R="<< Rz <<"\n";
    // tout<<"MAtrix  Q="<< Qz <<"\n";
    // tout<<"MAtrix  A=Q*R--outside"<< Qz*Rz <<"\n";
#endif
    /*
    Normal equations
    A^T A x = A^T b

    These equations are called the normal equations of the least squares problem

    Coefficient matrix  A^TA is the Gram matrix of A

    Equivalent to del f(x) = 0 where f(x) = ||Ax-b||^2

    All solutions of the least squares problem satisfy the normal equations.

    if A has linearly independent columns, then:

    A^T is non-singular

    Normal equations have unique solution xcap=(A^T A)^-1 A^T b
     */
    //Rewriting least squares solution using QR/LQ factorization A = QR, QR==LQ(transpose(A)).
    //  x = R^-1 Q^T x b
    /*  1. compute QR factorization A = QR(2mn^2 flops if A is m × n)
        2. matrix - vector product d = Q^T b(2mn flops)
        3. solve Rx = d by back substitution(n^2 flops)
        complexity: 2mn^2 flops
    */
    /*
       Example

         | 3 -6|    |-1 |
       A=| 4 -8|, b=| 7 |
	 | 0  1|    | 2 |

    1. QR/LQ factorization A = QR/LQ with

          |3/5 0|
    	Q=|4/5 0|, R=|5 -10|
     	  | 0  1|    |0	 1 |

    2. calculate d=Q^Tb = (5,2)

    3. solve Rx=d

    	|5 -10| |x1| = |5|
	|0  1 | |x2|   |2|

	solution is x1=5, x2=2

     */
    //Problem with regular method occurs when forming Gram matrix A^T A as it is unstable
    //QR factorization method is more stable because it avoids forming A^T A
    dense2D<double> x(A[iall][iall]),d(A[iall][iall]),b(A[iall][iall]);
    hessian_setup(x, 3.0); hessian_setup(d, 1.0);
    hessian_setup(b, 3.0);
    b[0][0] = -1, b[1][0] = 7, b[2][0] = 2;
    boost::tie(Q, R) = lq(A);
    mult(Q,b,d);
    d = trans(Q) * b;
    //d = trans(Q) * b;
    //lq decomposition is performed here so R==L here
    x = inv(R) * d;
    tout << "Solution to least square's problem is:" << x;
    return 0;
}


//
// Created by Sabar Nimmagadda on 4/25/20.
//
#include "Linear Algebra/computations.h"
#include <Eigen/LU>

#include <Eigen/QR>
#include <Eigen/Householder>
#include <Eigen/Eigenvalues>

using Eigen::MatrixXd;
using std::string;

  MatrixXd Computations::ComputeL(MatrixXd m) {
     typedef Eigen::Matrix<double, 3, 3 > L_Matrix;
     Eigen::FullPivLU<L_Matrix> lu(m);
     //Since the L matrix is a square matrix with the rowsize of the original matrix.
     L_Matrix l = L_Matrix::Identity();
     l.block<3,3>(0,0).triangularView<Eigen::StrictlyLower>() = lu.matrixLU();
     return l;
  }
  MatrixXd Computations::ComputeU(MatrixXd m) {
     typedef Eigen::Matrix<double, 3, 3> U_Matrix;
     Eigen::FullPivLU<U_Matrix> lu(m);
     //Since the U matrix has the exact dimensions of the original matrix.
     U_Matrix u = lu.matrixLU().triangularView<Eigen::Upper>();
     return u;
  }
  MatrixXd Computations::ComputePermutationMatrix(MatrixXd m) {
      typedef Eigen::Matrix<double, 3, 3> P_Matrix;
      Eigen::FullPivLU<P_Matrix> lu(m);
      P_Matrix p = lu.matrixLU();
      return p;
  }
  MatrixXd Computations::ComputeInverse(const MatrixXd& m) {
      return m.inverse();
  }

  MatrixXd Computations::ComputeRREF(const MatrixXd& m) {
      if (m.rows() >= 1 && m.cols() >= 1) {
          int lead = 0;
          MatrixXd out = m;
          while (lead < m.rows()) {
              float divisor, multiplier;
              for (int r = 0; r < m.rows(); r++) {
                  if (out(lead, lead) != 0) {
                      divisor = out(lead, lead); //What divides a cell to make its value = 1.
                      multiplier = out(r, lead)/ out(lead, lead); //What is multiplied to a cell, so that when it is subtracted the value becomes 0.
                  } else {
                      divisor = 1;
                      multiplier = 1;
                  }
                  for(int c = 0; c < m.cols(); c++) {
                      if (lead == r) {
                          out(r,c) /= divisor;//This makes the pivots = 1.
                      } else {
                          out(r,c) -= out(lead, c) * multiplier; //Making everything that is not a pivot = 0;
                      }
                  }
              }
              lead++;
          }
          return out;
      }
  }

  MatrixXd Computations::ComputeMultiply(MatrixXd matrix1, MatrixXd matrix2) {
      if (matrix1.cols() == matrix2.rows()) {
          return matrix1*matrix2;
      }
  }

  MatrixXd Computations::ComputeR(const MatrixXd& matrix) {
      Eigen::HouseholderQR<M3X3> qr(matrix.rows(), matrix.cols());
      qr.compute(matrix);
      MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
      return R;
  }

  MatrixXd Computations::ComputeQ(const MatrixXd& matrix) {
      Eigen::HouseholderQR<M3X3> qr(matrix.rows(), matrix.cols());
      qr.compute(matrix);
      MatrixXd Q = qr.householderQ();
      return Q;
  }

  double Computations::ComputeDotProduct(MatrixXd matrix1, MatrixXd matrix2, int dim) {
      double product = 0;
      for (int i = 0; i < dim; i++) {
          for (int j = 0; j < dim; j++) {
              product += matrix1(i, j) * matrix2(i, j);
          }
      }
      return product;
  }

  VectorXcd Computations::ComputeEigenValues(const MatrixXd& matrix) {
      VectorXcd ret = matrix.eigenvalues();
      return ret;
  }

  MatrixXcd Computations::ComputeEigenVectors(const MatrixXd& matrix) {
      Eigen::EigenSolver<MatrixXd> es(matrix);
      return es.eigenvectors();
  }

  int Computations::ComputeDeterminant(const MatrixXd& matrix) {
      return matrix.determinant();
  }
  MatrixXd Computations::ComputeColSpace(MatrixXd matrix) {
      if (matrix.size() >= 1) {
          Eigen::ColPivHouseholderQR<MatrixXd> qr(matrix);
          MatrixXd new_mat;
          if (qr.rank() != 0) {
              for (auto i : qr.colsPermutation().indices()) {
                  new_mat.col(i) = matrix.col(i);
              }
              return new_mat;
          }
          return new_mat;
      }
  }

  MatrixXd Computations::ComputeRowSpace(MatrixXd matrix) {
      MatrixXd transp = matrix.transpose();
      Eigen::ColPivHouseholderQR<MatrixXd> qr(transp);
      MatrixXd new_mat;
      for (auto i : qr.colsPermutation().indices()) {
          new_mat.col(i) = transp.col(i);
      }
      return new_mat;
  }


// =============================================================================
//                    MetaProg Chapter III Solution 1
//                         Author : Xin Liu
// =============================================================================

#include <iostream>
#include <limits>
#include <type_traits>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/multiplies.hpp>

namespace mpl = boost::mpl;

////////////////////////////////////////////////////////////////////////////////
namespace dimension_analysis
{
    using mass                  = mpl::vector_c<int, 1, 0, 0, 0, 0, 0, 0>;
    using length                = mpl::vector_c<int, 0, 1, 0, 0, 0, 0, 0>;
    using time                  = mpl::vector_c<int, 0, 0, 1, 0, 0, 0, 0>;
    using charge                = mpl::vector_c<int, 0, 0, 0, 1, 0, 0, 0>;
    using temperature           = mpl::vector_c<int, 0, 0, 0, 0, 1, 0, 0>;
    using intensity             = mpl::vector_c<int, 0, 0, 0, 0, 0, 1, 0>;
    using amount_of_substance   = mpl::vector_c<int, 0, 0, 0, 0, 0, 0, 1>;

    using velocity              = mpl::vector_c<int, 0, 1, -1, 0, 0, 0, 0>; // l/t
    using acceleration          = mpl::vector_c<int, 0, 1, -2, 0, 0, 0, 0>; // l/(t^2)
    using momentum              = mpl::vector_c<int, 1, 1, -1, 0, 0, 0, 0>; // ml/t
    using force                 = mpl::vector_c<int, 1, 1, -2, 0, 0, 0, 0>; // ml/(t^2)

    using scalar                = mpl::vector_c<int, 0, 0, 0, 0, 0, 0, 0>;

    template<typename T, typename Dimensions>
    class quantity
    {
    public:
        explicit quantity(T x)
                : m_value(x)
        { }

        template <typename OtherDimensions>
        quantity(quantity<T, OtherDimensions> const& q)
                : m_value(q.value())
        {
            static_assert(mpl::equal<Dimensions, OtherDimensions>::type::value, "OtherDimensions should be same as Dimensions.");
        }

    public:
        T value() const
        {
            return m_value;
        }

    private:
        T m_value;
    };

    template <typename T, typename D>
    quantity<T, D>
    operator+ (const quantity<T, D>& lhs, const quantity<T, D>& rhs)
    {
        //static_assert(mpl::equal<D1, D2>(), "D1 and D2 dimensions should be same.");
        return quantity<T, D>(lhs.value() + rhs.value());
    }

    template <typename T, typename D>
    quantity<T, D>
    operator- (const quantity<T, D>& lhs, const quantity<T, D>& rhs)
    {
        //static_assert(mpl::equal<D1, D2>(), "D1 and D2 dimensions should be same.");
        return quantity<T, D>(lhs.value() - rhs.value());
    }

    using mpl::placeholders::_;

    template <typename D1, typename D2>
    struct multiply_dimensions
            : mpl::transform<D1, D2, mpl::plus<_, _> >
    { };

    template <typename T, typename D1, typename D2>
    quantity<T, typename multiply_dimensions<D1, D2>::type>
    operator* (const quantity<T, D1>& lhs, const quantity<T, D2>& rhs)
    {
        return quantity<T, typename multiply_dimensions<D1, D2>::type>(lhs.value() * rhs.value());
    }

    template <typename D1, typename D2>
    struct divide_dimensions
            : mpl::transform<D1, D2, mpl::minus<_, _>>
    { };

    template <typename T, typename D1, typename D2>
    quantity<T, typename divide_dimensions<D1, D2>::type>
    operator/ (const quantity<T, D1>& lhs, const quantity<T, D2>& rhs)
    {
        return quantity<T, typename divide_dimensions<D1, D2>::type>(lhs.value() / rhs.value());
    }
} // namespace dimension_analysis

int main()
{
    using namespace dimension_analysis;

    quantity<float, length> l{ 1.0f };
    quantity<float, mass> m{ 2.0f };

    // expected compile error
    // m = l;

    quantity<float, length> len1{ 1.0f };
    quantity<float, length> len2{ 2.0f };

    len1 = len1 + len2;

    // expected compile error
    //len1 = len2 + quantity<float, mass>{ 3.7f };

    quantity<float, mass> m1{ 5.0f };
    quantity<float, mass> m3{ 10.0f };
    quantity<float, acceleration> a1{ 9.8f };

    quantity<float, force> f1 = m1 * a1;

    quantity<float, mass> m2 = f1 / a1;
    float rounding_error = std::abs((m2 - m1).value());
    std::cout << "error: " << rounding_error << "\n";

    f1 = f1 + (quantity<float, force>)(m3 * a1);
    float rounding_error2 = std::abs((m2 - m1).value());
    std::cout << "error2: " << rounding_error2 << "\n";
    bool res = rounding_error2 <= std::numeric_limits<float>::min();
    if(res) return 0;
    else return -1;
}


#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <map>
#include <utility>
#include <vector>
// this header only for ch8
#ifndef STATISTICS_HPP
#define STATISTICS_HPP
using namespace std;

double genZValue(double alpha) {
    boost::math::normal Ndistribution(0, 1);
    auto Z = boost::math::quantile(complement(Ndistribution, alpha / 2));
    return Z;
}

double genTValue(int degree, double upperTailArea) {
    boost::math::students_t Tdistribution(degree);
    auto T = boost::math::quantile(complement(Tdistribution, upperTailArea));
    return T;
}

double genMean(vector<double>& _dataSet) {
    double sum = 0;
    for (auto i : _dataSet)
        sum += i;
    return sum / _dataSet.size();
}

double genSampleStandardDeviation(vector<double>& _dataSet, double sampleMean, double size) {
    double sxx = 0;
    for (auto i : _dataSet)
        sxx += (i - sampleMean) * (i - sampleMean);
    return sqrt(1 / (size - 1) * sxx);
}

double errorRadius(vector<double>& _dataSet, int degree, int sampleSize, double upperTailArea) {
    double mean = genMean(_dataSet);
    double ssd = genSampleStandardDeviation(_dataSet, mean, sampleSize);
    return ssd / sqrt(sampleSize) * genTValue(degree, upperTailArea);
}

double errorRadius(vector<double>& _dataSet, double upperTailArea = 0.025) {
    /*
    * giving data set and the upper tail area of Student-T distribution
    * return the error radius
    * for too lazy to only input data set
    */
    int sampleSize = _dataSet.size();
    return errorRadius(_dataSet, sampleSize - 1, sampleSize, upperTailArea);
}

double errorRadius(double knownSigma, double alpha, int sampleSize) {
    /*
    * @knownSigma: the population sigma, which over sqrt(n)
    * giving alpha return the error radius for known Sigma of Z distribution
    * return the error radius
    */
    return knownSigma / sqrt(sampleSize) * genZValue(alpha);
}

pair<double, double> genConfidenceInterval(double theta, double errorRadius) {
    /*
     * @theta: the sample variable
     * @errorRadius: a radius of error, which might greater than 1,
     *               means the standardDeviation times (z or t) over sqrt(n)
     */
    return make_pair(theta - errorRadius, theta + errorRadius);
}

template <typename T, typename S>
ostream& operator<<(ostream& os, const pair<T, S>& v) {
    os << "[" << v.first << ", " << v.second << "]";
    return os;
}

namespace needingSampleSize {
int p(double alpha, double p = 0.5, double marginError = 0.95) {
    /*
    * @p: the p bar of sample proportions, which might be iid Ber(P) (Bernoulli distribution)
    * @marginError: the margin error of confidence interval
    */
    double q = 1 - p, z = genZValue(alpha);
    auto sampleSize = p * q * z * z / marginError / marginError;
    return ceil(sampleSize);
}

int x_bar(double alpha, double knownTheta, double marginError) {
    /*
    * @knownTheta: the sigma which is the population standard deviation
    * @marginError: the margin error of confidence interval
    */
    double z = genZValue(alpha);
    auto sampleSize = z * z * knownTheta * knownTheta / marginError / marginError;
    return ceil(sampleSize);
}
}  // namespace needingSampleSize

template <class T>
string readSingleLineCSV(vector<T>& _dataSet, string fileName) {
    /* will return the file's title
    * read only single line csv file
    * @_dataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawdata;
    getline(inFile, title);
    while (inFile >> rawdata)
        _dataSet.push_back(rawdata);
    inFile.close();
    return title;
}

template <class T>
string readSingleLineCSV(map<T, int>& proportionDataSet, string fileName) {
    /*
    * only for reading proportion Data
    * will return the file's title
    * read only single line csv file
    * @proportionDataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawData;
    getline(inFile, title);
    while (getline(inFile, rawData)) {
        auto isInMap = proportionDataSet.find(rawData);
        if (isInMap != proportionDataSet.end())
            proportionDataSet.at(isInMap->first)++;
        else
            proportionDataSet.insert(pair<T, int>(rawData, 1));
    }
    inFile.close();
    return title;
}

#endif


#include <cmath>
#include <fftw3.h>
#include <tuple>
#include <vector>

#include "specgram.h"

std::vector<float> window_hanning(int size) {
    std::vector<float> window;
    float scale =  2 * M_PI / (size - 1);
    for (int n = 0; n < size; n++)
        window.push_back(0.5 * (1 - cos(scale * n)));
    return window;
}

std::pair<int, int> specgram_shape(int n, int NFFT,
                                   int noverlap) {
    int stride = NFFT - noverlap;
    int time_steps = (n - noverlap) / stride;
    int n_freqs = NFFT / 2 + 1;
    return std::make_pair(n_freqs, time_steps);
}

void log_cspecgram(const int16_t* const in, int n,
                  int NFFT, int Fs,
                  int noverlap, float* out,
                  float epsilon /* default=1e-7 */) {
    cspecgram(in, n, NFFT, Fs, noverlap, out);
    auto spec_shape = specgram_shape(n, NFFT, noverlap);
    int size = spec_shape.first * spec_shape.second;
    for (int i = 0; i < size; i++)
        out[i] = log(out[i] + epsilon);
}

void cspecgram(const int16_t* const in, int n,
              int NFFT, int Fs,
              int noverlap, float* out) {

    int stride = NFFT - noverlap;
    int n_freqs;
    int time_steps;
    std::tie(n_freqs, time_steps) = specgram_shape(n, NFFT, noverlap);

    std::vector<float> window(window_hanning(NFFT));

    float *x = new float[NFFT * time_steps];

    float* in_f = new float[n];
    for (int i = 0; i < n; i++)
        in_f[i] = static_cast<float>(in[i]);

    for (int i = 0; i < time_steps; i++) {
        for (int j = 0; j < NFFT; j++) {
            x[i * NFFT + j] = window[j]  * in_f[i * stride + j];
        }
    }

    // Do an fft for each NFFT seg of x
    fftwf_complex* res = (fftwf_complex*) fftwf_malloc(
                            sizeof(fftwf_complex) * n_freqs * time_steps);

    fftwf_plan plan = fftwf_plan_many_dft_r2c(1, &NFFT,
                              time_steps, x, NULL, 1, NFFT,
                              res, NULL, 1, n_freqs,
                              FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    fftwf_execute(plan);
    fftwf_destroy_plan(plan);

    for (int i = 0; i < time_steps; i++) {
        int s = i * n_freqs;
        for (int j = 0; j < n_freqs; j++) {
            out[s + j] = res[s + j][0] * res[s + j][0] +
                            res[s + j][1] * res[s + j][1];
        }
    }

    // Cleanup temporaries
    fftwf_free(res);
    delete[] x;
    delete[] in_f;

    // Scale output in the same style as matplotlib
    float scale = 0;
    for (float w : window)
        scale += w * w;
    scale *= Fs;
    for (int i = 0; i < time_steps; i++) {
        int s = i * n_freqs;
        out[s] /= scale;
        for (int j = 1; j < n_freqs - 1; j++) {
            out[s + j] *= (2.0 / scale);
        }
        if (NFFT % 2 == 0) {
            out[s + n_freqs - 1] /= scale;
        } else {
            out[s + n_freqs - 1] *= (2.0 / scale);
        }
    }
}

#ifdef PYTHON
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/extract.hpp>
#include <numpy/noprefix.h>

namespace py = boost::python;
namespace np = boost::python::numeric;

void specgram(np::array input, int NFFT, int Fs,
               int noverlap, np::array output) {

    py::object i_shape = input.attr("shape");
    py::object o_shape = output.attr("shape");

    if (len(i_shape) != 1 || len(o_shape) != 2)
        throw std::invalid_argument("Bad input or ouput shape");
    unsigned int n = py::extract<unsigned int>(i_shape[0]);

    int16_t* in_dat = static_cast<int16_t*>(PyArray_DATA(input.ptr()));
    float* out_dat = static_cast<float*>(PyArray_DATA(output.ptr()));
    cspecgram(in_dat, n, NFFT, Fs, noverlap, out_dat);
}

BOOST_PYTHON_MODULE(cspecgram) {
    np::array::set_module_and_type("numpy", "ndarray");
    py::def("specgram", &specgram);
}
#endif


//
// Created by Vlad Argunov on 19/10/2021.
//

#include "Regression.h"

#include <Eigen/Dense>
#include <string>
#include <iostream>

Regression::Regression(std::string type) {
    type_regression = type;
}

void Regression::set_data(Eigen::MatrixXd x_values_, Eigen::VectorXd y_values_) {
    x_values = x_values_;
    y_values = y_values_;
}

void Regression::add_constant() {
    Eigen::MatrixXd x_values_with_constant;
    x_values_with_constant.resize(x_values.rows(), x_values.cols() + 1);
    x_values_with_constant.rightCols(x_values.cols()) = x_values;

    x_values_with_constant.col(0) = Eigen::VectorXd::Ones(x_values.rows());
    x_values = x_values_with_constant;

}

void Regression::print_data() {
    std::cout << "Data Matrix:\n";
    std::cout << x_values << "\n";
}

void Regression::print_responses() {
    std::cout << "Responses vector:\n";
    std::cout << y_values << "\n";
}

void Regression::solve() {
    if (type_regression == "Linear"){
        Eigen::MatrixXd computation_matrix;
        computation_matrix.resize(x_values.rows(), x_values.rows());
        computation_matrix = (x_values.transpose() * x_values).inverse();
        parameters = computation_matrix * x_values.transpose() * y_values;
    }
}

void Regression::print_parameters() {
    std::cout << "Parameters vector:\n";
    std::cout << parameters << "\n";
}


#pragma once
#ifndef MANIFOLDMATH_H
#define MANIFOLDMATH_H

#include <vector>
#include <memory>

#include <Eigen/Core>

#include <engine/Vectormath_Defines.hpp>

namespace Engine
{
    // Basically all math inside this namespace assumes that the given
    //      vectorfields contain unit vectors. They are therefore interpreted
    //      as living in a manifold which is the direct product space of N
    //      spheres.
    // The only exception are functions which interpret the vectorfield
    //      as a single 3N-dimensional vector.
    // TODO: cleanly separate these two cases!
    namespace Manifoldmath
    {
        // Get the norm of a vectorfield (interpreted as a 3N-vector)
        scalar norm(const vectorfield & vf);
        // Normalize a vectorfield (interpreted as a 3N-vector)
        void normalize(vectorfield & vf);

        // Project v1 to be parallel to v2
        //      Note: this assumes normalized vectorfields
        void project_parallel(vectorfield & vf1, const vectorfield & vf2);
        // Project v1 to be orthogonal to v2
        //      Note: this assumes normalized vectorfields
        void project_orthogonal(vectorfield & vf1, const vectorfield & vf2);
        // Invert v1's component parallel to v2
        //      Note: this assumes normalized vectorfields
        void invert_parallel(vectorfield & vf1, const vectorfield & vf2);
        // Invert v1's component orthogonal to v2
        //      Note: this assumes normalized vectorfields
        void invert_orthogonal(vectorfield & vf1, const vectorfield & vf2);
        // Project vf1's vectors into the tangent plane of vf2
        //      Note: vf2 must have normalized vectors
        void project_tangential(vectorfield & vf1, const vectorfield & vf2);

        // The tangential projector is a matrix which projects any vector into the tangent
        //      space of a vectorfield, considered to live on the direct product of N unit
        //      spheres. It is a 3N x 3N matrix.
        MatrixX tangential_projector(const vectorfield & image);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will be the spherical unit vectors, except at the poles.
        //      The basis will be a 3Nx2N matrix.
        void tangent_basis_spherical(const vectorfield & vf, MatrixX & basis);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will be generated from cross products with euclidean basis
        //      vectors. The basis will be a 3Nx2N matrix.
        void tangent_basis_cross(const vectorfield & vf, MatrixX & basis);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will form righthanded sets with the vectors of vf.
        //      The basis will be a 3Nx2N matrix.
        void tangent_basis_righthanded(const vectorfield & vf, MatrixX & basis);

        // Calculate a matrix of orthonormal basis vectors that span the tangent space to
        //      a vectorfield, considered to live on the direct product of N unit spheres.
        //      The basis vectors will be calculated from orthonormalizations with respect
        //      to a random vector. The basis will be a 3Nx2N matrix.
        // void tangent_basis_random(const vectorfield & vf, MatrixX & basis);

        // This is the derivatives of the coordinate transformation from (unit-)spherical to cartesian
        // i.e. d_cartesian/d_spherical
        void spherical_to_cartesian_jacobian(const vectorfield & vf, MatrixX & jacobian);

        // This is the second derivatives of the coordinate transformation from (unit-)spherical to cartesian
        // i.e. d^2_cartesian/d^2_spherical
        void spherical_to_cartesian_hessian(const vectorfield & vf, MatrixX & gamma_x, MatrixX & gamma_y, MatrixX & gamma_z);

        //
        void spherical_to_cartesian_coordinate_basis(const vectorfield & vf, MatrixX & basis);

        //
        void spherical_coordinate_christoffel_symbols(const vectorfield & vf, MatrixX & gamma_theta, MatrixX & gamma_phi);

        // Calculate Hessian for a vectorfield constrained to unit length, at any extremum (i.e. where vectors || gradient)
        void hessian_bordered(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out);
        // Calculate tangential derivatives and correction terms according to the projector approach
        void hessian_projected(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out);
        // Calculate tangential derivatives and correction terms according to the projector and Weingarten map approach
        void hessian_weingarten(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out);
        // Calculate spherical derivatives using coordinate transformations via jacobians
        void hessian_spherical(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out);
        // Calculate spherical derivates and correction terms using jacobians and christoffel symbols
        void hessian_covariant(const vectorfield & image,  const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out);

        // Geodesic distance between two vectorfields
        scalar dist_geodesic(const vectorfield & v1, const vectorfield & v2);

        // Calculate the "tangent" vectorfields pointing between a set of configurations
        void Tangents(std::vector<std::shared_ptr<vectorfield>> configurations, const std::vector<scalar> & energies, std::vector<vectorfield> & tangents);
    }
}

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

// Author: Marc Hartung

#ifndef MTL_MATRIX_QR_GIVENS_INCLUDE
#define MTL_MATRIX_QR_GIVENS_INCLUDE

#include <cmath>
#include <utility>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

namespace mtl { namespace matrix {

/// General Given's transformator
/**
 * Input-matrix A will be splitted in A = Q*R. Q and R are accessible from getQ() and getR().
 * \sa givens
 */
template <typename Matrix>
class qr_givens_solver {

    typedef typename Collection<Matrix>::value_type value_type;
    typedef typename Collection<Matrix>::size_type size_type;

public:

   /** \brief Constructor needs a sqare-matrix as input
     * \param MTL-Matrix type
     *
     */

    qr_givens_solver(const Matrix& IN) : R(IN), G(2,2), Q(num_cols(IN), num_rows(IN)) {
	value_type one = math::one(c);
	Q = one;
	eps = 1.0e-8;
    }

    /** \brief Returns Q as the input-matrix-type
     *
     * \return Matrix Q
     */

    Matrix& getQ() {
	return Q;
    }

    /** \brief Returns R as the input-matrix-type
     *
     * \return Matrix R
     */
    Matrix& getR() {
	return R;
    }

    /** \brief Sets the distance to zero
     *
     * \param value_type tolerance to zero
     */
    void setTolerance(value_type tol) {
	eps = tol;
    }

    /** \brief Starts QR decompostion
     */
    void calc()
    {
	value_type zero= math::zero(c);
	Matrix RIter(2, num_rows(R)), Iter(2, num_rows(R));
	for(size_type i= 0; i < R.num_cols() - 1; i++) {
	    irange r(i, num_cols(R));
 	    for(size_type j= num_rows(R)-1;j>i;j--) {
		if (std::abs(R[j][i]) > eps) {
		    set_rotation(R[i][i], R[j][i]);
		    Iter[0][r] = R[i][r];
		    Iter[1][r] = R[j][r];
		    RIter[iall][r] = G*Iter[iall][r];
		    R[i][r] = RIter[0][r];
		    R[j][r] = RIter[1][r];
		    Iter[0][iall] = Q[i][iall];
		    Iter[1][iall] = Q[j][iall];
		    RIter = G*Iter;
		    Q[i][iall] = RIter[0][iall];
		    Q[j][iall] = RIter[1][iall];
		}
		R[j][i] = zero;
	    }

	}

	for(size_type i= 0; i < num_cols(R) - 1; i++)
	    R[i+1][i] = zero;

    }

  private:

    Matrix     R, G, Q;
    value_type c, s, eps;

    template <typename T>
    inline T square(T x) const { return x * x; }

    /// Coefficients for Given's rotation
    void set_rotation(value_type a, value_type b)
    {
	using std::abs;
	using mtl::conj;

	value_type zero= math::zero(a), one= math::one(b), t;

	if ( b == zero ) {
	    c= one; s= zero;
	} else if ( abs(b) > abs(a) ) {
	    t = a / abs(b);
	    s = one/ sqrt(one + square(abs(t)));
	    c = s * t;
	    s *= (b/abs(b));
	} else {
	    t = b / abs(a);
	    c = one / sqrt(one + square(abs(t)));
	    s = c * t;
	    c = (a/abs(a))*c;
	}

	G= conj(c), conj(s),
	  -s, c;
    }

};

/// QR-Factorization of matrix A(m x n) based on Givens' rotation
template <typename Matrix>
std::pair<Matrix, Matrix>
inline qr_givens(const Matrix& A)
{
    qr_givens_solver<Matrix> solver(A);
    solver.calc();
    return std::make_pair(solver.getQ(), solver.getR());
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_QR_GIVENS_INCLUDE


#ifndef InitCondLV_CC_
#define InitCondLV_CC_
/**
 * @file initcondlv.cc
 * @brief NPDE homework InitCondLV code
 * @author lfilippo, tille, jgacon, dcasati
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>
#include <utility>

#include "../../../lecturecodes/Ode45/ode45.h"

namespace InitCondLV {

/* Compute the maps Phi(t,y0) and W(t,y0) at final time T.
 * Use initial data given by u0 and v0. */
/* SAM_LISTING_BEGIN_1 */
std::pair<Eigen::Vector2d, Eigen::Matrix2d> PhiAndW(double u0, double v0,
                                                    double T) {
  // Save the values of Phi and W at time T in PaW.first and PaW.second resp.
  std::pair<Eigen::Vector2d, Eigen::Matrix2d> PaW;

#if SOLUTION
  auto f = [](const Eigen::VectorXd& w) {
    Eigen::VectorXd temp(6);
    temp(0) = (2. - w(1)) * w(0);
    temp(1) = (w(0) - 1.) * w(1);
    temp(2) = (2. - w(1)) * w(2) - w(0) * w(3);
    temp(3) = w(1) * w(2) + (w(0) - 1.) * w(3);
    temp(4) = (2. - w(1)) * w(4) - w(0) * w(5);
    temp(5) = w(1) * w(4) + (w(0) - 1.) * w(5);
    return temp;
  };

  Eigen::VectorXd w0(6);
  w0 << u0, v0, 1., 0, 0, 1.;

  // Construct ode solver with r.h.s
  Ode45<Eigen::VectorXd> O(f);
  // Set options
  O.options.rtol = 1e-14;
  O.options.atol = 1e-12;
  // Solve ODE
  auto sol = O.solve(w0, T);
  // Extract needed component
  Eigen::VectorXd wT = sol.back().first;

  PaW.first << wT(0), wT(1);
  PaW.second << wT(2), wT(4), wT(3), wT(5);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return PaW;
}
/* SAM_LISTING_END_1 */

}  // namespace InitCondLV

#endif  // #define InitCondLV_CC_


#include <boost/multiprecision/gmp.hpp>
#include <boost/format.hpp>
#include <iostream>
namespace mp = boost::multiprecision;

typedef mp::mpf_float_1000 MPFloat;

struct MPConstant {
  const char *name {};
  const char *expr {};
  MPFloat value;
};

int main() {
  const MPFloat one(1);
  const MPFloat two(2);
  const MPFloat ten(10);
  const MPFloat e = mp::exp(one);
  const MPFloat ln2 = mp::log(two);
  const MPFloat ln10 = mp::log(ten);
  const MPFloat pi = 4 * mp::atan(one);
  const MPFloat sqrtpi = mp::sqrt(pi);
  const MPFloat sqrt2 = mp::sqrt(two);

  const MPConstant constants[] = {
    {"E", "e", e},
    {"LOG2E", "log2(e)", mp::log2(e)},
    {"LOG10E", "log10(e)", mp::log10(e)},
    {"LN2", "log(2)", mp::log(two)},
    {"LN10", "log(10)", mp::log(ten)},
    {"PI", "pi", pi},
    {"2PI", "2*pi", 2 * pi},
    {"4PI", "4*pi", 4 * pi},
    {"PI_2", "pi/2", pi / 2},
    {"PI_4", "pi/4", pi / 4},
    {"1_PI", "1/pi", 1 / pi},
    {"2_PI", "2/pi", 2 / pi},
    {"4_PI", "4/pi", 4 / pi},
    {"1_SQRTPI", "1/sqrt(pi)", 1 / sqrtpi},
    {"2_SQRTPI", "2/sqrt(pi)", 2 / sqrtpi},
    {"SQRT2", "sqrt(2)", sqrt2},
    {"1_SQRT2", "1/sqrt(2)", 1 / sqrt2},
  };

  for (const MPConstant &c: constants) {
    std::cout << "#define " <<
        boost::format("K_%s%|15t|::coreutil::math_constant(%s)%|85t|// %s\n")
        % c.name % (c.value.str(36) + 'L') % c.expr;
  }

  return 0;
}


#include "project/LinearRegression.hpp"
#include <armadillo>

LinearRegression::LinearRegression(arma::mat &x,
                                   arma::vec &y){

    X = x;
    Y = y;
}

void LinearRegression::fit(bool fit_inter = true){
    fit_intercept = fit_inter;
    if(fit_intercept){
        coef = arma::solve(
                arma::join_rows(arma::ones(X.n_rows, 1), X),
                Y);
    } else {
        coef = arma::solve(X, Y);
    }
}

arma::colvec LinearRegression::predict(arma::mat &X_pred){
    if(fit_intercept) {
        return arma::join_rows(
                arma::ones(X_pred.n_rows, 1),
                X_pred)*coef;
    }
    else{
        return X_pred*coef;
    }
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "ResizeBilinear.hpp"

#include "TensorBufferArrayView.hpp"

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

#include <cmath>
#include <algorithm>

using namespace armnnUtils;

namespace armnn
{

namespace
{

inline float Lerp(float a, float b, float w)
{
    return w * b + (1.f - w) * a;
}

}

void ResizeBilinear(const float*      in,
                    const TensorInfo& inputInfo,
                    float*            out,
                    const TensorInfo& outputInfo,
                    DataLayoutIndexed dataLayout)
{
    // We follow the definition of TensorFlow and AndroidNN: the top-left corner of a texel in the output
    // image is projected into the input image to figure out the interpolants and weights. Note that this
    // will yield different results than if projecting the centre of output texels.

    const unsigned int batchSize = inputInfo.GetShape()[0];
    const unsigned int channelCount = inputInfo.GetShape()[dataLayout.GetChannelsIndex()];

    const unsigned int inputHeight = inputInfo.GetShape()[dataLayout.GetHeightIndex()];
    const unsigned int inputWidth = inputInfo.GetShape()[dataLayout.GetWidthIndex()];
    const unsigned int outputHeight = outputInfo.GetShape()[dataLayout.GetHeightIndex()];
    const unsigned int outputWidth = outputInfo.GetShape()[dataLayout.GetWidthIndex()];

    // How much to scale pixel coordinates in the output image, to get the corresponding pixel coordinates
    // in the input image.
    const float scaleY = boost::numeric_cast<float>(inputHeight) / boost::numeric_cast<float>(outputHeight);
    const float scaleX = boost::numeric_cast<float>(inputWidth) / boost::numeric_cast<float>(outputWidth);

    TensorBufferArrayView<const float> input(inputInfo.GetShape(), in, dataLayout);
    TensorBufferArrayView<float> output(outputInfo.GetShape(), out, dataLayout);

    for (unsigned int n = 0; n < batchSize; ++n)
    {
        for (unsigned int c = 0; c < channelCount; ++c)
        {
            for (unsigned int y = 0; y < outputHeight; ++y)
            {
                // Corresponding real-valued height coordinate in input image.
                const float iy = boost::numeric_cast<float>(y) * scaleY;

                // Discrete height coordinate of top-left texel (in the 2x2 texel area used for interpolation).
                const float fiy = floorf(iy);
                const unsigned int y0 = boost::numeric_cast<unsigned int>(fiy);

                // Interpolation weight (range [0,1]).
                const float yw = iy - fiy;

                for (unsigned int x = 0; x < outputWidth; ++x)
                {
                    // Real-valued and discrete width coordinates in input image.
                    const float ix = boost::numeric_cast<float>(x) * scaleX;
                    const float fix = floorf(ix);
                    const unsigned int x0 = boost::numeric_cast<unsigned int>(fix);

                    // Interpolation weight (range [0,1]).
                    const float xw = ix - fix;

                    // Discrete width/height coordinates of texels below and to the right of (x0, y0).
                    const unsigned int x1 = std::min(x0 + 1, inputWidth - 1u);
                    const unsigned int y1 = std::min(y0 + 1, inputHeight - 1u);

                    // Interpolation
                    const float ly0 = Lerp(input.Get(n, c, y0, x0), input.Get(n, c, y0, x1), xw); // lerp along row y0.
                    const float ly1 = Lerp(input.Get(n, c, y1, x0), input.Get(n, c, y1, x1), xw); // lerp along row y1.
                    const float l = Lerp(ly0, ly1, yw);

                    output.Get(n, c, y, x) = l;
                }
            }
        }
    }
}

} //namespace armnn


#include <QatGenericFunctions/Variable.h>
#include <QatGenericFunctions/Parameter.h>
#include <QatGenericFunctions/Square.h>
#include <QatGenericFunctions/Sqrt.h>
#include <QatGenericFunctions/Exp.h>
#include <QatGenericFunctions/Sin.h>
#include <QatGenericFunctions/Cos.h>
#include <QatGenericFunctions/VoigtDistribution.h>
#include <QatGenericFunctions/RKIntegrator.h>
#include <QatDataAnalysis/Hist1D.h>
#include <QatDataAnalysis/Table.h>
#include <QatDataModeling/TableLikelihoodFunctional.h>
#include <QatDataModeling/MinuitMinimizer.h>
#include <QatPlotWidgets/PlotView.h>
#include <QatPlotWidgets/MultipleViewWindow.h>
#include <QatPlotting/PlotStream.h>
#include <QatPlotting/PlotHist1D.h>
#include <QatPlotting/PlotFunction1D.h>
#include <QatPlotting/PlotProfile.h>
#include <QatPlotting/PlotKey.h>
#include <QatPlotting/PlotOrbit.h>
#include <Eigen/Dense>
#include <QApplication>
#include <QMainWindow>
#include <QAction>
#include <QToolBar>
#include <QFont>
#include <cmath>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
#include <random>

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange,  const double Width=1200, const double Height=800);

typedef std::vector<double> Data;
typedef std::unique_ptr<Genfun::AbsFunction> FuncPtr;
typedef std::weak_ptr<Genfun::Parameter> ParamPtr;

class PlanetOrbit
{
private:
    Genfun::RKIntegrator integrator;

public:
    Genfun::Parameter* GM;
    Genfun::Parameter* e;
    Genfun::Parameter* a;
    Genfun::Parameter* theta0;
    Genfun::Parameter* inclination;
    Genfun::Parameter* orientation;
    FuncPtr X;
    FuncPtr Y;

    PlanetOrbit()
    {
        Genfun::Variable theta;
        Genfun::Sin qSin;
        Genfun::Cos qCos;
        Genfun::Sqrt qSqrt;

        GM = integrator.createControlParameter("GM", 0.0002959, 0);
        e = integrator.createControlParameter("eccentricity", 0, 0, 1);
        a = integrator.createControlParameter("semimajor axis", 1);
        theta0 = integrator.createControlParameter("Theta0", 0, -2*M_PI, 2*M_PI);
        orientation = new Genfun::Parameter("Orientation", 0, -2*M_PI, 2*M_PI);
        inclination = new Genfun::Parameter("Inclination", 0, 0, M_PI);

        Genfun::GENFUNCTION r = (*a) * (1 - (*e)*(*e)) / (1 + (*e) * qCos(theta - (*theta0)));
        Genfun::GENFUNCTION DTheta = qSqrt((*GM) * (*a) * (1 - (*e)*(*e))) / (r*r);

        integrator.addDiffEquation(&DTheta);

        Genfun::GENFUNCTION Theta = *integrator.getFunction(theta);
        Genfun::GENFUNCTION Radius = (*a) * (1 - (*e) * (*e)) / (1 + (*e) * qCos(Theta - (*theta0)));

        X = FuncPtr((Radius * qCos(Theta - (*theta0) + (*orientation))).clone());
        Y = FuncPtr((Radius * qSin(Theta - (*theta0) + (*orientation))).clone());
    }

    ~PlanetOrbit()
    {
        delete orientation;
    }
};

const double MAX_ANGLE = 3 * M_PI;

double Time2Radian(const double hour, const double minute, const double second)
{
    const double Hour2Radian = 2 * M_PI / 24;
    const double Minute2Radian = Hour2Radian / 60;
    const double Second2Radian = Minute2Radian/ 60;
    double angle = Hour2Radian*std::abs(hour) + Minute2Radian*std::abs(minute) + Second2Radian*std::abs(second);
    return std::signbit(hour) ? -angle : angle;
}

double Degree2Radian(const double degree, const double minute, const double second)
{
    const double Degree2Radian = 2 * M_PI / 360;
    const double Minute2Radian = Degree2Radian / 60;
    const double Second2Radian = Minute2Radian / 60;
    double angle = Degree2Radian*std::abs(degree) + Minute2Radian*std::abs(minute) + Second2Radian*std::abs(second);
    return std::signbit(degree) ? -angle : angle;
}

void readData(const std::string& file, Data& TIME, Data& RA, Data& DEC)
{
    std::ifstream inFile(file);
    std::string day, time;

    for (size_t i = 1; inFile >> day >> time; i++)
    {
        TIME.push_back(i);

        double hour, minute, second;
        inFile >> hour >> minute >> second;
        RA.push_back(Time2Radian(hour, minute, second));
        inFile >> hour >> minute >> second;
        DEC.push_back(Degree2Radian(hour, minute, second));
    }
}

Data refineData(const Data& rawData, const double lowerBound, const double upperBound);
Hist1D binData(const Data& data);
Table tableData(const Data& data);

const std::string dataFileName = "Mars01.dat";
const bool verbose = true;

int main(int argc, char **argv)
{
    QApplication app(argc, argv);
    MultipleViewWindow window;

    QToolBar *toolBar = window.addToolBar("Tools");
    QAction *quitAction = toolBar->addAction("Quit (q)");
    quitAction->setShortcut(QKeySequence("q"));
	QObject::connect(quitAction, SIGNAL(triggered()), &app, SLOT(quit()));

    PlanetOrbit Earth;
    Earth.e->setValue(0.8);
    Earth.theta0->setValue(M_PI/3);
    Earth.orientation->setValue(-M_PI/3);
    PlotOrbit earthOrbit(*(Earth.X), *(Earth.Y), 0, 360);
    PlotView *earthView = createPlotView("Earth", "X", "Y", PRectF(-2, 2, -2, 2), 800, 800);
    earthView->add(&earthOrbit);
    window.add(earthView, "Earth");

    Data TIME, RA, DEC;
    readData(dataFileName, TIME, RA, DEC);

    std::cout << "RA @ 0: " << RA[0] << std::endl;
    std::cout << "DEC @ 0: " << DEC[0] << std::endl;

    PlotView *RAView = createPlotView("Right Ascension", "Days", "", PRectF(-10, 760, -0.5, 2*M_PI + 0.5));
    window.add(RAView, "RA");

    PlotProfile RAProfile;
    for (size_t i = 0; i < TIME.size(); i++)
    {
        RAProfile.addPoint(TIME[i],RA[i]);
    }
    {
        PlotProfile::Properties prop;
        prop.pen.setWidth(1);
        RAProfile.setProperties(prop);
    }
    RAView->add(&RAProfile);

    PlotView *DECView = createPlotView("Declination", "Days", "", PRectF(-10, 760, -1, 1));
    window.add(DECView, "DEC");

    PlotProfile DECProfile;
    for (size_t i = 0; i < TIME.size(); i++)
    {
        DECProfile.addPoint(TIME[i], DEC[i]);
    }
    {
        PlotProfile::Properties prop;
        prop.pen.setWidth(1);
        DECProfile.setProperties(prop);
    }
    DECView->add(&DECProfile);

	window.show();
	app.exec();

    return 0;
}

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange, const double Width, const double Height)
{
    PlotView *view_ptr = new PlotView(viewRange);
    view_ptr->setFixedWidth(Width);
    view_ptr->setFixedHeight(Height);

    PlotStream titleStream(view_ptr->titleTextEdit());
    titleStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << title
                << PlotStream::EndP();

    PlotStream xLabelStream(view_ptr->xLabelTextEdit());
    xLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << xLabel
                << PlotStream::EndP();

    PlotStream yLabelStream(view_ptr->yLabelTextEdit());
    yLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << yLabel
                << PlotStream::EndP();

    return view_ptr;
}