//
//  cvx_pose_estimation.cpp
//  LoopClosure
//
//  Created by jimmy on 2016-03-31.
//  Copyright © 2016 jimmy. All rights reserved.
//

#include "cvx_pose_estimation.hpp"
#include <iostream>
#include <Eigen/Geometry>
#include "cvx_calib3d.hpp"

using std::cout;
using std::endl;
using cv::Mat;

struct HypotheseLoss
{
    double loss_;
    Mat rvec_;       // rotation     for 3D --> 2D projection
    Mat tvec_;       // translation  for 3D --> 2D projection
    Mat affine_;     //              for 3D --> 3D camera to world transformation
    vector<unsigned int> inlier_indices_;         // camera coordinate index
    vector<unsigned int> inlier_candidate_world_pts_indices_; // candidate world point index

    // store all inliers from preemptive ransac
    vector<cv::Point3d> camera_pts_;
    vector<cv::Point3d> wld_pts_;

    HypotheseLoss()
    {
        loss_ = INT_MAX;
    }
    HypotheseLoss(const double loss)
    {
        loss_  = loss;
    }

    HypotheseLoss(const HypotheseLoss & other)
    {
        loss_ = other.loss_;
        rvec_ = other.rvec_;
        tvec_ = other.tvec_;
        affine_ = other.affine_;
        inlier_indices_.clear();
        inlier_indices_.resize(other.inlier_indices_.size());
        inlier_candidate_world_pts_indices_.clear();
        inlier_candidate_world_pts_indices_.resize(other.inlier_candidate_world_pts_indices_.size());
        for(int i = 0; i<other.inlier_indices_.size(); i++) {
            inlier_indices_[i] = other.inlier_indices_[i];
        }
        for(int i = 0; i<other.inlier_candidate_world_pts_indices_.size(); i++){
            inlier_candidate_world_pts_indices_[i] = other.inlier_candidate_world_pts_indices_[i];
        }
        if(inlier_candidate_world_pts_indices_.size() != 0){
            assert(inlier_indices_.size() == inlier_candidate_world_pts_indices_.size());
        }

        // copy camera points and world coordinate points
        if(other.camera_pts_.size() > 0)
        {
            camera_pts_.resize(other.camera_pts_.size());
            wld_pts_.resize(other.wld_pts_.size());

            assert(camera_pts_.size() == wld_pts_.size());
            for(int i = 0; i<camera_pts_.size(); i++)
            {
                camera_pts_[i] = other.camera_pts_[i];
                wld_pts_[i] = other.wld_pts_[i];
            }
        }
    }

    bool operator < (const HypotheseLoss & other) const
    {
        return loss_ < other.loss_;
    }

    HypotheseLoss & operator = (const HypotheseLoss & other)
    {
        if (&other == this) {
            return *this;
        }
        loss_ = other.loss_;
        rvec_ = other.rvec_;
        tvec_ = other.tvec_;
        affine_ = other.affine_;
        inlier_indices_.clear();
        inlier_indices_.resize(other.inlier_indices_.size());
        inlier_candidate_world_pts_indices_.clear();
        inlier_candidate_world_pts_indices_.resize(other.inlier_candidate_world_pts_indices_.size());
        for(int i = 0; i<other.inlier_indices_.size(); i++) {
            inlier_indices_[i] = other.inlier_indices_[i];
        }
        for(int i = 0; i<other.inlier_candidate_world_pts_indices_.size(); i++){
            inlier_candidate_world_pts_indices_[i] = other.inlier_candidate_world_pts_indices_[i];
        }
        if(inlier_candidate_world_pts_indices_.size() != 0){
            assert(inlier_indices_.size() == inlier_candidate_world_pts_indices_.size());
        }

        // copy camera points and world coordinate points
        if(other.camera_pts_.size() > 0)
        {
            camera_pts_.resize(other.camera_pts_.size());
            wld_pts_.resize(other.wld_pts_.size());

            assert(camera_pts_.size() == wld_pts_.size());
            for(int i = 0; i<camera_pts_.size(); i++)
            {
                camera_pts_[i] = other.camera_pts_[i];
                wld_pts_[i] = other.wld_pts_[i];
            }
        }

        return *this;
    }
};

bool CvxPoseEstimation::preemptiveRANSAC3DOneToMany(const vector<cv::Point3d> & camera_pts,
                                                    const vector<vector<cv::Point3d>> & candidate_wld_pts,
                                                    const PreemptiveRANSAC3DParameter & param,
                                                    cv::Mat & camera_pose)
{
    assert(camera_pts.size() == candidate_wld_pts.size());
    if (camera_pts.size() < 500) {
        return false;
    }

    const int num_iteration = 2048;
    int K = 1024;
    const int N = (int)camera_pts.size();
    const int B = param.sample_number_;

    vector<cv::Mat > affine_candidate;
    for (int i = 0; i<num_iteration; i++) {

        int k1 = 0;
        int k2 = 0;
        int k3 = 0;
        int k4 = 0;

        do{
            k1 = rand()%N;
            k2 = rand()%N;
            k3 = rand()%N;
            k4 = rand()%N;
        }while (k1 == k2 || k1 == k3 || k1 == k4 ||
                k2 == k3 || k2 == k4 || k3 == k4);

        vector<cv::Point3d> sampled_camera_pts;
        vector<cv::Point3d> sampled_wld_pts;

        sampled_camera_pts.push_back(camera_pts[k1]);
        sampled_camera_pts.push_back(camera_pts[k2]);
        sampled_camera_pts.push_back(camera_pts[k3]);
        sampled_camera_pts.push_back(camera_pts[k4]);

        sampled_wld_pts.push_back(candidate_wld_pts[k1][0]);
        sampled_wld_pts.push_back(candidate_wld_pts[k2][0]);
        sampled_wld_pts.push_back(candidate_wld_pts[k3][0]);
        sampled_wld_pts.push_back(candidate_wld_pts[k4][0]);

        Mat affine;
        CvxCalib3D::KabschTransform(sampled_camera_pts, sampled_wld_pts, affine);
        affine_candidate.push_back(affine);
        if (affine_candidate.size() > K) {
            printf("initialization repeat %d times\n", i);
            break;
        }
    }
    printf("init camera parameter number is %lu\n", affine_candidate.size());

    vector<HypotheseLoss> losses;
    for (int i = 0; i<affine_candidate.size(); i++) {
        HypotheseLoss hyp(0.0);
        hyp.affine_ = affine_candidate[i];
        losses.push_back(hyp);
    }

    double threshold = param.dis_threshold_;
    while (losses.size() > 1) {
        // sample random set
        vector<cv::Point3d> sampled_camera_pts;
        vector< vector<cv::Point3d> > sampled_wld_pts;  // one camera point may have multiple world points correspondences
        vector<int> sampled_indices;
        for (int i =0; i<B; i++) {
            int index = rand()%N;
            sampled_camera_pts.push_back(camera_pts[index]);
            sampled_wld_pts.push_back(candidate_wld_pts[index]);
            sampled_indices.push_back(index);
        }

        // count outliers
        for (int i = 0; i<losses.size(); i++) {
            // evaluate the accuracy by check transformation
            vector<cv::Point3d> transformed_pts;
            CvxCalib3D::rigidTransform(sampled_camera_pts, losses[i].affine_, transformed_pts);

            // check minimum distance from transformed points to world coordiante
            for (int j = 0; j<transformed_pts.size(); j++) {
                double min_dis = threshold * 2;
                int min_index = -1;
                for (int k = 0; k<sampled_wld_pts[j].size(); k++) {
                    cv::Point3d dif = transformed_pts[j] - sampled_wld_pts[j][k];
                    double dis = cv::norm(dif);
                    if (dis < min_dis) {
                        min_dis = dis;
                        min_index = k;
                    }
                } // end of k

                if (min_dis > threshold) {
                    losses[i].loss_ += 1.0;
                }
                else {
                    losses[i].inlier_indices_.push_back(sampled_indices[j]);
                    losses[i].inlier_candidate_world_pts_indices_.push_back(min_index);
                }
            } // end of j
            assert(losses[i].inlier_indices_.size() == losses[i].inlier_candidate_world_pts_indices_.size());
            // printf("inlier number is %lu\n", losses[i].inlier_indices_.size());
        }
        //getchar();

        std::sort(losses.begin(), losses.end());
        losses.resize(losses.size()/2);

        for (int i = 0; i<losses.size(); i++) {
         //   printf("after: loss is %lf\n", losses[i].loss_);
         //   printf("inlier number is %lu\n", losses[i].inlier_indices_.size());
        }
        // printf("\n\n");

        // refine by inliers
        for (int i = 0; i<losses.size(); i++) {
            // number of inliers is larger than minimum configure
            if (losses[i].inlier_indices_.size() > 4) {
                vector<cv::Point3d> inlier_camera_pts;
                vector<cv::Point3d> inlier_wld_pts;
                for (int j = 0; j < losses[i].inlier_indices_.size(); j++) {
                    int index = losses[i].inlier_indices_[j];
                    int wld_index = losses[i].inlier_candidate_world_pts_indices_[j];
                    inlier_camera_pts.push_back(camera_pts[index]);
                    inlier_wld_pts.push_back(candidate_wld_pts[index][wld_index]);
                }
                Mat affine;

                CvxCalib3D::KabschTransform(inlier_camera_pts, inlier_wld_pts, affine);
                losses[i].affine_ = affine;
                losses[i].inlier_indices_.clear();
                losses[i].inlier_candidate_world_pts_indices_.clear();
            }
        }
    }
    assert(losses.size() == 1);

    camera_pose = cv::Mat::eye(4, 4, CV_64F);
    losses[0].affine_.copyTo(camera_pose(cv::Rect(0, 0, 4, 3)));
    //cout<<"camera pose\n"<<camera_pose<<endl;
    return true;
}

Mat CvxPoseEstimation::rotationToEularAngle(const cv::Mat & rot)
{
    assert(rot.rows == 3 && rot.cols == 3);
    assert(rot.type() == CV_64FC1);

    // https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf
    double m00 = rot.at<double>(0, 0);
    double m01 = rot.at<double>(0, 1);
    double m02 = rot.at<double>(0, 2);
    double m10 = rot.at<double>(1, 0);
    double m11 = rot.at<double>(1, 1);
    double m12 = rot.at<double>(1, 2);
    double m20 = rot.at<double>(2, 0);
    double m21 = rot.at<double>(2, 1);
    double m22 = rot.at<double>(2, 2);
    double theta1 = atan2(m12, m22);
    double c2 = sqrt(m00 * m00 + m01 * m01);
    double theta2 = atan2(-m02, c2);
    double s1 = sin(theta1);
    double c1 = cos(theta1);
    double theta3 = atan2(s1*m20 - c1 * m10, c1*m11 - s1*m21);

    double scale = 180.0/3.14159;
    theta1 *= scale;
    theta2 *= scale;
    theta3 *= scale;

    Mat eular_angle = cv::Mat::zeros(3, 1, CV_64FC1);
    eular_angle.at<double>(0, 0) = theta1;
    eular_angle.at<double>(1, 0) = theta2;
    eular_angle.at<double>(2, 0) = theta3;
    return eular_angle;
    //printf("Eular angle %lf %lf %lf\n", theta1, theta2, theta3);
}

void CvxPoseEstimation::poseDistance(const cv::Mat & src_pose,
                                     const cv::Mat & dst_pose,
                                     double & angle_distance,
                                     double & euclidean_disance)
{
    // http://chrischoy.github.io/research/measuring-rotation/
    assert(src_pose.type() == CV_64F);
    assert(dst_pose.type() == CV_64F);

    Mat src_R = src_pose(cv::Rect(0, 0, 3, 3));
    Mat dst_R = dst_pose(cv::Rect(0, 0, 3, 3));

    double scale = 180.0/3.14159;

    Mat q1 = CvxPoseEstimation::rotationToQuaternion(src_R);
    Mat q2 = CvxPoseEstimation::rotationToQuaternion(dst_R);
    double val_dot = fabs(q1.dot(q2));

    //double dot = r1.dot(r2);
    //angle_distance = acos(dot) * scale;
    angle_distance = 2.0 * acos(val_dot) * scale;

    euclidean_disance = 0.0;
    double dx = src_pose.at<double>(0, 3) - dst_pose.at<double>(0, 3);
    double dy = src_pose.at<double>(1, 3) - dst_pose.at<double>(1, 3);
    double dz = src_pose.at<double>(2, 3) - dst_pose.at<double>(2, 3);
    euclidean_disance += dx * dx;
    euclidean_disance += dy * dy;
    euclidean_disance += dz * dz;
    euclidean_disance = sqrt(euclidean_disance);
 //   printf("location distance are %f %f %f\n", dx, dy, dz);
}

Mat CvxPoseEstimation::rotationToQuaternion(const cv::Mat & rot)
{
    assert(rot.type() == CV_64FC1);
    assert(rot.rows == 3 && rot.cols == 3);

    Mat ret = cv::Mat::zeros(4, 1, CV_64FC1);

    float r11 = rot.at<double>(0, 0);
    float r12 = rot.at<double>(0, 1);
    float r13 = rot.at<double>(0, 2);
    float r21 = rot.at<double>(1, 0);
    float r22 = rot.at<double>(1, 1);
    float r23 = rot.at<double>(1, 2);
    float r31 = rot.at<double>(2, 0);
    float r32 = rot.at<double>(2, 1);
    float r33 = rot.at<double>(2, 2);

    float q0 = ( r11 + r22 + r33 + 1.0f) / 4.0f;
    float q1 = ( r11 - r22 - r33 + 1.0f) / 4.0f;
    float q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
    float q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
    if(q0 < 0.0f) q0 = 0.0f;
    if(q1 < 0.0f) q1 = 0.0f;
    if(q2 < 0.0f) q2 = 0.0f;
    if(q3 < 0.0f) q3 = 0.0f;
    q0 = sqrt(q0);
    q1 = sqrt(q1);
    q2 = sqrt(q2);
    q3 = sqrt(q3);
    if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
        q0 *= +1.0f;
        q1 *= CvxPoseEstimation::SIGN(r32 - r23);
        q2 *= CvxPoseEstimation::SIGN(r13 - r31);
        q3 *= CvxPoseEstimation::SIGN(r21 - r12);
    } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
        q0 *= CvxPoseEstimation::SIGN(r32 - r23);
        q1 *= +1.0f;
        q2 *= CvxPoseEstimation::SIGN(r21 + r12);
        q3 *= CvxPoseEstimation::SIGN(r13 + r31);
    } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
        q0 *= CvxPoseEstimation::SIGN(r13 - r31);
        q1 *= CvxPoseEstimation::SIGN(r21 + r12);
        q2 *= +1.0f;
        q3 *= CvxPoseEstimation::SIGN(r32 + r23);
    } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
        q0 *= CvxPoseEstimation::SIGN(r21 - r12);
        q1 *= CvxPoseEstimation::SIGN(r31 + r13);
        q2 *= CvxPoseEstimation::SIGN(r32 + r23);
        q3 *= +1.0f;
    } else {
        printf("q0, q1, q2, q3: %f %f %f %f\n", q0, q1, q2, q3);
        printf("Error: rotation matrix quaternion.\n");
        cout<<"rotation matrix is \n"<<rot<<endl;
        assert(0);
    }
    float r = CvxPoseEstimation::NORM(q0, q1, q2, q3);
    q0 /= r;
    q1 /= r;
    q2 /= r;
    q3 /= r;

    ret.at<double>(0, 0) = q0;
    ret.at<double>(1, 0) = q1;
    ret.at<double>(2, 0) = q2;
    ret.at<double>(3, 0) = q3;
    return ret;
}

Mat CvxPoseEstimation::quaternionToRotation(const cv::Mat & q)
{
    assert(q.type() == CV_64FC1);
    assert(q.rows == 4);
    assert(q.cols == 1);

    double x = q.at<double>(0, 0);
    double y = q.at<double>(1, 0);
    double z = q.at<double>(2, 0);
    double w = q.at<double>(3, 0);

    Eigen::Quaterniond quat(w, x, y, z);

    Eigen::Matrix<double, 3, 3> eig_mat = quat.matrix();
    cv::Mat rot = cv::Mat::zeros(3, 3, CV_64FC1);
    for (int r = 0; r<3; r++) {
        for (int c = 0; c<3; c++) {
            rot.at<double>(r, c) = eig_mat(r, c);
        }
    }
    return rot;
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-arithmetic
    This function object returns  its parameter decremented by 1.

    @par Header <boost/simd/function/dec.hpp>

    @par Decorators

    - saturated_ ensures that @c saturated_(dec)(x) will never be
      strictly greater than @c x,
      avoiding the wrap around from @ref Valmin to
      @ref Valmax with integer types

    @see inc, minus

    @par Example:

      @snippet dec.cpp dec

    @par Possible output:

      @snippet dec.txt dec
  **/
  Value dec(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/dec.hpp>
#include <boost/simd/function/simd/dec.hpp>

#endif


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:	 NonDGlobalReliability
//- Description: Implementation code for NonDGlobalReliability class
//- Owner:       Barron Bichon, Mike Eldred
//- Checked by:
//- Version:

#include "dakota_system_defs.hpp"
#include "dakota_data_io.hpp"
#include "NonDGlobalReliability.hpp"
#include "NonDLHSSampling.hpp"
//#include "DDACEDesignCompExp.hpp"
//#include "FSUDesignCompExp.hpp"
#include "NonDAdaptImpSampling.hpp"
//#ifdef HAVE_ACRO
//#include "COLINOptimizer.hpp"
//#endif
#include "RecastModel.hpp"
#include "DataFitSurrModel.hpp"
#include "DakotaApproximation.hpp"
#include "ProblemDescDB.hpp"
#ifdef HAVE_NCSU
#include "NCSUOptimizer.hpp"
#endif
#include "pecos_stat_util.hpp"
#include <boost/lexical_cast.hpp>

//#define DEBUG
//#define DEGUG_PLOTS

static const char rcsId[] = "@(#) $Id: NonDGlobalReliability.cpp 4058 2006-10-25 01:39:40Z mseldre $";

extern "C" {
#ifdef DAKOTA_F90
  #if defined(HAVE_CONFIG_H) && !defined(DISABLE_DAKOTA_CONFIG_H)

  // Deprecated; continue to support legacy, clashing macros for ONE RELEASE
  #define BVLS_WRAPPER_FC FC_FUNC_(bvls_wrapper,BVLS_WRAPPER)
  void BVLS_WRAPPER_FC( Dakota::Real* a, int& m, int& n, Dakota::Real* b,
		        Dakota::Real* bnd, Dakota::Real* x, Dakota::Real& rnorm,
		        int& nsetp, Dakota::Real* w, int* index, int& ierr );
  #else

  // Use the CMake-generated fortran name mangling macros (eliminate warnings)
  #include "dak_f90_config.h"
  #define BVLS_WRAPPER_FC DAK_F90_GLOBAL_(bvls_wrapper,BVLS_WRAPPER)
  void BVLS_WRAPPER_FC( Dakota::Real* a, int& m, int& n, Dakota::Real* b,
		        Dakota::Real* bnd, Dakota::Real* x, Dakota::Real& rnorm,
		        int& nsetp, Dakota::Real* w, int* index, int& ierr );
  #endif // HAVE_CONFIG_H and !DISABLE_DAKOTA_CONFIG_H
#endif // DAKOTA_F90
}

namespace Dakota {

// initialization of statics
NonDGlobalReliability* NonDGlobalReliability::nondGlobRelInstance(NULL);

NonDGlobalReliability::
NonDGlobalReliability(ProblemDescDB& problem_db, Model& model):
  NonDReliability(problem_db, model),
  meritFunctionType(AUGMENTED_LAGRANGIAN_MERIT), dataOrder(1)
{
  if (mppSearchType < EGRA_X) {
    Cerr << "Error: only x-space and u-space EGRA are currently supported in "
	 << "global_reliability."<< std::endl;
    abort_handler(-1);
  }

  // standard reliability indices are not defined and should be precluded
  // via the input spec.  requestedRelLevels is default sized in NonD and
  // cannot be used for the empty() test below.
  if (!probDescDB.get_rva("method.nond.reliability_levels").empty() ||
      respLevelTarget == RELIABILITIES) {
    Cerr << "Error: reliability indices are not defined for global reliability "
	 << "methods.  Use generalized reliability instead." << std::endl;
    abort_handler(-1);
  }

  // requestedProbLevels & requestedGenRelLevels are not yet supported
  // since PMA EGRA requires additional R&D.  Note: requestedProbLevels and
  // requestedGenRelLevels are default sized in NonD and cannot be used for
  // the empty() tests below.
  if (!probDescDB.get_rva("method.nond.probability_levels").empty() ||
      !probDescDB.get_rva("method.nond.gen_reliability_levels").empty()) {
    Cerr << "Error: Inverse reliability mappings not currently supported in "
	 << "global_reliability."<< std::endl;
    abort_handler(-1);
  }

#ifndef DAKOTA_F90
  if (meritFunctionType == LAGRANGIAN_MERIT) {
    Cerr << "Error: F90 required for standard Lagrangian merit function in "
	 << "global_reliability."<< std::endl;
    abort_handler(-1);
  }
#endif // DAKOTA_F90

  // Size the output arrays, augmenting sizing in NonDReliability.  Relative to
  // other NonD methods, the output storage for reliability methods is greater
  // since there may be differences between requested and computed levels for
  // the same level type (the request is not always achieved) and since
  // probability and reliability are carried along in parallel (due to their
  // direct correspondence).  Relative to NonDLocalReliability, RelLevels are
  // ignored since probabilities are estimated directly from MMAIS.
  size_t i;
  for (i=0; i<numFunctions; i++) {
    size_t num_levels = requestedRespLevels[i].length() +
      requestedProbLevels[i].length() + requestedGenRelLevels[i].length();
    computedRespLevels[i].resize(num_levels);
    computedProbLevels[i].resize(num_levels);
    computedGenRelLevels[i].resize(num_levels);
  }

  // The Gaussian process model of the limit state in u-space [G-hat(u)] is
  // constructed here one time.

  // Always build a global Gaussian process model.  No correction is needed.
  String approx_type = "global_kriging";
  if (probDescDB.get_short("method.nond.emulator") == GP_EMULATOR)
    approx_type = "global_gaussian";

  unsigned short sample_type = SUBMETHOD_DEFAULT;
  UShortArray approx_order; // not used for GP/kriging
  short corr_order = -1, corr_type = NO_CORRECTION,
    active_view = iteratedModel.current_variables().view().first;
  if (probDescDB.get_bool("method.derivative_usage")) {
    if (approx_type == "global_gaussian") {
      Cerr << "\nError: efficient_global does not support gaussian_process "
	   << "when derivatives present; use kriging instead." << std::endl;
      abort_handler(-1);
    }
    if (iteratedModel.gradient_type() != "none") dataOrder |= 2;
    if (iteratedModel.hessian_type()  != "none") dataOrder |= 4;
  }
  String sample_reuse
    = (active_view == RELAXED_ALL || active_view == MIXED_ALL) ? "all" : "none";

  int db_samples = probDescDB.get_int("method.samples");
  int samples = (db_samples > 0) ? db_samples :
    (numContinuousVars+1)*(numContinuousVars+2)/2;

  int lhs_seed = probDescDB.get_int("method.random_seed");
  const String& rng = probDescDB.get_string("method.random_number_generator");
  bool vary_pattern = false; // for consistency across outer loop invocations
  // get point samples file
  const String& import_pts_file
    = probDescDB.get_string("method.import_build_points_file");
  if (!import_pts_file.empty())
    { samples = 0; sample_reuse = "all"; }

  //int symbols = samples; // symbols needed for DDACE
  Iterator dace_iterator;
  NonDLHSSampling* lhs_sampler_rep;
  // instantiate the Nataf Recast and Gaussian Process DataFit recursions
  if (mppSearchType == EGRA_X) { // Recast( DataFit( iteratedModel ) )

    // The following uses on the fly derived ctor:
    lhs_sampler_rep = new NonDLHSSampling(iteratedModel, sample_type, samples,
      lhs_seed, rng, vary_pattern, ACTIVE_UNIFORM);
    //unsigned short dace_method = SUBMETHOD_LHS; // submethod enum
    //lhs_sampler_rep = new DDACEDesignCompExp(iteratedModel, samples, symbols,
    //                                         lhs_seed, dace_method);
    //unsigned short dace_method = FSU_HAMMERSLEY;
    //lhs_sampler_rep = new FSUDesignCompExp(iteratedModel, samples, lhs_seed,
    //                                       dace_method);
    dace_iterator.assign_rep(lhs_sampler_rep, false);

    // Construct g-hat(x) using a GP approximation over the active/uncertain
    // vars (same view as iteratedModel: not the typical All view for DACE).
    // For RBDO with GP over {u}+{d}, view set to All from all_variables spec.
    Model g_hat_x_model;
    IntSet surr_fn_indices; // Only want functions with requested levels
    ActiveSet set = iteratedModel.current_response().active_set();// copy
    set.request_values(0);
    for (i=0; i<numFunctions; ++i)
      if (!computedRespLevels[i].empty()) // sized to total req levels above
    	{ set.request_value(dataOrder, i); surr_fn_indices.insert(i); }
    dace_iterator.active_set(set);
    //const Variables& curr_vars = iteratedModel.current_variables();
    ActiveSet gp_set = iteratedModel.current_response().active_set(); // copy
    gp_set.request_values(1);// no surr deriv evals, but GP may be grad-enhanced
    g_hat_x_model.assign_rep(new DataFitSurrModel(dace_iterator, iteratedModel,
      gp_set, approx_type, approx_order, corr_type, corr_order, dataOrder,
      outputLevel, sample_reuse, import_pts_file,
      probDescDB.get_ushort("method.import_build_format"),
      probDescDB.get_bool("method.import_build_active_only"),
      probDescDB.get_string("method.export_approx_points_file"),
      probDescDB.get_ushort("method.export_approx_format")), false);
    g_hat_x_model.surrogate_function_indices(surr_fn_indices);

    // Recast g-hat(x) to G-hat(u)
    transform_model(g_hat_x_model, uSpaceModel, true, 5.);// truncated dist bnds
  }
  else { // DataFit( Recast( iteratedModel ) )

    // Recast g(x) to G(u)
    Model g_u_model;
    transform_model(iteratedModel, g_u_model, true, 5.); // truncated dist bnds

    // For additional generality, could develop on the fly envelope ctor:
    //Iterator dace_iterator(g_u_model, dace_method, ...);

    // The following use on-the-fly derived ctors:
    lhs_sampler_rep = new NonDLHSSampling(g_u_model, sample_type,
      samples, lhs_seed, rng, vary_pattern, ACTIVE_UNIFORM);
    //unsigned short dace_method = SUBMETHOD_LHS; // submethod enum
    //lhs_sampler_rep = new DDACEDesignCompExp(g_u_model, samples, symbols,
    //                                         lhs_seed, dace_method);
    //unsigned short dace_method = FSU_HAMMERSLEY;
    //lhs_sampler_rep = new FSUDesignCompExp(g_u_model, samples, lhs_seed,
    //                                       dace_method);
    dace_iterator.assign_rep(lhs_sampler_rep, false);
    // share nataf instance to provide data for performing inverse transforms
    lhs_sampler_rep->initialize_random_variables(natafTransform); // shared rep

    // Construct G-hat(u) using a GP approximation over the active/uncertain
    // variables (using the same view as iteratedModel/g_u_model: not the
    // typical All view for DACE).
    // For RBDO with GP over {u}+{d}, view set to All from all_variables spec.
    IntSet surr_fn_indices; // Only want functions with requested levels
    ActiveSet set = iteratedModel.current_response().active_set();// copy
    set.request_values(0);
    for (i=0; i<numFunctions; ++i)
      if (!computedRespLevels[i].empty()) // sized to total req levels above
    	{ set.request_value(dataOrder, i); surr_fn_indices.insert(i); }
    dace_iterator.active_set(set);

    //const Variables& g_u_vars = g_u_model.current_variables();
    ActiveSet gp_set = g_u_model.current_response().active_set(); // copy
    gp_set.request_values(1);// no surr deriv evals, but GP may be grad-enhanced
    uSpaceModel.assign_rep(new DataFitSurrModel(dace_iterator, g_u_model,
      gp_set, approx_type, approx_order, corr_type, corr_order, dataOrder,
      outputLevel, sample_reuse, import_pts_file,
      probDescDB.get_ushort("method.import_build_format"),
      probDescDB.get_bool("method.import_build_active_only"),
      probDescDB.get_string("method.export_approx_points_file"),
      probDescDB.get_ushort("method.export_approx_format")), false);
    uSpaceModel.surrogate_function_indices(surr_fn_indices);
  }

  // Following this ctor, IteratorScheduler::init_iterator() initializes the
  // parallel configuration for NonDGlobalReliability + iteratedModel using
  // NonDGlobalReliability's maxEvalConcurrency. During uSpaceModel construction
  // above, DataFitSurrModel::derived_init_communicators() initializes the
  // parallel configuration for dace_iterator + iteratedModel using
  // dace_iterator's maxEvalConcurrency.  The only iteratedModel concurrency
  // currently exercised is that used by dace_iterator within the initial GP
  // construction, but the NonDGlobalReliability maxEvalConcurrency must still
  // be set so as to avoid parallel configuration errors resulting from
  // avail_procs > max_concurrency within IteratorScheduler::init_iterator().
  // A max of the local derivative concurrency and the DACE concurrency is used
  // for this purpose.
  maxEvalConcurrency = std::max(maxEvalConcurrency,
				dace_iterator.maximum_evaluation_concurrency());

  // Configure a RecastModel with one objective and no constraints using the
  // alternate minimalist constructor.  The RIA/PMA expected improvement/
  // expected feasibility formulations may vary with the level requests, so
  // the recast fn pointers are reset for each level within the run fn.
  SizetArray recast_vars_comps_total;  // default: empty; no change in size
  BitArray all_relax_di, all_relax_dr; // default: empty; no discrete relaxation
  short recast_resp_order = 1; // nongradient-based optimizers
  mppModel.assign_rep(
    new RecastModel(uSpaceModel, recast_vars_comps_total, all_relax_di,
		    all_relax_dr, 1, 0, 0, recast_resp_order), false);

  // For formulations with one objective and one equality constraint,
  // use the following instead:
  //mppModel.assign_rep(new RecastModel(uSpaceModel, ..., 1, 1, 0, ...), false);
  //RealVector nln_eq_targets(1, 0.);
  //mppModel.nonlinear_eq_constraint_targets(nln_eq_targets);

  // must use alternate NoDB ctor chain
  int max_iter = 1000, max_eval = 10000;
  double min_box_size = 1.e-15, vol_box_size = 1.e-15;
#ifdef HAVE_NCSU
  mppOptimizer.assign_rep(new NCSUOptimizer(mppModel, max_iter, max_eval,
					    min_box_size, vol_box_size), false);
  //#ifdef HAVE_ACRO
  //int coliny_seed = 0; // system-generated, for now
  //mppOptimizer.assign_rep(new
  //  COLINOptimizer<coliny::DIRECT>(mppModel, coliny_seed), false);
  //mppOptimizer.assign_rep(new
  //  COLINOptimizer<coliny::EAminlp>(mppModel, coliny_seed), false);
  //#endif
#else
  Cerr << "NCSU DIRECT Optimizer is not available to use in the MPP search "
       << "in global reliability optimization:  aborting process." << std::endl;
        abort_handler(-1);
#endif //HAVE_NCSU

  // The importance sampler uses uSpaceModel (without additional recasting)
  // and may be constructed here.  Thus, NonDGlobal applies integration
  // refinement to the G-hat(u) surrogate.  Behavior needs to be repeatable
  // and AIS is not part of the EGRA spec: either reuse lhs_seed or hardwire.
  int refine_samples = 1000, refine_seed = 123457;
  // we pass a u-space model and enforce the EGRA GP bounds on the samples;
  // extreme values are needed to define bounds for outer PDF bins.
  bool x_model_flag = false, use_model_bounds = true, track_extreme = pdfOutput;
  integrationRefinement = MMAIS; vary_pattern = true;

  NonDAdaptImpSampling* importance_sampler_rep = new
    NonDAdaptImpSampling(uSpaceModel, sample_type, refine_samples, refine_seed,
			 rng, vary_pattern, integrationRefinement, cdfFlag,
			 x_model_flag, use_model_bounds, track_extreme);
  importanceSampler.assign_rep(importance_sampler_rep, false);

  // if approximation is built in x-space, then importanceSampler must perform
  // inverse transformations on gp_inputs; if approximation is built in u-space,
  // only the cdfFlag is needed to define which samples are failures
  if (mppSearchType == EGRA_X) // share the ProbabilityTransformation rep
    importance_sampler_rep->initialize_random_variables(natafTransform);
}

NonDGlobalReliability::~NonDGlobalReliability()
{ }

bool NonDGlobalReliability::resize()
{
  bool parent_reinit_comms = NonDReliability::resize();

  Cerr << "\nError: Resizing is not yet supported in method "
       << method_enum_to_string(methodName) << "." << std::endl;
  abort_handler(METHOD_ERROR);

  return parent_reinit_comms;
}

void NonDGlobalReliability::derived_init_communicators(ParLevLIter pl_iter)
{
  iteratedModel.init_communicators(pl_iter, maxEvalConcurrency);

  // mppModel.init_communicators() recursion is currently sufficient for
  // uSpaceModel.  An additional uSpaceModel.init_communicators() call would be
  // motivated by special parallel usage of uSpaceModel below that is not
  // otherwise covered by the recursion.
  //uSpaceMaxConcurrency = maxEvalConcurrency; // local derivative concurrency
  //uSpaceModel.init_communicators(pl_iter, uSpaceMaxConcurrency);

  // mppOptimizer and importanceSampler use NoDBBaseConstructor, so no
  // need to manage DB list nodes at this level
  mppOptimizer.init_communicators(pl_iter);
  importanceSampler.init_communicators(pl_iter);
}

void NonDGlobalReliability::derived_set_communicators(ParLevLIter pl_iter)
{
  NonD::derived_set_communicators(pl_iter);

  //uSpaceMaxConcurrency = maxEvalConcurrency; // local derivative concurrency
  //uSpaceModel.set_communicators(pl_iter, uSpaceMaxConcurrency);

  // mppOptimizer and importanceSampler use NoDBBaseConstructor, so no
  // need to manage DB list nodes at this level
  mppOptimizer.set_communicators(pl_iter);
  importanceSampler.set_communicators(pl_iter);
}

void NonDGlobalReliability::derived_free_communicators(ParLevLIter pl_iter)
{
  // deallocate communicators for MMAIS on uSpaceModel
  importanceSampler.free_communicators(pl_iter);

  // deallocate communicators for DIRECT on mppModel
  mppOptimizer.free_communicators(pl_iter);

  //uSpaceMaxConcurrency = maxEvalConcurrency; // local derivative concurrency
  //uSpaceModel.free_communicators(pl_iter, uSpaceMaxConcurrency);

  iteratedModel.free_communicators(pl_iter, maxEvalConcurrency);
}

void NonDGlobalReliability::core_run()
{
  // initialize the random variable arrays and the correlation Cholesky factor
  initialize_random_variable_parameters();
  transform_correlations();

  // set the object instance pointer for use within static member functions
  NonDGlobalReliability* prev_grel_instance = nondGlobRelInstance;
  nondGlobRelInstance = this;

  // Optimize the GP for all levels and then use MAIS for _all_ levels
  optimize_gaussian_process();
  importance_sampling();
  numRelAnalyses++;

  // restore in case of recursion
  nondGlobRelInstance = prev_grel_instance;
}

void NonDGlobalReliability::optimize_gaussian_process()
{
  // now that variables/labels/bounds/targets have flowed down at run-time from
  // any higher level recursions, propagate them up the instantiate-on-the-fly
  // Model recursion so that they are correct when they propagate back down.
  mppModel.update_from_subordinate_model(); // depth = max

  if (mppSearchType == EGRA_X) {
    // assign non-default global variable bounds for use in DACE.
    // This does not affect any uncertain variable distribution bounds.
    // Note 1: the interval defined in x-space will be sampled uniformly by
    // DACE methods, which could result in very irregular coverage in u-space.
    // It would be better to sample uniformly in u-space.
    // Note 2: it would be preferable to set this up in the constructor.  The
    // EGRA_U case can do this by setting the bounds in the actualModel of the
    // DataFitSurrModel and then having them copied in the DataFitSurrModel
    // ctor.  The EGRA_X case needs trans_U_to_X, which isn't available until
    // after initialize_random_variable_parameters() is executed at run time.
    // Therefore, this case sets the bounds for the DataFitSurrModel, which are
    // then propagated to actualModel in DataFitSurrModel::update_actual_model()
    RealVector u_l_bnds(numContinuousVars, false), x_l_bnds,
               u_u_bnds(numContinuousVars, false), x_u_bnds;
    u_l_bnds = -5.; u_u_bnds = 5.;
    for (size_t i=0; i<numContDesVars; i++)
      { u_l_bnds[i] = -1.; u_u_bnds[i] =  1.; }
    for (size_t i=numContDesVars+numContAleatUncVars; i<numContinuousVars; i++)
      { u_l_bnds[i] = -1.; u_u_bnds[i] =  1.; }
    natafTransform.trans_U_to_X(u_l_bnds, x_l_bnds);
    natafTransform.trans_U_to_X(u_u_bnds, x_u_bnds);
    Model& g_hat_x_model = uSpaceModel.subordinate_model();
    g_hat_x_model.continuous_lower_bounds(x_l_bnds);
    g_hat_x_model.continuous_upper_bounds(x_u_bnds);
  }

  // Build initial GP once for all response functions
  uSpaceModel.build_approximation();

  // Loop over each response function in the responses specification.  It is
  // important to note that the MPP iteration is different for each response
  // function, and it is not possible to combine the model evaluations for
  // multiple response functions.
  ParLevLIter pl_iter = methodPCIter->mi_parallel_level_iterator(miPLIndex);
  for (respFnCount=0; respFnCount<numFunctions; respFnCount++) {

    // The most general case is to allow a combination of response, probability,
    // and reliability level specifications for each response function.
    size_t rl_len = requestedRespLevels[respFnCount].length(),
           pl_len = requestedProbLevels[respFnCount].length(),
           gl_len = requestedGenRelLevels[respFnCount].length(),
           num_levels = rl_len + pl_len + gl_len;

    // Loop over response/probability/reliability levels
    for (levelCount=0; levelCount<num_levels; levelCount++) {

      // The rl_len response levels are performed first using the RIA
      // formulation, followed by the pl_len probability levels and the
      // gl_len generalized reliability levels using the PMA formulation.
      bool ria_flag = (levelCount < rl_len) ? true : false;
      if (ria_flag) {
        requestedTargetLevel = requestedRespLevels[respFnCount][levelCount];
	Cout << "\n>>>>> Reliability Index Approach (RIA) for response level "
	     << levelCount+1 << " = " << requestedTargetLevel << '\n';
      }
      else if (levelCount < rl_len + pl_len) {
	size_t index = levelCount-rl_len;
	Real p = requestedProbLevels[respFnCount][index];
	Cout << "\n>>>>> Performance Measure Approach (PMA) for probability "
	     << "level " << index+1 << " = " << p << '\n';
	requestedTargetLevel = -Pecos::NormalRandomVariable::inverse_std_cdf(p);
	Real gen_beta_cdf = (cdfFlag) ?
	  requestedTargetLevel : -requestedTargetLevel;
	pmaMaximizeG = (gen_beta_cdf < 0.);
      }
      else {
	size_t index = levelCount-rl_len-pl_len;
	requestedTargetLevel = requestedGenRelLevels[respFnCount][index];
	Cout << "\n>>>>> Performance Measure Approach (PMA) for reliability "
	     << "level " << index+1 << " = " << requestedTargetLevel << '\n';
	Real gen_beta_cdf = (cdfFlag) ?
	  requestedTargetLevel : -requestedTargetLevel;
	pmaMaximizeG = (gen_beta_cdf < 0.);
      }

      bool pma_aug_lag_flag
	= (!ria_flag && meritFunctionType == AUGMENTED_LAGRANGIAN_MERIT);
      if (pma_aug_lag_flag) {
	augLagrangeMult         = 0.;      penaltyParameter    = 1.;
	lastConstraintViolation = DBL_MAX; lastIterateAccepted = false;
      }

      // Iterate until EGRA converges
      approxIters = 0;
      approxConverged = false;
      while (!approxConverged) {

	approxIters++;

	// construct global optimizer and its EI/EF recast model
	Sizet2DArray vars_map, primary_resp_map, secondary_resp_map;
	BoolDequeArray nonlinear_resp_map(1, BoolDeque(1, true));
	RecastModel* mpp_model_rep = (RecastModel*)mppModel.model_rep();
	if (ria_flag) {
	  // Standard RIA : min u'u  s.t. g = z_bar
	  // use RIA evaluators to recast g into global opt subproblem
	  //primary_resp_map.reshape(1);   // one objective, no contributors
	  //secondary_resp_map.reshape(1); // one constraint, one contributor
	  //secondary_resp_map[0].reshape(1);
	  //secondary_resp_map[0][0] = respFnCount;
	  //BoolDequeArray nonlinear_resp_map(2);
	  //nonlinear_resp_map[1] = BoolDeque(1, false);
	  //mpp_model_rep->init_maps(vars_map, false, NULL, NULL,
	  //  primary_resp_map, secondary_resp_map, nonlinear_resp_map,
	  //  RIA_objective_eval, RIA_constraint_eval);

	  // EFF formulation : max EFF  s.t. bound constraints
	  // use EFF evaluators to recast g into global opt subproblem
	  primary_resp_map.resize(1);
	  primary_resp_map[0].resize(1);
	  primary_resp_map[0][0] = respFnCount;
	  mpp_model_rep->init_maps(vars_map, false, NULL, NULL,
	    primary_resp_map, secondary_resp_map, nonlinear_resp_map,
	    EFF_objective_eval, NULL);
	}
	else {
	  // Standard PMA : min/max g  s.t. u'u = beta_bar^2
	  // use PMA evaluators to recast g into global opt subproblem
	  //void (*set_map) (const ShortArray& recast_asv,
	  //                 ShortArray& sub_model_asv) =
	  //  (integrationOrder == 2) ? PMA2_set_mapping : NULL;
	  //primary_resp_map.reshape(1);   // one objective, one contributor
	  //primary_resp_map[0].reshape(1);
	  //primary_resp_map[0][0] = respFnCount;
	  //secondary_resp_map.reshape(1); // one constraint, no contributors
	  //BoolDequeArray nonlinear_resp_map(2);
	  //nonlinear_resp_map[0] = BoolDeque(1, false);
	  //mpp_model_rep->init_maps(vars_map, false, NULL, set_map,
	  //  primary_resp_map, secondary_resp_map, nonlinear_resp_map,
	  //  PMA_objective_eval, PMA_constraint_eval);

	  // EIF formulation : max Phi(EIF) [merit function on EIF]
	  // determine fnStar from among sample data
	  get_best_sample();
	  // use EIF evaluators to recast g into global opt subproblem
	  primary_resp_map.resize(1);
	  primary_resp_map[0].resize(1);
	  primary_resp_map[0][0] = respFnCount;
	  mpp_model_rep->init_maps(vars_map, false, NULL, NULL,
	    primary_resp_map, secondary_resp_map, nonlinear_resp_map,
	    EIF_objective_eval, NULL);
	}

	// Execute GLOBAL search and retrieve u-space results
	Cout << "\n>>>>> Initiating global reliability optimization\n";
	mppOptimizer.run(pl_iter);
	// Use these two lines for COLINY optimizers
	//const VariablesArray& vars_star
	//  = mppOptimizer.variables_array_results();
	//const RealVector& c_vars_u = vars_star[0].continuous_variables();
	// Use these two lines for NCSU DIRECT
	const Variables& vars_star = mppOptimizer.variables_results();
	const RealVector& c_vars_u = vars_star.continuous_variables();

	// Get expected value at u* for output
	uSpaceModel.continuous_variables(c_vars_u);
	uSpaceModel.evaluate();
	const RealVector& g_hat_fns
	  = uSpaceModel.current_response().function_values();

	// Re-evaluate the expected improvement/feasibility at vars_star
	Real beta_star = 0.,/* TO DO */  exp_fns_star = (ria_flag) ?
	  expected_feasibility(g_hat_fns, vars_star) :
	  expected_improvement(g_hat_fns, vars_star);

	Cout << "\nResults of EGRA iteration:\nFinal point (u-space)   =\n";
	write_data(Cout, c_vars_u);
	size_t wpp7 = write_precision+7;
	if (ria_flag) {
	  Cout << "Expected Feasibility    =\n                     "
	       << std::setw(wpp7) << -exp_fns_star << "\n                     "
	       << std::setw(wpp7) << g_hat_fns[respFnCount]-requestedTargetLevel
	       << " [G_hat(u) - z]\n";
	//     << "                     " << std::setw(wpp7) << beta_star
	//     << " [beta*]\n";
	//Cout << "RIA optimum             =\n                     "
	//     << std::setw(wpp7) << exp_fns_star << " [u'u]\n"
	//     << "                     " << std::setw(wpp7) << exp_fns_star[1]
	//     << " [G(u) - z]\n";
	}
	else {
	  // Calculate beta^2 for output (and aug_lag update)
	  Cout << "Expected Improvement    =\n                     "
	       << std::setw(wpp7) << -exp_fns_star << "\n                     "
	       << std::setw(wpp7) << beta_star - requestedTargetLevel
	       << " [beta* - bar-beta*]\n                     "
	       << std::setw(wpp7) << g_hat_fns[respFnCount] << " [G_hat(u)]\n";
	//Cout << "PMA optimum             =\n                     "
	//     << std::setw(wpp7) << exp_fns_star << " [";
	//if (pmaMaximizeG) Cout << '-';
	//Cout << "G(u)]\n                     " << std::setw(wpp7)
	//     << exp_fns_star[1] << " [u'u - B^2]\n";
	}

	// Update parameters for the augmented Lagrangian merit function
	if (pma_aug_lag_flag) {
	  // currently only used for PMA with EIF
	  Real c_violation = beta_star - requestedTargetLevel;
	  if (c_violation < lastConstraintViolation)
	    lastIterateAccepted = true;
	  lastConstraintViolation = c_violation;
	}

	// Check for convergence based on max EIF/EFF
	// BMA: was previously hard-wired: convergenceTol = .001;
        if (maxIterations < 0)
          maxIterations  = 25*numContinuousVars;
	if (approxIters >= maxIterations || -exp_fns_star < convergenceTol)
	  approxConverged = true;
	else {
	  // Evaluate response_star_truth
	  uSpaceModel.component_parallel_mode(TRUTH_MODEL);
	  RealVector c_vars_x;
	  natafTransform.trans_U_to_X(c_vars_u, c_vars_x);
	  iteratedModel.continuous_variables(c_vars_x);
	  ActiveSet set = iteratedModel.current_response().active_set();
	  set.request_values(0); set.request_value(dataOrder, respFnCount);
	  iteratedModel.evaluate(set);
	  IntResponsePair resp_star_truth(iteratedModel.evaluation_id(),
					  iteratedModel.current_response());

	  // Update the GP approximation
	  if (mppSearchType == EGRA_X) // update with x-space current vars
	    uSpaceModel.append_approximation(
	      iteratedModel.current_variables(), resp_star_truth, true);
	  else                         // update with u-space vars_star
	    uSpaceModel.append_approximation(vars_star, resp_star_truth, true);
	}
      } // end approx convergence while loop

      if (ria_flag)
	Cout << "\n<<<<< GP model has converged for response level ";
      else
 	Cout << "\n<<<<< GP model has converged for response level ";
      Cout << levelCount+1 << " = " << requestedTargetLevel << '\n';

    } // end loop over levels

    if (num_levels)
      Cout << "\n<<<<< GP model has converged for all requested levels of "
	   << "response function " << respFnCount+1 << '\n';

#ifdef DEBUG
    // DEBUG - output set of samples used to build the GP
    // If problem is 2d, output a grid of points on the GP, variance,
    //   eff, and truth (if requested)
    if (num_levels) {  // can only plot if GP was built!
      std::string samsfile("egra_sams");
      std::string tag = "_" + boost::lexical_cast<std::string>(respFnCount+1) +
                        ".out";
      samsfile += tag;
      std::ofstream samsOut(samsfile.c_str(),std::ios::out);
      samsOut << std::scientific;
      const Pecos::SurrogateData& gp_data
	= uSpaceModel.approximation_data(respFnCount);
      size_t num_data_pts = gp_data.size(), num_vars = uSpaceModel.cv();
      for (size_t i=0; i<num_data_pts; ++i) {
	const RealVector& sams = gp_data.continuous_variables(i); // view
	Real true_fn = gp_data.response_function(i);

	if (mppSearchType == EGRA_X) {
	  RealVector sams_u(num_vars);
	  natafTransform.trans_X_to_U(sams,sams_u);

	  samsOut << '\n';
	  for (size_t j=0; j<num_vars; j++)
	    samsOut << std::setw(13) << sams_u[j] << ' ';
	  samsOut<< std::setw(13) << true_fn;
	}
	else {
	  samsOut << '\n';
	  for (size_t j=0; j<num_vars; j++)
	    samsOut << std::setw(13) << sams[j] << ' ';
	  samsOut<< std::setw(13) << true_fn;
	}
      }
      samsOut << std::endl;

#ifdef DEBUG_PLOTS
      // Plotting the GP, etc over a grid is intended for visualization and
      //   is therefore only available for 2D problems
      if (num_vars==2) {
	bool true_plot = false;
        std::string truefile("egra_true"), gpfile("egra_gp"),
                    varfile("egra_var"), efffile("egra_eff");
	truefile += tag; gpfile += tag; varfile += tag; efffile += tag;
	std::ofstream trueOut(truefile.c_str(), std::ios::out),
	                gpOut(gpfile.c_str(),   std::ios::out),
                       varOut(varfile.c_str(),  std::ios::out),
                       effOut(efffile.c_str(),  std::ios::out);
	gpOut  << std::scientific; varOut << std::scientific;
	effOut << std::scientific; if (true_plot) trueOut << std::scientific;
	RealVector u_pt(2), x_pt(2);
	Real lbnd =  -5., ubnd =  5.;
	//Real lbnd = -10., ubnd = 10.;
	Real interval = (ubnd-lbnd)/100.;
	for (size_t i=0; i<101; i++){
	  u_pt[0] = lbnd + float(i)*interval;
	  for (size_t j=0; j<101; j++){
	    u_pt[1] = lbnd + float(j)*interval;

	    uSpaceModel.continuous_variables(u_pt);
	    ActiveSet set = uSpaceModel.current_response().active_set();
	    set.request_values(0); set.request_value(1, respFnCount);
	    uSpaceModel.evaluate(set);
	    const Response& gp_resp = uSpaceModel.current_response();
	    const RealVector& gp_fn = gp_resp.function_values();

	    gpOut << '\n' << std::setw(13) << u_pt[0] << ' ' << std::setw(13)
		  << u_pt[1] << ' ' << std::setw(13) << gp_fn[respFnCount];

	    RealVector variance;
	    if (mppSearchType == EGRA_X) { // Recast( DataFit( iteratedModel ) )
	      // RecastModel::derived_evaluate() propagates u_pt to x_pt
	      Model& dfs_model = uSpaceModel.subordinate_model();
	      variance = dfs_model.approximation_variances(
		dfs_model.current_variables()); // x_pt
	    }
	    else // EGRA_U: DataFit( Recast( iteratedModel ) )
	      variance = uSpaceModel.approximation_variances(
		uSpaceModel.current_variables()); // u_pt

	    varOut << '\n' << std::setw(13) << u_pt[0] << ' ' << std::setw(13)
		   << u_pt[1] << ' ' << std::setw(13) << variance[respFnCount];

	    Real eff = expected_feasibility(gp_fn, u_pt);

	    effOut << '\n' << std::setw(13) << u_pt[0] << ' ' << std::setw(13)
		   << u_pt[1] << ' ' << std::setw(13) << -eff;

	    // plotting the true function can be expensive, but is available
	    if (true_plot) {
	      uSpaceModel.component_parallel_mode(TRUTH_MODEL);
	      natafTransform.trans_U_to_X(u_pt,x_pt);
	      iteratedModel.continuous_variables(x_pt);
	      set = iteratedModel.current_response().active_set();
	      set.request_values(0); set.request_value(1, respFnCount);
	      iteratedModel.evaluate(set);
	      const Response& true_resp = iteratedModel.current_response();
	      const RealVector& true_fn = true_resp.function_values();

	      trueOut << '\n' << std::setw(13) << u_pt[0] << ' '
		      << std::setw(13) << u_pt[1] << ' '
		      << std::setw(13) << true_fn[respFnCount];
	    }
	  }
	  gpOut << std::endl; varOut << std::endl; effOut << std::endl;
	  if (true_plot) trueOut << std::endl;
	}
      }
#endif //DEBUG_PLOTS
    }
#endif //DEBUG

  } // end loop over response fns
}

void NonDGlobalReliability::importance_sampling()
{
  bool x_data_flag = (mppSearchType == EGRA_X);
  size_t i;
  statCount = 0;
  const ShortArray& final_res_asv = finalStatistics.active_set_request_vector();
  ParLevLIter pl_iter = methodPCIter->mi_parallel_level_iterator(miPLIndex);
  // rep needed for access to functions not mapped to Iterator level
  NonDAdaptImpSampling* importance_sampler_rep
    = (NonDAdaptImpSampling*)importanceSampler.iterator_rep();

  for (respFnCount=0; respFnCount<numFunctions; respFnCount++) {

    // The most general case is to allow a combination of response, probability,
    // and reliability level specifications for each response function.
    size_t rl_len = requestedRespLevels[respFnCount].length(),
           pl_len = requestedProbLevels[respFnCount].length(),
           gl_len = requestedGenRelLevels[respFnCount].length(),
           num_levels = rl_len + pl_len + gl_len;

    RealVectorArray gp_inputs;
    if (num_levels==0) {
      uSpaceModel.component_parallel_mode(TRUTH_MODEL);
      // don't use derivatives in the importance sampling
      ActiveSet set = iteratedModel.current_response().active_set();
      set.request_values(0); set.request_value(1, respFnCount);
      iteratedModel.evaluate(set);
      const Response& true_resp = iteratedModel.current_response();
      const RealVector& true_fn = true_resp.function_values();
      finalStatistics.function_value(true_fn[respFnCount], statCount);
    }
    else {
      // extract the approximation data from the surrogate model.
      // Note 1: this data is either x-space (EGRA_X) or u-space (EGRA_U).
      // Note 2: this returns _all_ truth model data for this response fn,
      //         including initial DACE and EGRA added points for _all_ levels.
      // TO DO:  likely need to remove DACE and partition remaining data among
      //         levels for importance sampling efficiency.
      const Pecos::SurrogateData& gp_data
	= uSpaceModel.approximation_data(respFnCount);
      size_t num_data_pts = gp_data.points();
      gp_inputs.resize(num_data_pts);
      for (i=0; i<num_data_pts; ++i)
	gp_inputs[i] = gp_data.continuous_variables(i); // view OK
    }
    statCount++;

    // Standard deviations & sensitivities not available, skip
    statCount++;

    // Loop over response/probability/reliability levels
    for (levelCount=0; levelCount<num_levels; levelCount++) {

      Cout << "\n<<<<< Performing importance sampling for response function "
	   << respFnCount+1 << " level " << levelCount+1 << '\n';

      bool ria_flag = (levelCount < rl_len) ? true : false;
      if (ria_flag) {
	Real z =  computedRespLevels[respFnCount][levelCount]
	       = requestedRespLevels[respFnCount][levelCount];
	importance_sampler_rep->
	  initialize(gp_inputs, x_data_flag, respFnCount, 0., z);
      }
      else // not operational (see error traps in ctor)
	importance_sampler_rep->initialize(gp_inputs, x_data_flag,
	  respFnCount, 0., computedRespLevels[respFnCount][levelCount]);

      importanceSampler.run(pl_iter);

      Real p = importance_sampler_rep->final_probability();
#ifdef DEBUG
      Cout << "\np = " << p << std::endl;
#endif // DEBUG
      // RIA z-bar -> p
      computedProbLevels[respFnCount][levelCount] = p;
      // RIA z-bar -> generalized beta
      Real gen_beta = -Pecos::NormalRandomVariable::inverse_std_cdf(p);
      computedGenRelLevels[respFnCount][levelCount] = gen_beta;
      switch (respLevelTarget) {
      case PROBABILITIES:
	finalStatistics.function_value(p, statCount++);        break;
      case GEN_RELIABILITIES:
	finalStatistics.function_value(gen_beta, statCount++); break;
      }
    }
  }
  // post-process level mappings to define PDFs (using prob_refined and
  // all_levels_computed modes)
  if (pdfOutput)
    compute_densities(importance_sampler_rep->extreme_values(), true, true);
}

void NonDGlobalReliability::
EIF_objective_eval(const Variables& sub_model_vars,
		   const Variables& recast_vars,
		   const Response& sub_model_response,
		   Response& recast_response)
{
  const ShortArray& recast_asv = recast_response.active_set_request_vector();
  if (recast_asv[0] & 1) {
    Real ei = nondGlobRelInstance->expected_improvement(
      sub_model_response.function_values(), recast_vars);
    recast_response.function_value(ei, 0);
  }
}

void NonDGlobalReliability::
EFF_objective_eval(const Variables& sub_model_vars,
		   const Variables& recast_vars,
		   const Response& sub_model_response,
		   Response& recast_response)
{
  const ShortArray& recast_asv = recast_response.active_set_request_vector();
  if (recast_asv[0] & 1) {
    Real ef = nondGlobRelInstance->expected_feasibility(
      sub_model_response.function_values(), recast_vars);
    recast_response.function_value(ef, 0);
  }
}

Real NonDGlobalReliability::
expected_improvement(const RealVector& expected_values,
		     const Variables& recast_vars)
{
  // Get variance from the GP; Expected values are passed in
  // If GP built in x-space, transform input point to x-space to get variance
  RealVector variances;
  if (mppSearchType == EGRA_X) { // uSpaceModel = Recast(DataFit(iteratedModel))
    Model& dfs_model = uSpaceModel.subordinate_model();
    // assume recast_vars have been propagated to GP just prior to call
    variances
      = dfs_model.approximation_variances(dfs_model.current_variables());
  }
  else                   // EGRA_U: uSpaceModel = DataFit(Recast(iteratedModel))
    variances = uSpaceModel.approximation_variances(recast_vars);

  Real mean = expected_values[respFnCount];
  Real stdv = std::sqrt(variances[respFnCount]);

  // Calculate and apply penalty to the mean
  Real beta_star = 0.; // TO DO
  // calculate the equality constraint: beta* - bar-beta*
  Real cfn = beta_star - requestedTargetLevel;
  Real penalty = constraint_penalty(cfn, recast_vars.continuous_variables());
  // Calculation of EI will have different form for the CDF/CCDF cases
  Real penalized_mean = (pmaMaximizeG) ? mean - penalty : mean + penalty;

  // Calculate the expected improvement
  // Note: This is independent of the CDF/CCDF check

  Real ei, cdf, pdf;
  Real snv = (fnStar-penalized_mean); // not normalized yet
  if(std::fabs(snv)>=std::fabs(stdv)*50.0) {
    //this will trap the denominator=0.0 case even if numerator=0.0
    pdf = 0.;
    cdf = (snv > 0.) ? 1. : 0.;
  }
  else{
    snv /= stdv; // now snv is the standard normal variate
    cdf = Pecos::NormalRandomVariable::std_cdf(snv);
    pdf = Pecos::NormalRandomVariable::std_pdf(snv);
  }
  ei = (pmaMaximizeG) ? (penalized_mean - fnStar)*(1.-cdf) + stdv*pdf
                      : (fnStar - penalized_mean)*cdf      + stdv*pdf;
  return -ei; // return -EI because we are maximizing EI
}

Real NonDGlobalReliability::
expected_feasibility(const RealVector& expected_values,
		     const Variables& recast_vars)
{
  // Get variance from the GP; Expected values are passed in
  // If GP built in x-space, transform input point to x-space to get variance
  RealVector variances;
  if (mppSearchType == EGRA_X) { // uSpaceModel = Recast(DataFit(iteratedModel))
    Model& dfs_model = uSpaceModel.subordinate_model();
    // assume recast_vars have been propagated to GP just prior to call
    variances
      = dfs_model.approximation_variances(dfs_model.current_variables());
  }
  else                   // EGRA_U: uSpaceModel = DataFit(Recast(iteratedModel))
    variances = uSpaceModel.approximation_variances(recast_vars);

  Real mean  = expected_values[respFnCount],
       stdv  = std::sqrt(variances[respFnCount]),
       zbar  = requestedTargetLevel,
       alpha = 2.; // may want to try values other than 2

  // calculate standard normal variate +/- alpha

  Real cdfz, pdfz, cdfp, pdfp, cdfm, pdfm;
  Real snvz = (zbar - mean);
  if (std::fabs(snvz) >= std::fabs(stdv)*50.) {
    pdfm = pdfp = pdfz = 0.;
    cdfm = cdfp = cdfz = (snvz > 0.) ? 1. : 0.;
  }
  else {
    snvz /= stdv; Real snvp = snvz + alpha, snvm = snvz - alpha;
    pdfz = Pecos::NormalRandomVariable::std_pdf(snvz);
    cdfz = Pecos::NormalRandomVariable::std_cdf(snvz);
    pdfp = Pecos::NormalRandomVariable::std_pdf(snvp);
    cdfp = Pecos::NormalRandomVariable::std_cdf(snvp);
    pdfm = Pecos::NormalRandomVariable::std_pdf(snvm);
    cdfm = Pecos::NormalRandomVariable::std_cdf(snvm);
  }
  // calculate expected feasibility function
  Real ef = (mean - zbar)*(2.*cdfz - cdfm - cdfp) //exploit
          - stdv*(2.*pdfz - pdfm - pdfp //explore
	  - alpha*cdfp + alpha*cdfm);

  return -ef;  // return -EF because we are maximizing
}

void NonDGlobalReliability::get_best_sample()
{
  // Pull the samples and responses from data used to build latest GP
  //   and apply any penalties to calculate fnStar for use in the
  //   expected improvement function
  // This is only done for PMA - there is no "best solution" for
  //   the expected feasibility function used in RIA

  Iterator&             dace_iterator  = uSpaceModel.subordinate_iterator();
  const RealMatrix&     true_vars_x    = dace_iterator.all_samples();
  const IntResponseMap& true_responses = dace_iterator.all_responses();
  size_t i, j, num_samples = true_vars_x.numCols(),
    num_vars = true_vars_x.numRows();

  // If GP built in x-space, transform true_vars_x to u-space to calculate beta
  RealVectorArray true_c_vars_u(num_samples); RealVector true_vars_x_cv;
  for (i=0; i<num_samples; i++) {
    true_vars_x_cv = Teuchos::getCol(Teuchos::View,
      const_cast<RealMatrix&>(true_vars_x), (int)i);
    if (mppSearchType == EGRA_X)
      natafTransform.trans_X_to_U(true_vars_x_cv, true_c_vars_u[i]);
    else
      true_c_vars_u[i] = true_vars_x_cv; // view OK
  }

  // Calculation of fnStar will have different form for CDF/CCDF cases
  fnStar = (pmaMaximizeG) ? -DBL_MAX : DBL_MAX; IntRespMCIter it;
  for (i=0, it=true_responses.begin(); i<num_samples; i++, ++it) {
    // calculate the reliability index (beta)
    Real beta_star = 0., penalized_response; // TO DO
    // calculate the equality constraint: u'u - beta_target^2
    Real cfn = beta_star - requestedTargetLevel;
    Real penalty = constraint_penalty(cfn, true_c_vars_u[i]);
    penalized_response = (pmaMaximizeG) ?
      it->second.function_value(0) - penalty :
      it->second.function_value(0) + penalty;
    if ( ( pmaMaximizeG && penalized_response > fnStar) ||
	 (!pmaMaximizeG && penalized_response < fnStar) )
      fnStar = penalized_response;
  }
}

Real NonDGlobalReliability::
constraint_penalty(const Real& c_viol, const RealVector& u)
{
  if (meritFunctionType == PENALTY_MERIT)
    return exp(approxIters/10.)*c_viol*c_viol;  // try other schedules
  else if (meritFunctionType == AUGMENTED_LAGRANGIAN_MERIT) {
    if (lastIterateAccepted)
      augLagrangeMult += 2.*penaltyParameter*c_viol;
    else
      penaltyParameter *= 2.;
    return augLagrangeMult*c_viol + penaltyParameter*c_viol*c_viol;
  }
  else if (meritFunctionType == LAGRANGIAN_MERIT) {
#ifdef DAKOTA_F90
    // form [A] = grad[u'u - beta^2]
    RealVector A(numContAleatUncVars, false);
    for (size_t i=0; i<numContAleatUncVars; i++)
      A[i] = 2.*u[i];

    // form -{grad_f} = m_grad_f = -grad[G_hat(u)]
    uSpaceModel.continuous_variables(u);
    uSpaceModel.evaluate();
    const Real* grad_f = uSpaceModel.current_response().function_gradient(0);
    RealVector m_grad_f(numContAleatUncVars, false);
    for (size_t i=0; i<numContAleatUncVars; ++i)
      m_grad_f[i] = -grad_f[i];

    // solve for lambda : [A]{lambda} = {m_grad_f}
    int ierr, nsetp, m = numContAleatUncVars, n = 1;
    Real res_norm;
    IntVector index(1);
    RealVector lambda(1), w(1), bnd(2);
    // lawson_hanson2.f90: BVLS ignore bounds based on huge(), so +/-DBL_MAX
    // is sufficient here
    bnd[0] = -DBL_MAX; bnd[1] = DBL_MAX;
    BVLS_WRAPPER_FC(A.values(), m, n, m_grad_f.values(), bnd.values(),
		    lambda.values(), res_norm, nsetp, w.values(),
		    index.values(), ierr);
    if (ierr) {
      Cerr << "\nError: BVLS failed in constraint_penalty() in NonDGR"
	   << std::endl;
      abort_handler(-1);
    }

    lagrangeMult = lambda[0];
    return lagrangeMult*c_viol;
#endif // DAKOTA_F90
  }
  else
    return 0.; // add NO_PENALTY to the enum instead?
}

void NonDGlobalReliability::print_results(std::ostream& s, short results_state)
{
  size_t i, j, wpp7 = write_precision + 7;
  const StringArray& fn_labels = iteratedModel.response_labels();
  s << "-----------------------------------------------------------------------"
    << "------";

  print_densities(s);

  // output CDF/CCDF level mappings (replaces NonD::print_level_mappings())
  s << std::scientific << std::setprecision(write_precision)
    << "\nLevel mappings for each response function:\n";
  for (i=0; i<numFunctions; i++) {

    size_t num_levels = computedRespLevels[i].length();
    if (num_levels) {
      if (cdfFlag)
        s << "Cumulative Distribution Function (CDF) for ";
      else
        s << "Complementary Cumulative Distribution Function (CCDF) for ";
      s << fn_labels[i] << ":\n     Response Level  Probability Level  "
	<< "Reliability Index  General Rel Index\n     --------------  "
	<< "-----------------  -----------------  -----------------\n";
      for (j=0; j<num_levels; j++)
        s << "  " << std::setw(wpp7) << computedRespLevels[i][j]
	  << "  " << std::setw(wpp7) << computedProbLevels[i][j]
	  << std::setw(2*write_precision+18) << computedGenRelLevels[i][j]
	  << '\n';
    }
  }

  s << "-----------------------------------------------------------------------"
    << "------" << std::endl;
}

} // namespace Dakota


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

#include "gui/camera.h"
#include <cmath>
#include <mutex>
#include <Eigen/LU>
#include <Eigen/Dense>
#include <QDebug>
#include <QTimer>
#include <iostream>

using Eigen::Vector3f;
using Eigen::Vector2f;
using Eigen::AngleAxis;

Camera::Camera() {
    //mtx_ = new std::mutex();
    fov_current_ = 60.0f;
    fov_future_ = 60.0f;
    aspect_ = 1.0f;
    depth_near_ = 1.0f;
    depth_far_ = 1000.f;

    roll_correction_ = true;
    always_update_ = false;

    rotation_current_ = AngleAxis<float>(0, Vector3f(1.0f, 0.0f, 0.0f));
    rotation_future_ = AngleAxis<float>(0, Vector3f(1.0f, 0.0f, 0.0f));

    translation_current_ = Vector3f(0, 0, 0);
    translation_future_ = Vector3f(0, 0, 0);

    translation_speed_ = 1;

    projection_dirty_ = true;

    timer_ = new QTimer();
    timer_->connect(timer_, &QTimer::timeout, [&] () {
        Eigen::Vector3f trans_diff = translation_future_ - translation_current_;
        bool good_enough_rotation = rotation_current_.isApprox(rotation_future_);
        float fov_diff = fov_future_ - fov_current_;

        if(trans_diff.norm() > 1e-4 || !good_enough_rotation || fabs(fov_diff) > 1e-10) {

            translation_current_ = translation_current_ + trans_diff * 0.5;
            rotation_current_ = rotation_current_.slerp(0.5, rotation_future_);
            fov_current_ = fov_future_ * 0.5 + fov_current_ * 0.5;

            if(fabs(fov_diff) > 1e-10)
                projection_dirty_ = true;

            emit updated();
        }
        else {
            if(always_update_)
                emit updated();
            translation_speed_ = 1;
        }

    });

    timer_->start(1000/60);
}

Camera::~Camera() {
    //delete mtx_;
}

double angle (Vector3f a, Vector3f b){
    double dotp = a.dot(b);
    if(dotp >= 1 || dotp <= -1){
        return 0.0;
    }
    return acos(dotp);
}

double normalizeRad(double angle){
    if(angle < -M_PI_2){
        return -M_PI_2 - (angle + M_PI_2);
    } else if(angle > M_PI_2) {
        return M_PI_2 - (angle - M_PI_2);
    }
    return angle;
}

Eigen::Quaternionf rollcorrect(Eigen::Quaternionf rotation){
    Vector3f local_z_axis = rotation.inverse() * -Vector3f::UnitZ();
    Vector3f local_x_axis = rotation.inverse() * Vector3f::UnitX();
    Vector3f local_y_axis = rotation.inverse() * Vector3f::UnitY();

    Vector3f z_proj_zx = local_z_axis; z_proj_zx(1) = 0; z_proj_zx.normalize();
    Vector3f x_proj_zx = local_x_axis; x_proj_zx(1) = 0; x_proj_zx.normalize();
    Vector3f y_proj_zx = local_y_axis; y_proj_zx(1) = 0; y_proj_zx.normalize();

    double pitch = angle(local_z_axis, z_proj_zx);
    double roll = angle(local_x_axis, x_proj_zx);

    int proj_side = local_x_axis.cross(x_proj_zx).dot(local_z_axis) > 0 ? -1 : 1;
    int side_up = local_y_axis.dot(Vector3f::UnitY()) > 0 ? 1 : -1;

    if(side_up == -1){
        roll = M_PI - roll;
    }

    roll = roll * proj_side * side_up;

    double norm_pitch = normalizeRad(pitch);
    double correction_factor = (M_PI_2 - fabs(norm_pitch)) / M_PI_2;
    correction_factor = pow(correction_factor - 0.5, 1);

    if(pitch > 0.7){
        correction_factor = 0;
    }

    AngleAxis<float> roll_correction(correction_factor*-roll, Vector3f::UnitZ());
    rotation = roll_correction * rotation;

    return rotation;
}

void Camera::setFoV(float fov) {
    fov_future_ = fov;
    projection_dirty_ = true;
}

void Camera::setAspect(float aspect) {
    aspect_ = aspect;
    projection_dirty_ = true;
}

void Camera::setDepthRange(float near, float far) {
    depth_near_ = near;
    depth_far_ = far;
    projection_dirty_ = true;
}

void Camera::setPosition(const Eigen::Vector3f& pos) {
    translation_future_ = pos;
}

void Camera::recalculateProjectionMatrix() {
    // Code from Mesa project, src/glu/sgi/libutil/project.c
    projection_matrix_.setIdentity();
    float radians = fov_current_ / 2 * M_PI / 180;

    float deltaZ = depth_far_ - depth_near_;
    float sine = sin(radians);
    if ((deltaZ == 0) || (sine == 0) || (aspect_ == 0)) {
        return;
    }
    float cotangent = cos(radians) / sine;

    projection_matrix_(0, 0) = cotangent / aspect_;
    projection_matrix_(1, 1) = cotangent;
    projection_matrix_(2, 2) = -(depth_far_ + depth_near_) / deltaZ;
    projection_matrix_(3, 2) = -1;
    projection_matrix_(2, 3) = -2 * depth_near_ * depth_far_ / deltaZ;
    projection_matrix_(3, 3) = 0;
    projection_dirty_ = false;
}

Eigen::Affine3f Camera::modelviewMatrix() {
    return rotation_current_  * Eigen::Translation3f(translation_current_) * Eigen::Affine3f::Identity();
}

Eigen::Affine3f Camera::projectionMatrix() const {
    if (projection_dirty_) {
        const_cast<Camera*>(this)->recalculateProjectionMatrix();
    }
    return projection_matrix_;
}

void Camera::translate(const Eigen::Vector3f& pos) {
    translation_future_ = Eigen::Translation3f(rotation_current_.inverse() * (translation_speed_ * pos)) * translation_current_;

    if(translation_speed_ < 10){
        translation_speed_ *= 1.1f; // If succesive tranlations are performed, speed things up
    }

    emit modified();
}

void Camera::setRotate3D(const Eigen::Vector3f& rot) {
    Eigen::AngleAxis<float> aaZ(rot.z(), Eigen::Vector3f::UnitZ());
    Eigen::AngleAxis<float> aaY(rot.y(), Eigen::Vector3f::UnitY());
    Eigen::AngleAxis<float> aaX(rot.x(), Eigen::Vector3f::UnitX());

    rotation_future_ = aaZ * aaY * aaX;
    emit modified();
}

void Camera::rotate2D(float x, float y) {
    Vector2f rot = Vector2f(x, y);

    AngleAxis<float> rotX(rot.x(), Vector3f::UnitY()); // look left right
    AngleAxis<float> rotY(rot.y(), Vector3f::UnitX()); // look up down

    rotation_future_ = (rotX * rotY) * rotation_current_;

    if(roll_correction_){
        rotation_future_ = rollcorrect(rotation_future_);
    }

    emit modified();
}

void Camera::rotate3D(float _yaw, float _pitch, float _roll) {

    AngleAxis<float> rotX(_yaw, Vector3f::UnitY()); // look left right
    AngleAxis<float> rotY(_pitch, Vector3f::UnitX()); // look up down

    rotation_future_ = (rotX * rotY) * rotation_future_;

    auto clamp = [] (double num, double low, double high) {
        if (num > high)
            return high;
        if (num < low)
            return low;
        return num;
    };

    Eigen::Matrix3f r = rotation_future_.toRotationMatrix();
    double roll = -atan2(r(0,2), r(1, 2));
    double pitch = acos(r(2,2));
    //double yaw = atan2(r(2, 0), r(2, 1));

    Vector3f dir = rotation_future_ * Vector3f::UnitZ();
    double dotp = dir.dot(Vector3f::UnitZ());

    double sign = dir.dot(Vector3f::UnitY()) > 0 ? 1.0 : -1.0;
    double angle = sign * acos(dotp);

/*
    qDebug() << "Y angle" << angle;
    qDebug() << "Y angle (DEG)" << (angle/M_PI) * 180;
    qDebug() << "Roll" << roll;
    qDebug() << "Pitch" << pitch;
    qDebug() << "Yaw" << yaw;
*/
    double correction_factor = 1.0 - fabs(pitch-M_PI/2)/(M_PI/2);
    correction_factor = -0.5 + 1.5 * correction_factor;
    correction_factor = clamp(correction_factor, 0, 1);

    if(angle < 0)
        correction_factor = 0;

    //qDebug() << "Correction factor:" << correction_factor;

    AngleAxis<float> roll_correction(correction_factor*-roll, Vector3f::UnitZ());
    rotation_future_ = roll_correction * rotation_future_;
    rotation_future_.normalize();

    emit modified();
}

void Camera::adjustFov(int val) {
    // Mouse seems to move in increments of 120
    val = -val/60.0f;
    if (fov_future_ + val < 100.0f && fov_future_ + val > 2.0f)
        setFoV(fov_future_ + val);

}

void Camera::birds_eye() {
    rotation_future_ = AngleAxis<float>(0, Vector3f(1, 0, 0));
    translation_future_ = 20 * -Vector3f::UnitZ();
}

void Camera::toggleRollCorrection(){
    roll_correction_ = !roll_correction_;
}

void Camera::toggleRollCorrection(bool on){
    roll_correction_ = on;
}


#ifndef INVKIN_H_INCLUDED
#define INVKIN_H_INCLUDED

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include "pinocchio/math/rpy.hpp"
#include "pinocchio/spatial/explog.hpp"

class InvKin {
  /* Planner that outputs current and future locations of footsteps, the
     reference trajectory of the base and the position, velocity, acceleration
     commands for feet in swing phase based on the reference velocity given by
     the user and the current position/velocity of the base */

  /*private:
   // Inputs of the constructor
   double dt;       // Time step of the contact sequence (time step of the MPC)
   double dt_tsid;  // Time step of TSID
   double T_gait;   // Gait period
   double T_mpc;    // MPC period (prediction horizon)
   double h_ref;    // Reference height for the trunk
   int k_mpc;       // Number of TSID iterations for one iteration of the MPC
   bool on_solo8;   //  Whether we are working on solo8 or not

   // Predefined quantities
   double k_feedback = 0.03;  // Feedback gain for the feedback term of the
   planner double g = 9.81;           // Value of the gravity acceleartion
   double L = 0.155;          // Value of the maximum allowed deviation due to
   leg length bool is_static = false;    // Flag for static gait

   // Number of time steps in the prediction horizon
   int n_steps; // T_mpc / time step of the MPC

   // Feet index vector
   std::vector<int> feet;
   std::vector<double> t0s;
   double t_remaining = 0.0;
   double t_swing[4] = {0.0, 0.0, 0.0, 0.0};

   // Constant sized matrices
   Eigen::MatrixXd fsteps = Eigen::MatrixXd::Zero(N0_gait, 13);
   Eigen::Matrix<double, 3, 4> shoulders = Eigen::Matrix<double, 3, 4>::Zero();
   // Position of shoulders in local frame Eigen::Matrix<double, 19, 1> q_static
   = Eigen::Matrix<double, 19, 1>::Zero(); Eigen::Matrix<double, 3, 1>
   RPY_static = Eigen::Matrix<double, 3, 1>::Zero(); Eigen::Matrix<double, 1,
   12> o_feet_contact = Eigen::Matrix<double, 1, 12>::Zero();  // Feet matrix in
   world frame Eigen::Matrix<double, 3, 4> next_footstep = Eigen::Matrix<double,
   3, 4>::Zero();  // To store the result of the compute_next_footstep function
   Eigen::Matrix<double, 3, 3> R =
       Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for
   compute_footstep function Eigen::Matrix<double, 3, 3> R_1 =
       Eigen::Matrix<double, 3, 3>::Zero();  // Predefined matrices for
   compute_next_footstep function Eigen::Matrix<double, 3, 3> R_2 =
   Eigen::Matrix<double, 3, 3>::Zero(); Eigen::Matrix<double, N0_gait, 1> dt_cum
   = Eigen::Matrix<double, N0_gait, 1>::Zero(); Eigen::Matrix<double, N0_gait,
   1> angle = Eigen::Matrix<double, N0_gait, 1>::Zero(); Eigen::Matrix<double,
   N0_gait, 1> dx = Eigen::Matrix<double, N0_gait, 1>::Zero();
   Eigen::Matrix<double, N0_gait, 1> dy = Eigen::Matrix<double, N0_gait,
   1>::Zero(); Eigen::Matrix<double, 3, 1> q_tmp = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 3, 1> q_dxdy = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 3, 1> RPY = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 3, 1> b_v_cur = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 6, 1> b_v_ref = Eigen::Matrix<double, 6,
   1>::Zero(); Eigen::Matrix<double, 3, 1> cross = Eigen::Matrix<double, 3,
   1>::Zero(); Eigen::Matrix<double, 6, 1> vref_in = Eigen::Matrix<double, 6,
   1>::Zero();

   Eigen::Matrix<double, N0_gait, 5> gait_p = Eigen::Matrix<double, N0_gait,
   5>::Zero();  // Past gait Eigen::MatrixXd gait_f =
   Eigen::MatrixXd::Zero(N0_gait, 5);                                // Current
   and future gait Eigen::Matrix<double, N0_gait, 5> gait_f_des =
   Eigen::Matrix<double, N0_gait, 5>::Zero();  // Future desired gait

   // Time interval vector
   Eigen::Matrix<double, 1, Eigen::Dynamic> dt_vector;

   // Reference trajectory matrix of size 12 by (1 + N)  with the current state
   of
   // the robot in column 0 and the N steps of the prediction horizon in the
   others Eigen::MatrixXd xref;

   // Foot trajectory generator
   double max_height_feet = 0.05;  // * (1000/312.5);  // height * correction
   coefficient double t_lock_before_touchdown = 0.07; std::vector<TrajGen>
   myTrajGen;

   // Variables for foot trajectory generator
   int i_end_gait = 0;
   Eigen::Matrix<double, 1, 4> t_stance =
       Eigen::Matrix<double, 1, 4>::Zero();  // Total duration of current stance
   phase for each foot Eigen::Matrix<double, 2, 4> footsteps_target =
   Eigen::Matrix<double, 2, 4>::Zero(); Eigen::MatrixXd goals =
   Eigen::MatrixXd::Zero(3, 4);   // Store 3D target position for feet
   Eigen::MatrixXd vgoals = Eigen::MatrixXd::Zero(3, 4);  // Store 3D target
   velocity for feet Eigen::MatrixXd agoals = Eigen::MatrixXd::Zero(3, 4);  //
   Store 3D target acceleration for feet Eigen::Matrix<double, 6, 4> mgoals =
       Eigen::Matrix<double, 6, 4>::Zero();  // Storage variable for the
   trajectory generator

   Eigen::Matrix<double, 11, 4> res_gen = Eigen::Matrix<double, 11, 4>::Zero();
   // Result of the generator
   */

 private:
  // Inputs of the constructor
  double dt;  // Time step of the contact sequence (time step of the MPC)

  // Matrices initialisation
  Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();
  Eigen::Matrix<double, 3, 3> base_orientation_ref = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_angularvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_angularacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_position_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_linearvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> base_linearacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 6, 1> x_ref = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
  Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
  Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
  Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
  Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();

  Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
  Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
  Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();

  Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
  Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
  Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);

  // Gains
  double Kp_base_orientation = 100.0;
  double Kd_base_orientation = 2.0 * std::sqrt(Kp_base_orientation);

  double Kp_base_position = 100.0;
  double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);

  double Kp_flyingfeet = 1000.0;
  double Kd_flyingfeet = 5.0 * std::sqrt(Kp_flyingfeet);

 public:
  InvKin();
  InvKin(double dt_in);

  Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);

  Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd &x_cmd, const Eigen::MatrixXd &contacts,
                                    const Eigen::MatrixXd &goals, const Eigen::MatrixXd &vgoals, const Eigen::MatrixXd &agoals,
                                    const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
                                    const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb,
                                    const Eigen::MatrixXd &ab, const Eigen::MatrixXd &Jb);
  Eigen::MatrixXd computeInvKin(const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
                                const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb, const Eigen::MatrixXd &ab,
                                const Eigen::MatrixXd &Jb);
  Eigen::MatrixXd get_q_step();
  Eigen::MatrixXd get_dq_cmd();
  /*void Print();

  int create_walk();
  int create_trot();
  int create_gait_f();
  int roll(int k);
  int compute_footsteps(Eigen::MatrixXd q_cur, Eigen::MatrixXd v_cur,
  Eigen::MatrixXd v_ref); double get_stance_swing_duration(int i, int j, double
  value); int compute_next_footstep(int i, int j); int
  getRefStates(Eigen::MatrixXd q, Eigen::MatrixXd v, Eigen::MatrixXd vref,
  double z_average); int update_target_footsteps(); int
  update_trajectory_generator(int k, double h_estim); int run_planner(int k,
  const Eigen::MatrixXd &q, const Eigen::MatrixXd &v, const Eigen::MatrixXd
  &b_vref, double h_estim, double z_average);*/

  // Accessors (to retrieve C data from Python)
  /*Eigen::MatrixXd get_xref();
  Eigen::MatrixXd get_fsteps();
  Eigen::MatrixXd get_gait();
  Eigen::MatrixXd get_goals();
  Eigen::MatrixXd get_vgoals();
  Eigen::MatrixXd get_agoals();*/
};

template<typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
{
	Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
	double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) *svd.singularValues().array().abs()(0);
	return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
}

#endif  // INVKIN_H_INCLUDED


// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

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

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Julius Ziegler <ziegler@fzi.de>
 * \date    2012-03-06
 *
 */
//----------------------------------------------------------------------

#if !defined(LOCALGEOGRAPHICCS_HPP)
#define LOCALGEOGRAPHICCS_HPP

#include "convert_coordinates.hpp"

#include <boost/tuple/tuple.hpp>

#include <utility>

struct LocalGeographicCS
{
  LocalGeographicCS();
  LocalGeographicCS( double lat0, double lon0 );

  void set_origin( double lat0, double lon0 );

  void ll2xy( double lat, double lon, double& x, double& y ) const;
  void xy2ll( double x, double y, double& lat, double& lon ) const;

  boost::tuple<double, double> ll2xy( double lat, double lon ) const;
  boost::tuple<double, double> xy2ll( double x, double y ) const;

  // operate on containers
  template<class ItIn, class ItOut>
  void ll2xy( const ItIn& lat_begin, const ItIn& lat_end, const ItIn& lon_begin, const ItOut& x_begin, const ItOut& y_begin ) const;

  template<class ItIn, class ItOut>
  void xy2ll( const ItIn& x_begin, const ItIn& x_end, const ItIn& y_begin, const ItOut& lat_begin, const ItOut& lon_begin ) const;

private:
  double _scale;
  double _x0, _y0;
};

inline LocalGeographicCS::LocalGeographicCS( double lat0, double lon0 )
{
  set_origin( lat0, lon0 );
}

inline LocalGeographicCS::LocalGeographicCS()
{}

inline void LocalGeographicCS::set_origin( double lat0, double lon0 )
{
  _scale = convert_coordinates::lat_to_scale( lat0 );
  convert_coordinates::latlon_to_mercator( lat0, lon0, _scale, _x0, _y0 );
}

inline void LocalGeographicCS::ll2xy( double lat, double lon, double& x, double& y ) const
{
  convert_coordinates::latlon_to_mercator( lat, lon, _scale, x, y );
  x -= _x0;
  y -= _y0;
}

inline boost::tuple<double, double> LocalGeographicCS::ll2xy( double lat, double lon ) const
{
  double x, y;
  ll2xy( lat, lon, x, y );
  return boost::make_tuple( x, y );
}

inline void LocalGeographicCS::xy2ll( double x, double y, double& lat, double& lon ) const
{
  x += _x0;
  y += _y0;

  convert_coordinates::mercator_to_latlon( x, y, _scale, lat, lon );
}

inline boost::tuple<double, double> LocalGeographicCS::xy2ll( double x, double y ) const
{
  double lat, lon;
  xy2ll( x, y, lat, lon );
  return boost::make_tuple( lat, lon );
}

// operate on containers
template<class ItIn, class ItOut>
void LocalGeographicCS::ll2xy( const ItIn& lat_begin, const ItIn& lat_end, const ItIn& lon_begin, const ItOut& x_begin, const ItOut& y_begin ) const
{
  ItIn lat = lat_begin;
  ItIn lon = lon_begin;
  ItOut x = x_begin;
  ItOut y = y_begin;

  for( ; lat != lat_end; lat++, lon++, x++, y++ )
    {
      ll2xy( *lat, *lon, *x, *y );
    }
}

template<class ItIn, class ItOut>
void LocalGeographicCS::xy2ll( const ItIn& x_begin, const ItIn& x_end, const ItIn& y_begin, const ItOut& lat_begin, const ItOut& lon_begin ) const
{
  ItIn x = x_begin;
  ItIn y = y_begin;

  ItOut lat = lat_begin;
  ItOut lon = lon_begin;

  for( ; x != x_end; lat++, lon++, x++, y++ )
    xy2ll( *x, *y, *lat, *lon );
}

#endif


//=============================================================================================================
/**
* @file     spectrogram.cpp
* @author   Martin Henfling <martin.henfling@tu-ilmenau.de>;
*           Daniel Knobl <daniel.knobl@tu-ilmenau.de>;
* @version  1.0
* @date     September, 2015
*
* @section  LICENSE
*
* Copyright (C) 2014, Martin Henfling and Daniel Knobl. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    Implementation of spectrogram class.
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "spectrogram.h"
#include "math.h"

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/SparseCore>
#include <unsupported/Eigen/FFT>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

VectorXd Spectrogram::gauss_window (qint32 sample_count, qreal scale, quint32 translation)
{
    VectorXd gauss = VectorXd::Zero(sample_count);

    for(qint32 n = 0; n < sample_count; n++)
    {
        qreal t = (qreal(n) - translation) / scale;
        gauss[n] = exp(-3.14 * pow(t, 2))*pow(sqrt(scale),(-1))*pow(qreal(2),(0.25));
    }

    return gauss;
}

//-----------------------------------------------------------------------------------------------------------------

MatrixXd Spectrogram::make_spectrogram(VectorXd signal, qint32 window_size = 0)
{
    if(window_size == 0)
        window_size = signal.rows()/4;

    Eigen::FFT<double> fft;
    MatrixXd tf_matrix = MatrixXd::Zero(signal.rows()/2, signal.rows());

    for(qint32 translate = 0; translate < signal.rows(); translate++)
    {
        VectorXd envelope = gauss_window(signal.rows(), window_size, translate);

        VectorXd windowed_sig = VectorXd::Zero(signal.rows());
        VectorXcd fft_win_sig = VectorXcd::Zero(signal.rows());

        VectorXd real_coeffs = VectorXd::Zero(signal.rows()/2);

        for(qint32 sample = 0; sample < signal.rows(); sample++)
            windowed_sig[sample] = signal[sample] * envelope[sample];

        fft.fwd(fft_win_sig, windowed_sig);

        for(qint32 i= 0; i<signal.rows()/2; i++)
        {
            qreal value = pow(abs(fft_win_sig[i]), 2.0);
            real_coeffs[i] = value;
        }

        tf_matrix.col(translate) = real_coeffs;
    }
    return tf_matrix;
}