/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
------------------ Author:       Marnix Volckaert               -------------------
------------------ Affiliation:  TUDelft                        -------------------
-----------------------------------------------------------------------------------
 Modified by Tiziana Sabatini on May-July 2009
*/

#include <QDir>
#include <QTextStream>

#include "EntryTrajectory.h"
#include "Astro-Core/EODE/eode.h"
#include <Eigen/Core>
#include <QProcess>
#include <QDebug>

/*Function containing the equations of motion*/
//=================================================================================================
void derivstate (VectorXd state, double time, VectorXd parameters, VectorXd& derivative)
{
        derivative[0] = state[3];
        derivative[1] = state[4];
        derivative[2] = state[5];
        derivative[3] = parameters[3] + parameters[0] + pow(parameters[6],2)*state[0] + 2.0*parameters[6]*state[4];
        derivative[4] = parameters[4] + parameters[1] + pow(parameters[6],2)*state[1] - 2.0*parameters[6]*state[3];
        derivative[5] = parameters[5] + parameters[2];
        derivative[6] = parameters[7];
        derivative[7] = parameters[8];
}

class EntryDerivativeCalculator : public DerivativeCalculator<8>
{
public:
    EntryDerivativeCalculator(EntryTrajectory *trajectory):
            m_trajectory(trajectory)
            //m_perturbationList(perturbationList)
    {
    }

    virtual void compute(const State& state, double t, State& derivatives) const
    {
        Vector3d position = state.segment<3>(0);
        Vector3d velocity = state.segment<3>(3);
        sta::StateVector stateVector=StateVector(position,velocity);
        Vector3d gravity;                   //Components of gravitational acceleration in appropriate elements
        Vector3d aero;                     	//Components of aerodynamic acceleration in appropriate elements
        VectorXd parameters(9);             //List of parameters to send to integration: 0,1,2 = gravity, 3,4,5 = aero, 6 = omega, 7 = Qdot
        double Cdc;
        double tau = m_trajectory->settings.stepsize;

        m_trajectory->body.updateGravity(stateVector, gravity);
        m_trajectory->body.updateAero(stateVector, m_trajectory->state2, m_trajectory->atmosphere, m_trajectory->capsule, aero, Cdc);
        /*
        Vector3d perturbingAcceleration(0, 0, 0);
        foreach (Perturbations* perturbation, perturbationList)
        {
            perturbingAcceleration += perturbation->calculateAcceleration(stateVector, time, tau);
        }
        gravity += perturbingAcceleration;
        */
        m_trajectory->calculateState2(aero, Cdc);
        m_trajectory->updateEndstate();

        //Test for status
        m_trajectory->checkStatus();
        if ( m_trajectory->status != OK) {
            m_trajectory->result.status = m_trajectory->status;
        }
        parameters << gravity, aero, m_trajectory->body.getOmega(), m_trajectory->state2(3), m_trajectory->state2(4);
        //TO DO : re-insert the heat load integration (now heat flux is always set to zero)
        VectorXd state3(8);
                state3 << stateVector.position, stateVector.velocity, 0, 0;

                derivatives[0] = state3[3];
                derivatives[1] = state3[4];
                derivatives[2] = state3[5];
                derivatives[3] = parameters[3] + parameters[0] + pow(parameters[6],2)*state3[0] + 2.0*parameters[6]*state3[4];
                derivatives[4] = parameters[4] + parameters[1] + pow(parameters[6],2)*state3[1] - 2.0*parameters[6]*state3[3];
                derivatives[5] = parameters[5] + parameters[2];
                derivatives[6] = parameters[7];
                derivatives[7] = parameters[8];
    }

private:
    EntryTrajectory* m_trajectory;

    //const QList<Perturbations*>& m_perturbationList;

};

EntryTrajectory::EntryTrajectory(EntrySettings _settings)
{
    //Set the entry trajectory's settings to the input settings
    //---------------------------------------------------------
    settings = _settings;
    //---------------------------------------------------------

    //Assign body, atmosphere and heat rate, and capsule Cd profile
    //---------------------------------------------------------
    atmosphere.selectModel(settings.modelname);
    body.selectBody(settings.bodyname);
    heatrate.selectBody(settings.bodyname);
    capsule.flag = 0;
    capsule.selectCdCprofile(settings.CdCprofilename);
    capsule.selectClCprofile(settings.ClCprofilename);
    capsule.selectCsCprofile(settings.CsCprofilename);

    //capsule.selectCdPprofile(settings.CdPprofilename);
    //capsule.Sp = settings.parachuteArea;

    state.resize(8);
    state2.resize(10);
}

sta::StateVector EntryTrajectory::initialise(EntryParameters _parameters,sta::StateVector initialState, double startTime)
{

    parameters = _parameters;           //Save the parameters as a member of the trajectory, to possibly use later in plotting

    capsule.Sc = parameters.Sref;
    capsule.m = parameters.m;
    capsule.Rn = parameters.Rn;

    double theta = getGreenwichHourAngle(startTime);
    int celestialbody;
    if (settings.bodyname == "Earth")
         celestialbody = 0;
    else if (settings.bodyname == "Mars")
         celestialbody = 3;
    //inertialTOfixed(celestialbody, theta, initialState.position.x(), initialState.position.y(), initialState.position.z(), initialState.velocity.x(), initialState.velocity.y(), initialState.velocity.z(),
      //          state(0), state(1), state(2), state(3), state(4), state(5));

    //const Eigen::Vector3d position(initialState.position.x(),initialState.position.y(),initialState.position.z());
    //const Eigen::Vector3d velocity(state(3), state(4), state(5));
    //initialState = sta::StateVector(position,velocity);
    double omega=7.29211585494e-5;
    initialState.velocity.x()+=initialState.position.y()*omega;
    initialState.velocity.y()+=-initialState.position.x()*omega;

    //----- Initialise the heat loads to zero
    state(6) = 0.0;
    state(7) = 0.0;
    //---------------------------------------------------------

    //Initialise the rest
    //---------------------------------------------------------
    time = sta::daysToSecs(sta::JdToMjd(startTime));
    for (int i=0; i<10; i++) {
        state2(i) = 0.0;
    }
    state2(5) = parameters.inputstate[0];          //Initial altitude
    status = OK;                                   //Status = OK
    result.Machone_transition_altitude = 0.0;      //Needs to be zero at first so that it only gets set once
    result.parachutedeploy_altitude = 0.0;         //Needs to be zero at first so that it only gets set once
    result.maxconvheatrate.value = 0.0;
    result.maxradheatrate.value = 0.0;
    result.maxtotalheatrate.value = 0.0;
    result.maxloadfactor.value = 0.0;

    trajectory.open("data/REMTrajectory.stae");

    return initialState;
}

Endstate EntryTrajectory::getEndstate()
{
    save = false;
    QList<double> sampleTimesTemp;
    QList<sta::StateVector> samplesTemp;
    //integrate(sampleTimesTemp, samplesTemp);
    return (result);
}

void EntryTrajectory::saveTrajectory (QList<double>& sampleTimes, QList<sta::StateVector>& samples)
{
    save = true;
    trajectory.open("data/REMTrajectory.stae");
    misc.open("data/REMMiscellaneous.stam");
    //integrate(sampleTimes, samples);
    misc.close();
    trajectory.close();
    printReport();
}

sta::StateVector EntryTrajectory::integrate(sta::StateVector stateVector, QList<Perturbations*> perturbationsList)//Modified by Dominic to improve integration stability
{
    double tau = settings.stepsize;

    /*
    Vector3d gravity;                   //Components of gravitational acceleration in appropriate elements
    Vector3d aero;                     	//Components of aerodynamic acceleration in appropriate elements
    VectorXd parameters(9);             //List of parameters to send to integration: 0,1,2 = gravity, 3,4,5 = aero, 6 = omega, 7 = Qdot
    double Cdc;

    //Update gravity, perturbations and aerodynamic accelerations
    body.updateGravity(stateVector, gravity);
    body.updateAero(stateVector, state2, atmosphere, capsule, aero, Cdc);

    //Vector3d perturbingAcceleration(0, 0, 0);
    //foreach (Perturbations* perturbation, perturbationsList)
    //{
    //    perturbingAcceleration += perturbation->calculateAcceleration(stateVector, time, tau);
    //}
    //gravity += perturbingAcceleration;

    calculateState2(aero, Cdc);
    updateEndstate();

    //Test for status
    checkStatus();
    if ( status != OK) {
        result.status = status;
    }

    //Build the parameters vector to send to equations of motion
    parameters << gravity, aero, body.getOmega(), state2(3), state2(4);
    //TO DO : re-insert the heat load integration (now heat flux is always set to zero)
    VectorXd state(8);
            state << stateVector.position, stateVector.velocity, 0, 0;

    //if (settings.integrator == "RK4")
        rk4 (state, 8, time, tau, derivstate, parameters);
    */
    EntryDerivativeCalculator entryCalculator(this);// entryCalculator(this,QList<Perturbations*> perturbationsList);
    Matrix<double, 8, 1> state;
    state<<stateVector.position, stateVector.velocity, 0, 0;
    rk4(state, time, tau, &entryCalculator);

    const Vector3d vec1(state(0), state(1), state(2));
    const Vector3d vec2(state(3), state(4), state(5));
    sta::StateVector vector = sta::StateVector(vec1, vec2);

    stateVector = vector;

    time += tau;

    //------ Calculate the rest of the endstate structure ------
    calculateEndstate();

    return stateVector;
}

void EntryTrajectory::calculateState2(Vector3d& aero, double Cdc)
{
    state2(2) = aero.norm() / 9.81; //n = magnitude of resultant vector of accelerations / g
    //------ calculate heat rate at stagnation point ------
    state2(3) = heatrate.ConvectiveHeatRate(state2(6), state2(0), capsule.Rn);
    state2(4) = heatrate.RadiativeHeatRate(state2(6), state2(0), capsule.Rn);
    state2(8) = state2(3) + state2(4);
    state2(9) = state(6) + state(7);
}

inline void EntryTrajectory::checkStatus()
{
    if (state2(5) < 0.0) {status = landed;};
    if (state2(5) > settings.maxaltitude) {status = gone;};     //If higher than x times initial altitude -> gone
    if (state2(8) > settings.maxheatrate) {status = burnt;};
    if (result.maxloadfactor.value > settings.maxloadfactor) {status = crushed;};
}

Endstate EntryTrajectory::updateEndstate()
{
    if (state2(1) < 1.0 && result.Machone_transition_altitude == 0.0) {
        result.Machone_transition_time = time;
        result.Machone_transition_altitude = state2(5);
    }//-------------------------------------
    if (state2(1) < settings.parachuteDeployMach && result.parachutedeploy_altitude == 0.0) {
        result.parachutedeploy_time = time;
        result.parachutedeploy_altitude = state2(5);
        state2(7) = 1.0;
    }//-------------------------------------
    if (state2(2) > result.maxloadfactor.value && state2(7) != 1.0) {         //find the maximum load factor
        result.maxloadfactor.value = state2(2);
        result.maxloadfactor.time = time;
        result.maxloadfactor.altitude = state2(5);
    }//-------------------------------------
    if (state2(3) > result.maxconvheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxconvheatrate.value = state2(3);
        result.maxconvheatrate.time = time;
        result.maxconvheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(4) > result.maxradheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxradheatrate.value = state2(4);
        result.maxradheatrate.time = time;
        result.maxradheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(8) > result.maxtotalheatrate.value) {         //find the maximum total heat rate
        result.maxtotalheatrate.value = state2(8);
        result.maxtotalheatrate.time = time;
        result.maxtotalheatrate.altitude = state2(5);
    }
    return(result);
}

Endstate EntryTrajectory::calculateEndstate() {

    //------ Update the maximum values one more time ------
    if (state2(2) > result.maxloadfactor.value) {         //find the maximum heat rate
        result.maxloadfactor.value = state2(2);
        result.maxloadfactor.time = time;
        result.maxloadfactor.altitude = state2(5);
    }//-------------------------------------
    if (state2(3) > result.maxconvheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxconvheatrate.value = state2(3);
        result.maxconvheatrate.time = time;
        result.maxconvheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(4) > result.maxradheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxradheatrate.value = state2(3);
        result.maxradheatrate.time = time;
        result.maxradheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(8) > result.maxtotalheatrate.value) {         //find the maximum total heat rate

        result.maxtotalheatrate.value = state2(4);
        result.maxtotalheatrate.time = time;
        result.maxtotalheatrate.altitude = state2(5);
    }
    //------ Compute time of flight, total heat load and impact velocity ------
    result.timeofflight = time;
    result.totalheatload = state2(9);
    result.tpsmass = (0.0009*pow(state(6),0.5306)) * parameters.m;
    result.impactvelocity = state2(6);

    //The following is only applicable for a successful landing
    //---------------------------------------------------------
    if (status == 0) {
        //------ Compute the landing longitude ------
        result.longitude = atan2(state(1), state(0)) / acos(-1.0) * 180;
        //------ Compute the landing latitude ------
        //lambda = asin( state(2) / sqrt(pow(state(0),2) + pow(state(1),2) + pow(state(2),2)) );
        //result.latitude = body.geocentric2geodetic_latitude(lambda) / acos(-1) * 180;
        result.latitude = asin( state(2) / sqrt(pow(state(0),2) + pow(state(1),2) + pow(state(2),2)) ) / acos(-1.0) * 180;
    }
    //---------------------------------------------------------
    return(result);
}

inline void EntryTrajectory::saveOutput (double time)
{

    double stateSpherical[6];
    cartesianTOspherical(state(0), state(1), state(2), state(3), state(4), state(5),
                         stateSpherical[0], stateSpherical[1], stateSpherical[2], stateSpherical[3], stateSpherical[4], stateSpherical[5]);
    stateSpherical[2] = body.altitude(stateSpherical[2] , stateSpherical[1] );
    double theta = getGreenwichHourAngle(sta::MjdToJd(sta::secsToDays(time)));
    double a,b,c,d,e,f;
    int celestialbody;
    if (settings.bodyname == "Earth")
         celestialbody = 0;
    else if (settings.bodyname == "Mars")
         celestialbody = 3;
    fixedTOinertial(celestialbody, theta, state(0), state(1), state(2), state(3), state(4), state(5),
                    a,b,c,d,e,f);
    double v = sqrt(d*d + e*e + f*f);
    double g,h,i;
    cartesianTOspherical(a, b, c, d, e, f,
                         g, h, i, stateSpherical[3], stateSpherical[4], stateSpherical[5]);

    trajectory << setfill('0');
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << state(0) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << state(1) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << state(2) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(8) << state(3) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(8) << state(4) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(8) << state(5) << "     ";
        trajectory << stateSpherical[0]*1000 << "     ";
        trajectory << stateSpherical[1]*1000 << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << stateSpherical[2]*1000 << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << v << "     ";
        trajectory << stateSpherical[4]*1000 << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << stateSpherical[5]*1000 << "     ";
        trajectory << state2[0]*1000 << "     ";//density
        trajectory << state2[1] << "     ";//Ma
        trajectory << state2[2]*1000 << "     ";//load factor
        trajectory << state2[3] << "     ";//qdot_c
        trajectory << state2[4] << "     ";//qdot_r
        trajectory << state[7] << "     ";//parachute
        trajectory << state2[8]*1000 << "     ";//totalheatrate
        trajectory << time << endl;
}

inline void EntryTrajectory::printReport ()
{

    angle longitude_angle;
    angle latitude_angle;
    angle entrylong_angle;
    angle entrylat_angle;
    AngleConversion angleconversion;
    //angleconversion.decimalToDegMinSec(result.longitude, longitude_angle.degrees, longitude_angle.minutes, longitude_angle.seconds);
    //angleconversion.decimalToDegMinSec(result.latitude, latitude_angle.degrees, latitude_angle.minutes, latitude_angle.seconds);
    angleconversion.decimalToDegMinSec(parameters.inputstate[1], entrylong_angle.degrees, entrylong_angle.minutes, entrylong_angle.seconds);
    angleconversion.decimalToDegMinSec(parameters.inputstate[2], entrylat_angle.degrees, entrylat_angle.minutes, entrylat_angle.seconds);

//    angle longitude_angle = double2angle (result.longitude);
//    angle latitude_angle = double2angle (result.latitude);
//    angle entrylong_angle = double2angle (parameters.inputstate[1]);
//    angle entrylat_angle = double2angle (parameters.inputstate[2]);
    //ofstream report ("C:/Users/Tizy/Desktop/Code/trunk/sta-src/Entry/REMReport.txt");
    ofstream report("data/REMReport.txt");

    //ofstream summary ("data/REMSummary.txt");

    switch (status)
       {
       case landed:
            cout << "Landed" << endl;
            report << "Status report: succesful landing" << endl;
            report << "================================" << endl;
            report << " " << endl;
            report << "Capsule data and initial conditions" << endl;
            report << "--------------------------------" << endl;
            //report << "Capsule base radius:\t" << parameters.R << " m" << endl; Make consistent with enw schema
            report << "Capsule mass:\t\t" << parameters.m << " kg" << endl;
            report << "Capsule nose radius:\t" << parameters.Rn << " m" << endl;
            report << "Entry altitude:\t\t" << parameters.inputstate[0] / 1000.0 << " km" << endl;
            report << "Entry longitude:\t" << entrylong_angle.degrees << "° " << entrylong_angle.minutes << "' " << entrylong_angle.seconds << "'' " << "(" << parameters.inputstate[1] << "°)" << endl;
            report << "Entry latitude:\t\t" << entrylat_angle.degrees << "° " << entrylat_angle.minutes << "' " << entrylat_angle.seconds << "'' " << "(" << parameters.inputstate[2] << "°)"<< endl;
            report << "Velocity:\t\t" << parameters.inputstate[3] << " m/s" << endl;
            report << "Flight path angle:\t" << parameters.inputstate[4] << " deg" << endl;
            report << "Heading:\t\t" << parameters.inputstate[5] << " deg" << endl;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Trajectory data" << endl;
            report << "--------------------------------" << endl;
            //report << "Impact longitude:\t" << longitude_angle.degrees << "° " << longitude_angle.minutes << "' " << longitude_angle.seconds << "'' " << "(" << result.longitude << "°)" << endl;
            //report << "Impact latitude:\t" << latitude_angle.degrees << "° " << latitude_angle.minutes << "' " << latitude_angle.seconds << "'' " << "(" << result.latitude << "°)" << endl;
            report << setprecision (3);
            report << "Impact velocity:\t" << result.impactvelocity << " m/s" << endl;
            report << fixed;
            report << "Total time of flight:\t" << result.timeofflight << " s" << endl;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Load data" << endl;
            report << "--------------------------------" << endl;
            report << "Maximum G load:\t\t\t" << result.maxloadfactor.value << " G (limit: " << settings.maxloadfactor << " G)" << endl;
            report << "Maximum convective heat rate:\t" << result.maxconvheatrate.value << " W/cm²" << endl;
            report << "Maximum radiative heat rate:\t" << result.maxradheatrate.value << " W/cm²" << endl;
            report << "Maximum total heat rate:\t" << result.maxtotalheatrate.value << " W/cm² (limit: " << settings.maxheatrate << " W/cm²)" << endl;
            report << "Total heat load:\t\t" << result.totalheatload << " J/cm²" << endl;
            report << setprecision (3);
            report << "TPS mass estimate:\t\t" << result.tpsmass << " kg (" << (0.0009 * pow(result.totalheatload,0.5306))*100.0 << " % of capsule mass)" << endl;
            report << fixed;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Entry profile: altitude" << endl;
            report << "--------------------------------" << endl;
            report << "Maximum radiative heat rate:\t" << result.maxradheatrate.altitude / 1000.0 << " km" << endl;
            report << "Maximum total heat rate:\t" << result.maxtotalheatrate.altitude / 1000.0 << " km" << endl;
            report << "Maximum convective heat rate:\t" << result.maxconvheatrate.altitude / 1000.0 << " km" << endl;
            report << "Maximum G:\t\t\t" << result.maxloadfactor.altitude / 1000.0 << " km" << endl;
            report << "Mach 1 transition:\t\t" << result.Machone_transition_altitude / 1000.0 << " km" << endl;
            report << "Parachute deployment:\t\t" << result.parachutedeploy_altitude / 1000.0 << " km (M " << settings.parachuteDeployMach << ")" << endl;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Entry profile: time (TE = time of entry, TI = time of impact)" << endl;
            report << "--------------------------------" << endl;
            report << "Maximum radiative heat rate:\tTE + " << result.maxradheatrate.time << " s, TI - " << (result.timeofflight - result.maxradheatrate.time) << " s" << endl;
            report << "Maximum total heat rate:\tTE + " << result.maxtotalheatrate.time << " s, TI - " << (result.timeofflight - result.maxtotalheatrate.time) << " s" << endl;
            report << "Maximum convective heat rate:\tTE + " << result.maxconvheatrate.time << " s, TI - " << (result.timeofflight - result.maxconvheatrate.time) << " s" << endl;
            report << "Maximum G:\t\t\tTE + " << result.maxloadfactor.time << " s, TI - " << (result.timeofflight - result.maxloadfactor.time) << " s" << endl;
            report << "Mach 1 transition:\t\tTE + " << result.Machone_transition_time << " s, TI - " << (result.timeofflight - result.Machone_transition_time) << " s" << endl;
            report << "Parachute deployment:\t\tTE + " << result.parachutedeploy_time << " s, TI - " << (result.timeofflight - result.parachutedeploy_time) << " s (M " << settings.parachuteDeployMach << ")" << endl;
            report << "================================";
            //    summary << longitude_angle.degrees << endl; summary << longitude_angle.minutes << endl; summary << longitude_angle.seconds << endl;
            //    summary << latitude_angle.degrees << endl; summary << latitude_angle.minutes << endl; summary << latitude_angle.seconds << endl;
            //    summary << result.maxconvheatrate.value << endl; summary << result.maxconvheatrate.altitude << endl; summary << result.maxconvheatrate.time << endl;
            //    summary << result.maxradheatrate.value << endl; summary << result.maxradheatrate.altitude << endl; summary << result.maxradheatrate.time << endl;
            //    summary << result.maxtotalheatrate.value << endl; summary << result.maxtotalheatrate.altitude << endl; summary << result.maxtotalheatrate.time << endl;
            //    summary << result.maxloadfactor.value << endl; summary << result.maxloadfactor.altitude << endl; summary << result.maxloadfactor.time << endl;
            //    summary << result.Machone_transition_altitude << endl; summary << result.Machone_transition_time << endl;
            //    summary << result.parachutedeploy_altitude << endl;  summary << result.parachutedeploy_time << endl;
            //    summary << result.totalheatload << endl; summary << result.timeofflight << endl; summary << result.impactvelocity << endl;
            break;
        case gone:
            cout << "Gone" << endl;
            report << "Status report: unsuccesful landing" << endl;
            report << "==================================" << endl;
            report << "The capsule reached maximum altitude after " << result.timeofflight << " s" << endl;
            break;
        case crushed:
            cout << "Crushed" << endl;
            break;
        case burnt:
            cout << "Burnt" << endl;
            break;
        case OK:
            cout << "In air" << endl;
            break;

        }

    report.close();
    //summary.close();
}


#include <iostream>
#include <valarray> // No nice indexing.
// We load Eigen just to have nice indexing & printing!
// Download Eigen & place Eigen/ -> ./Eigen/
//                        unsupported/ -> Eigen_unsupported/
#include <Eigen/Dense>
#include <Eigen_unsupported/Eigen/CXX11/Tensor>

using namespace std;
using namespace Eigen;

typedef TensorMap<Tensor<int64_t, 3>> TensorMap3d;
typedef Matrix<int64_t, 3, 1> Vector3i64;
typedef Matrix<int64_t, Dynamic, 1> VectorXi64;
typedef Matrix<int64_t, Dynamic, Dynamic> MatrixXi64;

extern "C" {
  int density_gradient(
                       double* point_ptr,
                       const int64_t nmo,
                       const int64_t natm,
                       const int64_t nprims,
                       const int64_t mgrp,
                       const int64_t ngto_h,
                       int64_t* ngroup_ptr,
                       int64_t* ityp_ptr,
                       int64_t* nzexp_ptr,
                       int64_t* nlm_ptr,
                       int64_t* nuexp_ptr,
                       double* occ_ptr,
                       double* oexp_ptr,
                       double* xyz_ptr,
                       double* rcutte_ptr,
                       double* coef_ptr,
                       double* grad_ptr
                      ) ;
}

int density_gradient(
                     double* point_ptr,
                     const int64_t nmo,
                     const int64_t natm,
                     const int64_t nprims,
                     const int64_t mgrp,
                     const int64_t ngto_h,
                     int64_t* ngroup_ptr,
                     int64_t* ityp_ptr,
                     int64_t* nzexp_ptr,
                     int64_t* nlm_ptr,
                     int64_t* nuexp_ptr,
                     double* occ_ptr,
                     double* oexp_ptr,
                     double* xyz_ptr,
                     double* rcutte_ptr,
                     double* coef_ptr,
                     double* grad_ptr
										) {

  // input & output variables
  Map<Vector3d>   point(point_ptr, 3);
  Map<VectorXi64> ngroup(ngroup_ptr, natm);
  Map<VectorXi64> ityp(ityp_ptr, nprims);
  Map<MatrixXi64> nzexp(nzexp_ptr, natm, mgrp);
  Map<MatrixXi64> nlm(nlm_ptr, 56, 3); // could be static ... => () -> []
  TensorMap3d     nuexp(nuexp_ptr, natm, mgrp, ngto_h);
  Map<VectorXd>   occ(occ_ptr, nmo);
  Map<VectorXd>   oexp(oexp_ptr, nprims);
  Map<MatrixXd>   xyz(xyz_ptr, natm, 3);
  Map<MatrixXd>   rcutte(rcutte_ptr, natm, mgrp);
  Map<MatrixXd>   coef(coef_ptr, 2*nmo, nprims);
  Map<Vector3d>   grad(grad_ptr, 3);
  // local variables
  Vector3d fun   = Vector3d::Zero(3);
  Vector3d fun1  = Vector3d::Zero(3);
  Vector3d xcoor = Vector3d::Zero(3);
  VectorXd gun   = VectorXd::Zero(nmo);
  MatrixXd gun1  = MatrixXd::Zero(nmo,3);
  int k, i, itip, n;
  double dis2, ori, dp2, aexp, x2, x;
  double f12, f123, fa, fb, fc, cfj;
  double fac, facgun;

  grad.setZero();

  // Run over centers
  for ( int ic=0; ic<natm; ic++) {
    // Atomic coordinates of this center
    xcoor(0) = point(0) - xyz(ic,0);
    xcoor(1) = point(1) - xyz(ic,1);
    xcoor(2) = point(2) - xyz(ic,2);
    dis2 = xcoor.cwiseAbs2().sum();
    // Loop over different shell in this atom
    for ( int m=0; m<ngroup(ic); m++) {
      k = nuexp(ic, m, 0)-1;
      // Skip to compute this primitive if distance is too big.
      if (dis2 > rcutte(ic, m) * rcutte(ic, m) ) {
        continue;
      }
      ori = -oexp(k);
      dp2 = 2.0*ori;
      // All primitives in a shell share the same exponent.
      aexp = exp( ori * dis2 );
      // Loop over the different primitives in this shell.
      for ( int jj=0; jj<nzexp(ic,m); jj++) {
        // "i" is the original index of the primitive in the WFN.
        i = nuexp(ic, m, jj)-1;
        itip = ityp(i)-1;
        // Integer coefficients.
        Vector3i64 it = Vector3i64::Zero();
        it(0) = nlm(itip,0);
        it(1) = nlm(itip,1);
        it(2) = nlm(itip,2);

        for ( int j=0; j<3; j++) {
          n = it(j);
          x = xcoor(j);
          if (n == 0) {
                  fun1(j) = dp2 * x;
                  fun(j) = 1.0;
          } else if (n == 1) {
                  fun1(j) = 1.0 + dp2 * x * x;
                  fun(j) = x;
          } else if (n == 2) {
                  x2 = x * x;
                  fun1(j) = x * ( 2.0 + dp2 * x2 );
                  fun(j) = x2;
          } else if (n == 3) {
                  x2 = x * x;
                  fun1(j) = x2 * ( 3.0 + dp2 * x2 );
                  fun(j) = x * x2;
          } else if (n == 4) {
                  x2 = x * x;
                  fun1(j) = x2 * x * ( 4.0 + dp2 * x2 );
                  fun(j) = x2 * x2;
          } else if (n == 5) {
                  x2 = x * x;
                  fun1(j) = x2 * x2 * ( 5.0 + dp2 * x2 );
                  fun(j) = x2 * x2 * x;
          }
        } // endfor j
        f12 = fun(0) * fun(1) * aexp;
        f123 = f12 * fun(2);
        fa = fun1(0) * fun(1) * fun(2) * aexp;
        fb = fun1(1) * fun(0) * fun(2) * aexp;
        fc = fun1(2) * f12;

        // run over orbitals
        for ( int j=0; j<nmo; j++) {
          cfj = coef(j,i);
          gun(j) = gun(j) + cfj * f123;
          gun1(j,0) += cfj * fa;
          gun1(j,1) += cfj * fb;
          gun1(j,2) += cfj * fc;
        }

      } // endfor jj
    } // endfor m
  } // endfor ic

  // Run again over orbitals
  for ( int i=0; i<nmo; i++) {
    fac = occ(i);
    facgun = fac * gun(i);
    for ( int j=0; j<3; j++) {
      grad(j) += facgun * gun1(i,j);
    }
  }
  grad *= 2.0;

  return EXIT_SUCCESS;
}


/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cmath>
#include <boost/functional.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/tools/roots.hpp>

#include <idmlib/lshkit/common.h>
#include <idmlib/lshkit/mplsh.h>

#define ABS_ERROR   1e-5l
#define REL_ERROR   1e-5l
#define MAX_SHAPE   1000.0l
#define LIMIT       40000

namespace lshkit {

static GaussianDouble normal;

/* Maximum Likelihood Estimation of gamma distribution */
class GammaDoubleMleHelper
{
    double rate_; // G / M
public:
    GammaDoubleMleHelper(double rate): rate_(rate) {}

    double operator () (double k)
    {
        return std::log(k) - boost::math::digamma(k) + std::log(rate_);
    }
};

bool GammaDoubleMleTol (double first, double second)
{
    return (second - first) < ABS_ERROR;
}

GammaDouble GammaDoubleMLE (double M, double G)
{
    GammaDoubleMleHelper hlp(G/M);
    std::pair<double,double> pair = boost::math::tools::bisect(hlp, ABS_ERROR, MAX_SHAPE, GammaDoubleMleTol);
    double k = (pair.first + pair.second) / 2.0;
    return GammaDouble(k, M/k);
}

/* estimate the cost and miss of LSH */
static inline double col_helper (double x)
{
    double result;
    result = 2.0*boost::math::cdf(normal, x) - 1.0;
    result += std::sqrt(2.0/M_PI) * (std::exp(-x*x/2.0)-1.0)/x;
    return result;
}

static inline double p_col_helper (double x, double k)
{
    return boost::math::cdf(normal,(1.0 + k) * x)
        - boost::math::cdf(normal, k*x);
}

double MultiProbeLshModel::recall (double x) const
{
/*
    double x2 = W_ / x;
    double p = col_helper(x2);

    unsigned MT =  __probeSequenceTemplates[M_].size();
    if (MT > T_) MT = T_;

    double result = 0;
    for (unsigned i = 0; i < MT; i++)
    {
        double r = 1.0;
        for (unsigned j = 0; j < M_; j++)
        {
            Probe &probe = __probeSequenceTemplates[M_][i];
            if (probe.mask & leftshift(j))
            {
                double delta = (j + 1.0) / (M_ + 1.0) * 0.5; // expected value
                if (probe.shift & leftshift(j))
                {
                    r *= p_col_helper(x2, 1.0 - delta);
                }
                else
                {
                    r *= p_col_helper(x2, delta);
                }
            }
            else r *= p;
        }
        result += r;
    }
    return 1.0 - std::exp(std::log(1.0 - result) * L_);
*/
    return 0;
}

struct __MpLshMdlHlpr
{
    const MultiProbeLshModel *model;
    const GammaDouble *gamma;
};
/*
static double recall_helper (double xsqr, void *_param)
{
    __MpLshMdlHlpr *param = reinterpret_cast<__MpLshMdlHlpr
        *>(_param);
    return boost::math::pdf(*param->gamma, xsqr) * param->model->recall(std::sqrt(xsqr));
}
*/
static double recall (__MpLshMdlHlpr *param)
{
/*
    static gsl_integration_workspace *workspace = NULL;
    double f, error;
    gsl_function I;
    if (workspace == NULL)
    {
        workspace = gsl_integration_workspace_alloc(LIMIT);
        BOOST_VERIFY(workspace != NULL);
    }
    I.params = param;
    I.function = recall_helper;
    if (gsl_integration_qagiu(&I, 0.0, ABS_ERROR, REL_ERROR, LIMIT, workspace, &f, &error) != 0) f = 1.0;
    return f;
*/
    return 0;
}

double MultiProbeLshDataModel::avgRecall () const
{
    double f = 0.0;
    __MpLshMdlHlpr param;
    param.model = this;
    for (unsigned k = 0; k < topkDists_.size(); k++)
    {
        param.gamma = &topkDists_[k];
        f += lshkit::recall(&param);
    }
    f /= topkDists_.size();
    return f;
}

double MultiProbeLshDataModel::cost () const
{
    __MpLshMdlHlpr param;
    param.model = this;
    param.gamma = &globalDist_;
    return lshkit::recall(&param);
}

}


// Flatten.cpp : Defines the entry point for the console application.
//

#ifdef WIN32
#define NOMINMAX
#include <windows.h>
#endif

#if defined (__APPLE__) || defined (OSX)
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glut.h>
#endif

#include "GA/c3ga.h"
#include "GA/c3ga_util.h"
#include "GA/gl_util.h"

#include "primitivedraw.h"
#include "gahelper.h"
#include "Laplacian.h"

#include <memory>

#include <vector>
#include <queue>
#include <map>
#include "numerics.h"
#include "HalfEdge/Mesh.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/Geometry>

// #include <ppl.h>

#include "Benchs.h"

const char *WINDOW_TITLE = "Interactive 3D Shape Deformation using Conformal Geometric Algebra";

// GLUT state information
int g_viewportWidth = 800;
int g_viewportHeight = 600;

void display();
void reshape(GLint width, GLint height);
void MouseButton(int button, int state, int x, int y);
void MouseMotion(int x, int y);
void KeyboardUpFunc(unsigned char key, int x, int y);
void SpecialFunc(int key, int x, int y);
void SpecialUpFunc(int key, int x, int y);
void Idle();
void DestroyWindow();

//using namespace boost;
using namespace c3ga;
using namespace std;
using namespace numerics;

class Camera
{
public:
	float		pos[3];
	float		fw[3];
	float		up[3];
	float		translateVel;
	float		rotateVel;

	Camera()
	{
		float		_pos[] = { 0, 0, -2};
		float		_fw[] = { 0, 0, 1 };
		float		_up[] = { 0, 1, 0 };

		translateVel = 0.005;
		rotateVel = 0.005;
		memcpy(pos, _pos, sizeof(float)*3);
		memcpy(fw, _fw, sizeof(float)*3);
		memcpy(up, _up, sizeof(float)*3);
	}

	void glLookAt()
	{
		gluLookAt( pos[0], pos[1], pos[2], fw[0],  fw[1],  fw[2], up[0],  up[1],  up[2] );
	}
};

class VertexBuffer
{
public:
	std::vector<Eigen::Vector3d> deformedPositions; //deformed mesh positions
	std::map<int, Eigen::Vector3d> constrainedPositions; //positional constraints
	std::vector<Eigen::Vector3d> laplacianCoordinates; //laplacian Coordinates
	std::vector<Eigen::Vector3d> normals; //for rendering (lighting)
	std::vector<Eigen::Vector3d> normalsOrig; //original normals
	std::vector<Eigen::Quaterniond> rotors;
	std::vector<Eigen::Matrix4d> JtJ;
	std::vector<Eigen::Vector4d> b;
	int size;

	VertexBuffer() : size(0)
	{
	}

	void resize(int size)
	{
		this->size = size;
		deformedPositions.resize(size);
		laplacianCoordinates.resize(size);
		normals.resize(size);
		normalsOrig.resize(size);
		rotors.resize(size);
		JtJ.resize(size);
		b.resize(size);
	}
	int get_size() { return size; }

};

class IndexBuffer {
public:
	std::vector<int> faces;
	int size;

	IndexBuffer() : size(0)
	{
	}

	void resize(int size)
	{
		this->size = size;
		faces.resize(size);
	}
	int get_size() { return size; }

};

class Handle
{
public:
	rotor R;
	translator T;
	translator Tcenter;
	dualSphere dS;
	std::set<int> constraints;

	Handle() {
		T = _rotor(1.0);
		R = _translator(1.0);
		Tcenter = _translator(1.0);
	}
	Handle(dualSphere dS)
	{
		normalizedPoint x = DualSphereCenter(dS);
		Tcenter = exp(-0.5 * _vectorE3GA(x) * ni);
		T = _translator(1.0);
		R = _rotor(1.0);
		this->dS = dS;
	}

	TRversor GetTRVersor()
	{
		return _TRversor(T * _TRversor(Tcenter * R * inverse(Tcenter)));
	}
};

Camera g_camera;
Mesh mesh, meshLow;
map<int, int> mapping, rotorClusters;
vectorE3GA g_prevMousePos;
bool g_rotateModel = false;
bool g_rotateModelOutOfPlane = false;
rotor g_modelRotor = _rotor(1.0);
bool g_rotateKeyRotors = false;
bool g_translateKeyRotors = false;
bool g_computeBasis = false;
float g_dragDistance = -1.0f;
int g_dragObject;
std::shared_ptr<SparseMatrix> A;
std::shared_ptr<SparseMatrix> AHi;
Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solverLow, solverHi;
int systemType = LaplaceBeltrami; //MeanValue; //LaplaceBeltrami
bool g_showSpheres = true;
bool g_showWires = false;
bool g_iterateManyTimes = false;
Eigen::MatrixXd b3Low;
Eigen::MatrixXd xyzLow;
Eigen::MatrixXd b3Hi;
Eigen::MatrixXd xyzHi;
double g_meshArea = 0.0;

bool g_automaticAnimation = false;
bool g_convergence = false;
Benchmarks benchs;

VertexBuffer vertexDescriptors;
VertexBuffer vertexDescriptorsLow;
IndexBuffer trianglesHi;
std::vector<Handle> handles;

Eigen::Affine3d MotorToMatrix(const TRversor& R) {
	TRversor Ri = inverse(R);

	// compute images of basis vectors:
	c3ga::flatPoint imageOfE1NI = _flatPoint(R * c3ga::e1ni * Ri);
	c3ga::flatPoint imageOfE2NI = _flatPoint(R * c3ga::e2ni * Ri);
	c3ga::flatPoint imageOfE3NI = _flatPoint(R * c3ga::e3ni * Ri);
	c3ga::flatPoint imageOfNONI = _flatPoint(R * c3ga::noni * Ri);

	// create matrix representation:
	Eigen::Affine3d M;
	M(0, 0) = imageOfE1NI.m_c[0];
	M(1, 0) = imageOfE1NI.m_c[1];
	M(2, 0) = imageOfE1NI.m_c[2];
	M(3, 0) = imageOfE1NI.m_c[3];
	M(0, 1) = imageOfE2NI.m_c[0];
	M(1, 1) = imageOfE2NI.m_c[1];
	M(2, 1) = imageOfE2NI.m_c[2];
	M(3, 1) = imageOfE2NI.m_c[3];
	M(0, 2) = imageOfE3NI.m_c[0];
	M(1, 2) = imageOfE3NI.m_c[1];
	M(2, 2) = imageOfE3NI.m_c[2];
	M(3, 2) = imageOfE3NI.m_c[3];
	M(0, 3) = imageOfNONI.m_c[0];
	M(1, 3) = imageOfNONI.m_c[1];
	M(2, 3) = imageOfNONI.m_c[2];
	M(3, 3) = imageOfNONI.m_c[3];
	return M;
}

void ComputeLaplacianCoordinates(std::shared_ptr<SparseMatrix> A, Mesh* mesh, std::vector<Eigen::Vector3d>& laplacianCoordinates)
{
	std::fill(laplacianCoordinates.begin(), laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	auto numRows = A->numRows();

	for (int i = 0; i < numRows; ++i)
	{
		SparseMatrix::RowIterator aIter = A->iterator(i);
		for (; !aIter.end(); ++aIter)
		{
			auto j = aIter.columnIndex();
			laplacianCoordinates[i] += mesh->vertexAt(j).p * aIter.value();
		}
	}
}

bool is_constrained(std::set<int>& constraints, int vertex)
{
	return constraints.find(vertex) != constraints.end();
}

void PreFactor(std::shared_ptr<SparseMatrix> A, std::set<int>& constraints, Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>& solver)
{

	Eigen::SparseMatrix<double> Lc = Eigen::SparseMatrix<double>(A->numRows(), A->numColumns());

	auto numRows = A->numRows();
	for (int i = 0; i < numRows; ++i)
	{
		if (!is_constrained(constraints, i))
		{
			SparseMatrix::RowIterator aIter = A->iterator(i);
			for (; !aIter.end(); ++aIter)
			{
				auto j = aIter.columnIndex();
				Lc.insert(i, j) = (*A)(i, j);
			}
		}
		else
		{
			Lc.insert(i, i) = 1.0;
		}
	}

	Lc.makeCompressed();
	solver.compute(Lc);
	if (solver.info() != Eigen::Success) {
		// TODO: error handling
	}

}

double meshArea(Mesh *mesh)
{
	double area = 0.0;
	for(Face& face :  mesh->getFaces())
	{
		Eigen::Vector3d	u = face.edge->next->vertex->p - face.edge->vertex->p;
		Eigen::Vector3d	v = face.edge->next->next->vertex->p - face.edge->vertex->p;
		area += 0.5 * u.cross(v).norm();
	}
	return area;
}

void meshMapping(Mesh *meshLow, Mesh *mesh, map<int, int>& mapping)
{
	for (Vertex& vertexLow : meshLow->getVertices())
	{
		double minDist = 1e38;
		int mappedPoint = -1;
		Eigen::Vector3d& pLow = vertexLow.p;
		for (Vertex& vertexHi : mesh->getVertices())
		{
			Eigen::Vector3d& p = vertexHi.p;
			double dist = (pLow - p).norm();
			if (dist < minDist) {
				minDist = dist;
				mappedPoint = vertexHi.ID;
			}
		}
		mapping[vertexLow.ID] = mappedPoint;
	}

	vector<bool> visited = vector<bool>(mesh->numVertices(), false);
	queue<int> assigned;

	for (auto&& pair : mapping){
		visited[pair.second] = true;
		rotorClusters[pair.second] = pair.first;
		assigned.push(pair.second);
	}

	while (!assigned.empty())
	{
		int i = assigned.front();
		assigned.pop();
		visited[i] = true;

		for (Vertex::EdgeAroundIterator edgeAroundIter = mesh->vertexAt(i).iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			if (visited[j] == false)
			{
				rotorClusters[j] = rotorClusters[i];
				assigned.push(j);
			}
		}
	}
}

int main(int argc, char* argv[])
{
	mesh.readOBJ("cactus2.obj"); //armadillo-5k-smooth.obj female.obj david.obj rabbit.obj tyra.obj horse.obj cylinder.obj bar.obj planewithpeaks.obj dragon.obj catHead.obj  cactus.obj  bunny.obj  764_hand-olivier-10kf.obj armadillo.obj
	mesh.CenterAndNormalize();
	mesh.computeNormals();

	meshLow.readOBJ("cactus1.obj"); //armadillo-5k-smooth.obj female.obj david.obj rabbit.obj tyra.obj horse.obj cylinder.obj bar.obj planewithpeaks.obj dragon.obj catHead.obj  cactus.obj  bunny.obj  764_hand-olivier-10kf.obj armadillo.obj
	meshLow.CenterAndNormalize();
	meshLow.computeNormals();

	meshMapping(&meshLow, &mesh, mapping);

	// GLUT Window Initialization:
	glutInit (&argc, argv);
	glutInitWindowSize(g_viewportWidth, g_viewportHeight);
	glutInitDisplayMode( GLUT_RGB | GLUT_ALPHA | GLUT_DOUBLE | GLUT_DEPTH);
	glutCreateWindow(WINDOW_TITLE);

	// Register callbacks:
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(MouseButton);
	glutMotionFunc(MouseMotion);
	glutKeyboardUpFunc(KeyboardUpFunc);
	glutSpecialFunc(SpecialFunc);
	glutSpecialUpFunc(SpecialUpFunc);
	glutIdleFunc(Idle);
	atexit(DestroyWindow);

	InitializeDrawing();

	//cactus.obj
	handles.push_back(Handle(_dualSphere(c3gaPoint(.0, .04, 0.7) - 0.5*SQR(0.15)*ni)));
	handles.push_back(Handle(_dualSphere(c3gaPoint(.0, .04, -0.8) - 0.5*SQR(0.15)*ni)));

	////cylinder.obj
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, 0.9, .0) - 0.5*SQR(0.25)*ni), false, P1)));
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, -0.9, .0) - 0.5*SQR(0.25)*ni), true, P2)));

	//Armadillo Pie y Mano
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(-.5, 0.45, -.3) - 0.5*SQR(0.15)*ni), false, P1)));
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.2, -0.6, .1) - 0.5*SQR(0.15)*ni), true, P2)));
	//Armadillo Pubis y Cabeza
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, 0.4, -.2) - 0.5*SQR(0.15)*ni), false, P1)));
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, -0.05, .1) - 0.5*SQR(0.15)*ni), true, P2)));

	//handles[0]->extrema = 0;
	//handles[1]->extrema = 1;
	//extremas[0]->handle = 0;
	//extremas[1]->handle = 1;

	vertexDescriptorsLow.resize(meshLow.numVertices());
	vertexDescriptors.resize(mesh.numVertices());
	trianglesHi.resize(mesh.numFaces() * 3);

	for( Vertex& vertexLow : meshLow.getVertices())
	{
		vertexDescriptorsLow.rotors[vertexLow.ID] = Eigen::Quaterniond::Identity();
		normalizedPoint position = c3gaPoint(vertexLow.p.x(), vertexLow.p.y(), vertexLow.p.z());

		for (Handle& handle : handles)
		{
			TRversor TR = handle.GetTRVersor();

			if (_double(position << (TR * handle.dS * inverse(TR))) > 0) //inside the sphere
			{
				handle.constraints.insert(vertexLow.ID);
				vertexDescriptorsLow.constrainedPositions[vertexLow.ID] = vertexLow.p;
			}
		}
	}

	A = CreateLaplacianMatrix( &meshLow, systemType );

	ComputeLaplacianCoordinates(A, &meshLow, vertexDescriptorsLow.laplacianCoordinates);

	b3Low = Eigen::MatrixXd(A->numRows(), 3);

	std::set<int> allconstraints;
	for (Handle& handle : handles) {
		allconstraints.insert(handle.constraints.begin(), handle.constraints.end());
	}
	PreFactor(A, allconstraints, solverLow);

	g_meshArea = meshArea(&meshLow);

	for (Vertex& vertexHi : mesh.getVertices()) {
		vertexDescriptors.normalsOrig[vertexHi.ID] = vertexHi.n;
		vertexDescriptors.rotors[vertexHi.ID] = Eigen::Quaterniond::Identity();
	}

	for (Face& face : mesh.getFaces()) {
		int i = face.ID;
		int	v1 = face.edge->vertex->ID;
		int	v2 = face.edge->next->vertex->ID;
		int	v3 = face.edge->next->next->vertex->ID;
		trianglesHi.faces[i * 3 + 0] = v1;
		trianglesHi.faces[i * 3 + 1] = v2;
		trianglesHi.faces[i * 3 + 2] = v3;
	}

	AHi = CreateLaplacianMatrix(&mesh, systemType);
	ComputeLaplacianCoordinates(AHi, &mesh, vertexDescriptors.laplacianCoordinates);
	std::set<int> constraintsHi;
	for (map<int, int>::iterator iter = mapping.begin(); iter != mapping.end(); iter++){
		constraintsHi.insert(iter->second);
	}
	b3Hi = Eigen::MatrixXd(AHi->numRows(), 3);
	PreFactor(AHi, constraintsHi, solverHi);

	glutMainLoop();

	return 0;
}

void transferRotations(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
	for (auto&& pair : rotorClusters) {
		vertexDescriptors.rotors[pair.first] = vertexDescriptorsLow.rotors[pair.second];
	}

	std::fill(vertexDescriptors.laplacianCoordinates.begin(), vertexDescriptors.laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			double wij = (*A)(i, j);
			Eigen::Quaterniond &Ri = vertexDescriptors.rotors[i];
			Eigen::Quaterniond &Rj = vertexDescriptors.rotors[j];
			Eigen::Vector3d V = mesh->vertexAt(j).p - mesh->vertexAt(i).p;
			vertexDescriptors.laplacianCoordinates[i] += 0.5 * wij * (Ri._transformVector(V) + Rj._transformVector(V));;
		}
	}
	//});
}

void SolveLinearSystemTaucs(VertexBuffer& vertexDescriptors)
{
	int n = vertexDescriptors.get_size();

	for (int i = 0; i < n; ++i) {
		b3Low.row(i) = vertexDescriptors.laplacianCoordinates[i];
	}

	for (Handle& handle : handles) {
		Eigen::Affine3d M = MotorToMatrix(handle.GetTRVersor());
		for (int i : handle.constraints) {
			b3Low.row(i) = M * vertexDescriptors.constrainedPositions[i];
		}
	}

	xyzLow = solverLow.solve(b3Low);

	for( int i = 0 ; i < n ; ++i ) {
		vertexDescriptors.deformedPositions[i] = xyzLow.row(i);
	}
}

void SolveLinearSystemTaucsHi(VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
	int n = vertexDescriptors.get_size();

	for (int i = 0; i < n; ++i) {
		b3Hi.row(i) = vertexDescriptors.laplacianCoordinates[i];
	}

	for (auto&& pair : mapping){
		b3Hi.row(pair.second) = vertexDescriptorsLow.deformedPositions[pair.first];
	}

	xyzHi = solverHi.solve(b3Hi);

	for (int i = 0; i < n; ++i) {
		vertexDescriptors.deformedPositions[i] = xyzHi.row(i);
		vertexDescriptors.normals[i] = vertexDescriptors.rotors[i]._transformVector(vertexDescriptors.normalsOrig[i]);
	}
}

void E3GA_Prep(const vector<Eigen::Vector3d>& P, const vector<Eigen::Vector3d>& Q, double wij2, const int N, Eigen::Matrix4d &JtJ, Eigen::Vector4d &b)
{
	JtJ.setZero();

	double q3p3;
	double q2p2;
	double p1q1;
	double q1p1;
	double p2q2;
	double p3q3;
	double p1q1_2;
	double p2q2_2;
	double p3q3_2;
	double q2p2_2;
	double q1p1_2;
	double q3p3_2;

	for (int i = 0; i < N; ++i)
	{
		const Eigen::Vector3d& Pi = P[i];
		const Eigen::Vector3d& Qi = Q[i];

		q1p1 = Qi.x() + Pi.x();
		q2p2 = Qi.y() + Pi.y();
		q3p3 = Qi.z() + Pi.z();
		p1q1 = Pi.x() - Qi.x();
		p2q2 = Pi.y() - Qi.y();
		p3q3 = Pi.z() - Qi.z();

		p1q1_2 = p1q1 * p1q1;
		p2q2_2 = p2q2 * p2q2;
		p3q3_2 = p3q3 * p3q3;
		q2p2_2 = q2p2 * q2p2;
		q1p1_2 = q1p1 * q1p1;
		q3p3_2 = q3p3 * q3p3;

		JtJ(0, 0) += p1q1_2 + p2q2_2 + p3q3_2;
		JtJ(0, 1) += p1q1 * q2p2 - p2q2 * q1p1;
		JtJ(0, 2) += p1q1 * q3p3 - p3q3 * q1p1;
		JtJ(0, 3) += p2q2 * q3p3 - p3q3 * q2p2;
		JtJ(1, 1) += q2p2_2 + q1p1_2 + p3q3_2;
		JtJ(1, 2) += q2p2 * q3p3 - p3q3 * p2q2;
		JtJ(1, 3) += -q1p1 * q3p3 + p3q3 * p1q1;
		JtJ(2, 2) += q3p3_2 + q1p1_2 + p2q2_2;
		JtJ(2, 3) += q1p1 * q2p2 - p2q2 * p1q1;
		JtJ(3, 3) += q3p3_2 + q2p2_2 + p1q1_2;
	}

	b(0) = -JtJ(0, 0);
	b(1) = -JtJ(0, 1);
	b(2) = -JtJ(0, 2);
	b(3) = -JtJ(0, 3);

	const double nwij2 = N * wij2 + 1e-6;

	JtJ(0, 0) += nwij2;
	JtJ(1, 1) += nwij2;
	JtJ(2, 2) += nwij2;
	JtJ(3, 3) += nwij2;

	JtJ(1, 0) = JtJ(0, 1);
	JtJ(2, 0) = JtJ(0, 2);
	JtJ(2, 1) = JtJ(1, 2);
	JtJ(3, 0) = JtJ(0, 3);
	JtJ(3, 1) = JtJ(1, 3);
	JtJ(3, 2) = JtJ(2, 3);

	JtJ = JtJ.inverse().eval();
}

Eigen::Quaterniond E3GA_Fast5(double wij, const vector<Eigen::Quaterniond>& Rq, const Eigen::Quaterniond& Rx, const int N, const Eigen::Matrix4d &JtJ, const Eigen::Vector4d &b)
{
	Eigen::Vector4d x;
	Eigen::Vector4d b2;
	b2.setZero();
	//const double wij2 = wij;// *wij;

	for (int i = 0; i < N; ++i)
	{
		const Eigen::Quaterniond& Rqi = Rq[i];
		b2(0) += Rqi.w() - 1.0; // 1.0
		b2(1) += -Rqi.z(); // e1 ^ e2
		b2(2) += Rqi.y(); // e1 ^ e3
		b2(3) += -Rqi.x(); // e2 ^ e3
	}

	b2 *= wij;

	b2(0) += b(0) + 1e-6 * (Rx.w() - 1.0); // 1.0
	b2(1) += b(1) + 1e-6 * -Rx.z(); // e1 ^ e2
	b2(2) += b(2) + 1e-6 * Rx.y(); // e1 ^ e3
	b2(3) += b(3) + 1e-6 * -Rx.x(); // e2 ^ e3

	x.noalias() = JtJ * b2;

	return Eigen::Quaterniond(1.0 + x(0), -x(3), x(2), -x(1)); //rotor(rotor_scalar_e1e2_e2e3_e3e1, 1.0 + x(0), x(1), x(3), -x(2));
}

void UpdateLaplaciansRotationPrep(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors)
{
	//Matrix3x3 m;
	//Matrix3x3 U, W, V, Ut;
	//Matrix3x3 M;
	//ICP icp;
	vector<Eigen::Vector3d> P;
	vector<Eigen::Vector3d> Q;

	P.resize(32);
	Q.resize(32);

	const double alpha = 0.14;
	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d &pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); vertexDegree < 32 && !edgeAroundIter.end(); edgeAroundIter++, vertexDegree++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;

			Eigen::Vector3d &tpj = vertexDescriptors.deformedPositions[j];
			Eigen::Vector3d &pj = mesh->vertexAt(j).p;
			Eigen::Vector3d eij = (pj - pi) * (*A)(i, j);
			Eigen::Vector3d teij = tpj - tpi;

			P[vertexDegree] = eij;
			Q[vertexDegree] = teij;
		}
		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;
		//rotor M = E3GA4(P, Q, vertexDescriptors[i]->M, vertexDegree);
		//rotor M = E3GA5(P, Q, vertexDegree);
		E3GA_Prep(P, Q, invVertexDegree, vertexDegree, vertexDescriptors.JtJ[i], vertexDescriptors.b[i]);
	}
	//});
}

void UpdateLaplaciansRotationExec(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors, bool updateLaplacian)
{
	const double alpha = 0.14;
	vector<Eigen::Quaterniond> Rq;

	Rq.resize(32);
	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d &pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); vertexDegree < 32 && !edgeAroundIter.end(); edgeAroundIter++, vertexDegree++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			Rq[vertexDegree] = vertexDescriptors.rotors[j];
		}
		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;
		Eigen::Quaterniond M = E3GA_Fast5(invVertexDegree, Rq, vertexDescriptors.rotors[i], vertexDegree, vertexDescriptors.JtJ[i], vertexDescriptors.b[i]);
		vertexDescriptors.rotors[i] = M.normalized();
	}
	//});

	if (!updateLaplacian) {
		return;
	}

	std::fill(vertexDescriptors.laplacianCoordinates.begin(), vertexDescriptors.laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			double wij = (*A)(i, j);
			Eigen::Quaterniond&Ri = vertexDescriptors.rotors[i];
			Eigen::Quaterniond&Rj = vertexDescriptors.rotors[j];
			Eigen::Vector3d V = mesh->vertexAt(j).p - mesh->vertexAt(i).p;
			vertexDescriptors.laplacianCoordinates[i] += 0.5 * wij * (Ri._transformVector(V) + Rj._transformVector(V));;
		}
	}
	//});
}

double ComputeEnergy(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors)
{
	double arapEnergy = 0.0;
	const double alpha = 0.08;
	//concurrency::parallel_for_each(mesh->vertices.begin(), mesh->vertices.end(), [&](Vertex * vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d& pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];
		auto& Ri = vertexDescriptors.rotors[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			vertexDegree++;
		}

		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;

		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;

			Eigen::Vector3d &tpj = vertexDescriptors.deformedPositions[j];
			Eigen::Vector3d &pj = mesh->vertexAt(j).p;

			Eigen::Vector3d eij = (pj - pi);
			auto q_ij = tpj - tpi;
			auto p_ij = eij;
			auto& Rj = vertexDescriptors.rotors[j];

			arapEnergy += (*A)(i, j) * (Ri._transformVector(p_ij) - q_ij).squaredNorm();
			arapEnergy += invVertexDegree * (Rj.coeffs() - Ri.coeffs()).squaredNorm();
		}
	}//);

	return arapEnergy;
}

void display()
{
	/*
	 *	matrices
	 */
	glViewport( 0, 0, g_viewportWidth, g_viewportHeight );
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	pickLoadMatrix();
	GLpick::g_frustumFar = 1000.0;
	GLpick::g_frustumNear = .1;
	gluPerspective( 60.0, (double)g_viewportWidth/(double)g_viewportHeight, GLpick::g_frustumNear, GLpick::g_frustumFar );
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glShadeModel(GL_SMOOTH);	//gouraud shading
	glClearDepth(1.0f);
	glClearColor( .75f, .75f, .75f, .0f );
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );

	/*
	 *	estados
	 */
	glEnable(GL_CULL_FACE);		//face culling
	glCullFace( GL_BACK );
	glFrontFace( GL_CCW );
	glEnable(GL_DEPTH_TEST);	//z-buffer
	glDepthFunc(GL_LEQUAL);

	/*
	 *	iluminacion
	 */
	float		ambient[] = { .3f, .3f, .3f, 1.f };
	float		diffuse[] = { .3f, .3f, .3f, 1.f };
	float		position[] = { .0f, 0.f, -150.f, 1.f };
	float		specular[] = { 1.f, 1.f, 1.f };

	glLightfv( GL_LIGHT0, GL_AMBIENT, ambient );
	glLightfv( GL_LIGHT0, GL_DIFFUSE, diffuse );
	glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 0);
	glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.0125);
	glEnable(  GL_LIGHT0   );
	glEnable(  GL_LIGHTING );
	glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, specular );
	glMaterialf( GL_FRONT_AND_BACK, GL_SHININESS, 50.f );

	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	glLoadIdentity();

	g_camera.glLookAt();

	glLightfv( GL_LIGHT0, /*GL_POSITION*/GL_SPOT_DIRECTION, position );

	glPushMatrix();

	rotorGLMult(g_modelRotor);

	benchs.Begin();

	if(!g_computeBasis)
	{
		static bool oneTime = true;
		if (oneTime || (g_rotateKeyRotors || g_translateKeyRotors || g_automaticAnimation))
		{
			if(oneTime == true)
			{
				SolveLinearSystemTaucs(vertexDescriptorsLow);
			}

			oneTime = false;

			for(int i = 0 ; i < 3 ; ++i)
			{
				UpdateLaplaciansRotationPrep(&meshLow, A, vertexDescriptorsLow);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, true);
				SolveLinearSystemTaucs(vertexDescriptorsLow);
			}
			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolveLinearSystemTaucsHi(vertexDescriptors, vertexDescriptorsLow);
		}
	}
	if(g_iterateManyTimes)
	{
		if (g_convergence)
		{
			clock_t begin;
			clock_t end;
			double avgTime = 0.0;

			auto arapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
			int count = 0;
			for (; count < 2500; ++count)
			{
				begin = clock();

				UpdateLaplaciansRotationPrep(&meshLow, A, vertexDescriptorsLow);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, true);
				SolveLinearSystemTaucs(vertexDescriptorsLow);

				end = clock();

				double secDeform = (double)(end - begin) / CLOCKS_PER_SEC;

				avgTime += secDeform;

				auto newArapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
				if (fabs(arapEnergy - newArapEnergy) < 1e-4)
				{
					std::cout << "convergence after " << count + 1 << " iterations" << std::endl;
					break;
				}
				arapEnergy = newArapEnergy;
			}
			arapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
			std::cout << arapEnergy << std::endl;
			std::cout << "Average iteration time (sec): " << avgTime / (double)(count + 1) << endl;
			std::cout << "Total accum time (sec): " << avgTime << endl;

			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolveLinearSystemTaucsHi(vertexDescriptors, vertexDescriptorsLow);
		}
		else
		{
			for (int i = 0; i < 100; ++i)
			{
				UpdateLaplaciansRotationPrep(&meshLow, A, vertexDescriptorsLow);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, true);
				SolveLinearSystemTaucs(vertexDescriptorsLow);
			}
			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolveLinearSystemTaucsHi(vertexDescriptors, vertexDescriptorsLow);
		}
		g_iterateManyTimes = false;
	}

	benchs.End();

	if (GLpick::g_pickActive) glLoadName((GLuint)-1);

	double alpha = 1.0;

	//glEnable (GL_BLEND);
	//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	//alpha = 0.5;

	//Mesh-Faces Rendering
	glPolygonMode( GL_FRONT_AND_BACK, GL_FILL /*GL_LINE GL_FILL GL_POINT*/);
	glEnable (GL_POLYGON_OFFSET_FILL);
	glPolygonOffset (1., 1.);
	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
	glEnable( GL_COLOR_MATERIAL );
	if (GLpick::g_pickActive) glLoadName((GLuint)10);

	glColor4d(1, 1, 1, alpha);
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_DOUBLE, 0, &vertexDescriptors.deformedPositions[0]);
	glNormalPointer(GL_DOUBLE, 0, &vertexDescriptors.normals[0]);
	// draw the model
	glDrawElements(GL_TRIANGLES, trianglesHi.get_size(), GL_UNSIGNED_INT, &trianglesHi.faces[0]);
	// deactivate vertex arrays after drawing
	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);

	if (g_showWires)
	{
		if (!GLpick::g_pickActive)
		{
			//Mesh-Edges Rendering (superimposed to faces)
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE /*GL_LINE GL_FILL GL_POINT*/);
			glColor4d(.5, .5, .5, alpha);
			glDisable(GL_LIGHTING);
			glEnableClientState(GL_VERTEX_ARRAY);
			glVertexPointer(3, GL_DOUBLE, 0, &vertexDescriptors.deformedPositions[0]);
			// draw the model
			glDrawElements(GL_TRIANGLES, trianglesHi.get_size(), GL_UNSIGNED_INT, &trianglesHi.faces[0]);
			// deactivate vertex arrays after drawing
			glDisableClientState(GL_VERTEX_ARRAY);
			glEnable(GL_LIGHTING);
		}
	}

	//for (map<int, int>::iterator iter = mapping.begin(); iter != mapping.end(); iter++){
	//	DrawPoint(c3gaPoint(vertexDescriptors[iter->second]->deformedPosition));
	//}

	glDisable( GL_COLOR_MATERIAL );
	glDisable(GL_POLYGON_OFFSET_FILL);

	//glDisable (GL_BLEND);

	if(g_showSpheres)
	{
		//Handles rendering
		glPolygonMode( GL_FRONT_AND_BACK, GL_FILL /*GL_LINE GL_FILL GL_POINT*/);

		float	turcoise[] = { .0f, .5f, .5f, 0.3f };
		float	red[] = { .5f, .0f, .0f, 0.3f };

		for( int k = 0 ; k < handles.size() ; ++k)
		{
			if (GLpick::g_pickActive) glLoadName((GLuint)k);
			TRversor R = handles[k].GetTRVersor();
			DrawTransparentDualSphere( _dualSphere( R * handles[k].dS * inverse(R) ), turcoise );
		}
	}

	glPopMatrix();

	glutSwapBuffers();
}

void reshape(GLint width, GLint height)
{
	g_viewportWidth = width;
	g_viewportHeight = height;

	// redraw viewport
	glutPostRedisplay();
}

vectorE3GA mousePosToVector(int x, int y) {
	x -= g_viewportWidth / 2;
	y -= g_viewportHeight / 2;
	return _vectorE3GA((float)-x * e1 - (float)y * e2);
}

void MouseButton(int button, int state, int x, int y)
{
	g_rotateModel = false;
	g_rotateKeyRotors = false;
	g_translateKeyRotors = false;

	if (button == GLUT_LEFT_BUTTON)
	{
		g_prevMousePos = mousePosToVector(x, y);

		GLpick::g_pickWinSize = 1;
		g_dragObject = pick(x, g_viewportHeight - y, display, &g_dragDistance);

		if(g_dragObject == -1 || g_dragObject == 10 )
		{
			vectorE3GA mousePos = mousePosToVector(x, y);
			g_rotateModel = true;

			if ((_Float(norm_e(mousePos)) / _Float(norm_e(g_viewportWidth * e1 + g_viewportHeight * e2))) < 0.2)
				g_rotateModelOutOfPlane = true;
			else g_rotateModelOutOfPlane = false;
		}
		else if(g_dragObject >= 0 && g_dragObject < handles.size())
		{
			g_rotateKeyRotors = true;
		}
	}

	if (button == GLUT_RIGHT_BUTTON)
	{
		g_prevMousePos = mousePosToVector(x, y);

		GLpick::g_pickWinSize = 1;
		g_dragObject = pick(x, g_viewportHeight - y, display, &g_dragDistance);

		if(g_dragObject >= 0 && g_dragObject < handles.size())
			g_translateKeyRotors = true;
	}
}

void MouseMotion(int x, int y)
{
	if (g_rotateModel || g_rotateKeyRotors || g_translateKeyRotors )
	{
		// get mouse position, motion
		vectorE3GA mousePos = mousePosToVector(x, y);
		vectorE3GA motion = mousePos - g_prevMousePos;

		if (g_rotateModel)
		{
			// update rotor
			if (g_rotateModelOutOfPlane)
				g_modelRotor = exp(g_camera.rotateVel * (motion ^ e3) ) * g_modelRotor;
			else
				g_modelRotor = exp(0.00001f * (motion ^ mousePos) ) * g_modelRotor;
		}
		if(g_rotateKeyRotors)
		{
			//rotor R1 =  _rotor( inverse(g_modelRotor) * exp(-g_camera.rotateVel * (motion ^ e3) ) * g_modelRotor);
			rotor R1 =  _rotor( exp(-g_camera.rotateVel * (motion ^ e3) ) );
			if(g_dragObject < handles.size())
			{
				rotor R = handles[g_dragObject].R;
				handles[g_dragObject].R = normalize(_TRversor( R1 * R  ) );
			}
		}

		if(g_translateKeyRotors)
		{
			normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp( _freeVector(-g_camera.translateVel*motion*ni) ) * g_modelRotor);
			if(g_dragObject < handles.size())
			{
				translator T = handles[g_dragObject].T;
				handles[g_dragObject].T = normalize(_TRversor( T1 * T ));
			}
		}

		// remember mouse pos for next motion:
		g_prevMousePos = mousePos;

		// redraw viewport
		glutPostRedisplay();
	}
}

void SpecialFunc(int key, int x, int y)
{
	switch(key) {
		case GLUT_KEY_F1 :
			{
				int mod = glutGetModifiers();
				if(mod == GLUT_ACTIVE_CTRL || mod == GLUT_ACTIVE_SHIFT )
				{
				}
			}
			break;
		case GLUT_KEY_UP:
			{
				if(g_rotateKeyRotors)
				{
					handles[g_dragObject].dS = ChangeDualSphereRadiusSize(handles[g_dragObject].dS, 0.025);

					// redraw viewport
					glutPostRedisplay();
				}
			}
			break;
		case GLUT_KEY_DOWN:
			{
				if(g_rotateKeyRotors)
				{
					handles[g_dragObject].dS = ChangeDualSphereRadiusSize(handles[g_dragObject].dS, -0.025);

					// redraw viewport
					glutPostRedisplay();
				}
			}
			break;
	}
}

void SpecialUpFunc(int key, int x, int y)
{
}

void KeyboardUpFunc(unsigned char key, int x, int y)
{
	if (key == 'c' || key == 'C')
	{
		g_convergence = !g_convergence;
		vectorE3GA motion = _vectorE3GA(-70.0, -60.0, 0);
		normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp(_freeVector(-g_camera.translateVel*motion*ni)) * g_modelRotor);
		translator T = handles[0].T;
		handles[0].T = normalize(_TRversor(T1 * T));

		glutPostRedisplay();
	}

	if (key == 'a' || key == 'A')
	{
		g_automaticAnimation = !g_automaticAnimation;
		if (g_automaticAnimation)
		{
			benchs.Start();
		}
		else
		{
			benchs.Stop();
		}

		glutPostRedisplay();
	}

	if(key == 'w' || key == 'W')
	{
		g_showWires = !g_showWires;
		glutPostRedisplay();
	}

	if( key == 'h' || key == 'H' )
	{
		g_showSpheres = !g_showSpheres;
		glutPostRedisplay();
	}

	if( key == 'x' || key == 'X' )
	{
		g_iterateManyTimes = true;
		glutPostRedisplay();
	}
}

void Idle()
{
	// redraw viewport
	if (g_automaticAnimation)
	{
		const int minCount = -50;
		const int maxCount = 50;
		static int mycount = 0;
		static int direction = 0;
		vectorE3GA motion = _vectorE3GA(1.0, 0, 0);
		if (mycount == maxCount)
		{
			direction = 1;
		}
		if (mycount == minCount)
		{
			direction = 0;
		}

		if (mycount < maxCount && direction == 0)
		{
			motion = _vectorE3GA(1.0, 0, 0);
			mycount++;
		}
		if (mycount > minCount && direction == 1)
		{
			motion = _vectorE3GA(-1.0, 0, 0);
			mycount--;
		}

		normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp(_freeVector(-g_camera.translateVel*motion*ni)) * g_modelRotor);
		translator T = handles[0].T;
		handles[0].T = normalize(_TRversor(T1 * T));

		if (!benchs.IsStarted())
		{
			benchs.Start();
		}

		glutPostRedisplay();
	}
}

void DestroyWindow()
{
	ReleaseDrawing();
}


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/Types.h"
#include "latbuilder/Parser/SizeParam.h"
#include "latbuilder/Parser/Weights.h"
#include "latbuilder/Parser/FigureOfMerit.h"
#include "latbuilder/Parser/MeritFilterList.h"
#include "latbuilder/TextStream.h"
#include "latbuilder/LFSR258.h"

#include "latbuilder/GenSeq/VectorCreator.h"
#include "latbuilder/GenSeq/CoprimeIntegers.h"
#include "latbuilder/LatSeq/Combiner.h"

#include "latbuilder/MeritSeq/CBC.h"
#include "latbuilder/MeritSeq/CoordUniformCBC.h"
#include "latbuilder/MeritSeq/CoordUniformInnerProd.h"
#include "latbuilder/MeritSeq/LatSeqOverCBC.h"

#include <boost/lexical_cast.hpp>

using namespace LatBuilder;
using TextStream::operator<<;

template <typename T1, typename T2, typename T3, typename T4>
void printTableRow(
      const T1& x1,
      const T2& x2,
      const T3& x3,
      const T4& x4
      )
{
   using namespace std;
   cout << x1 << '\t';
   cout << scientific << setprecision(8) << x2 << '\t';
   cout << scientific << setprecision(8) << x3 << '\t';
   cout << scientific << setprecision(8) << x4 << endl;;
}

struct Execute {
   template <class FIG2>
   void operator()(
         FIG2 fig2,
         const std::string& fig1,
         std::unique_ptr<LatticeTester::Weights> weights1,
         LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size,
         Dimension dimension,
         MeritFilterList<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> filters,
         size_t nrand
         ) const
   {
      Parser::FigureOfMerit<LatticeType::ORDINARY>::parse(
	    "2",
            fig1,
            1,
            std::move(weights1),
            *this,
            std::move(fig2),
            std::move(size),
            dimension,
            std::move(filters),
            nrand
            );
   }

   template <class FIG1, class FIG2>
   void operator()(
         FIG1 fig1,
         FIG2 fig2,
         LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size,
         Dimension dimension,
         MeritFilterList<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> filters,
         size_t nrand
         ) const
   {
      Storage<LatticeType::ORDINARY, EmbeddingType::UNILEVEL, fig1.suggestedCompression()> storage1(size);
      Storage<LatticeType::ORDINARY, EmbeddingType::UNILEVEL, fig2.suggestedCompression()> storage2(size);

      typedef GenSeq::CoprimeIntegers<fig1.suggestedCompression(), Traversal::Random<LFSR258>> Coprime;
      auto genSeqs = GenSeq::VectorCreator<Coprime>::create(size, dimension, nrand);
      genSeqs[0] = GenSeq::Creator<Coprime>::create(SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>(2));

      auto latSeq = LatSeq::combine<Zip>(size, std::move(genSeqs));

      auto lsoc1 = MeritSeq::latSeqOverCBC(MeritSeq::cbc(storage1, fig1));
      auto lsoc2 = MeritSeq::latSeqOverCBC(MeritSeq::cbc(storage2, fig2));

      auto fseq1 = filters.apply(lsoc1.meritSeq(latSeq));
      auto fseq2 = filters.apply(lsoc2.meritSeq(latSeq));

      printTableRow("gen", fig1.name(), fig2.name(),
            "rel" + fig1.name() + "m" + fig2.name());

      auto it1 = fseq1.begin();
      auto it2 = fseq2.begin();
      while (it1 != fseq1.end()) {
         printTableRow(
               it1.base().base()->gen(),
               *it1,
               *it2,
               (*it1 - *it2) / *it1
               );
         ++it1;
         ++it2;
      }

   }
};

int main(int argc, const char *argv[])
{
   if (argc < 7 + 1) {
      std::cerr << "usage: correlation <nrand> <size> <dimension> <figure1> <weights1> <figure2> <weights2> [<filter> [...]]" << std::endl;
      return 1;
   }

   int iarg = 1;
   auto nrand = boost::lexical_cast<size_t>(argv[iarg++]);
   auto size = Parser::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>::parse(argv[iarg++]);
   auto dimension = boost::lexical_cast<Dimension>(argv[iarg++]);
   std::string figSpec1 = argv[iarg++];
   std::string weightsSpec1 = argv[iarg++];
   std::string figSpec2 = argv[iarg++];
   std::string weightsSpec2 = argv[iarg++];
   std::vector<std::string> filtersSpec(&argv[iarg], &argv[argc]);

   auto weights1 = Parser::Weights::parse(weightsSpec1);
   auto weights2 = Parser::Weights::parse(weightsSpec2);

   auto filters = Parser::MeritFilterList<LatticeType::ORDINARY>::parse("", filtersSpec, size, *weights1, 2);

   Parser::FigureOfMerit<LatticeType::ORDINARY>::parse(
         "2",
         figSpec2,
         1,
         std::move(weights2),
         Execute(),
         figSpec1,
         std::move(weights1),
         std::move(size),
         dimension,
         std::move(filters),
         nrand
         );

   return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <scitbx/matrix/transpose_multiply.h>
#include <scitbx/math/utils.h>
#include <boost/python/make_constructor.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/format.hpp>
#include "flex_helpers.h"

namespace scitbx { namespace serialization { namespace single_buffered {

  inline
  char* to_string(char* start, vec3<double> const& value)
  {
    return
      to_string(to_string(to_string(start, value[0]), value[1]), value[2]);
  }

  template <>
  struct from_string<vec3<double> >
  {
    from_string(const char* start)
    {
      end = start;
      for(std::size_t i=0;i<3;i++) {
        from_string<double> proxy(end);
        value[i] = proxy.value;
        end = proxy.end;
      }
    }

    vec3<double> value;
    const char* end;
  };

}}} // namespace scitbx::serialization::single_buffered

#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>

namespace scitbx { namespace af {
namespace {

  flex<vec3<double> >::type*
  join(
    af::const_ref<double> const& x,
    af::const_ref<double> const& y,
    af::const_ref<double> const& z)
  {
    SCITBX_ASSERT(y.size() == x.size());
    SCITBX_ASSERT(z.size() == x.size());
    af::shared<vec3<double> > result((af::reserve(x.size())));
    for(std::size_t i=0;i<x.size();i++) {
      result.push_back(vec3<double>(x[i],y[i],z[i]));
    }
    return new flex<vec3<double> >::type(result, result.size());
  }

  flex<vec3<double> >::type*
  from_double(
    af::const_ref<double> const& x)
  {
    SCITBX_ASSERT(x.size() % 3 == 0);
    std::size_t result_size = x.size() / 3;
    af::shared<vec3<double> > result((af::reserve(result_size)));
    const double* d = x.begin();
    for(std::size_t i=0;i<result_size;i++) {
      result.push_back(vec3<double>(d));
      d += 3;
    }
    return new flex<vec3<double> >::type(result, result.size());
  }

  boost::python::tuple
  part_names()
  {
    return boost::python::make_tuple("x", "y", "z");
  }

  boost::python::tuple
  parts(
    versa<vec3<double>, flex_grid<> > const& O)
  {
    tiny<versa<double, flex_grid<> >, 3> result;
    std::size_t n = O.size();
    for(std::size_t i=0;i<3;i++) {
      result[i].resize(O.accessor());
      for(std::size_t j=0;j<n;j++) {
        result[i][j] = O[j][i];
      }
    }
    return boost::python::make_tuple(result[0], result[1], result[2]);
  }

  af::shared<vec3<double> >
  rotate_around_origin(
    flex<vec3<double> >::type const& a,
    vec3<double> const& direction,
    double const& angle)
  {
    SCITBX_ASSERT(direction.length() > 0)(direction.length());
    vec3<double> unit = direction.normalize();
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i].unit_rotate_around_origin(
        unit, angle));
    }
    return result;
  }

  af::shared<vec3<double> >
  rotate_around_origin(
    flex<vec3<double> >::type const& a,
    vec3<double> const& direction,
    flex<double>::type const& angles)
  {
    SCITBX_ASSERT(direction.length() > 0)(direction.length());
    vec3<double> unit = direction.normalize();
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i].unit_rotate_around_origin(
        unit, angles[i]));
    }
    return result;
  }

  af::shared<vec3<double> >
  rotate_around_origin(
    flex<vec3<double> >::type const& a,
    flex<vec3<double> >::type const& directions,
    flex<double>::type const& angles)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    SCITBX_ASSERT(directions.size() == a.size());
    SCITBX_ASSERT(angles.size() == a.size());
    for(std::size_t i=0;i<a.size();i++) {
      SCITBX_ASSERT(directions[i].length() > 0)(directions[i].length());
      vec3<double> unit = directions[i].normalize();
      result.push_back(a[i].unit_rotate_around_origin(
        unit, angles[i]));
    }
    return result;
  }

  af::shared<double> angle(
      af::const_ref< vec3<double> > const& self,
      vec3<double> other, bool deg) {
    af::shared<double> result(self.size());
    for (std::size_t i = 0; i < self.size(); ++i) {
      boost::optional<double> oa = self[i].angle_rad(other);
      if (oa) {
        double a = *oa;
        if (deg) {
          a = rad_as_deg(a);
        }
        result[i] = a;
      } else {
        result[i] = 0.0;
      }
    }
    return result;
  }

  af::shared<double> angle(
      af::const_ref< vec3<double> > const& self,
      af::const_ref< vec3<double> > const& other,
      bool deg) {
    SCITBX_ASSERT(self.size() == other.size());
    af::shared<double> result(self.size());
    for (std::size_t i = 0; i < self.size(); ++i) {
      boost::optional<double> oa = self[i].angle_rad(other[i]);
      if (oa) {
        double a = *oa;
        if (deg) {
          a = rad_as_deg(a);
        }
        result[i] = a;
      } else {
        result[i] = 0.0;
      }
    }
    return result;
  }

  flex_double
  as_double(flex<vec3<double> >::type const& a)
  {
    SCITBX_ASSERT(a.accessor().is_trivial_1d());
    flex_double result(a.size()*3, init_functor_null<double>());
    double* r = result.begin();
    const_ref<vec3<double> > a_ref = a.const_ref().as_1d();
    for(std::size_t i=0;i<a_ref.size();i++) {
      for(std::size_t j=0;j<3;j++) {
        *r++ = a_ref[i][j];
      }
    }
    return result;
  }

  vec3<double>
  vec3_min(flex<vec3<double> >::type const& a)
  {
    SCITBX_ASSERT(!a.accessor().is_padded());
    vec3<double> result(0,0,0);
    af::const_ref<vec3<double>, af::flex_grid<> > a_ref = a.const_ref();
    if (a_ref.size() > 0) {
      result = a_ref[0];
      for(std::size_t i=1;i<a_ref.size();i++) {
        result.each_update_min(a_ref[i]);
      }
    }
    return result;
  }

  vec3<double>
  vec3_max(flex<vec3<double> >::type const& a)
  {
    SCITBX_ASSERT(!a.accessor().is_padded());
    vec3<double> result(0,0,0);
    af::const_ref<vec3<double>, af::flex_grid<> > a_ref = a.const_ref();
    if (a_ref.size() > 0) {
      result = a_ref[0];
      for(std::size_t i=1;i<a_ref.size();i++) {
        result.each_update_max(a_ref[i]);
      }
    }
    return result;
  }

  af::shared<vec3<double> >
  round(
    af::const_ref<vec3<double> > const& a,
    int n_digits)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for (std::size_t i=0; i<a.size(); i++) {
      vec3<double> const& src = a[i];
      vec3<double> result_i;
      for (std::size_t j=0; j<3; j++) {
        result_i[j] = math::round(src[j], n_digits);
      }
      result.push_back(result_i);
    }
    return result;
  }

  af::shared<vec3<int> >
  iround(
    af::const_ref<vec3<double> > const& a)
  {
    af::shared<vec3<int> > result((af::reserve(a.size())));
    for (std::size_t i=0; i<a.size(); i++) {
      vec3<double> const& src = a[i];
      vec3<int> result_i;
      for (std::size_t j=0; j<3; j++) {
        result_i[j] = math::iround(src[j]);
      }
      result.push_back(result_i);
    }
    return result;
  }

  vec3<double>
  mean_weighted_a_a(
    af::const_ref<vec3<double> > const& self,
    af::const_ref<double> const& weights)
  {
    return af::mean_weighted(self, weights);
  }

  af::shared<vec3<double> >
  mul_a_scalar(
    af::const_ref<vec3<double> > const& a,
    double f)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * f);
    }
    return result;
  }

  af::shared<vec3<double> >
  mul_a_a_scalar(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<double> const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec3<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs[i]);
    }
    return result;
  }

  void
  imul_a_scalar(
    af::ref<vec3<double> > const& a,
    double f)
  {
    for(std::size_t i=0;i<a.size();i++) a[i] *= f;
  }

  af::shared<vec3<double> >
  div_a_as(
    af::ref<vec3<double> > const& lhs,
    af::ref<double> const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec3<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      SCITBX_ASSERT(rhs[i] != 0);
      result.push_back(lhs[i] / rhs[i]);
    }
    return result;
  }

  af::shared<vec3<double> >
  mul_a_mat3(
    af::const_ref<vec3<double> > const& a,
    mat3<double> const& m)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * m);
    }
    return result;
  }

  af::shared<vec3<double> >
  rmul_a_mat3(
    af::const_ref<vec3<double> > const& a,
    mat3<double> const& m)
  {
    mat3<double> m_transposed = m.transpose();
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * m_transposed);
    }
    return result;
  }

  af::shared<double>
  dot_a_a(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs[i]);
    }
    return result;
  }

  af::shared<double>
  dot_a_s(
    af::const_ref<vec3<double> > const& lhs,
    vec3<double> rhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs);
    }
    return result;
  }

  af::shared<double>
  dot_a(
    af::const_ref<vec3<double> > const& lhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * lhs[i]);
    }
    return result;
  }

  af::shared<vec3<double> >
  cross_a_a(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec3<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i].cross(rhs[i]));
    }
    return result;
  }

  af::shared<double>
  norms_(
    af::const_ref<vec3<double> > const& lhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(std::sqrt(lhs[i] * lhs[i]));
    }
    return result;
  }

  double
  sum_sq_(
    af::const_ref<vec3<double> > const& self)
  {
    double result = 0;
    for(std::size_t i=0;i<self.size();i++) {
      result += self[i] * self[i];
    }
    return result;
  }

  double
  norm_(
    af::const_ref<vec3<double> > const& self)
  {
    return std::sqrt(sum_sq_(self));
  }

  af::shared<vec3<double> >
  each_normalize(
    af::const_ref<vec3<double> > const& a,
    bool raise_if_length_zero=true)
  {
    af::shared<vec3<double> > result(a.begin(), a.end());
    vec3<double>* r = result.begin();
    std::size_t n_zero = 0;
    for(std::size_t i=0;i<a.size();i++) {
      double length = r[i].length();
      if (length == 0) n_zero++;
      else r[i] *= (1 / length);
    }
    if (n_zero != 0 && raise_if_length_zero) {
      throw std::runtime_error((boost::format(
        "flex.vec3_double.each_normalize():"
        " number of vectors with length zero: %lu of %lu")
          % n_zero % a.size()).str());
    }
    return result;
  }

  double
  min_distance_between_any_pair(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    if (lhs.size() == 0) return 0;
    if (rhs.size() == 0) return 0;
    double min_length_sq = (lhs[0]-rhs[0]).length_sq();
    for(std::size_t i=0;i<lhs.size();i++) {
      for(std::size_t j=0;j<rhs.size();j++) {
        math::update_min(min_length_sq, (lhs[i]-rhs[j]).length_sq());
      }
    }
    return std::sqrt(min_length_sq);
  }

  boost::python::tuple
  min_distance_between_any_pair_with_id(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    if (lhs.size() == 0) return boost::python::make_tuple(0, 0, 0);
    if (rhs.size() == 0) return boost::python::make_tuple(0, 0, 0);
    double min_length_sq = (lhs[0]-rhs[0]).length_sq();
    double working_length_sq = 0;
    int best_i=0;
    int best_j=0;
    for(std::size_t i=0;i<lhs.size();i++) {
      for(std::size_t j=0;j<rhs.size();j++) {
        working_length_sq=(lhs[i]-rhs[j]).length_sq();
        if (working_length_sq < min_length_sq) {
          best_i=i;
          best_j=j;
          min_length_sq=working_length_sq;
        }
      }
    }
    return boost::python::make_tuple(std::sqrt(min_length_sq), best_i, best_j);
  }

  double
  max_distance(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    if (lhs.size() == 0) return 0;
    double max_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      math::update_max(max_length_sq, (lhs[i]-rhs[i]).length_sq());
    }
    return std::sqrt(max_length_sq);
  }

  double
  rms_difference(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    if (lhs.size() == 0) return 0;
    double sum_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      sum_length_sq += (lhs[i]-rhs[i]).length_sq();
    }
    return std::sqrt(sum_length_sq / lhs.size());
  }

  double
  rms_length(
    af::const_ref<vec3<double> > const& lhs)
  {
    if (lhs.size() == 0) return 0;
    double sum_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      sum_length_sq += lhs[i].length_sq();
    }
    return std::sqrt(sum_length_sq / lhs.size());
  }

} // namespace <anonymous>

namespace boost_python {

  template <>
  struct flex_default_element<vec3<double> >
  {
    static vec3<double>
    get() { return vec3<double>(0,0,0); }
  };

  void wrap_flex_vec3_double()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef flex_wrapper<vec3<double> > f_w;
    f_w::plain("vec3_double")
      .def_pickle(flex_pickle_single_buffered<vec3<double>,
        3*pickle_size_per_element<double>::value>())
      .def("__init__", make_constructor(join))
      .def("__init__", make_constructor(from_double))
      .def("part_names", part_names)
      .staticmethod("part_names")
      .def("parts", parts)
      .def("rotate_around_origin",
        (af::shared<vec3<double> >(*)(
          flex<vec3<double> >::type const&,
          vec3<double> const&,
          double const&)) rotate_around_origin)
      .def("rotate_around_origin",
        (af::shared<vec3<double> >(*)(
          flex<vec3<double> >::type const&,
          vec3<double> const&,
          flex<double>::type const&)) rotate_around_origin)
      .def("rotate_around_origin",
        (af::shared<vec3<double> >(*)(
          flex<vec3<double> >::type const&,
          flex<vec3<double> >::type const&,
          flex<double>::type const&)) rotate_around_origin)
      .def("angle",
        (af::shared<double>(*)(
          af::const_ref< vec3<double> > const&,
          vec3<double>, bool)) &angle, (
            arg("other"),
            arg("deg") = false))
      .def("angle",
        (af::shared<double>(*)(
          af::const_ref< vec3<double> > const&,
          af::const_ref< vec3<double> > const&,
          bool)) &angle, (
            arg("other"),
            arg("deg") = false))
      .def("as_double", as_double)
      .def("add_selected",
        (object(*)(
          object const&,
          af::const_ref<std::size_t> const&,
          af::const_ref<vec3<double> > const&)) add_selected_unsigned_a, (
            arg("indices"), arg("values")))
      .def("min", vec3_min)
      .def("max", vec3_max)
      .def("sum", f_w::sum_a)
      .def("mean", f_w::mean_a)
      .def("mean_weighted", mean_weighted_a_a, (arg("weights")))
      .def("__add__", f_w::add_a_s)
      .def("__add__", f_w::add_a_a)
      .def("__iadd__", f_w::iadd_a_s)
      .def("__iadd__", f_w::iadd_a_a)
      .def("__sub__", f_w::sub_a_s)
      .def("__sub__", f_w::sub_a_a)
      .def("__isub__", f_w::isub_a_s)
      .def("__mul__", mul_a_scalar)
      .def("__rmul__", mul_a_scalar)
      .def("__mul__", mul_a_a_scalar)
      .def("__rmul__", mul_a_a_scalar)
      .def("__imul__", imul_a_scalar, return_self<>())
      .def("__div__", div_a_as)
      .def("__truediv__", div_a_as)
      .def("__mul__", mul_a_mat3)
      .def("__rmul__", rmul_a_mat3)
      .def("round", round)
      .def("iround", iround)
      .def("dot", dot_a_s)
      .def("dot", dot_a_a)
      .def("dot", dot_a)
      .def("cross", cross_a_a)
      .def("norms", norms_)
      .def("transpose_multiply",
        (mat3<double>(*)(
          af::const_ref<vec3<double> > const&,
          af::const_ref<vec3<double> > const&)) matrix::transpose_multiply)
      .def("sum_sq", sum_sq_)
      .def("norm", norm_)
      .def("each_normalize", each_normalize, (
        arg("raise_if_length_zero")=true))
      .def("min_distance_between_any_pair", min_distance_between_any_pair)
      .def("min_distance_between_any_pair_with_id",
            min_distance_between_any_pair_with_id)
      .def("max_distance", max_distance)
      .def("rms_difference", rms_difference)
      .def("rms_length", rms_length)
    ;
  }

}}} // namespace scitbx::af::boost_python