/**
    @file .cpp

    @author Terence Henriod

    Project Name

    @brief This program...

    @version Original Code 1.00 (10/29/2013) - T. Henriod
*/

/*
bool extensionIsGood( const string& file_name, const string& extension )
{
    return ( file_name.find( extension, file_name.length() - 5 ) !=
             string::npos );
}
 */

/*==============================================================================
=======     HEADER FILES     ===================================================
==============================================================================*/
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cstdio>
#include <cmath>
#include <ctime>
#include <string>
#include <vector>

#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib
#include "my_stopwatch.h"

using namespace std;
using namespace Eigen;

/*==============================================================================
=======     GLOBAL CONSTANTS     ===============================================
==============================================================================*/
#define DEBUG 1

const int STD_STR_LEN = 100;

const char MATCH = 'X';
const char NO_MATCH = '-';

const string TRAINING_RESULTS_DIRECTORY = "/training_results/";

enum Status
{
    NO_ERRORS = 0,
    ERROR
};

enum Mode
{
    TRAINING_MODE = 0,
    TESTING_MODE = 1,
    INVALID = 666
};

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

=======     USER DEFINED TYPES     =============================================
==============================================================================*/

/**
@struct

Description

@var
*/
typedef struct
{
    char** training_image_names;
    int num_training_images;
    char** training_eigenweight_names;
    int num_training_eigenweights;
    char** testing_image_names;
    int num_testing_images;
    int pixels_per_image;
    int width_in_pixels;
    int height_in_pixels;
} JobInfo;

typedef struct
{
    char tested_image_name[ STD_STR_LEN ];
    int test_image_index;
    char matched_image_name[ STD_STR_LEN ];
    int matched_image_index;
    double distance_between_images;
    char is_match_by_test;
    char is_actual_match;
} MatchResult;

/*==============================================================================
=======     GLOBAL VARIABLES     ===============================================
==============================================================================*/

/*==============================================================================
=======     FUNCTION PROTOTYPES     ============================================
==============================================================================*/
/**
FunctionName

A short description

@param

@return

@pre
-#

@post
-#

@detail @bAlgorithm
-#

@exception

@code
@endcode
*/
int processCommandLineArguments( const int argc, char** argv,
                                 JobInfo& info );

int trainingMode( JobInfo& info );

int findSampleImageSize( int& image_width, int& image_height,
                         const char* file_name );

int allocateArrays( double**& face_data_matrix, double*& mean_face, const JobInfo& info );

int loadTrainingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info );

int readPgmToVector( VectorXd& image_vector, const char* file_name, const JobInfo& info );

int trainMeanFace( VectorXd& mean_face,  const Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info );

int writeVectorToFile( const char* file_name, const VectorXd& data_vector );

int centerFaceData( Matrix<double, Dynamic, Dynamic>& centered_data, Matrix<double, Dynamic, Dynamic>& face_data, const VectorXd& mean_face, const JobInfo& info );

int findFaceCovariance( Matrix<double, Dynamic, Dynamic>& face_covariance,
                        const Matrix<double, Dynamic, Dynamic>& centered_data,
                        const JobInfo& info );

int findEigenValuesVectors( VectorXd& eigenvalues, MatrixXd& eigenvectors,
                            const MatrixXd& A_matrix, const JobInfo& info );

bool compareEigenvalIndexPairs( const pair< double, int >& one,
                                const pair< double, int >& other );

double getVectorMagnitude( const VectorXd the_vector );

int writePgmImage( const string& file_name, const VectorXd& image_data,
                   int image_width, int image_height, int image_shades );

VectorXd scaleImageToPgm( const VectorXd& image_data, const JobInfo& info );

int storeEigenData( const VectorXd& eigenvalues, const MatrixXd& eigenvectors );

int promptForNumEigens( const VectorXd& eigenvalues );

int storeEigenProjections( const int eigens_to_keep, const MatrixXd& centered_data, const MatrixXd& face_eigenvectors, const JobInfo& info );

int writeEigenweightFile( const char* file_name, const VectorXd& eigen_weights );

int testingMode( JobInfo& info );

int loadTrainingOutcomes( VectorXd& mean_face, VectorXd& eigenvalues, MatrixXd& eigenvectors, MatrixXd& training_eigenweights, const JobInfo& info );

int readDataVectorFromTxt( VectorXd& data_vector, const char* file_name );

int readMatrixFromTxt( MatrixXd& data_matrix, const char* file_name );

int loadTestingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info );

int centerTestData( MatrixXd& centered_data, MatrixXd& face_data, const VectorXd& mean_face, const JobInfo& info );

int projectDataInEigenspace( MatrixXd& eigenweights, const MatrixXd& data, const MatrixXd& eigenvectors );

VectorXd projectImageInEigenspace( const VectorXd& centered_image, const MatrixXd& eigenvectors  );

double promptForThreshold();

int matchAllImages( vector< MatchResult >& matching_results,
                 const MatrixXd& training_eigenweights,
                 const MatrixXd& testing_eigenweights,
                 const MatrixXd& eigenvalues,
                 const double threshold,
                 const int num_eigens_to_use,
                 const JobInfo& info );

int matchImage( MatchResult& result, const VectorXd& test_weights, const MatrixXd& training_weights );

int matchImage( MatchResult& result, const VectorXd& test_weights,
                const MatrixXd& training_weights, const VectorXd& eigenvalues,
                const int num_eigens_to_use, const double threshold,
                const JobInfo& info );

double computeMahalanobisDistance( const VectorXd& test_weights,
                                   const VectorXd& training_weights,
                                   const VectorXd& eigenvalues,
                                   const int num_eigens_to_use );

int outputDataSummary( vector< MatchResult >& results, double threshold, int num_eigens_to_use, bool explicit_summary );

char* stripFilePath( char* file_name );

/*==============================================================================
=======     MAIN FUNCTION     ==================================================
==============================================================================*/

/**
FunctionName

A short description

@param

@return

@pre
-#

@post
-#

@detail @bAlgorithm
-#

@exception

@code
@endcode
*/
int main( int argc, char** argv )
{
    // variables
    int program_status = NO_ERRORS;
    int program_mode = TRAINING_MODE;
    JobInfo info;

    // process the command line arguments
    program_mode = processCommandLineArguments( argc, argv, info );

info.pixels_per_image = 2880; // 48 x 60 for hi-res samples

    // case: the program is running in training mode
    if( program_mode == TRAINING_MODE )
    {
        // indicate that the program is running in training mode
        cout << char( 0x0C )
             << "    =====================" << endl
             << "    |   TRAINING MODE   |" << endl
             << "    =====================" << endl << endl;

        // run program in training mode
        trainingMode( info );
    }
    // case: the program is running in "testing" mode
    else if( program_mode == TESTING_MODE )
    {
        // indicate that the program is running in training mode
        cout << char( 0x0C )
             << "       ====================" << endl
             << "       |   TESTING MODE   |" << endl
             << "       ====================" << endl << endl;

        testingMode( info );
    }
    // case: bad command line arguments were used
    else
    {
        puts( "Bad command line arguments result in program termination.\n" );
    }

    // return a program operation status signal
    return program_status;
}

/*==============================================================================
=======     FUNCTION IMPLEMENTATIONS     =======================================
==============================================================================*/
int processCommandLineArguments( const int argc, char** argv,
                                 JobInfo& info )
{
    // variables
    int program_mode = INVALID;

    // case: a large enough number of command line arguments were given
    if( argc >= 3 )
    {
        // collect the numerical value of the first command line argument
        program_mode = atoi( argv[1] );

        // case: the given mode is not a valid one
        if( !( program_mode == TRAINING_MODE ||
               program_mode == TESTING_MODE    ) )
        {
            // set the mode to indicate an invalid mode
            program_mode = INVALID;
        }

        if( program_mode == TRAINING_MODE )
        {
            // set up the job information
            info.num_training_images = argc - 2;
            info.training_image_names = argv + 2;
        }
        else if( program_mode == TESTING_MODE )
        {
            // set up the job information
            info.training_image_names = argv + 2;
            info.num_training_images = 0;
            while( strstr( info.training_image_names[ info.num_training_images ], ".pgm" ) != NULL )
            {
                info.num_training_images++;
            }

            info.training_eigenweight_names = info.training_image_names + info.num_training_images;
            info.num_training_eigenweights = 0;
            while( strstr( info.training_eigenweight_names[ info.num_training_eigenweights ], ".txt" ) != NULL )
            {
                info.num_training_eigenweights++;
            }

            info.testing_image_names = info.training_eigenweight_names + info.num_training_eigenweights;
            info.num_testing_images = 0;
            while( ( info.num_testing_images < 1196 ) &&
                   ( strstr( info.testing_image_names[ info.num_testing_images ], ".pgm" ) != NULL ) )
            {
                info.num_testing_images++;
            }
        }
    }
    // case: too few arguments were used
    else
    {
        // give a stern message
        cout << "Something's wrong there guy... You need more arguments."
             << endl << endl;
    }

    // return the mode the program will be running in
    return program_mode;
}

int trainingMode( JobInfo& info )
{
    // variables
    int training_success = NO_ERRORS;

    VectorXd mean_face;
    VectorXd face_eigenvalues;
    Matrix< double, Dynamic, Dynamic > face_eigenvectors;
    Matrix< double, Dynamic, Dynamic > face_data;
    Matrix< double, Dynamic, Dynamic > centered_data; // A matrix
    Matrix< double, Dynamic, Dynamic > face_covariance;
    Matrix< double, Dynamic, Dynamic > eigen_faces;
    pair< VectorXd, MatrixXd > eigen_values_vectors;

    int eigens_to_keep = 1;

    info.pixels_per_image = findSampleImageSize( info.width_in_pixels,
                                                 info.height_in_pixels,
                                                 info.training_image_names[0] );

    puts( "Loading data...\n" );
    loadTrainingData( face_data, info );

    puts( "Training mean...\n" );
    trainMeanFace( mean_face, face_data, info );

    puts( "\"Centering\" faces...\n" );
    centerFaceData( centered_data, face_data, mean_face, info );

/*    puts( "Training covariance matrix...\n" );
    findFaceCovariance( face_covariance, centered_data, info );
 */

    // compute the Eigen values/vectors
    puts( "Finding Eigenvalues/Eigenvectors...\n" );
    findEigenValuesVectors( face_eigenvalues, face_eigenvectors,
                            centered_data, info );

    // create data/coefficients file
    puts( "Storing the found eigen-data...\n" );
    storeEigenData( face_eigenvalues, face_eigenvectors );

    // prompt user for the amount of information to keep
    eigens_to_keep = promptForNumEigens( face_eigenvalues );

    // create the reduced dimensionality eigenfaces, store them to a file
    puts( "Storing the training faces projected into eigenspace...\n" );
    storeEigenProjections( eigens_to_keep, centered_data, face_eigenvectors, info );

    // return a signal as to whether or not training was successful
    return training_success;
}

int findSampleImageSize( int& image_width, int& image_height,
                         const char* file_name )
{
    ifstream fin;
    string magic_number;
    int width = 0;
    int height = 0;

    fin.clear();
    fin.open( file_name );
    fin >> magic_number >> image_width >> image_height;
    fin.close();

    return ( image_width * image_height );
}

int loadTrainingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info )
{
    // variables
    int image_num = 0;
    VectorXd temp;

    face_data.resize( info.pixels_per_image, info.num_training_images );

    // for every image
    for( image_num = 0; image_num < info.num_training_images; image_num++ )
    {
        readPgmToVector( temp, info.training_image_names[image_num], info );
        face_data.col( image_num ) = temp;
    }

#if 0
writePgmImage( info.training_image_names[20], face_data.col( 20 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif

}

int readPgmToVector( VectorXd& image_vector, const char* file_name, const JobInfo& info )
{
    int read_success = NO_ERRORS;
    int pixel_num = 0;
    ifstream fin;
    string magic_number;
    int width;
    int height;
    int granularity;
    unsigned char pixel_byte;

    // open the file
    fin.clear();
    fin.open( file_name );

    // size the vector appropriately
    image_vector.resize( info.pixels_per_image );

    // eat the header
    fin >> magic_number >> width >> height >> granularity;
    fin.get(); // and an endline char

    // read in the data to a column vector
    for( pixel_num = 0; pixel_num < info.pixels_per_image; pixel_num++ )
    {
        // read in the byte, convert to float
        pixel_byte = fin.get();
        image_vector( pixel_num ) = (double) pixel_byte;
    }

    // close the file
    fin.close();

    return read_success;
}

int trainMeanFace( VectorXd& mean_face,  const Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info )
{
    int mean_success = NO_ERRORS;
    int image_num = 0, pixel_num = 0;

    mean_face.resize( info.pixels_per_image );

    for( pixel_num = 0; pixel_num < info.pixels_per_image; pixel_num++ )
    {
        for( image_num = 0, mean_face( pixel_num ) = 0;
             image_num < info.num_training_images;
             image_num++ )
        {
            mean_face( pixel_num ) += face_data( pixel_num, image_num );
        }

        mean_face( pixel_num ) /= info.num_training_images;
    }

#if 1
writePgmImage( "mean_training_face.pgm", mean_face, info.width_in_pixels,
               info.height_in_pixels, 255 );

writeVectorToFile( "mean_training_face_data.txt", mean_face );
#endif

    return mean_success;
}

int writeVectorToFile( const char* file_name, const VectorXd& data_vector )
{
    int write_success = NO_ERRORS;
    ofstream fout;
    int i = 0;

    fout.clear();
    fout.open( file_name );
    fout << data_vector.size() << endl;
    fout<< data_vector << endl;

    return write_success;
}

int centerFaceData( Matrix<double, Dynamic, Dynamic>& centered_data, Matrix<double, Dynamic, Dynamic>& face_data, const VectorXd& mean_face, const JobInfo& info )
{
    int centering_success = NO_ERRORS;
    int image_number = 0, pixel_number = 0;

    centered_data.resize( face_data.rows(), face_data.cols() );

    for( image_number = 0; image_number < info.num_training_images; image_number++ )
    {
        for( pixel_number = 0; pixel_number < info.pixels_per_image; pixel_number++ )
        {
            centered_data( pixel_number, image_number ) = face_data( pixel_number, image_number ) - mean_face( pixel_number );
        }
    }

#if 0
writePgmImage( "test_center.pgm", centered_data.col( 5 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif

    return centering_success;
}

int findFaceCovariance( Matrix<double, Dynamic, Dynamic>& face_covariance,
                        const Matrix<double, Dynamic, Dynamic>& centered_data,
                        const JobInfo& info )
{
    int covariance_success = NO_ERRORS;
    double inverse_num_training_images = 1.0 / info.num_training_images;

    face_covariance.resize( info.pixels_per_image, info.pixels_per_image );

    face_covariance = inverse_num_training_images *
                      ( centered_data * centered_data.transpose().eval() );

    return covariance_success;
}

bool compareEigenvalIndexPairs( const pair< double, int >& one,
                                const pair< double, int >& other )
{
    // this makes for a high to low sort when used with std::sort
    return ( one.first > other.first );
}

double getVectorMagnitude( const VectorXd the_vector )
{
    double magnitude = 0.0;
    int i = 0;

    for( i = 0; i < the_vector.size(); i++ )
    {
        magnitude += ( the_vector( i ) * the_vector( i ) );
    }

    return sqrt( magnitude );
}

int writePgmImage( const string& file_name, const VectorXd& image_data, int image_width, int image_height, int image_shades )
{
    int write_success = NO_ERRORS;
    ofstream fout;
    int pixel_num = 0;
    int row_sizer = 0;
    int num_pixels = image_width * image_height;

    fout.clear();
    fout.open( file_name.c_str() );

    // write header with PGM "magic number" for binary type
    fout << "P5" << '\n' << image_width << ' '
         << image_height << '\n' << image_shades << '\n';

    // write the data
    for( pixel_num = 0; pixel_num < num_pixels; pixel_num++ )
    {
        fout << (unsigned char) ((unsigned int)image_data( pixel_num ) % (image_shades + 1));
    }

#if 0
    // write header with PGM "magic number" for ASCII type
    fout << "P2" << ' ' << image_width << ' '
         << image_height << ' ' << image_shades << '\n';

    // write the data
    for( pixel_num = 0; pixel_num < num_pixels; pixel_num++ )
    {
        fout << (unsigned int) image_data( pixel_num ) % image_shades;
        row_sizer++;
        if( row_sizer < image_width )
        {
            fout << ' ';
        }
        else
        {
            row_sizer = 0;
            fout << '\n';
        }
    }
#endif
    fout.close();

    return write_success;
}

int findEigenValuesVectors( VectorXd& eigenvalues, MatrixXd& eigenvectors, const MatrixXd& A_matrix, const JobInfo& info )
{
    EigenSolver<MatrixXd> eigen_solver;
    Matrix<double, Dynamic, Dynamic> At_A_matrix;   // A^t * A matrix
    Matrix<double, Dynamic, Dynamic> interim_eigenvectors;
    vector< pair< double, int> > eigen_indices;
    pair< double, int > temp;
    int i = 0;
    char buffer[100];

    // compute the A^t * A matrix to save on computations vs A * A^t
    At_A_matrix = A_matrix.transpose().eval() * A_matrix;

    // compute the eigen-stuff of the matrix
    eigen_solver.compute( At_A_matrix );

    // identify the strictly real eigen values and vectors
    for( i = 0; i < eigen_solver.eigenvalues().size(); i++ )
    {
        if( ( eigen_solver.eigenvalues().imag()( i ) == 0.0 ) )
        {
            temp.first = eigen_solver.eigenvalues().real()( i );
            temp.second = i;
            eigen_indices.push_back( temp );
        }
    }

    // sort the eigen stuff in descending order by eigen value
    sort( eigen_indices.begin(), eigen_indices.end(),
          compareEigenvalIndexPairs );

    // store the eigen-stuff in usable objects
    eigenvalues.resize( eigen_indices.size() );
    interim_eigenvectors.resize( A_matrix.cols(), eigen_indices.size() );
    eigenvectors.resize( A_matrix.rows(), eigen_indices.size() );
    for( i = 0; i < eigen_indices.size(); i++ )
    {
        eigenvalues( i ) = eigen_indices[i].first;
        interim_eigenvectors.col( i ) = eigen_solver.eigenvectors().col( eigen_indices[i].second ).real();
    }

    // get from Av to u for actual eigenvectors
    for( i = 0; i < eigenvectors.cols(); i++ )
    {
        // get the u vector
        eigenvectors.col( i ) = (A_matrix * interim_eigenvectors.col( i ));

        // make it a unit u vector
        eigenvectors.col( i ) /= getVectorMagnitude( eigenvectors.col( i ) );
    }

    for( i = 0; i < eigenvalues.size(); i++ )
    {
        sprintf( buffer, "eigenfaces/eigenface_%d.pgm", i );

        writePgmImage( buffer, scaleImageToPgm( eigenvectors.col( i ), info ), info.width_in_pixels,
               info.height_in_pixels, 255 );
    }

    return 0;
}

VectorXd scaleImageToPgm( const VectorXd& image_data, const JobInfo& info )
{
    VectorXd scaled_vector;
    double minimum_value = image_data( 0 );
    double maximum_value = image_data( 0 );
    int pixel_num = 0;

    scaled_vector.resize( image_data.size() );
    scaled_vector = image_data;

    // find the minimum and maximum values for scaling
    for( pixel_num = 0; pixel_num < image_data.size(); pixel_num++ )
    {
        if( image_data( pixel_num ) < minimum_value )
        {
            minimum_value = image_data( pixel_num );
        }

        if( image_data( pixel_num ) > maximum_value )
        {
            maximum_value = image_data( pixel_num );
        }
    }

    // scale the pixel values properly
    for( pixel_num = 0; pixel_num < image_data.size(); pixel_num++ )
    {
        scaled_vector( pixel_num ) = image_data( pixel_num ) - minimum_value;
    }
    maximum_value -= minimum_value;
    scaled_vector *= ( 255.0 / (maximum_value ));//- minimum_value ));

    return scaled_vector;
}

int storeEigenData( const VectorXd& eigenvalues, const MatrixXd& eigenvectors )
{
    int storage_success = NO_ERRORS;
    ofstream fout;
    int i = 0;
    int j = 0;

    fout.clear();
    fout.open( "EIGENVALUES.txt" );
    fout << eigenvalues.size() << endl;
    for( i = 0; i < eigenvalues.size(); i++ )
    {
        fout << eigenvalues( i ) << endl;
    }
    fout.close();

    fout.clear();
    fout.open( "EIGENVECTORS.txt" );
    fout << eigenvectors.rows() << ' ' << eigenvectors.cols() << endl;

    fout << eigenvectors << endl;

    fout.close();

    return storage_success;
}

int promptForNumEigens( const VectorXd& eigenvalues )
{
    int num_eigens_to_keep = 0;
    double eigenvalue_sum = 0;
    double kept_eigenvalue_sum = 0;
    char response;
    bool keep_prompting = true;
    int i = 0;

    for( i = 0; i < eigenvalues.size(); i++ )
    {
        eigenvalue_sum += eigenvalues( i );
    }

    while( keep_prompting )
    {
        cout << "How many eigen values (faces) should be kept? ("
             << eigenvalues.size() << " available): ";
        cin >> num_eigens_to_keep;
 // use 16 for ~80%

        for( kept_eigenvalue_sum = 0, i = 0; i < num_eigens_to_keep; i++ )
        {
            kept_eigenvalue_sum += eigenvalues( i );
        }

        cout << ( kept_eigenvalue_sum * 100 / eigenvalue_sum )
             << "% of the information will be kept. Is this acceptable? (y/n): ";
        cin >> response;

        if( response == 'y' )
        {
            keep_prompting = false;
        }
    }

    return num_eigens_to_keep;
}

int storeEigenProjections( const int eigens_to_keep, const MatrixXd& centered_data, const MatrixXd& eigenvectors, const JobInfo& info )
{
    int eigenprojection_success = NO_ERRORS;
    VectorXd projected_face;
    MatrixXd eigen_weights;
    char buffer[100];
    ofstream fout;
    int i = 0;
    int j = 0;

    // initial setup and resizing
    eigen_weights.resize( eigens_to_keep, centered_data.cols() );
    projected_face.resize( info.pixels_per_image );

    for( j = 0; j < centered_data.cols(); j++ )
    {
       // eigen_weights.col( j ) = projectImageInEigenspace( centered_data.col( j ), eigenvectors );

        for( i = 0; i < eigens_to_keep; i++ )
        {
            eigen_weights( i, j ) = eigenvectors.col( i ).transpose().eval() * centered_data.col( j );
        }

    }

    for( j = 0; j < centered_data.cols(); j++ )
    {
        projected_face *= 0;

        for( i = 0; i < eigens_to_keep; i++ )
        {
            projected_face += eigen_weights( i, j ) * eigenvectors.col( i );
        }

        sprintf( buffer, "training_eigenprojections/projected_face_%d_coefficients.txt", j );
        writeEigenweightFile( buffer, eigen_weights.col( j ) );

        sprintf( buffer, "training_eigenprojections/projected_face_%d.pgm", j );
        writePgmImage( buffer, scaleImageToPgm( projected_face, info ), info.width_in_pixels,
               info.height_in_pixels, 255 );
    }

    return eigenprojection_success;
}

int writeEigenweightFile( const char* file_name, const VectorXd& eigen_weights )
{
    int write_success = NO_ERRORS;
    ofstream fout;

    fout.clear();
    fout.open( file_name );

    fout << eigen_weights.size() << endl;
    fout << eigen_weights << endl;

    fout.close();

    return write_success;
}

int testingMode( JobInfo& info )
{
    int testing_success = NO_ERRORS;
    VectorXd mean_face;
    VectorXd eigenvalues;
    MatrixXd eigenvectors;
    MatrixXd testing_faces;
    MatrixXd centered_test_faces;
    MatrixXd training_eigenweights;
    MatrixXd testing_eigenweights;
    int num_eigens_to_use = 0;
    vector< MatchResult > matching_results;
    double threshold = 1.0;
    ofstream fout;
    int i = 0;
    int true_positive = 0, false_positive = 0, total_positive = 0, total_negative = 0;

    info.pixels_per_image = findSampleImageSize( info.width_in_pixels,
                                                 info.height_in_pixels,
                                                 info.training_image_names[0] );

    puts( "Loading training outcomes...\n" );
    loadTrainingOutcomes( mean_face, eigenvalues, eigenvectors, training_eigenweights, info );

    puts( "Projecting inputs onto eigenspace...\n" );
    loadTestingData( testing_faces, info );
    centerTestData( centered_test_faces, testing_faces, mean_face, info );
    projectDataInEigenspace( testing_eigenweights, centered_test_faces, eigenvectors );

    puts( "Matching inputs to training images...\n" );
    fout.clear()
    fout.open( "ROC.txt" );
    fout << "True positives, total positives, false negatives, total negatives" << endl;
    for( threshold = 0.0001, true_positive = 0, false_positive = 0, total_positive = 0, total_negative = 0;
           threshold < 0.004; threshold += 0.0005 )
    {
        num_eigens_to_use = 114; //promptForNumEigens( eigenvalues );
        // threshold = 1.5; // promptForThreshold();
        matchAllImages( matching_results, training_eigenweights, testing_eigenweights, eigenvalues, threshold, num_eigens_to_use, info );

        puts( "Outputting matching data...\n" );
        outputDataSummary( matching_results, threshold, num_eigens_to_use, false );

        for( i = 0; i < matching_results.size(); i++ );

    }

    return testing_success;
}

int loadTrainingOutcomes( VectorXd& mean_face, VectorXd& eigenvalues, MatrixXd& eigenvectors, MatrixXd& training_eigenweights, const JobInfo& info )
{
    int load_success = NO_ERRORS;
    int i = 0;
    VectorXd temp;

    readDataVectorFromTxt( mean_face, "mean_training_face_data.txt" );
    readDataVectorFromTxt( eigenvalues, "EIGENVALUES.txt" );
    readMatrixFromTxt( eigenvectors, "EIGENVECTORS.txt" );

cout << eigenvalues.size() << endl;

    training_eigenweights.resize( 1204, eigenvectors.cols() );
    for( i = 0; i < info.num_training_eigenweights; i++ )
    {
        readDataVectorFromTxt( temp, info.training_eigenweight_names[i] );
        training_eigenweights.col( i ) = temp;
    }

    return load_success;
}

int readDataVectorFromTxt( VectorXd& data_vector, const char* file_name )
{
    int read_success = NO_ERRORS;
    ifstream fin;
    int size;
    double value;
    int i;

    fin.clear();
    fin.open( file_name );

    if( fin.good() )
    {
        fin >> size;
        data_vector.resize( size );

        for( i = 0; i < size; i++ )
        {
            fin >> value;
            data_vector( i ) = value;
        }
    }

    return read_success;
}

int readMatrixFromTxt( MatrixXd& data_matrix, const char* file_name )
{
    int read_success = NO_ERRORS;
    ifstream fin;
    int rows, cols;
    double value;
    int i = 0, j = 0;

    fin.clear();
    fin.open( file_name );

    if( fin.good() )
    {
        fin >> rows >> cols;
        data_matrix.resize( rows, cols );

        for( i = 0; i < rows; i++ )
        {
            for( j = 0; j < cols; j++ )
            {
                fin >> value;
                data_matrix( i, j ) = value;
            }
        }
    }

    return read_success;
}

int loadTestingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info )
{
    // variables
    int image_num = 0;
    VectorXd temp;

    face_data.resize( info.pixels_per_image, info.num_testing_images );

    // for every image
    for( image_num = 0; image_num < info.num_testing_images; image_num++ )
    {
        readPgmToVector( temp, info.testing_image_names[image_num], info );
        face_data.col( image_num ) = temp;
    }

#if 1
writePgmImage( info.training_image_names[20], face_data.col( 20 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif
}

int centerTestData( MatrixXd& centered_data, MatrixXd& face_data, const VectorXd& mean_face, const JobInfo& info )
{
    int centering_success = NO_ERRORS;
    int image_number = 0, pixel_number = 0;

    centered_data.resize( face_data.rows(), face_data.cols() );

    for( image_number = 0; image_number < info.num_testing_images; image_number++ )
    {
        for( pixel_number = 0; pixel_number < info.pixels_per_image; pixel_number++ )
        {
            centered_data( pixel_number, image_number ) = face_data( pixel_number, image_number ) - mean_face( pixel_number );
        }
    }

#if 0

writePgmImage( "test_center.pgm", centered_data.col( 5 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif

    return centering_success;
}

int projectDataInEigenspace( MatrixXd& eigenweights, const MatrixXd& data, const MatrixXd& eigenvectors )
{
    int eigenprojection_success = NO_ERRORS;
    int j = 0;

    // initial setup and resizing
    eigenweights.resize( eigenvectors.cols(), data.cols() );

    for( j = 0; j < data.cols(); j++ )  // for each image
    {
        eigenweights.col( j ) = projectImageInEigenspace( data.col( j ), eigenvectors );

/*
        for( i = 0; i < eigenvectors.cols(); i++ ) // for each feature/weight
        {
            eigenweights( i, j ) = eigenvectors.col( i ).transpose().eval() * data.col( j );
        }
*/
    }

    return eigenprojection_success;
}

VectorXd projectImageInEigenspace( const VectorXd& centered_image, const MatrixXd& eigenvectors  )
{
    VectorXd weight_vector;
    int i = 0;

    weight_vector.resize( eigenvectors.cols() );

    for( i = 0; i < eigenvectors.cols(); i++ ) // for every eigenvector
    {
        weight_vector( i ) = eigenvectors.col( i ).transpose().eval() * centered_image;
    }

    return weight_vector;
}

double promptForThreshold()
{
    double threshold;

    cout << "Enter the threshold value to be used: ";
    cin >> threshold;

    return threshold;
}

int matchAllImages( vector< MatchResult >& matching_results,
                 const MatrixXd& training_eigenweights,
                 const MatrixXd& testing_eigenweights,
                 const MatrixXd& eigenvalues,
                 const double threshold,
                 const int num_eigens_to_use,
                 const JobInfo& info )
{
    int matching_success;
    int i = 0;
    int j = 0;
    double temp_distance = 0;
    double distance = 0;
    MatchResult temp;

    for( i = 0; i < testing_eigenweights.cols(); i++ )
    {
        strcpy( temp.tested_image_name, stripFilePath( info.testing_image_names[i] ) );

        matchImage( temp, testing_eigenweights.col( i ), training_eigenweights, eigenvalues, num_eigens_to_use, threshold, info );

        matching_results.push_back( temp );
    }

    return matching_success;
}

int matchImage( MatchResult& result, const VectorXd& test_weights,
                const MatrixXd& training_weights, const VectorXd& eigenvalues,
                const int num_eigens_to_use, const double threshold,
                const JobInfo& info )
{
    int match_function_success = NO_ERRORS;
    int i = 0;
    double temp_distance = 2E64;
    int CMC_counter = 0;

    // check against all other sets of weights
    for( i = 0, result.distance_between_images = 2E64; i < training_weights.cols(); i++ )
    {
        // find the distance between the image and a training one
        temp_distance = computeMahalanobisDistance( test_weights, training_weights.col( i ), eigenvalues, num_eigens_to_use );

        // case: the distance is better than what was thought to be the previous best
        if( temp_distance < result.distance_between_images )
        {
            // update the index of the match and the new, lower distance
            result.distance_between_images = temp_distance;
            result.matched_image_index = i;
        }

        if( atoi(result.tested_image_name) == atoi( stripFilePath( info.training_image_names[ i ] ) )
            && temp_distance < threshold )
        {
            CMC_counter++;
        }

    }

cout << "CMC" << CMC_counter << endl;

    // indicate which image the test one is closest to
    strcpy( result.matched_image_name,
            stripFilePath( info.training_image_names[ result.matched_image_index ] ) );

    // case: the best distance could be considered a match
    if( result.distance_between_images < threshold )
    {
        // indicate that there was a match, get matching image's name
        result.is_match_by_test = MATCH;
    }
    // case: the distance does not indicate a match
    else
    {
        result.is_match_by_test = NO_MATCH;
    }

    // determine if the images are an actual match
    // NOTE: I expect the atoi to read just the first segment of the image name, up to the first underscore
    if( atoi( result.tested_image_name ) == atoi( result.matched_image_name ) )
    {
        result.is_actual_match = MATCH;

cout << result.tested_image_name << "  " << result.matched_image_name << "  " << result.is_actual_match << ' ' <<  MATCH << endl;

    }
    else
    {
        result.is_actual_match = NO_MATCH;
    }

    return match_function_success;
}

double computeMahalanobisDistance( const VectorXd& test_weights,
                                   const VectorXd& training_weights,
                                   const VectorXd& eigenvalues,
                                   const int num_eigens_to_use )
{
    double mahalanobis_distance = 0;
    double euclidean_component;
    int i = 0;

    for( i = 0; i < num_eigens_to_use; i++ )
    {
        euclidean_component = ( test_weights( i ) - training_weights( i ) );
        euclidean_component *= euclidean_component;
        mahalanobis_distance += euclidean_component / eigenvalues( i );
    }

    return mahalanobis_distance;
}

int outputDataSummary( vector< MatchResult >& results, double threshold, int num_eigens_to_use, bool explicit_summary )
{
    ofstream fout;
    int i = 0;

    if( explicit_summary )
    {
        fout.clear();
        fout.open( "RESULTS.txt" );

        fout << "Test Image Name,"
                "Matched Image Name,"
                "Distance Between Images,"
                "Is Match By Test,"
                "Is Actually A Match,"
             << endl;

        for( i = 0; i < results.size(); i++ )
        {
            fout << results[i].tested_image_name << ", "
                 << results[i].matched_image_name << ", "
                 << results[i].distance_between_images << ", "
                 << results[i].is_match_by_test << ", "
                 << results[i].is_actual_match << endl;
        }
    }
}

char* stripFilePath( char* file_name )
{
    char* stripped_string = file_name;
    char* temp;

    temp = strstr( stripped_string, "/" );
    while( temp != NULL )
    {
        stripped_string = temp + 1;
        temp = strstr( stripped_string, "/" );
    }

    return stripped_string;
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    GeneratesEuler constant.

    @par Header <boost/simd/constant/euler.hpp>

    @par Semantic:
    The Euler constant can be defined as \f$\displaystyle \lim_{n \rightarrow \infty} \left(\sum_1^n \frac1n -\log n\right)\f$

    @code
    T r = Euler<T>();
    @endcode

    is similar to:

    @code
      r =  T(0.577215664901532860606512090082402431042159335939923598805767234884867726777664670936947063291746749);
    @endcode

**/
  template<typename T> T Euler();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      GeneratesEuler constant.

      Generate the  constant euler.

      @return The Euler constant for the proper type
    **/
    Value Euler();
  }
} }
#endif

#include <boost/simd/constant/scalar/euler.hpp>
#include <boost/simd/constant/simd/euler.hpp>

#endif


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

/*
 Copyright (C) 2016 Sebastian Schlenkrich

*/

#ifndef quantlib_templateauxilliaries_choleskyfactorisation_hpp
#define quantlib_templateauxilliaries_choleskyfactorisation_hpp

//#include <ql/types.hpp>
//#include <boost/function.hpp>

#include <stdlib.h>
#include <math.h>

namespace TemplateAuxilliaries {

    // This method is unsafe! Memory allocation need to be ensured by user
    // A(i,j) (and L) is stored row-wise as A[i*n+j]
    // L(i,j) is lower triangular matrix with L L^T = A
    template <typename Type>
    inline void cholesky(std::vector< Type >& A, std::vector< Type >& L, size_t n) {
        for (size_t i = 0; i < n; ++i) {
            for (size_t j = 0; j < (i+1); ++j) {
                Type s = 0;
                for (size_t k = 0; k < j; ++k) s += L[i * n + k] * L[j * n + k];
                if (i==j) {
                    if (A[i * n + i] < s) throw std::exception();
                    L[i * n + j] = sqrt(A[i * n + i] - s);
                }
                else L[i * n + j] = (1.0 / L[j * n + j] * (A[i * n + j] - s));
            }
        }
    }

    template <typename Type>
    std::vector< std::vector< Type > > cholesky(const std::vector< std::vector< Type > >& A) {
        std::vector<Type> arrayA(A.size()*A.size());
        std::vector<Type> arrayL(A.size()*A.size());
        for (size_t i = 0; i < A.size(); ++i) {
            if (A.size()!=A[i].size()) throw std::exception();
            for (size_t j = 0; j < A[i].size(); ++j) arrayA[i*A[i].size() + j] = A[i][j];
        }
        cholesky(arrayA, arrayL, A.size());
        std::vector< std::vector< Type > > L(A.size(), std::vector< Type >(A.size(), 0.0));
        for (size_t i = 0; i < L.size(); ++i) {
            for (size_t j = 0; j <= i; ++j) L[i][j] = arrayL[i*L[i].size() + j];
        }
        return L;
    }

    // alternative implementation of Cholesky decomposition
    template<class T>
    void performCholesky(std::vector< std::vector<T> >& matrix, size_t dimIn, bool flexible) {
        size_t dim = dimIn;
        for (size_t i = 0; i < dim; i++) {
            for (size_t j = i; j < dim; j++) {
                if (abs(matrix[i][j] - matrix[j][i])>QL_EPSILON)
                    QL_FAIL(std::string("A symmetrix matrix is necessary to apply Cholesky Decomposition"));
            }
        }

        //Perform Decomposition as described in script of Trottenberg (S. 47):
        //Because script delivers Upper right matrix, we interchange indizes in order to end up with lower left matrix.

        //First step:
        matrix[0][0] = sqrt(matrix[0][0]);
        if (abs(matrix[0][0]) < QL_EPSILON && !flexible)
            QL_FAIL("No positive definite Correlation Matrix because rank is not full.");
        for (size_t i = 1; i < dim; i++) {
            matrix[i][0] = abs(matrix[0][0]) < QL_EPSILON ? 0.0 : matrix[i][0] / matrix[0][0];
        }

        //Now iterate:
        for (size_t i = 1; i < dim; i++) {
            for (size_t k = 0; k < i; k++) {
                matrix[i][i] = matrix[i][i] - matrix[i][k] * matrix[i][k];
            }
            matrix[i][i] = sqrt(matrix[i][i]);
            if (abs(matrix[i][i]) < QL_EPSILON && !flexible)
                QL_FAIL(std::string("No positive definite Correlation Matrix because rank is not full."));
            if (matrix[i][i] != matrix[i][i] && !flexible) {
                QL_FAIL(std::string("No positive definite Correlation Matrix as diagonal square entry is negative."));
            }
            matrix[i][i] = (matrix[i][i] != matrix[i][i]) ? 0.0 : matrix[i][i];
            for (size_t j = 0; j < dim; j++) {
                if (j >= i + 1) {
                    for (size_t k = 0; k < i; k++) {
                        matrix[j][i] = matrix[j][i] - matrix[i][k] * matrix[j][k];
                    }
                    matrix[j][i] = (abs(matrix[i][i]) < QL_EPSILON || matrix[i][i] != matrix[i][i]) ? 0.0 : matrix[j][i] / matrix[i][i];
                }
                else if (j<i) {
                    matrix[j][i] = 0;
                }
                else {
                    //Nothing because diagonale.
                }
            }
        }
    }

}

#endif  /* ifndef quantlib_templateauxilliaries_qrfactorisation_hpp */


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

int main()
{
using Eigen::Quaterniond;
using Eigen::Vector3d;
Eigen::Quaterniond q(2, 0, 1, -3);

  std::cout << "This quaternion consists of a scalar " << q.w() << " and a vector " << std::endl << q.vec() << std::endl;

  q.normalize();

  std::cout << "To represent rotation, we need to normalize it such that its length is " << q.norm() << std::endl;

  Eigen::Vector3d v(1, 2, -1);

  Eigen::Quaterniond p;

  p.w() = 0;

  p.vec() = v;

  Eigen::Quaterniond rotatedP = q * p * q.inverse();

  Eigen::Vector3d rotatedV = rotatedP.vec();

  std::cout << "We can now use it to rotate a vector " << std::endl << v << " to " << std::endl << rotatedV << std::endl;

  Eigen::Matrix3d R = q.toRotationMatrix(); // convert a quaternion to a 3x3 rotation matrix

  std::cout << "Compare with the result using an rotation matrix " << std::endl << R * v << std::endl;

  Eigen::Quaterniond a = Eigen::Quaterniond::Identity();

  Eigen::Quaterniond b = Eigen::Quaterniond::Identity();

  Eigen::Quaterniond c; // Adding two quaternion as two 4x1 vectors is not supported by the EIgen API. That is, c = a + b is not allowed. We have to do this in a hard way

  c.w() = a.w() + b.w();

  c.x() = a.x() + b.x();

  c.y() = a.y() + b.y();

  c.z() = a.z() + b.z();
}


#include <iostream>
#include <armadillo>
#include <ctime>

using namespace std;
using namespace arma;

const double pi  =3.141592653589793238462;
int n,k;  // nrow(X),ncol(X)
mat z,X,y,XXi,Xt;
const double a=1;
const double b=1;
const int B=pow(10,5);
const double s2 = 10;

double ll(mat be, double sig2) {
  mat c(1,k), out(k,1);

  c = y-X*be;
  out = (c.t()*c / sig2 + n*log(sig2))/-2;

  return as_scalar(out);
}

double lpb(mat be) {
  return as_scalar(-be.t()*XXi*be/(2*s2));
}

double lps(double sig2) {
  return (a-1) * log(sig2) - sig2/b;
}

mat mvrnorm(mat M, mat S) {
  int n = M.n_rows;
  mat e = randn(n);
  return M + chol(S).t()*e;
}

int main(int argc, char** argv) {
  mat mle;

  //posteriors:
  mat bb;
  mat ss;

  // candidate sigma:
  mat csb;
  const double css = 1;

  //candidate values:
  mat candb;
  double cands;

  //acceptance rates:
  int accb = 0;
  int accs = 0;

  //metropolis ratio:
  double q;

  z.load("../data/dat.txt");
  n = z.n_rows;
  k = z.n_cols-1;

  y = z.col(0);
  X = z.cols(1,k); // columns 2 to k+1 of z
  Xt = X.t();
  XXi = (Xt*X).i();
  csb = 4*XXi;
  mle = XXi * Xt * y;

  bb.set_size(B,k);
  ss.set_size(B,1);
  bb.zeros();
  ss.ones();

  //current vals:
  mat bc = bb.row(0);
  double sc = 1.0;

  cout << "Starting Metropolis:" <<endl;
  clock_t t1 = clock();
  for (int i=1; i<B; i++) {
    // Set Initial Values:
    bb.row(i) = bb.row(i-1);
    ss.at(i,0) = sc;
    bc = bb.row(i).t();

    //Update Beta:
    candb = mvrnorm(bc,csb);
    q = ll(candb,sc)+lpb(candb) -ll(bc,sc)-lpb(bc);
    if (q>log(randu())) {
      bc = candb;
      bb.row(i) = bc.t();
      accb++;
    }

    //Update sigma2:
    cands = randn()*sqrt(css)+sc;
    if (cands>0){
      q = ll(bc,cands)+lps(cands) -ll(bc,sc)-lps(sc);
      if (q>log(randu())) {
        sc = cands;
        accs++;
      }
    }

    cout << "\r" << i*100/B <<"%";
  }
  clock_t t2 = clock();
  double elapsed = double(t2-t1) / CLOCKS_PER_SEC;
  cout << "Elapsed Time: " <<elapsed<<"s. \n"<<endl;

  cout << "Posterior Means Beta: \n" <<
          mean(bb.rows(90000,100000-1)).t()<<endl;
  cout << "Posterior Mean Sigma2: \n" <<
          mean(ss.rows(90000,100000-1))<<endl;
  cout << "Beta Acceptance:   "<< 100*accb/B <<"%"<<endl;
  cout << "Sigma2 Acceptance: "<< 100*accs/B <<"%"<<endl;
  cout <<endl;
  //ss.save("s2.txt",raw_ascii);

  return 0;
}


#ifndef _COCONUT_PULP_MATH_ANGLE_HPP_
#define _COCONUT_PULP_MATH_ANGLE_HPP_

#include <iosfwd>

#include <boost/operators.hpp>

namespace coconut {
namespace pulp {
namespace math {

const float PI = static_cast<float>(3.14159264f);

class Angle :
	boost::less_than_comparable<Angle,
	boost::equality_comparable<Angle,
	boost::additive<Angle,
	boost::multiplicative<Angle, float>>>>
{
public:

	static const Angle RIGHT;

	static const Angle HALF_FULL;

	static const Angle FULL;

	constexpr float radians() const noexcept {
		return radians_;
	}

	constexpr float degrees() const noexcept {
		return radians_ * (180.0f / PI);
	}

	constexpr bool operator==(const Angle& rhs) const noexcept {
		return radians_ == rhs.radians_;
	}

	constexpr bool operator<(const Angle& rhs) const noexcept {
		return radians_ < rhs.radians_;
	}

	Angle& operator+=(const Angle& rhs) noexcept {
		radians_ += rhs.radians_;
		return *this;
	}

	Angle& operator-=(const Angle& rhs) noexcept {
		radians_ -= rhs.radians_;
		return *this;
	}

	Angle& operator*=(float rhs) noexcept {
		radians_ *= rhs;
		return *this;
	}

	Angle& operator/=(float rhs) noexcept {
		radians_ /= rhs;
		return *this;
	}

	Angle operator-() const noexcept {
		return -1.0f * (*this);
	}

	friend const Angle radians(float radians) noexcept;

	friend const Angle degrees(float degrees) noexcept;

private:

	constexpr explicit Angle(float radians) noexcept :
		radians_(radians)
	{
	}

private:

	float radians_;

};

static_assert(sizeof(Angle) == sizeof(float), "Angle should have no extra data");

std::ostream& operator<<(std::ostream& os, const Angle& angle);

inline const Angle radians(float radians) noexcept {
	return Angle(radians);
}

inline const Angle degrees(float degrees) noexcept {
	return Angle(degrees * (PI / 180.0f));
}

inline const Angle operator""_rad(long double r) noexcept {
	return radians(static_cast<float>(r));
}

inline const Angle operator""_deg(long double d) noexcept {
	return degrees(static_cast<float>(d));
}

} // namespace math

using math::PI;
using math::Angle;
using math::radians;
using math::degrees;

namespace math_literals {

using math::operator ""_deg;
using math::operator ""_rad;

} // namespace math_literals

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_ANGLE_HPP_ */


#include <iostream>
#include <armadillo>
#include <cmath>

double calculate_anisotropy(const arma::mat &m) {

    const double iso = arma::mean(arma::eig_sym(m));
    double aniso = arma::accu(m % m);
    aniso = std::sqrt(std::abs(1.5*(aniso - (3.0*iso*iso))));

    return aniso;
}

int main() {

    arma::mat tensor(3, 3);
    tensor(0, 0) = 71.96979730;
    tensor(0, 1) = -7.19617988;
    tensor(0, 2) = 1.01470321;
    tensor(1, 0) = -7.19617833;
    tensor(1, 1) = 65.74361807;
    tensor(1, 2) = -5.62003645;
    tensor(2, 0) = 1.01469269;
    tensor(2, 1) = -5.62002592;
    tensor(2, 2) = 65.05385723;

    // arma::cx_vec principal_components_cx;
    // arma::cx_mat orientation_cx;

    // arma::eig_gen(principal_components_cx, orientation_cx, tensor);
    // principal_components_cx.print("principal components (complex)");
    // orientation_cx.print("orientation (complex)");

    arma::vec principal_components;
    arma::mat orientation;

    arma::eig_sym(principal_components, orientation, tensor);

    principal_components.print("principal components (real)");
    orientation.print("orientation (real)");

    double isotropic = arma::mean(principal_components);
    std::cout << "isotropic  : " << isotropic << std::endl;
    double anisotropic = calculate_anisotropy(tensor);
    std::cout << "anisotropic: " << anisotropic << std::endl;

    return 0;
}


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

#include "crossprod.h"

int main() {
  double T = 1.;
  int N = 1;

  Eigen::Vector3d y0(0.1, 0.2, 0.4);

  auto f = [](Eigen::Vector3d y) -> Eigen::Vector3d {
    return Eigen::Vector3d(y(0) * y(1), y(1) * y(2), y(2) - y(0));
  };

  auto Jf = [](Eigen::Vector3d y) -> Eigen::Matrix3d {
    Eigen::Matrix3d J;
    J << y(1), y(0), 0, 0, y(2), y(1), -1, 0, 1;
    return J;
  };
  // test implicit midpoint
  std::vector<Eigen::VectorXd> test_imp =
      CrossProd::solve_imp_mid(f, Jf, T, y0, N);
  std::cout << "Implicit midpoint:\n"
            << test_imp.back() << std::endl
            << std::endl;

  // test linear implicit midpoint
  std::vector<Eigen::VectorXd> test_lin =
      CrossProd::solve_lin_mid(f, Jf, T, y0, N);
  std::cout << "Implicit linear midpoint:\n"
            << test_lin.back() << std::endl
            << std::endl;

  // CrossProd::tab_crossprod();

  return 0;
}


#include "drake/math/quadratic_form.h"

#include <algorithm>

#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>

#include "drake/math/matrix_util.h"

namespace drake {
namespace math {
Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX(
    const Eigen::Ref<const Eigen::MatrixXd>& Y, double zero_tol) {
  if (Y.rows() != Y.cols()) {
    throw std::runtime_error("Y is not square.");
  }
  if (zero_tol < 0) {
    throw std::runtime_error("zero_tol should be non-negative.");
  }
  Eigen::LLT<Eigen::MatrixXd> llt_Y(Y);
  if (llt_Y.info() == Eigen::Success) {
    return llt_Y.matrixU();
  } else {
    // TODO(hongkai.dai) Switch to use robust Choleskly decomposition instead
    // of Eigen value decomposition, when the bug in
    // http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1479 is fixed.
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es_Y(Y);
    if (es_Y.info() == Eigen::Success) {
      Eigen::MatrixXd X(Y.rows(), Y.cols());
      int X_row_count = 0;
      for (int i = 0; i < es_Y.eigenvalues().rows(); ++i) {
        if (es_Y.eigenvalues()(i) < -zero_tol) {
          throw std::runtime_error("Y is not positive definite.");
        } else if (es_Y.eigenvalues()(i) > zero_tol) {
          X.row(X_row_count++) = std::sqrt(es_Y.eigenvalues()(i)) *
                                 es_Y.eigenvectors().col(i).transpose();
        }
      }
      return X.topRows(X_row_count);
    }
  }
  throw std::runtime_error("Y is not PSD.");
}

std::pair<Eigen::MatrixXd, Eigen::MatrixXd> DecomposePositiveQuadraticForm(
    const Eigen::Ref<const Eigen::MatrixXd>& Q,
    const Eigen::Ref<const Eigen::VectorXd>& b, double c, double tol) {
  if (Q.rows() != Q.cols()) {
    throw std::runtime_error("Q should be a square matrix.");
  }
  if (b.rows() != Q.rows()) {
    throw std::runtime_error("b does not have the right size.");
  }
  // The quadratic form xᵀQx + bᵀx + c can also be written as
  // [x]ᵀ * [Q   b/2] * [x]
  // [1]    [b/2   c]   [1]
  // We will call the matrix in the middle as M
  Eigen::MatrixXd M(Q.rows() + 1, Q.rows() + 1);
  // clang-format on
  M << (Q + Q.transpose()) / 2, b / 2,
       b.transpose() / 2, c;
  // clang-format off

  const Eigen::MatrixXd A = DecomposePSDmatrixIntoXtransposeTimesX(M, tol);
  Eigen::MatrixXd R = A.leftCols(Q.cols());
  Eigen::VectorXd d = A.col(Q.cols());
  return std::make_pair(R, d);
}

Eigen::MatrixXd BalanceQuadraticForms(
    const Eigen::Ref<const Eigen::MatrixXd>& S,
    const Eigen::Ref<const Eigen::MatrixXd>& P) {
  const double tolerance = 1e-8;
  const int n = S.rows();
  DRAKE_THROW_UNLESS(P.rows() == n);
  DRAKE_THROW_UNLESS(IsPositiveDefinite(S, tolerance));
  DRAKE_THROW_UNLESS(IsSymmetric(P, tolerance));

  const Eigen::MatrixXd R =
      S.llt().matrixL().solve(Eigen::MatrixXd::Identity(n, n));

  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(R * P * R.transpose(),
                                              Eigen::ComputeThinU);
  // Check that P was full rank (hence RPR' full-rank).
  DRAKE_THROW_UNLESS(svd.singularValues()(svd.singularValues().size()-1) >=
                         tolerance*std::max(1., svd.singularValues()(0)));

  const Eigen::VectorXd sigmaRootN4 =
      svd.singularValues().array().pow(-0.25).matrix();
  return R.transpose() * svd.matrixU() * sigmaRootN4.asDiagonal();
}

}  // namespace math
}  // namespace drake


#include <Eigen/Dense>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/videoio/videoio.hpp>

Eigen::Affine2d get_svd_transform(const Eigen::Matrix<double, -1, 2> &src, const Eigen::Matrix<double, -1, 2> &tgt)
{

  assert(src.rows() == tgt.rows());
  Eigen::Affine2d result = Eigen::Affine2d::Identity();
  Eigen::Matrix<double, -1, 2> src_ = src;
  Eigen::Matrix<double, -1, 2> tgt_ = tgt;
  Eigen::Vector2d centroid_src, centroid_tgt;
  // std::cout << "getting mean" << std::endl;

  centroid_src = src.colwise().mean();
  centroid_tgt = tgt.colwise().mean();

  std::cout << centroid_src << std::endl
            << centroid_tgt << std::endl;

  src_.colwise() -= centroid_src;
  tgt_.colwise() -= centroid_tgt;
  // std::cout << "getting svd" << std::endl;
  Eigen::Matrix2d mat = tgt_.transpose() * src_;
  Eigen::JacobiSVD<Eigen::Matrix2d> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix2d u, v;
  u = svd.matrixU();
  v = svd.matrixV();
  // std::cout << "getting rot" << std::endl;
  result.linear() = u * v.transpose();
  if (result.linear().determinant() < 0)
  {
    Eigen::Matrix<double, 2, 2> inv;
    inv << 1, 0,
           0, -1;
    std::cout << "special case" << std::endl;
    // v.transpose().rowwise() *= v;
    v = v * inv;
    result.linear() = u * v.transpose();
  }
  // std::cout << "getting trans" << std::endl;
  result.translation() = centroid_tgt - result.linear() * centroid_src;
  return result;
}

// Return l2 distance of transformed points
double compare_point_sets(const Eigen::Matrix<double, -1, 2> &src, const Eigen::Matrix<double, -1, 2> &tgt, Eigen::Affine2d &result)
{
  int rows = src.rows();

  // std::cout << "src: \n" << src << std::endl;
  // std::cout << "tgt: \n" << tgt << std::endl;
  // std::cout << "getting tf" << std::endl;

  result = get_svd_transform(src, tgt);
  std::cout << "rot:" << std::endl
            << result.linear() << std::endl;
  std::cout << "trans:" << std::endl
            << result.translation() << std::endl;

  Eigen::Matrix<double, -1, 2> transformed_src = (result * src.transpose()).transpose();
  // transformed_src.resize(rows, 2);
  // for(int i=0; i<rows; i++){
  //   transformed_src.row(i) = result * src.row(i);
  // }

  std::cout << "before \n"
            << src << std::endl;
  std::cout << "after \n"
            << transformed_src << std::endl;

  Eigen::Matrix<double, -1, 2> diff = tgt - transformed_src.topLeftCorner(rows, 2);
  Eigen::Matrix<double, -1, 1> err = diff.rowwise().norm();

  cv::Mat before = cv::Mat::zeros(cv::Size(640, 480), CV_8UC3);
  cv::Mat after = cv::Mat::zeros(cv::Size(640, 480), CV_8UC3);
  for (int i = 0; i < src.rows(); i++)
  {
    cv::circle(before, cv::Point(src(i, 0), src(i, 1)), 2, cv::Scalar(0, 0, 255), 2);
  }

  for (int i = 0; i < tgt.rows(); i++)
  {
    cv::circle(before, cv::Point(tgt(i, 0), tgt(i, 1)), 2, cv::Scalar(255, 255, 0), 2);
    cv::circle(after, cv::Point(tgt(i, 0), tgt(i, 1)), 2, cv::Scalar(255, 255, 0), 2);
  }

  for (int i = 0; i < transformed_src.rows(); i++)
  {
    cv::circle(after, cv::Point(transformed_src(i, 0), transformed_src(i, 1)), 2, cv::Scalar(0, 0, 255), 2);
  }

  cv::imshow("before", before);
  cv::imshow("after", after);
  cv::waitKey(2);
  return err.sum();
}


#include <Eigen/Core>
#include <iostream>
#include <sys/stat.h>
#include <igl/readPLY.h>
#include <igl/writePLY.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/slice.h>
#include <igl/barycentric_coordinates.h>

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

igl::AABB<Eigen::MatrixXd,3> tree;

/*
float icp_step(Eigen::MatrixXd *Vmov, Eigen::MatrixXd Vref, Eigen::MatrixXi Fref)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  int i;
  float dist = 0;

  // compute closest points
  VectorXd sqrD;
  VectorXi I;
  MatrixXd Vout;

  igl::point_mesh_squared_distance(*Vmov, Vref, Fref, sqrD, I, Vout);

  // compute rotation and translation
  MatrixXd C = Matrix3d::Constant(0);
  VectorXd oout = Vout.colwise().sum()/Vout.rows();
  VectorXd omov = (*Vmov).colwise().sum()/(*Vmov).rows();

  for(i=0;i<(*Vmov).rows();i++)
    C += ((*Vmov).row(i) - omov.transpose()).transpose()
         * (Vout.row(i) - oout.transpose());

  JacobiSVD<MatrixXd> svd(C, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV().transpose();

  MatrixXd R = U*V;
  MatrixXd t = omov - R*oout;

  // apply rotation and translation
  (*Vmov)*=R;
  (*Vmov).transpose().colwise() -= t.col(0);

  // check convergence
  // angle_dist = arccos((trace(P*Q')-1)/2), P, Q rotation matrices
  // (here, Q=I)
  // http://www.boris-belousov.net/2016/12/01/quat-dist/
  dist = acos((R.trace()-1)/2.0);
  dist = dist + t.norm();

  return dist;
}

int icp(Eigen::MatrixXd Vref, Eigen::MatrixXi Fref,
        Eigen::MatrixXd *Vmov, Eigen::MatrixXi Fmov,
        float *diff)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  int i;
  float dist;
  int maxiter = 100;
  float tol = 1e-6;

  // init distance AABB tree
  tree.init(Vref, Fref);

  float dist0=-1;

  for(i=0;i<maxiter;i++)
  {
    dist = icp_step(Vmov, Vref, Fref);
    if(dist0<0)
    {
        *diff = dist;
        printf("initial distance: %g\n", *diff);
    }
    else
        *diff = fabs(dist-dist0);

    if(*diff < tol)
        break;

    dist0=dist;
  }
  printf("final distance: %g (after %i iterations)\n", *diff, i);

  return i;
}
*/

void read_vertices(char *path, Eigen::MatrixXd *V, Eigen::MatrixXi *E)
{
  using namespace Eigen;
  using namespace std;

  FILE *f;
  int i, nv, ne;
  float x, y, z;
  int a, b;

  f=fopen(path,"r");
  fscanf(f," %i %*i %i ", &nv, &ne);
  cout<<"verts: "<<nv<<endl;
  MatrixXd X(nv, 3);
  for(i=0;i<nv;i++)
  {
    fscanf(f, " %f %f %f ", &x, &y, &z);
    X.row(i)<<x,y,z;
  }
  MatrixXi Y(ne, 2);
  for(i=0;i<ne;i++)
  {
    fscanf(f, " %i %i ", &a, &b);
    Y.row(i)<<a,b;
  }
  (*V) = X;
  (*E) = Y;
}

void save_vertices(char *path, Eigen::MatrixXd V, Eigen::MatrixXi E)
{
  using namespace Eigen;
  using namespace std;

  FILE *f;
  int i;

  f=fopen(path,"w");
  fprintf(f,"%i 0 %i\n", (int)V.rows(), (int)E.rows());
  for(i=0;i<V.rows();i++)
  {
    fprintf(f, "%f %f %f\n", V(i,0), V(i,1), V(i,2));
  }
  for(i=0;i<E.rows();i++)
  {
    fprintf(f, "%i %i\n", E(i,0), E(i,1));
  }
  fclose(f);
}

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  /*
    Transform fold curves to sphere
  */
  char path_orig[] = "/Users/roberto/Documents/annex-foldgraph/data/raw/baboon/both.ply";
  char path_sph[] = "/Users/roberto/Documents/annex-foldgraph/data/derived/skeleton/baboon/both_spherical.ply";
  char path_verts[] = "/Users/roberto/Documents/annex-foldgraph/data/derived/skeleton/baboon/both_skel_curves.txt";
  char path_out[] = "/Users/roberto/Documents/annex-foldgraph/data/derived/skeleton/baboon/test.txt";

  struct stat info;
  if(stat(path_orig, &info))
      printf("File %s not found\n", path_orig);
  if(stat(path_sph, &info))
      printf("File %s not found\n", path_sph);
  if(stat(path_verts, &info))
      printf("File %s not found\n", path_verts);

  MatrixXd Vorig, Vsph;
  MatrixXi Forig, Fsph;
  MatrixXd TMP1, TMP2;

  igl::readPLY(path_orig, Vorig, Forig, TMP1, TMP2);
  cout<<"Original nv, nt: "<<Vorig.rows()<<", "<<Forig.rows()<<endl;

  igl::readPLY(path_sph, Vsph, Fsph, TMP1, TMP2);
  cout<<"Spherical nv, nt: "<<Vsph.rows()<<", "<<Fsph.rows()<<endl;

  MatrixXd V;
  MatrixXi E;
  Vector3d t;
  t<<32.5,38.74,23.92;
  read_vertices(path_verts, &V, &E);
  V*=0.26;
  V.transpose().colwise() -=t;
  cout<<"Verts: "<<V.rows()<<endl;

  // init distance AABB tree
  tree.init(Vorig, Forig);

  // compute closest points
  VectorXd sqrD;
  VectorXi I;
  MatrixXd Vout;
  igl::point_mesh_squared_distance(V, Vorig, Forig, sqrD, I, Vout);

  // get barycentric coordinates
  MatrixXd Vx, Vy, Vz, B;
  Vector3d xyz;
  xyz<<0,1,2;
  MatrixXd VV(Vout.rows(),3);
  MatrixXi FF;
  slice(Forig, I, xyz, FF);
  slice(Vorig, FF.col(0), xyz, Vx);
  slice(Vorig, FF.col(1), xyz, Vy);
  slice(Vorig, FF.col(2), xyz, Vz);
  barycentric_coordinates(Vout, Vx, Vy, Vz, B);

  // get coordinates in Vsph space
  slice(Vsph, FF.col(0), xyz, Vx);
  slice(Vsph, FF.col(1), xyz, Vy);
  slice(Vsph, FF.col(2), xyz, Vz);
  int i;
  for(i=0;i<Vout.rows();i++)
    VV.row(i) = Vx.row(i)*B(i, 0) + Vy.row(i)*B(i, 1) + Vz.row(i)*B(i, 2);

  // save result
  save_vertices(path_out, VV, E);

  return 0;
}


#include "registration.h"
#include <Eigen/Cholesky>
#include <vector>

struct SPoint2D
{
	double x[2];
};

double computeAngle(const Eigen::Vector2d& e_im, const Eigen::Vector2d& e_i)
{
	//compute theta
	//when e_im.dot(e_i) = 1.0, theta = pi;
	const double r_sinT = e_im(0)*e_i(1) - e_im(1)*e_i(0);
	const double r_cosT = -e_im.dot(e_i);
	return atan2(r_sinT, r_cosT);
}

double sgn(const double& x)
{
	return (x >= 0) ? 1.0 : -1.0;
}

double computeOmegaFromAngle(const double& theta)
{
	const double sinT = sin(theta);
	const double cosT = cos(theta);
	const double tan_phi_over_2 = sgn(sinT) * sqrt((1.0+cosT)/(max(0.0, 1.0-cosT) + 1.0e-10));
	return 2.0 * tan_phi_over_2;
}

double computeOmega(const Eigen::Vector2d& e_im, const Eigen::Vector2d& e_i)
{
	//compute 2.0 * tan(phi / 2.0)
	//phi = pi - theta
	return computeOmegaFromAngle(computeAngle(e_im, e_i));
}

double sampleDistanceField(const SImage<double, 1>& in_DF, const SRegion& in_Region, const Eigen::Vector2d& x)
{
	assert(in_DF.width == in_DF.height);
	assert(in_Region.right-in_Region.left == in_Region.top-in_Region.bottom);

	Eigen::Vector2d proj_x = x;
	proj_x(0) = std::max(in_Region.left, std::min(in_Region.right, x(0)));
	proj_x(1) = std::max(in_Region.bottom, std::min(in_Region.top, x(1)));

	const double dist_scale = (in_Region.right-in_Region.left) / in_DF.width;

	double fi = (proj_x(0) - in_Region.left) * in_DF.width / (in_Region.right - in_Region.left);
	double fj = (proj_x(1) - in_Region.bottom) * in_DF.height / (in_Region.top - in_Region.bottom);
	double _i = floor(fi);
	double _j = floor(fj);

	int i = std::max(0, std::min(in_DF.width-1, int(_i)));
	int j = std::max(0, std::min(in_DF.height-1, int(_j)));
	int ip = std::min(in_DF.width-1, i+1);
	int jp = std::min(in_DF.height-1, j+1);

	double s = fi - _i;
	double t = fj - _j;

	double dist_proj = (1.0 - t) * (s * in_DF.ptr[j*in_DF.width+ip] + (1.0-s) * in_DF.ptr[j*in_DF.width+i])
		+ t * (s * in_DF.ptr[jp*in_DF.width+ip] + (1.0-s) * in_DF.ptr[jp*in_DF.width+i]);

	return dist_proj * dist_scale + (proj_x - x).norm();
}

double integrateDF2OverSegment(const SParameters* in_Params, const Eigen::Vector2d& x1, const Eigen::Vector2d& x2, const double restL)
{
	double tot = 0.0;
	const double len = restL / in_Params->substep_fit;

	//printf("integ_DF: ");
	for(int i=0; i<in_Params->substep_fit; i++)
	{
		const Eigen::Vector2d& p1 = x1 + (x2-x1) * double(i)/double(in_Params->substep_fit);
		const Eigen::Vector2d& p2 = x1 + (x2-x1) * double(i+1)/double(in_Params->substep_fit);

		const double d1 = sampleDistanceField(in_Params->df, in_Params->region, p1);
		const double d2 = sampleDistanceField(in_Params->df, in_Params->region, p2);

		//printf("[%f, %f, %f], ", d1, d2, len);

		const double e1 = 0.5 * (exp(d1) + exp(-d1)) - 1.0;
		const double e2 = 0.5 * (exp(d2) + exp(-d2)) - 1.0;

		//tot += 0.5 * (d1*d1+d2*d2) * len;
		tot += 0.5 * (e1*e1+e2*e2) * len;
	}
	//printf("\n");

	return tot;
}

inline double computeSegmentElasticEnergy(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const double& YA, const double& restLength)
{
	const Eigen::Vector2d diff = p1 - p2;
	return 0.5 * YA * restLength
		* (diff.norm() / restLength - 1)
		* (diff.norm() / restLength - 1);
}

inline double computeSegmentBendingEnergy(const Eigen::Vector2d& pp, const Eigen::Vector2d& pc, const Eigen::Vector2d& pn,
	const double& alpha, const double& restLengthP, const double& restLengthN, const double& theta)
{
	const Eigen::Vector2d e_im = pc - pp;
	const Eigen::Vector2d e_i = pn - pc;

	/*
	double omega = 2.0 * fabs(e_im(0)*e_i(1)-e_im(1)*e_i(0)) / (e_im.norm()*e_i.norm() + e_im.dot(e_i));
	double omega_bar = 2.0 * fabs(tan(phi*0.5));
	//*/
	double omega = computeOmega(e_im, e_i);
	double omega_bar = computeOmegaFromAngle(theta);
	return alpha * (omega - omega_bar) * (omega - omega_bar) / (restLengthP + restLengthN);
}

double computeEnergy_elastic(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	double energy = 0.0;

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		energy += computeSegmentElasticEnergy(in_Position.col(ip), in_Position.col(i), in_Params->YA, in_Curve->restLengths(i));
	}

	return energy;
}

double computeEnergy_bending(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position)
{
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	double energy = 0.0;

	for(int i=0; i<nAngles; i++)
	{
		int iim = in_Curve->closed ?
			(i + in_Curve->nVertices - 1) % in_Curve->nVertices : i;
		int ii = in_Curve->closed ? i : i + 1;
		int iip = in_Curve->closed ? (i + 1) % in_Curve->nVertices : i + 2;

		double restL_m = in_Curve->closed ? in_Curve->restLengths((i + in_Curve->nVertices - 1) % in_Curve->nVertices) :
			in_Curve->restLengths(i);
		double restL = in_Curve->closed ? in_Curve->restLengths(i) :
			in_Curve->restLengths(i+1);

		energy += computeSegmentBendingEnergy(in_Position.col(iim), in_Position.col(ii), in_Position.col(iip),
			in_Params->alpha, restL_m, restL, in_Curve->restAngles(i));
	}

	return energy;
}

double computeEnergy_fit(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	double energy = 0.0;

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		double int_df_seg = integrateDF2OverSegment(in_Params, in_Position.col(i), in_Position.col(ip), in_Curve->restLengths(i));
		energy += 0.5 * in_Params->fit * int_df_seg;
	}

	return energy;
}

double computeEnergy(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_Vars)
{
	const double E_elastic = computeEnergy_elastic(in_Params, in_Curve, in_Vars.pos);
	const double E_bending = computeEnergy_bending(in_Params, in_Curve, in_Vars.pos);
	const double E_fit = computeEnergy_fit(in_Params, in_Curve, in_Vars.pos);
	return E_elastic + E_bending + E_fit;
}

void compute_f_elastic(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, Eigen::VectorXd& io_f, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_f.rows() >= nSegs + offset_row);
	assert(in_Curve->nVertices == in_Position.cols());

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		const Eigen::Vector2d diff = in_Position.col(ip) - in_Position.col(i);
		io_f(i+offset_row) = sqrt(0.5 * in_Params->YA * in_Curve->restLengths(i)) * (diff.norm() / in_Curve->restLengths(i) - 1);
	}
}

void compute_f_bending(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, Eigen::VectorXd& io_f, int offset_row)
{
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	assert(io_f.rows() >= nAngles + offset_row);
	assert(in_Curve->nVertices == in_Position.cols());

	for(int i=0; i<nAngles; i++)
	{
		int iim = in_Curve->closed ?
			(i + in_Curve->nVertices - 1) % in_Curve->nVertices : i;
		int ii = in_Curve->closed ? i : i + 1;
		int iip = in_Curve->closed ? (i + 1) % in_Curve->nVertices : i + 2;

		double restL_m = in_Curve->closed ? in_Curve->restLengths((i + in_Curve->nVertices - 1) % in_Curve->nVertices) :
			in_Curve->restLengths(i);
		double restL = in_Curve->closed ? in_Curve->restLengths(i) :
			in_Curve->restLengths(i+1);

		const Eigen::Vector2d e_im = in_Position.col(ii) - in_Position.col(iim);
		const Eigen::Vector2d e_i = in_Position.col(iip) - in_Position.col(ii);

		/*
		double omega = 2.0 * fabs(e_im(0)*e_i(1)-e_im(1)*e_i(0)) / (e_im.norm()*e_i.norm() + e_im.dot(e_i));
		double omega_bar = 2.0 * fabs(tan(in_Curve->restAngles(i)*0.5));
		//*/
		double omega = computeOmega(e_im, e_i);
		double omega_bar = computeOmegaFromAngle(in_Curve->restAngles(i));
		io_f(i+offset_row) = sqrt(in_Params->alpha/(restL_m + restL)) * (omega - omega_bar);
	}
}

void compute_f_fit(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, Eigen::VectorXd& io_f, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_f.rows() >= nSegs + offset_row);
	assert(in_Curve->nVertices == in_Position.cols());

	//printf("int_seg: ");
	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		double int_df_seg = integrateDF2OverSegment(in_Params, in_Position.col(i), in_Position.col(ip), in_Curve->restLengths(i));
		//printf("%f, ", int_df_seg);
		io_f(i+offset_row) = sqrt(in_Params->fit * 0.5) * sqrt(int_df_seg);
	}
	//printf("\n");
}

void compute_f(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_Vars, Eigen::VectorXd& io_f)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	int nElems = nSegs + nAngles + nSegs;

	assert(io_f.rows() == nElems);
	assert(in_Curve->nVertices == in_Vars.pos.cols());
	assert(in_Curve->nVertices == in_Vars.pos.cols());

	io_f.setZero();

	compute_f_elastic(in_Params, in_Curve, in_Vars.pos, io_f, 0);
	compute_f_bending(in_Params, in_Curve, in_Vars.pos, io_f, nSegs);
	compute_f_fit(in_Params, in_Curve, in_Vars.pos, io_f, nSegs+nAngles);

	/*
	printf("f: [");
	for(int i=0; i<nElems; i++)
	{
		if(i<nElems-1) printf("%f, ", io_f(i));
		else printf("%f]\n", io_f(i));
	}
	//*/
}

void computeNumericalDerivative_elastic(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, double epsilon, Eigen::MatrixXd& io_Jacobian, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_Jacobian.rows() >= nSegs + offset_row);
	assert(io_Jacobian.cols() == in_Curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dJ/dxi, dJ/dyi
	for(int i=0; i<nSegs; i++)
	{
		//l_i is a function of x_i, x_{i+1}, y_i and y_{i+1}
		int ip = (i+1) % in_Curve->nVertices;

		const Eigen::Vector2d xi = in_Position.col(i);
		const Eigen::Vector2d xip = in_Position.col(ip);

		const Eigen::Vector2d diff0 = xip - xi;
		const double fi = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff0.norm() / in_Curve->restLengths(i) - 1);

		//dJ/dxi_i
		const Eigen::Vector2d xi_dx = xi + dx;
		const Eigen::Vector2d diff1 = xip - xi_dx;
		const double fi_i_dx = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff1.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, i) = (fi_i_dx - fi) / epsilon;

		//dJ/dyi_i
		const Eigen::Vector2d xi_dy = xi + dy;
		const Eigen::Vector2d diff2 = xip - xi_dy;
		const double fi_i_dy = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff2.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, i+in_Curve->nVertices) = (fi_i_dy - fi) / epsilon;

		//dJ/dxi_ip
		const Eigen::Vector2d xip_dx = xip + dx;
		const Eigen::Vector2d diff3 = xip_dx - xi;
		const double fi_ip_dx = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff3.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, ip) = (fi_ip_dx - fi) / epsilon;

		//dJ/dyi_i
		const Eigen::Vector2d xip_dy = xip + dy;
		const Eigen::Vector2d diff4 = xip_dy - xi;
		const double fi_ip_dy = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff4.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, ip+in_Curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative_bending(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, double epsilon, Eigen::MatrixXd& io_Jacobian, int offset_row)
{
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	assert(io_Jacobian.rows() >= nAngles + offset_row);
	assert(io_Jacobian.cols() == in_Curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dB/dxi, dB/dyi
	for(int i=0; i<nAngles; i++)
	{
		int iim = in_Curve->closed ?
			(i + in_Curve->nVertices - 1) % in_Curve->nVertices : i;
		int ii = in_Curve->closed ? i : i + 1;
		int iip = in_Curve->closed ? (i + 1) % in_Curve->nVertices : i + 2;

		double restL_m = in_Curve->restLengths(iim);
		double restL = in_Curve->restLengths(ii);

		const Eigen::Vector2d xim = in_Position.col(iim);
		const Eigen::Vector2d xi = in_Position.col(ii);
		const Eigen::Vector2d xip = in_Position.col(iip);

		const Eigen::Vector2d xim_dx = xim + dx; const Eigen::Vector2d xim_dy = xim + dy;
		const Eigen::Vector2d xi_dx = xi + dx; const Eigen::Vector2d xi_dy = xi + dy;
		const Eigen::Vector2d xip_dx = xip + dx; const Eigen::Vector2d xip_dy = xip + dy;

		const Eigen::Vector2d e_prev = xi - xim;
		const Eigen::Vector2d e_next = xip - xi;

		const Eigen::Vector2d e_prev_im_dx = xi - xim_dx; const Eigen::Vector2d e_prev_im_dy = xi - xim_dy;
		const Eigen::Vector2d e_prev_i_dx = xi_dx - xim; const Eigen::Vector2d e_prev_i_dy = xi_dy - xim;

		const Eigen::Vector2d e_next_i_dx = xip - xi_dx; const Eigen::Vector2d e_next_i_dy = xip - xi_dy;
		const Eigen::Vector2d e_next_ip_dx = xip_dx - xi; const Eigen::Vector2d e_next_ip_dy = xip_dy - xi;

		/*
		const double omega = 2.0 * fabs(e_prev(0)*e_next(1)-e_prev(1)*e_next(0)) / (e_prev.norm()*e_next.norm() + e_prev.dot(e_next));

		const double omega_im_dx = 2.0 * fabs(e_prev_im_dx(0)*e_next(1)-e_prev_im_dx(1)*e_next(0)) / (e_prev_im_dx.norm()*e_next.norm() + e_prev_im_dx.dot(e_next));
		const double omega_im_dy = 2.0 * fabs(e_prev_im_dy(0)*e_next(1)-e_prev_im_dy(1)*e_next(0)) / (e_prev_im_dy.norm()*e_next.norm() + e_prev_im_dy.dot(e_next));
		const double omega_i_dx = 2.0 * fabs(e_prev_i_dx(0)*e_next_i_dx(1)-e_prev_i_dx(1)*e_next_i_dx(0)) / (e_prev_i_dx.norm()*e_next_i_dx.norm() + e_prev_i_dx.dot(e_next_i_dx));
		const double omega_i_dy = 2.0 * fabs(e_prev_i_dy(0)*e_next_i_dy(1)-e_prev_i_dy(1)*e_next_i_dy(0)) / (e_prev_i_dy.norm()*e_next_i_dy.norm() + e_prev_i_dy.dot(e_next_i_dy));
		const double omega_ip_dx = 2.0 * fabs(e_prev(0)*e_next_ip_dx(1)-e_prev(1)*e_next_ip_dx(0)) / (e_prev.norm()*e_next_ip_dx.norm() + e_prev.dot(e_next_ip_dx));
		const double omega_ip_dy = 2.0 * fabs(e_prev(0)*e_next_ip_dy(1)-e_prev(1)*e_next_ip_dy(0)) / (e_prev.norm()*e_next_ip_dy.norm() + e_prev.dot(e_next_ip_dy));
		//*/

		const double omega = computeOmega(e_prev, e_next);

		const double omega_im_dx = computeOmega(e_prev_im_dx, e_next);
		const double omega_im_dy = computeOmega(e_prev_im_dy, e_next);
		const double omega_i_dx = computeOmega(e_prev_i_dx, e_next_i_dx);
		const double omega_i_dy = computeOmega(e_prev_i_dy, e_next_i_dy);
		const double omega_ip_dx = computeOmega(e_prev, e_next_ip_dx);
		const double omega_ip_dy = computeOmega(e_prev, e_next_ip_dy);

		const double fi = sqrt(in_Params->alpha/(restL_m + restL)) * omega; //omega_bar will be cancelled out, so omit it
		const double fi_im_dx = sqrt(in_Params->alpha/(restL_m + restL)) * omega_im_dx;
		const double fi_im_dy = sqrt(in_Params->alpha/(restL_m + restL)) * omega_im_dy;
		const double fi_i_dx = sqrt(in_Params->alpha/(restL_m + restL)) * omega_i_dx;
		const double fi_i_dy = sqrt(in_Params->alpha/(restL_m + restL)) * omega_i_dy;
		const double fi_ip_dx = sqrt(in_Params->alpha/(restL_m + restL)) * omega_ip_dx;
		const double fi_ip_dy = sqrt(in_Params->alpha/(restL_m + restL)) * omega_ip_dy;

		io_Jacobian(i+offset_row, iim) = (fi_im_dx - fi) / epsilon;
		io_Jacobian(i+offset_row, iim+in_Curve->nVertices) = (fi_im_dy - fi) / epsilon;

		io_Jacobian(i+offset_row, ii) = (fi_i_dx - fi) / epsilon;
		io_Jacobian(i+offset_row, ii+in_Curve->nVertices) = (fi_i_dy - fi) / epsilon;

		io_Jacobian(i+offset_row, iip) = (fi_ip_dx - fi) / epsilon;
		io_Jacobian(i+offset_row, iip+in_Curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative_fit(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, double epsilon, Eigen::MatrixXd& io_Jacobian, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_Jacobian.rows() >= nSegs + offset_row);
	assert(io_Jacobian.cols() == in_Curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dfit/dxi, dfit/dyi
	for(int i=0; i<nSegs; i++)
	{
		//l_i is a function of x_i, x_{i+1}, y_i and y_{i+1}
		int ip = (i+1) % in_Curve->nVertices;

		const Eigen::Vector2d xi = in_Position.col(i);
		const Eigen::Vector2d xip = in_Position.col(ip);
		const double restL = in_Curve->restLengths(i);

		const double fi = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi, xip, restL));

		//dfit/dxi_i
		const Eigen::Vector2d xi_dx = xi + dx;
		const double fi_i_dx = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi_dx, xip, restL));
		io_Jacobian(i+offset_row, i) = (fi_i_dx - fi) / epsilon;

		//dfit/dyi_i
		const Eigen::Vector2d xi_dy = xi + dy;
		const double fi_i_dy = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi_dy, xip, restL));
		io_Jacobian(i+offset_row, i+in_Curve->nVertices) = (fi_i_dy - fi) / epsilon;

		//dfit/dxi_ip
		const Eigen::Vector2d xip_dx = xip + dx;
		const double fi_ip_dx = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi, xip_dx, restL));
		io_Jacobian(i+offset_row, ip) = (fi_ip_dx - fi) / epsilon;

		//dfit/dyi_i
		const Eigen::Vector2d xip_dy = xip + dy;
		const double fi_ip_dy = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi, xip_dy, restL));
		io_Jacobian(i+offset_row, ip+in_Curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_Vars, double epsilon, Eigen::MatrixXd& io_Jacobian)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	//assert(io_Jacobian.rows() == nSegs + nAngles + nSegs);
	//assert(io_Jacobian.cols() == in_Curve->nVertices * 2);
	//assert(in_Vars.pos.cols() == in_Curve->nVertices);
	//assert(in_Vars.conf.cols() == in_Curve->nVertices);

	io_Jacobian.setZero();

	computeNumericalDerivative_elastic(in_Params, in_Curve, in_Vars.pos, epsilon, io_Jacobian, 0);
	computeNumericalDerivative_bending(in_Params, in_Curve, in_Vars.pos, epsilon, io_Jacobian, nSegs);
	computeNumericalDerivative_fit(in_Params, in_Curve, in_Vars.pos, epsilon, io_Jacobian, nSegs+nAngles);

	/*
	printf("B: [\n");
	for(int j=0; j<io_Jacobian.rows(); j++)
	{
		printf("[");
		for(int i=0; i<io_Jacobian.cols(); i++)
		{
			if(i<io_Jacobian.cols()-1) printf("%f, ", io_Jacobian(j, i));
			else printf("%f],\n", io_Jacobian(j, i));
		}
	}
	printf("]\n");
	//*/
}

void updateRestLength(const Eigen::Matrix2Xd& in_Position, SCurve* io_Curve)
{
	int nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	assert(io_Curve->nVertices == in_Position.cols());

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % io_Curve->nVertices;
		const Eigen::Vector2d diff = in_Position.col(ip) - in_Position.col(i);
		io_Curve->restLengths(i) = diff.norm();
	}
}

void updateCurveSubdivision(const SParameters* in_Params, SVar& io_InitialVars, SVar& io_Vars, SCurve* io_Curve, SSolverVars& io_SolverVars)
{
	int nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	//assert(io_Curve->nVertices == in_Position.cols());

	std::vector<SPoint2D> pos;
	std::vector<double> angles;
	std::vector<int> vids;

	bool subdivided = false;

	for(int i=0; i<nSegs; i++)
	{
		SPoint2D p; p.x[0] = io_Vars.pos.col(i)(0); p.x[1] = io_Vars.pos.col(i)(1);
		pos.push_back(p);
		vids.push_back(io_Curve->vertexIDs(i));

		if(i!=0 || io_Curve->closed) angles.push_back(io_Curve->restAngles(i));

		int ip = (i+1) % io_Curve->nVertices;
		const Eigen::Vector2d diff = io_Vars.pos.col(ip) - io_Vars.pos.col(i);
		if(diff.norm() > in_Params->refLength)
		{
			SPoint2D p;
			p.x[0] = (io_Vars.pos.col(ip)(0) + io_Vars.pos.col(i)(0)) * 0.5;
			p.x[1] = (io_Vars.pos.col(ip)(1) + io_Vars.pos.col(i)(1)) * 0.5;
			pos.push_back(p);
			//angles.push_back((io_Curve->restAngles(i) + io_Curve->restAngles(ip)) * 0.5);
			angles.push_back(PI);
			vids.push_back(-1);
			subdivided = true;
		}
	}

	if(!io_Curve->closed)
	{
		int ilast = io_Curve->nVertices-1;
		SPoint2D p; p.x[0] = io_Vars.pos.col(ilast)(0); p.x[1] = io_Vars.pos.col(ilast)(1);
		pos.push_back(p);
		vids.push_back(io_Curve->vertexIDs(ilast));
	}

	io_Curve->nVertices = pos.size();
	nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	io_Curve->restLengths.resize(nSegs);
	int nAngles = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 2;
	io_Curve->restAngles.resize(nAngles);
	io_Curve->vertexIDs.resize(io_Curve->nVertices);

	resize(io_Vars, io_Curve->nVertices);
	resize(io_InitialVars, io_Curve->nVertices);

	for(int i=0; i<io_Curve->nVertices; i++)
	{
		io_Vars.pos.col(i)(0) = pos[i].x[0];
		io_Vars.pos.col(i)(1) = pos[i].x[1];
		io_Vars.conf(i) = 0.0;
		io_Curve->vertexIDs(i) = vids[i];
	}

	nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % io_Curve->nVertices;
		const Eigen::Vector2d diff = io_Vars.pos.col(ip) - io_Vars.pos.col(i);
		io_Curve->restLengths(i) = diff.norm();
	}

	for(int i=0; i<nAngles; i++)
		io_Curve->restAngles(i) = angles[i];

	io_InitialVars = io_Vars;

	initSolverVars(in_Params, io_Curve, io_InitialVars, io_SolverVars, true);
}

bool secantLMMethodSingleUpdate(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_InitialVars, SSolverVars& io_SolverVars, SVar& solution)
{
	if(io_SolverVars.found || io_SolverVars.k > in_Params->kmax)
		return true;

	io_SolverVars.k++;
	io_SolverVars.A_muI = io_SolverVars.B.transpose() * io_SolverVars.B + io_SolverVars.mu * io_SolverVars.I;
	io_SolverVars.h = io_SolverVars.A_muI.ldlt().solve(-io_SolverVars.g);

	if(io_SolverVars.h.norm() <= in_Params->epsilon_2 * (io_SolverVars.x.pos.norm() + in_Params->epsilon_2))
		io_SolverVars.found = true;
	else
	{
		for(int q=0; q<io_SolverVars.nV; q++)
		{
			io_SolverVars.xnew.pos(0, q) = io_SolverVars.x.pos(0, q) + io_SolverVars.h(q);
			io_SolverVars.xnew.pos(1, q) = io_SolverVars.x.pos(1, q) + io_SolverVars.h(q + io_SolverVars.nV);
		}
	}

	double Fnew = 0.5 * computeEnergy(in_Params, in_Curve, io_SolverVars.xnew);
	double F = 0.5 * computeEnergy(in_Params, in_Curve, io_SolverVars.x);

	double gain_denom = - io_SolverVars.h.dot(io_SolverVars.B.transpose() * io_SolverVars.f)
		- 0.5 * io_SolverVars.h.dot(io_SolverVars.B.transpose() * io_SolverVars.B * io_SolverVars.h);
	double gain = (F - Fnew) / gain_denom;

	if(gain > 0)
	{
		io_SolverVars.x = io_SolverVars.xnew;
		compute_f(in_Params, in_Curve, io_SolverVars.x, io_SolverVars.f);
		computeNumericalDerivative(in_Params, in_Curve, io_SolverVars.x, io_SolverVars.epsilon, io_SolverVars.B);
		io_SolverVars.g = io_SolverVars.B.transpose() * io_SolverVars.f;
		io_SolverVars.found = (io_SolverVars.g.lpNorm<Eigen::Infinity>() <= in_Params->epsilon_1);
		printf("k: %d, gain: %f, |g|_inf: %f\n", io_SolverVars.k, gain, io_SolverVars.g.lpNorm<Eigen::Infinity>());
		io_SolverVars.mu = io_SolverVars.mu * std::max(1.0/3.0, 1.0 - (2.0 * gain - 1.0) * (2.0 * gain - 1.0) * (2.0 * gain - 1.0));
		io_SolverVars.nu = 2.0;
	}
	else
	{
		io_SolverVars.mu = io_SolverVars.mu * io_SolverVars.nu;
		io_SolverVars.nu = io_SolverVars.nu * 2.0;
	}

	if(io_SolverVars.found)
		printf("found in %d steps\n", io_SolverVars.k);

	solution = io_SolverVars.x;

	return io_SolverVars.found || io_SolverVars.k > in_Params->kmax;
}

void showFeaturePoints(const SCurve* in_Curve, const SVar& solution)
{
	printf("Feature points:\n");
	for(int i=0; i<in_Curve->nVertices; i++)
	{
		if(in_Curve->vertexIDs(i) >= 0)
		{
			printf("%d: %f, %f\n", in_Curve->vertexIDs(i), solution.pos.col(i).x(), solution.pos.col(i).y());
		}
	}
}

void secantLMMethod(const SParameters* in_Params, SCurve* in_Curve, SVar& in_InitialVars, SSolverVars& io_SolverVars, SVar& solution)
{
	initSolverVars(in_Params, in_Curve, in_InitialVars, io_SolverVars);

	while(1)
	{
		if(secantLMMethodSingleUpdate(in_Params, in_Curve, in_InitialVars, io_SolverVars, solution))
			break;
		updateCurveSubdivision(in_Params, in_InitialVars, solution, in_Curve, io_SolverVars);
	}

	printf("found in %d steps\n", io_SolverVars.k);
	solution = io_SolverVars.x;
	showFeaturePoints(in_Curve, solution);
}


#include <iostream>
#include <rmagine/math/math.h>
#include <rmagine/math/types.h>
#include <rmagine/util/StopWatch.hpp>

#include <Eigen/Dense>

using namespace rmagine;

void print(Matrix3x3 M)
{
    for(size_t i=0; i<3; i++)
    {
        for(size_t j=0; j<3; j++)
        {
            std::cout << M(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

void print(Matrix4x4 M)
{
    for(size_t i=0; i<4; i++)
    {
        for(size_t j=0; j<4; j++)
        {
            std::cout << M(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

void print(Eigen::Matrix4f M)
{
    for(size_t i=0; i<4; i++)
    {
        for(size_t j=0; j<4; j++)
        {
            std::cout << M(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

void print(Vector v)
{
    std::cout << v.x << " " << v.y << " " << v.z << std::endl;
}

void print(Quaternion q)
{
    std::cout << q.x << " " << q.y << " " << q.z << " " << q.w << std::endl;
}

void print(Transform T)
{
    print(T.R);
    print(T.t);
}

bool rotationConversionTest()
{
    std::cout << std::endl;
    std::cout << "--------------------------------" << std::endl;
    std::cout << "---- rotationConversionTest ----" << std::endl;
    std::cout << "--------------------------------" << std::endl;
    std::cout << std::endl;

    EulerAngles e0;
    e0.roll = -0.1;
    e0.pitch = 0.1;
    e0.yaw = M_PI / 2.0;

    EulerAngles e;
    Quaternion q;
    Matrix3x3 R;

    Vector x1{1.0, 0.0, 0.0};
    Vector x2{0.0, 1.0, 0.0};
    Vector x3{0.0, 0.0, 1.0};

    std::cout << "Euler -> Quat" << std::endl;
    q = e0;
    print(q * x1);
    print(q * x2);
    print(q * x3);
    std::cout << std::endl;

    std::cout << "Quat -> Euler" << std::endl;
    e = q;
    std::cout << e.roll << " " << e.pitch << " " << e.yaw << std::endl;
    std::cout << std::endl;

    if(    fabs(e.roll - e0.roll) > 0.0001
        || fabs(e.pitch - e0.pitch) > 0.0001
        || fabs(e.yaw - e0.yaw) > 0.0001)
    {
        std::cout << "Euler -> Quat -> Euler error." << std::endl;
        return false;
    }

    std::cout << "Euler -> Matrix" << std::endl;
    R = e0;
    print(R * x1);
    print(R * x2);
    print(R * x3);
    std::cout << std::endl;

    std::cout << "Matrix -> Euler" << std::endl;
    e = R;
    std::cout << e.roll << " " << e.pitch << " " << e.yaw << std::endl;
    std::cout << std::endl;

    std::cout << "Quat -> Matrix" << std::endl;
    R = q;
    print(R * x1);
    print(R * x2);
    print(R * x3);
    std::cout << std::endl;

    std::cout << "Matrix -> Quat" << std::endl;
    q = R;
    print(q * x1);
    print(q * x2);
    print(q * x3);
    std::cout << std::endl;

    return true;
}

Eigen::Vector3f& eigenView(Vector3& v)
{
    return *reinterpret_cast<Eigen::Vector3f*>( &v );
}

Eigen::Matrix3f& eigenView(Matrix3x3& M)
{
    return *reinterpret_cast<Eigen::Matrix3f*>( &M );
}

Eigen::Matrix4f& eigenView(Matrix4x4& M)
{
    return *reinterpret_cast<Eigen::Matrix4f*>( &M );
}

bool checkMatrix3x3()
{
    std::cout << "---------- checkMatrix3x3" << std::endl;
    EulerAngles e{-0.1, 0.1, M_PI / 2.0};

    Matrix3x3 M;
    M = e;
    M(0,1) = 10.0;

    // shallow copy.
    Eigen::Matrix3f& Meig_shallow = eigenView(M);
    std::cout << Meig_shallow << std::endl;

    // deep copy
    Eigen::Matrix3f Meig(&M(0,0));

    // Eigen::Matrix3f Meig_inv = Meig.inverse();
    Matrix3x3 M_inv = ~M;
    Eigen::Matrix3f Meig_inv = Meig.inverse();

    // std::cout << Meig_inv << std::endl;
    // print(M_inv);
    Matrix3x3 I = M_inv * M;
    Eigen::Matrix3f Ieig = Meig_inv * Meig;

    std::cout << "M = " << std::endl;
    print(M);

    std::cout << "M_inv = " << std::endl;
    print(M_inv);

    std::cout << "M_inv * M = " << std::endl;
    print(I);

    std::cout << "Meig = " << std::endl;
    std::cout << Meig << std::endl;

    std::cout << "Meig_inv = " << std::endl;
    std::cout << Meig_inv << std::endl;

    std::cout << "Meig_inv * Meig =" << std::endl;
    std::cout << Meig_inv * Meig <<  std::endl;

    std::cout << "Eigen::Matrix3f stats: " << std::endl;
    std::cout << "- det: " << Meig.determinant() << std::endl;
    std::cout << "- trace: " << Meig.trace() << std::endl;

    std::cout << "Matrix3x3 stats:" << std::endl;
    std::cout << "- det: " << M.det() << std::endl;
    std::cout << "- trace: " << M.trace() << std::endl;

    return true;
}

bool checkMatrix4x4()
{
    std::cout << "------- checkMatrix4x4" << std::endl;
    EulerAngles e{-0.1, 0.1, M_PI / 2.0};

    Matrix4x4 M;
    M.setIdentity();
    M.setRotation(e);

    Eigen::Matrix4f& Meig_shallow = eigenView(M);

    // M(0,1) = 10.0;

    Vector trans{0.0, 0.0, 1.0};
    M.setTranslation(trans);

    std::cout << Meig_shallow << std::endl;

    Matrix4x4 M_inv = M.inv();
    Matrix4x4 I = M_inv * M;

    std::cout << "M = " << std::endl;
    print(M);
    std::cout << "M_inv = " << std::endl;
    print(M_inv);
    std::cout << "(invRigid)=" << std::endl;
    print(M.invRigid());

    std::cout << "M_inv * M = " << std::endl;
    print(I);

    Eigen::Matrix4f Meig(&M(0,0));
    Eigen::Matrix4f Meig_inv = Meig.inverse();

    std::cout << "Meig = " << std::endl;
    std::cout << Meig << std::endl;

    std::cout << "Meig_inv = " << std::endl;
    std::cout << Meig_inv << std::endl;

    std::cout << "Meig_inv * Meig =" << std::endl;
    std::cout << Meig_inv * Meig <<  std::endl;

    std::cout << "Eigen::Matrix4f stats: " << std::endl;
    std::cout << "- det: " << Meig.determinant() << std::endl;
    std::cout << "- trace: " << Meig.trace() << std::endl;

    std::cout << "Matrix4x4 stats:" << std::endl;
    std::cout << "- det: " << M.det() << std::endl;
    std::cout << "- trace: " << M.trace() << std::endl;

    return true;
}

int main(int argc, char** argv)
{
    std::cout << "Rmagine Test: Basic Math" << std::endl;
    // rotationConversionTest();

    // checkMatrix3x3();
    // checkMatrix4x4();

    Vector ab{1.0, 2.0, 3.0};
    Vector ac{4.0, 5.0, 6.0};
    Vector n{7.0, 8.0, 9.0};

    Eigen::Matrix3f R;
    R.col(0) = Eigen::Vector3f(ab.x, ab.y, ab.z);
    R.col(1) = Eigen::Vector3f(ac.x, ac.y, ac.z);
    R.col(2) = Eigen::Vector3f(n.x, n.y, n.z);

    std::cout << R << std::endl;

    R(0,0) = ab.x;
    R(1,0) = ab.y;
    R(2,0) = ab.z;

    R(0,1) = ac.x;
    R(1,1) = ac.y;
    R(2,1) = ac.z;

    R(0,2) = n.x;
    R(1,2) = n.y;
    R(2,2) = n.z;

    std::cout << R << std::endl;

    return 0;
}