/*!
MIT License

Copyright (c) 2022 Gotelli Andrea

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

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

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

#include <ros/ros.h>
#include <Eigen/Dense>

#include <std_msgs/Float32.h>

#include <dynamic_reconfigure/server.h>

#include <gazebo_msgs/GetLinkState.h>
#include <gazebo_msgs/ApplyBodyWrench.h>

#include "cpr_messages/PIDControllerTuningConfig.h"
#include "cpr_messages/IGMParams.h"
#include "cpr_messages/SE3Pose.h"

#include "cpr_geometry/skew_operations.h"

#include <yaml-cpp/yaml.h>
//#include <log2plot/logger.h>
//#include <ros/package.h>

using namespace std;

Eigen::Affine3d GetDesiredPose(const double &set_point, const int controlled_dof)
{
  Eigen::Affine3d desired_pose = Eigen::Affine3d::Identity();
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();

  //  Depending on the dof to control, define the desired pose from set point
  switch (controlled_dof) {
  case 0: // case x is controlled
    desired_pose.translation().x() = set_point;
    break;
  case 1: // case y is controlled
    desired_pose.translation().y() = set_point;
    break;
  case 2: // case z is controlled
    desired_pose.translation().z() = set_point;
    break;
  case 3: // case roll is controlled
    R = Eigen::AngleAxisd(set_point, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ());
    desired_pose.linear() = R;
    //LogRotationMatrix( R );
    break;
  case 4: // case pitch is controlled
    R = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(set_point, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
    desired_pose.linear() = R;
    //LogRotationMatrix( R );
    break;
  case 5: // case yaw is controlled
    R = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(set_point, Eigen::Vector3d::UnitZ());
    desired_pose.linear() = R;
    //LogRotationMatrix( R );
    break;
  }

  return desired_pose;
}

static Eigen::Affine3d desired_pose = Eigen::Affine3d::Identity();
static bool callback_executed = false;
static bool just_updated = false;
void IGMParametersCallback(const cpr_messages::IGMParams::ConstPtr &igm_msg)
{
  desired_pose.translation() = Eigen::Vector3d(igm_msg->distal_plate_pose.position.x,
                                                             igm_msg->distal_plate_pose.position.y,
                                                             igm_msg->distal_plate_pose.position.z);

  Eigen::Quaternion<double> q(igm_msg->distal_plate_pose.orientation.w,
                              igm_msg->distal_plate_pose.orientation.x,
                              igm_msg->distal_plate_pose.orientation.y,
                              igm_msg->distal_plate_pose.orientation.z);

  desired_pose.linear() = q.toRotationMatrix();

  just_updated = true;

  if(!callback_executed)
    callback_executed = true;
}

Eigen::Affine3d ToAffine3d( const geometry_msgs::Pose &pose)
{
  //  Directly compute the position
  Eigen::Affine3d eigen_pose = Eigen::Affine3d::Identity();
  eigen_pose.translation() << pose.position.x,
                              pose.position.y,
                              pose.position.z;

  //  Copy the quaternion for the  orientation
  Eigen::Quaternion<double> q(pose.orientation.w,
                              pose.orientation.x,
                              pose.orientation.y,
                              pose.orientation.z);

  //  Convert the quaternion to a rotation matrix
  eigen_pose.linear() = q.toRotationMatrix();

  return eigen_pose;
}

Eigen::VectorXd ComputeSE3Error(const Eigen::Affine3d &desired_pose, const geometry_msgs::Pose &current_pose)
{

  //  Directly compute the position error
  Eigen::Vector3d position_error(desired_pose.translation().x() - current_pose.position.x,
                                 desired_pose.translation().y() - current_pose.position.y,
                                 desired_pose.translation().z() - current_pose.position.z);

  //  Copy the quaternion for the current distal plate orientation
  Eigen::Quaternion<double> q_current(current_pose.orientation.w,
                                      current_pose.orientation.x,
                                      current_pose.orientation.y,
                                      current_pose.orientation.z);

  //  Convert the quaternion to a rotation matrix
  Eigen::Matrix3d R_current = q_current.toRotationMatrix();

  //  Obtain the desired rotation matrix
  Eigen::Matrix3d R_desired = desired_pose.linear().matrix();

  //  Compute the error in the orientation
  Eigen::Vector3d orientation_error = cpr_geometry::inv_hat(R_current.transpose()*R_desired -
                                                              R_current*R_desired.transpose());

  //  Compose the vector containing the vector in SE(3)
  Eigen::VectorXd SE3_error(6);
  SE3_error << position_error, orientation_error;
  return SE3_error;
}

Eigen::Matrix<double, 6, 6> ToGainMatrix(const double &e_x,
                                         const double &e_y,
                                         const double &e_z,
                                         const double &e_roll,
                                         const double &e_pitch,
                                         const double &e_yaw)
{
  Eigen::VectorXd proportional_gains(6);
  proportional_gains << e_x, e_y, e_z, e_roll, e_pitch, e_yaw;
  return proportional_gains.asDiagonal();
}

double LimitAngle(double angle)
{
  double corrected_angle = angle;
  if(angle > 3)
    corrected_angle = angle - M_PI;
  if(angle < -3)
    corrected_angle = angle + M_PI;

  return corrected_angle;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "controller");
  ros::NodeHandle nh;

  const double dt = 0.001;

  ros::Subscriber igm_parameters_listener = nh.subscribe<cpr_messages::IGMParams>("/igm_parameters",1, IGMParametersCallback);

  ros::ServiceClient link_state_client = nh.serviceClient<gazebo_msgs::GetLinkState>("/gazebo/get_link_state");
  gazebo_msgs::GetLinkStateRequest link_state_request;
  gazebo_msgs::GetLinkStateResponse link_state_response;
  link_state_request.link_name = "distal_plate::distal_plate";
  link_state_request.reference_frame = "world";

  ros::ServiceClient apply_wrench_client = nh.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench");
  gazebo_msgs::ApplyBodyWrenchRequest apply_body_wrench_request;
  gazebo_msgs::ApplyBodyWrenchResponse apply_body_wrench_response;
  apply_body_wrench_request.body_name = "distal_plate::distal_plate";
  apply_body_wrench_request.reference_frame = "world";
  apply_body_wrench_request.duration.fromSec(dt);

  ros::Publisher distal_plate_wrench_controller = nh.advertise<geometry_msgs::Wrench>("/controller_wrench", 1);
  geometry_msgs::Wrench correction_wrench;

  //  Here the declaration of the Server element.
  dynamic_reconfigure::Server<cpr_messages::PIDControllerTuningConfig> server;

  cpr_messages::SE3Pose desired_values;
  ros::Publisher desired_values_publisher = nh.advertise<cpr_messages::SE3Pose>("/desired_values", 1);

  cpr_messages::SE3Pose current_values;
  ros::Publisher current_values_publisher = nh.advertise<cpr_messages::SE3Pose>("/current_values", 1);

  ros::Publisher time_publisher = nh.advertise<std_msgs::Float32>("/time", 1);

  double set_point = 0.0;
  ros::Subscriber set_point_listener = nh.subscribe<std_msgs::Float32>("/desired_value", 1,
                                                                            [&set_point](const std_msgs::Float32::ConstPtr &msg){
                                                                              set_point = static_cast<double>(msg->data);
                                                                            });

  double kpe_x     = 2.5;
  double kpe_y     = 2.5;
  double kpe_z     = 2.5;
  double kpe_roll  = 0.04;
  double kpe_pitch = 0.04;
  double kpe_yaw   = 0.4;

  Eigen::Matrix<double, 6, 6> Kp = 2*ToGainMatrix(kpe_x, kpe_y, kpe_z, kpe_roll, kpe_pitch, kpe_yaw);

  double kde_x     = 2.2;
  double kde_y     = 2.2;
  double kde_z     = 2.5;
  double kde_roll  = 0.035;
  double kde_pitch = 0.035;
  double kde_yaw   = 0.4;

  Eigen::Matrix<double, 6, 6> Kd = 1.8*ToGainMatrix(kde_x, kde_y, kde_z, kde_roll, kde_pitch, kde_yaw);

//  double kie_x     = 0.0;
//  double kie_y     = 0.0;
//  double kie_z     = 0.0;
//  double kie_roll  = 0.0;
//  double kie_pitch = 0.0;
//  double kie_yaw   = 0.0;
  double kie_x     = 5;
  double kie_y     = 5;
  double kie_z     = 5;
  double kie_roll  = 0.5;
  double kie_pitch = 0.5;
  double kie_yaw   = 0.5;

  Eigen::Matrix<double, 6, 6> Ki = 0.01*ToGainMatrix(kie_x, kie_y, kie_z, kie_roll, kie_pitch, kie_yaw);

  int controlled_dof = 2;
  bool start = false;
  server.setCallback( [&](const cpr_messages::PIDControllerTuningConfig &config, uint32_t){
    start = config.start;

//    Kp = ToGainMatrix(config.kde_x, config.kde_y, config.kde_z, config.kde_roll, config.kde_pitch, config.kde_yaw);

//    Kd = ToGainMatrix(config.kde_x, config.kde_y, config.kde_z, config.kde_roll, config.kde_pitch, config.kde_yaw);

//    Ki = ToGainMatrix(config.kie_x, config.kie_y, config.kie_z, config.kie_roll, config.kie_pitch, config.kie_yaw);

  } );

  Eigen::VectorXd SE3_error = Eigen::VectorXd::Zero(6);
  Eigen::VectorXd previous_SE3_error = Eigen::VectorXd::Zero(6);
  Eigen::VectorXd SE3_error_derivative = Eigen::VectorXd::Zero(6);
  Eigen::VectorXd SE3_error_integral = Eigen::VectorXd::Zero(6);
  Eigen::VectorXd PID_output = Eigen::VectorXd::Zero(6);

  YAML::Emitter emitter;
  std::vector<double> z_des;
  std::vector<double> roll_des;
  std::vector<double> pitch_des;

  std::vector<double> z;
  std::vector<double> roll;
  std::vector<double> pitch;

  std::vector<double> time;

  std_msgs::Float32 time_msgs;

  ros::Rate rate(1.0/dt);
  while(ros::ok()){
    ros::spinOnce();

    time.push_back( ros::Time::now().toSec() );

    time_msgs.data = ros::Time::now().toSec();
    time_publisher.publish( time_msgs );

    if(!callback_executed /*|| !start*/) {
      cout << "Waiting..." << endl;
      PID_output = Eigen::VectorXd::Zero(6);
      SE3_error_integral = Eigen::VectorXd::Zero(6);
      cout << "Desired Pose : " << endl << GetDesiredPose(set_point, controlled_dof).matrix() << endl << endl;
      rate.sleep();
      continue;
    }

    //  Otherwise get the actual position of the distal plate
    if(!link_state_client.call(link_state_request, link_state_response)){
      //  If the call has failed just skip
      rate.sleep();
      ROS_WARN_STREAM(link_state_response.status_message);
      continue;
    }

    //  Comput the error in SE3
//    SE3_error = ComputeSE3Error(GetDesiredPose(set_point, controlled_dof),
//                                               link_state_response.link_state.pose);

    SE3_error = ComputeSE3Error(desired_pose, link_state_response.link_state.pose);

    //  Comput the integral
    SE3_error_integral += SE3_error * dt;

    //  Comput the Derivative
    SE3_error_derivative = (SE3_error - previous_SE3_error)/dt;
    previous_SE3_error = SE3_error;

    PID_output = Kp * SE3_error            +  //  P
                 Ki * SE3_error_integral   +  //  I
                 Kd * SE3_error_derivative ;  //  D

//    apply_body_wrench_request.start_time = ros::Time::now();
//    apply_body_wrench_request.wrench.force.x = PID_output[0];
//    apply_body_wrench_request.wrench.force.y = PID_output[1];
//    apply_body_wrench_request.wrench.force.z = PID_output[2];
//    apply_body_wrench_request.wrench.torque.x = PID_output[3];
//    apply_body_wrench_request.wrench.torque.y = PID_output[4];
//    apply_body_wrench_request.wrench.torque.z = PID_output[5];

//    if(!apply_wrench_client.call(apply_body_wrench_request, apply_body_wrench_response))
//      ROS_WARN_STREAM(apply_body_wrench_response.status_message);

    correction_wrench.force.x = PID_output[0];
    correction_wrench.force.y = PID_output[1];
    correction_wrench.force.z = PID_output[2];
    correction_wrench.torque.x = PID_output[3];
    correction_wrench.torque.y = PID_output[4];
    correction_wrench.torque.z = PID_output[5];

    distal_plate_wrench_controller.publish( correction_wrench );

    //  Publish the values for plotting
    desired_values.x = static_cast<float>( desired_pose.translation().x() );
    desired_values.y = static_cast<float>( desired_pose.translation().y() );
    desired_values.z = static_cast<float>( desired_pose.translation().z() );
    desired_values.roll = static_cast<float>( LimitAngle(desired_pose.linear().eulerAngles(0,1,2)[0]) );
    desired_values.pitch = static_cast<float>( LimitAngle(desired_pose.linear().eulerAngles(0,1,2)[1]) );
    desired_values.yaw = static_cast<float>( LimitAngle(desired_pose.linear().eulerAngles(0,1,2)[2]) );

    desired_values_publisher.publish( desired_values );

    Eigen::Affine3d current_pose = ToAffine3d( link_state_response.link_state.pose );

    current_values.x = static_cast<float>( current_pose.translation().x() );
    current_values.y = static_cast<float>( current_pose.translation().y() );
    current_values.z = static_cast<float>( current_pose.translation().z() );
    current_values.roll = static_cast<float>( LimitAngle(current_pose.linear().eulerAngles(0,1,2)[0]) );
    current_values.pitch = static_cast<float>( LimitAngle(current_pose.linear().eulerAngles(0,1,2)[1]) );
    current_values.yaw = static_cast<float>( LimitAngle(current_pose.linear().eulerAngles(0,1,2)[2]) );

    current_values_publisher.publish( current_values );

    rate.sleep();
  }

//  emitter << YAML::BeginMap;

//  emitter << YAML::Key << "desired_values";
//  emitter << YAML::Value << YAML::BeginMap;
//      emitter << YAML::Key << "z_desired" << YAML::Value << YAML::Flow << z_des;
//  emitter << YAML::EndMap;
//  emitter << YAML::Newline;
//  emitter << YAML::Value << YAML::BeginMap;
//      emitter << YAML::Key << "roll_desired" << YAML::Value << YAML::Flow << roll_des;
//  emitter << YAML::EndMap;
//  emitter << YAML::Newline;
//  emitter << YAML::Value << YAML::BeginMap;
//      emitter << YAML::Key << "pitch_desired" << YAML::Value << YAML::Flow << pitch_des;
//  emitter << YAML::EndMap;
//  emitter << YAML::Newline;

//  emitter << YAML::Key << "current_values";
//  emitter << YAML::Value << YAML::BeginMap;
//      emitter << YAML::Key << "z" << YAML::Value << YAML::Flow << z;
//  emitter << YAML::EndMap;
//  emitter << YAML::Newline;
//  emitter << YAML::Value << YAML::BeginMap;
//      emitter << YAML::Key << "roll" << YAML::Value << YAML::Flow << roll;
//  emitter << YAML::EndMap;
//  emitter << YAML::Newline;
//  emitter << YAML::Value << YAML::BeginMap;
//      emitter << YAML::Key << "pitch" << YAML::Value << YAML::Flow << pitch;
//  emitter << YAML::EndMap;
//  emitter << YAML::Newline;

//  emitter << YAML::EndMap;

  return 0;
}


#pragma once
#ifndef OPENGM_PARTITION_HXX
#define OPENGM_PARTITION_HXX

#include <vector>
#include <map>

#ifdef WITH_BOOST
#include <boost/unordered_map.hpp>
#endif

namespace opengm {

/// Disjoint set data structure with path compression.
/// \ingroup datastructures
template<class T = size_t>
class Partition {
public:
   typedef T value_type;

   Partition();
   Partition(const value_type&);

   // query
   value_type find(const value_type&) const; // without path compression
   value_type find(value_type); // with path compression
   value_type numberOfElements() const;
   value_type numberOfSets() const;
   template<class Iterator> void elementLabeling(Iterator) const;
   template<class Iterator> void representatives(Iterator) const;
   void representativeLabeling(std::map<value_type, value_type>&) const;

   #ifdef WITH_BOOST
   void representativeLabeling(boost::unordered_map<value_type, value_type>&) const;
   #endif

   // manipulation
   void reset(const value_type&);
   void merge(value_type, value_type);
   void insert(const value_type&);

private:
   std::vector<value_type> parents_;
   std::vector<value_type> ranks_;
   value_type numberOfElements_;
   value_type numberOfSets_;
};

/// Construct a partition.
template<class T>
Partition<T>::Partition()
: parents_(),
  ranks_(),
  numberOfElements_(0),
  numberOfSets_(0)
{}

/// Construct a partition.
///
/// \param size Number of distinct sets.
///
template<class T>
inline
Partition<T>::Partition
(
   const value_type& size
)
: parents_(static_cast<size_t>(size)),
  ranks_(static_cast<size_t>(size)),
  numberOfElements_(size),
  numberOfSets_(size)
{
   for(T j=0; j<size; ++j) {
      parents_[static_cast<size_t>(j)] = j;
   }
}

/// Reset a partition such that each set contains precisely one element
///
/// \param size Number of distinct sets.
///
template<class T>
inline void
Partition<T>::reset
(
   const value_type& size
)
{
   numberOfElements_ = size;
   numberOfSets_ = size;
   ranks_.resize(static_cast<size_t>(size));
   parents_.resize(static_cast<size_t>(size));
   for(T j=0; j<size; ++j) {
      ranks_[static_cast<size_t>(j)] = 0;
      parents_[static_cast<size_t>(j)] = j;
   }
}

/// Find the representative element of the set that contains the given element.
///
/// This constant function does not compress the search path.
///
/// \param element Element.
///
template<class T>
inline typename Partition<T>::value_type
Partition<T>::find
(
   const value_type& element
) const
{
   // find the root
   value_type root = element;
   while(parents_[static_cast<size_t>(root)] != root) {
      root = parents_[static_cast<size_t>(root)];
   }
   return root;
}

/// Find the representative element of the set that contains the given element.
///
/// This mutable function compresses the search path.
///
/// \param element Element.
///
template<class T>
inline typename Partition<T>::value_type
Partition<T>::find
(
   value_type element // copy to work with
)
{
   // find the root
   value_type root = element;
   while(parents_[static_cast<size_t>(root)] != root) {
      root = parents_[static_cast<size_t>(root)];
   }
   // path compression
   while(element != root) {
      value_type tmp = parents_[static_cast<size_t>(element)];
      parents_[static_cast<size_t>(element)] = root;
      element = tmp;
   }
   return root;
}

/// Merge two sets.
///
/// \param element1 Element in the first set.
/// \param element2 Element in the second set.
///
template<class T>
inline void
Partition<T>::merge
(
   value_type element1,
   value_type element2
)
{
   // merge by rank
   element1 = find(element1);
   element2 = find(element2);
   if(ranks_[static_cast<size_t>(element1)] < ranks_[static_cast<size_t>(element2)]) {
      parents_[static_cast<size_t>(element1)] = element2;
      --numberOfSets_;
   }
   else if(ranks_[static_cast<size_t>(element1)] > ranks_[static_cast<size_t>(element2)]) {
      parents_[static_cast<size_t>(element2)] = element1;
      --numberOfSets_;
   }
   else if(element1 != element2) {
      parents_[static_cast<size_t>(element2)] = element1;
      ++ranks_[static_cast<size_t>(element1)];
      --numberOfSets_;
   }
}

/// Insert new sets.
///
/// \param number Number of sets to insert.
///
template<class T>
inline void
Partition<T>::insert
(
   const value_type& number
)
{
   ranks_.insert(ranks_.end(), static_cast<size_t>(number), T(0));
   parents_.insert(parents_.end(), static_cast<size_t>(number), T(0));
   for(value_type j=numberOfElements_; j<numberOfElements_+number; ++j) {
      parents_[static_cast<size_t>(j)] = j;
   }
   numberOfElements_ += number;
   numberOfSets_ += number;
}

/// Output all elements which are set representatives.
///
/// \param it (Output) Iterator into a container.
///
template<class T>
template<class Iterator>
inline void
Partition<T>::representatives
(
   Iterator it
) const
{
   for(value_type j=0; j<numberOfElements(); ++j) {
      if(parents_[static_cast<size_t>(j)] == j) {
         *it = j;
         ++it;
      }
   }
}

/// Output a continuous labeling of the representative elements.
///
/// \param out (Output) A map that assigns each representative element to its label.
///
template<class T>
inline void
Partition<T>::representativeLabeling
(
   std::map<value_type, value_type>& out
) const
{
   out.clear();
   std::vector<value_type> r(static_cast<size_t>(numberOfSets()));
   representatives(r.begin());
   for(T j=0; j<numberOfSets(); ++j) {
      out[ r[static_cast<size_t>(j)] ] = j;
   }
}

#ifdef WITH_BOOST
/// Output a continuous labeling of the representative elements.
///
/// \param out (Output) A map that assigns each representative element to its label.
///
template<class T>
inline void
Partition<T>::representativeLabeling
(
   boost::unordered_map<value_type, value_type>& out
) const
{
   out.clear();
   std::vector<value_type> r(static_cast<size_t>(numberOfSets()));
   representatives(r.begin());
   for(T j=0; j<numberOfSets(); ++j) {
      out[ r[static_cast<size_t>(j)] ] = j;
   }
}

#endif

/// Output a continuous labeling of all elements.
///
/// \param out (Output) Iterator into a container in which the j-th entry is the label of the element j.
///
template<class T>
template<class Iterator>
inline void
Partition<T>::elementLabeling
(
   Iterator out
) const
{
   std::map<value_type, value_type> rl;
   representativeLabeling(rl);
   for(value_type j=0; j<numberOfElements(); ++j) {
      *out = rl[find(j)];
      ++out;
   }
}

template<class T>
inline typename Partition<T>::value_type
Partition<T>::numberOfElements() const
{
   return numberOfElements_;
}

template<class T>
inline typename Partition<T>::value_type
Partition<T>::numberOfSets() const
{
   return numberOfSets_;
}

} // namespace opengm

#endif // #ifndef OPENGM_PARTITION_HXX


/**
 *  Author: Cedric LE GENTIL
 *
 *  Copyright 2021 Cedric LE GENTIL
 *
 *  This is a simple example of how to generate the UGPMs
 *
 *  Disclaimer:
 *  This code is not optimised neither for performance neither for maintainability.
 *  There might still be errors in the code, logic or procedure. Every feedback is welcomed.
 *
 *  For any further question or if you see any problem in that code
 *  le.gentil.cedric@gmail.com
 **/

#include <iostream>
#include <string>
#include <stdio.h>
#include <fstream>
#include <iomanip>
#include <boost/program_options.hpp>
#include "sensor_input/imu_simulator.h"
#include "imu_preintegration/preintegration.h"
#include "common/random.h"
#include "common/utils.h"
#include <math.h>

std::string get_content(std::string &str, int N){

    int start_of_path;
    std::string path;
    for (int i = 0; i < N; i++) {
        if(int(str[i]) == 58){
            start_of_path = i+1;
        }
    }
    path = str.substr(start_of_path, N);
    path.erase(std::remove_if(path.begin(), path.end(), [](unsigned char x) { return std::isspace(x); }), path.end());

    return path;
}

std::vector<std::string> readYAML()
{
    /* This is a primitive yaml-reader custom build for the expected yaml-file*/

    std::ifstream f;

    f.open ("../config.yaml");   /* open file with filename as argument */
    if (! f.is_open()) {    /* validate file open for reading */
        std::cerr << "error: file open failed.\n";
    }

    std::vector<std::string> expected_strings = {"imu_raw:", "sampling_times:", "save_path", "sum_prior", "g_const"};
    std::vector<std::string> config_content;

    int expected_amount = expected_strings.size();
    int retrieved_amount = 0;

    bool in_ugpm = false;
    std::string line;

    while (std::getline (f, line)) {        /* read each line */
        if(line.find("ugpm:") != std::string::npos){
            in_ugpm = true;
        }
        if(in_ugpm){
            for(std::string str : expected_strings){
                if ( line.find(str) != std::string::npos && retrieved_amount<expected_amount) {
                    int N = line.length();
                    std::string content = get_content(line,N);
                    config_content.push_back(content);
                    retrieved_amount++;
                }
            }
        }
    }
    return config_content;
}

std::vector<std::vector<double>> readCSV(std::string path)
{
    std::ifstream f;
    std::cout << "reading csv file \n";

    f.open (path);   /* open file with filename as argument */
    if (! f.is_open()) {    /* validate file open for reading */
        std::cerr << "error: file open failed.\n";
    }

    std::string line, val;                  /* string for line & value */
    std::vector<std::vector<double>> array;    /* vector of vector<int>  */

    while (std::getline (f, line)) {        /* read each line */
        std::vector<double> v;                 /* row vector v */
        std::stringstream s (line);         /* stringstream line */
        while (getline (s, val, ','))       /* get each value (',' delimited) */
            v.push_back (std::stod (val));  /* add to row vector */
        array.push_back (v);                /* add row vector to array */
    }

    return array;
}

void saveCSV(std::string path, std::vector<std::vector<double>> *array, std::string seperator, int precision, std::string header){

    std::ofstream myfile (path);
    std::cout << "Saving csv to " << path << "\n";

    bool write_header = false;

    if (myfile.is_open()){
        if(!header.empty()){
            myfile << header << "\n";
        }
        for (const auto& inner : *array) {
            for (const auto& item : inner) {

                myfile << std::fixed << std::setprecision(precision) << std::scientific <<item << seperator << " ";
            }
            myfile << "\n";
        }
        myfile.close();
    }else{
        std::cout << "Unable to open file";}
}

celib::ImuData convertArray(std::vector<std::vector<double>> *array){

    std::cout << "converting csv array to ImuData format: \n";

    std::vector<double> imu_raw(7);     // {t, acc_x, acc_y, acc_z, gyr_x, gyr_y, gyr_z}
    celib::ImuData imu_data;

    int i;
    for (auto& row : *array) {               /* iterate over rows */
        celib::ImuSample acc, gyr;
        i = 0;
        for (auto& val : row){               /* iterate over vals */
            imu_raw[i] = val;
            i++;
        }

        double acc_data[3] = {imu_raw[1], imu_raw[2], imu_raw[3]};
        double gyr_data[3] = {imu_raw[4], imu_raw[5], imu_raw[6]};
        acc.t = imu_raw[0];
        gyr.t = imu_raw[0];
        memcpy(acc.data, acc_data, sizeof(acc_data));
        memcpy(gyr.data, gyr_data, sizeof(gyr_data));

        imu_data.acc.push_back(acc);
        imu_data.gyr.push_back(gyr);
        imu_data.acc_var = 1.25e-7;
        imu_data.gyr_var = 2.0e-4;

    }
    return imu_data;
}

std::vector<std::vector<double>> integrate_between_samples(celib::ImuData imu_data, std::vector<std::vector<double>> sample_time_array,
                                                           celib::PreintOption preint_opt){
    /*
        Format is

        t_i delta_t a_x a_y a_z w_x w_y w_z

        t_i is current time
        delta_t is t_i - t_i-1
        a_x, a_y, a_z is defined as v_i - v_i-1
        w_x, w_y, w_z is defined as angle_i - angle_i-1
    */

    double dpdt_x_prior, dpdt_y_prior, dpdt_z_prior = 0;
    double dpdt_x, dpdt_y, dpdt_z, dvdt_x, dvdt_y, dvdt_z, t_iter, t_iter_prev, dt;

    t_iter_prev = sample_time_array[0][0];

    int a = 0;
    int N = sample_time_array.size();

    std::vector<double> data_iter;
    std::vector<std::vector<double>> t, data_array;

    for(int i = 1; i < N; i++){

        data_iter.clear();

        t_iter = sample_time_array[i][0];

        t.clear();
        t.push_back({t_iter});

        celib::PreintPrior prior;
        celib::StopWatch stop_watch;
        stop_watch.start();
        celib::ImuPreintegration preint(imu_data, t_iter_prev, t, preint_opt, prior);
        stop_watch.stop();
        stop_watch.print();

        celib::PreintMeas preint_meas = preint.get(0,0);

        // This conversion between R and euler angles may be a possible source of error
        Eigen::Vector3d euler_t = preint_meas.delta_R.eulerAngles(2, 1, 0);

        dt = t_iter - t_iter_prev;

        data_iter.push_back(t_iter);
        data_iter.push_back(dt);

        for(int a = 0; a < 3; a++){
            for(int b = 0; b < 3; b++){
                data_iter.push_back(preint_meas.delta_R.coeff(a,b));
            }
        }
        for(int a = 0; a < 3; a++){
            data_iter.push_back(preint_meas.delta_v[a]);
        }
        for(int a = 0; a < 3; a++){
            data_iter.push_back(preint_meas.delta_p[a]);
        }
        for(int a = 0; a < 3; a++){
            for(int b = 0; b < 3; b++){
                data_iter.push_back(preint_meas.cov.coeff(a,b));
            }
        }
        for(int a = 3; a < 6; a++){
            for(int b = 3; b < 6; b++){
                data_iter.push_back(preint_meas.cov.coeff(a,b));
            }
        }
        for(int a = 6; a < 9; a++){
            for(int b = 6; b < 9; b++){
                data_iter.push_back(preint_meas.cov.coeff(a,b));
            }
        }

        data_array.push_back(data_iter);
        t_iter_prev = t_iter;
        dpdt_x_prior = dpdt_x;
        dpdt_y_prior = dpdt_y;
        dpdt_z_prior = dpdt_z;
    }
    return data_array;
}

void pre_integrate_between_frames(celib::PreintOption preint_opt){
    std::vector<std::string> config_content = readYAML();
    std::vector<std::vector<double>> imu_array, sample_time_array, data_array;

    imu_array = readCSV(config_content[0]);
    sample_time_array = readCSV(config_content[1]);

    celib::ImuData imu_data = convertArray(&imu_array);

    data_array = integrate_between_samples(imu_data, sample_time_array, preint_opt);
    std::string header_str = "t dt dR_11 dR_12 dR_13 dR_21 dR_22 dR_23 dR_31 dR_32 dR_33";
    header_str += " dv_x dv_y dv_z dp_x dp_y dp_z";
    header_str += " cov_dR_11 cov_dR_12 cov_dR_13 cov_dR_21 cov_dR_22 cov_dR_23 cov_dR_31 cov_dR_32 cov_dR_33";
    header_str += " cov_dv_11 cov_dv_12 cov_dv_13 cov_dv_21 cov_dv_22 cov_dv_23 cov_dv_31 cov_dv_32 cov_dv_33";
    header_str += " cov_dp_11 cov_dp_12 cov_dp_13 cov_dp_21 cov_dp_22 cov_dp_23 cov_dp_31 cov_dp_32 cov_dp_33";
    saveCSV(config_content[2], &data_array, "", 18, header_str);

}

int main(int argc, char* argv[]){

    celib::PreintOption preint_opt;
    bool test_jacobians;

    // Program options
    boost::program_options::options_description opt_description("Allowed options");
    opt_description.add_options()
        ("help,h", "Produce help message")
        ("method,m", boost::program_options::value< std::string >(), "LPM | GPM | UGPM(default)")
        ("length,l", boost::program_options::value< double >(), "Length simulated integration (default = 2.0)")
        ("train,t", boost::program_options::bool_switch(&preint_opt.train_gpm), "Flag to activate training")
        ("jacobian,j", boost::program_options::bool_switch(&test_jacobians), "Flag to activate the testing of the Jacobians")
        ("nb_inference,n", boost::program_options::value< int >(), "Nb of inferrences (to simulate lidar nb of inferences), Default = 1")
        ("quantum,q", boost::program_options::value< double >(), "Time quantum for piece-wise integration (useful for long integration intervals, -1 to deactivate (default = 0.2)")
        ;

    boost::program_options::variables_map var_map;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, opt_description), var_map);
    boost::program_options::notify(var_map);

    if(var_map.count("help")) {
        std::cout << opt_description << std::endl;
        return 1;
    }

    preint_opt.type = celib::UGPM;
    if(var_map.count("method"))
    {
        std::string type = var_map["method"].as<std::string>();
        std::transform(type.begin(), type.end(), type.begin(), [](unsigned char c){ return std::tolower(c); });
        if(type == "lpm") preint_opt.type = celib::LPM;
        else if(type == "gpm") preint_opt.type = celib::GPM;
        else if(type == "ugpm") preint_opt.type = celib::UGPM;
        else
        {
            std::cout << "ERROR: The type of preintegration method is unknown, program stopping now" << std::endl;
            return 1;
        }
    }
    std::string to_print;
    switch (preint_opt.type)
    {
    case celib::LPM:
        to_print = "LPM";
        break;

    case celib::GPM:
        to_print = "GPM";
        break;

    case celib::UGPM:
        to_print = "UGPM";
        break;

    default:
        break;
    }
    std::cout << "Preintegration demonstration with " << to_print << std::endl;

    double integration_length = 0.2;
    if(var_map.count("length"))
    {
        integration_length = var_map["length"].as<double>();
    }

    int nb_infer = 0;
    if(var_map.count("nb_inference"))
    {
        nb_infer = var_map["nb_inference"].as<int>() - 1;
    }

    preint_opt.quantum = -1;
    if(var_map.count("quantum"))
    {
        preint_opt.quantum = var_map["quantum"].as<double>();
    }

    preint_opt.min_freq = 1000;

    pre_integrate_between_frames(preint_opt);

    return 0;
}


/* ****************************************************************** **
**    OpenSees - Open System for Earthquake Engineering Simulation    **
**          Pacific Earthquake Engineering Research Center            **
**                                                                    **
**                                                                    **
** (C) Copyright 1999, The Regents of the University of California    **
** All Rights Reserved.                                               **
**                                                                    **
** Commercial use of this program without express permission of the   **
** University of California, Berkeley, is strictly prohibited.  See   **
** file 'COPYRIGHT'  in main directory for information on usage and   **
** redistribution,  and for a DISCLAIMER OF ALL WARRANTIES.           **
**                                                                    **
** Developed by:                                                      **
**   Frank McKenna (fmckenna@ce.berkeley.edu)                         **
**   Gregory L. Fenves (fenves@ce.berkeley.edu)                       **
**   Filip C. Filippou (filippou@ce.berkeley.edu)                     **
**                                                                    **
** ****************************************************************** */

// $Revision: 1.3 $
// $Date: 2005-12-19 22:43:36 $
// $Source: /usr/local/cvs/OpenSees/SRC/analysis/integrator/EigenIntegrator.cpp,v $

// Written: Jun Peng
// Created: Wed Jan 27, 1999
// Revision: A
//
// Description: This file contains the class definition of EigenIntegrator.
// EigenIntegrator is an algorithmic class for setting up the finite element
// equations for a eigen problem analysis.
//
// This class is inheritanted from the base class of Integrator which was
// created by fmk (Frank).

#include "EigenIntegrator.h"
#include <FE_Element.h>
#include <AnalysisModel.h>
#include <EigenSOE.h>
#include <Vector.h>
#include <DOF_Group.h>
#include <FE_EleIter.h>
#include <DOF_GrpIter.h>

EigenIntegrator::EigenIntegrator()
  :Integrator(EigenINTEGRATOR_TAGS_Eigen),
   theSOE(0), theAnalysisModel(0)
{

}

EigenIntegrator::~EigenIntegrator()
{

}

void
EigenIntegrator::setLinks(AnalysisModel &theModel, EigenSOE &theSysOE)
{
    theAnalysisModel = &theModel;
    theSOE = &theSysOE;
}

int
EigenIntegrator::formEleTangent(FE_Element *theEle)
{
  if (flagK == 0)
      return this->formEleTangK(theEle);
  else
      return this->formEleTangM(theEle);
}

int
EigenIntegrator::formNodTangent(DOF_Group *theDof)
{
  return this->formNodTangM(theDof);
}

int
EigenIntegrator::formEleResidual(FE_Element *theEle)
{
    return 0;
}

int
EigenIntegrator::formNodUnbalance(DOF_Group *theDof)
{
    return 0;
}

int
EigenIntegrator::newStep()
{
    return 0;
}

int
EigenIntegrator::getLastResponse(Vector &result, const ID &id)
{
    return 0;
}

int
EigenIntegrator::formK()
{
    if (theAnalysisModel == 0 || theSOE == 0) {
	opserr << "WARNING EigenIntegrator::formK -";
	opserr << " no AnalysisModel or EigenSOE has been set\n";
	return -1;
    }

    // the loops to form and add the tangents are broken into two for
    // efficiency when performing parallel computations

    // loop through the FE_Elements getting them to form the tangent
    // FE_EleIter &theEles1 = theAnalysisModel->getFEs();
    FE_Element *elePtr;

    flagK = 0;

    theSOE->zeroA();

    //while((elePtr = theEles1()) != 0)
    //  elePtr->formTangent(this);

   // loop through the FE_Elements getting them to add the tangent
    int result = 0;
    FE_EleIter &theEles2 = theAnalysisModel->getFEs();
    while((elePtr = theEles2()) != 0) {

        if (theSOE->addA(elePtr->getTangent(this), elePtr->getID()) < 0) {
	    opserr << "WARNING EigenIntegrator::formK -";
	    opserr << " failed in addA for ID " << elePtr->getID();
	    result = -2;
	}
    }

    return result;
}

int
EigenIntegrator::formM()
{
    if (theAnalysisModel == 0 || theSOE == 0) {
	opserr << "WARNING EigenIntegrator::formK -";
	opserr << " no AnalysisModel or EigenSOE has been set\n";
	return -1;
    }

    // the loops to form and add the tangents are broken into two for
    // efficiency when performing parallel computations

    // loop through the FE_Elements getting them to form the tangent
    // FE_EleIter &theEles1 = theAnalysisModel->getFEs();
    FE_Element *elePtr;

    flagK = 1;
    theSOE->zeroM();

    // while((elePtr = theEles1()) != 0)
    //     elePtr->formTangent(this);

   // loop through the FE_Elements getting them to add the tangent
    int result = 0;
    FE_EleIter &theEles2 = theAnalysisModel->getFEs();
    while((elePtr = theEles2()) != 0) {
	if (theSOE->addM(elePtr->getTangent(this), elePtr->getID()) < 0) {
	    opserr << "WARNING EigenIntegrator::formK -";
	    opserr << " failed in addA for ID " << elePtr->getID();
	    result = -2;
	}
    }

    DOF_Group *dofPtr;
    DOF_GrpIter &theDofs = theAnalysisModel->getDOFs();
    while((dofPtr = theDofs()) != 0) {
	//   	dofPtr->formTangent(this);
	if (theSOE->addM(dofPtr->getTangent(this),dofPtr->getID()) < 0) {
	    opserr << "WARNING EigenIntegrator::formM -";
	    opserr << " failed in addM for ID " << dofPtr->getID();
	    result = -3;
	}
    }

    return result;
}

int
EigenIntegrator::formEleTangK(FE_Element *theEle)
{
  theEle->zeroTangent();
  theEle->addKtToTang(1.0);
  return 0;
}

int
EigenIntegrator::formEleTangM(FE_Element *theEle)
{
  theEle->zeroTangent();
  theEle->addMtoTang(1.0);
  return 0;
}

int
EigenIntegrator::formNodTangM(DOF_Group *theDof)
{
  theDof->zeroTangent();
  theDof->addMtoTang(1.0);
  return 0;
}

int
EigenIntegrator::update(const Vector &deltaU)
{
    return 0;
}

EigenSOE *
EigenIntegrator::getEigenSOEPtr() const
{
    return theSOE;
}

AnalysisModel *
EigenIntegrator::getAnalysisModelPtr() const
{
    return theAnalysisModel;
}

int
EigenIntegrator::sendSelf(int commitTag, Channel &theChannel)
{
    return 0;
}

int
EigenIntegrator::recvSelf(int commitTag, Channel &theChannel,
			  FEM_ObjectBroker &theBroker)
{
    return 0;
}

void
EigenIntegrator::Print(OPS_Stream &s, int flag)
{
    s << "\t EigenIntegrator: \n";
}


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

#ifndef MTL_UPDATE_INCLUDE
#define MTL_UPDATE_INCLUDE

#include <boost/numeric/mtl/operation/assign_mode.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace operations {

template <typename Element>
struct update_store
{
    template <typename Value>
    Element& operator() (Element& x, Value const& y)
    {
    vampir_trace<13> tracer;
	return x= Element(y);
    }

    // How to fill empty entries; typically directly with /p y
    template <typename Value>
    Element init(Value const& y)
    {
	return y;
    }
};

template <typename Element>
struct update_plus
{
    template <typename Value>
    Element& operator() (Element& x, Value const& y)
    {
    vampir_trace<14> tracer;
	return x+= Element(y);
    }

    // How to fill empty entries; typically directly with /p y
    template <typename Value>
    Element init(Value const& y)
    {
	return y;
    }
};

template <typename Element>
struct update_minus
{
    template <typename Value>
    Element& operator() (Element& x, Value const& y)
    {
    vampir_trace<15> tracer;
	return x-= Element(y);
    }

    // How to fill empty entries. Here the inverse of /p y is needed!!!
    template <typename Value>
    Element init(Value const& y)
    {
	return -y;
    }
};

template <typename Element>
struct update_times
{
    template <typename Value>
    Element& operator() (Element& x, Value const& y)
    {
    vampir_trace<16> tracer;
	return x*= y;
    }

    // How to fill empty entries; typically directly with /p y
    template <typename Value>
    Element init(Value const& y)
    {
	return y;
    }
};

template <typename Element, typename MonoidOp>
struct update_adapter
{
    template <typename Value>
    Element& operator() (Element& x, Value const& y)
    {
    vampir_trace<17> tracer;
	return x= MonoidOp()(x, y);
    }

    // How to fill empty entries
    template <typename Value>
    Element init(Value const& y)
    {
	return y;
    }
};

template <typename Inserter, typename SizeType = std::size_t>
struct update_proxy
{
    typedef update_proxy          self;
    typedef typename Inserter::value_type  value_type;

    explicit update_proxy(Inserter& ins, SizeType row, SizeType col)
      : ins(ins), row(row), col(col) {}

    template <typename Value>
    self& operator<< (Value const& val)
    {
	vampir_trace<20> tracer;
	return lshift(val, typename ashape::ashape<Value>::type());
    }

    template <typename Value>
    self& operator= (Value const& val)
    {
	vampir_trace<21> tracer;
	ins.template modify<update_store<value_type> > (row, col, val);
	return *this;
    }

    template <typename Value>
    self& operator+= (Value const& val)
    {
	vampir_trace<22> tracer;
	ins.template modify<update_plus<value_type> > (row, col, val);
	return *this;
    }

  private:

    typedef typename Inserter::matrix_type                               matrix_type;
    typedef typename mtl::ashape::ashape<matrix_type>::type              matrix_shape;
    typedef typename mtl::ashape::ashape<typename matrix_type::value_type>::type value_shape;

    // Update scalar value as before
    template <typename Value>
    self& lshift (Value const& val, value_shape)
    {
	ins.update (row, col, val);
	return *this;
    }

    // Update an entire matrix considered as block
    template <typename MatrixSrc>
    self& lshift (const MatrixSrc& src, matrix_shape)
    {
	namespace traits = mtl::traits;
	typename traits::row<MatrixSrc>::type             row(src);
	typename traits::col<MatrixSrc>::type             col(src);
	typename traits::const_value<MatrixSrc>::type     value(src);

	typedef typename traits::range_generator<tag::major, MatrixSrc>::type  cursor_type;
	typedef typename traits::range_generator<tag::nz, cursor_type>::type   icursor_type;

	for (cursor_type cursor = begin<tag::major>(src), cend = end<tag::major>(src); cursor != cend; ++cursor)
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		ins.update(row(*icursor) + this->row, col(*icursor) + this->col, value(*icursor));
	return *this;
    }

    Inserter&  ins;
    SizeType   row, col;
};

/// Compute updater that corresponds to assign_mode
template <typename Assign, typename Value>
struct update_assign_mode {};

template <typename Value>
struct update_assign_mode<assign::assign_sum, Value>
{
    typedef update_plus<Value> type;
};

template <typename Value>
struct update_assign_mode<assign::plus_sum, Value>
{
    typedef update_plus<Value> type;
};

template <typename Value>
struct update_assign_mode<assign::minus_sum, Value>
{
    typedef update_minus<Value> type;
};

} // namespace operations

using operations::update_store;
using operations::update_plus;
using operations::update_minus;
using operations::update_times;

} // namespace mtl

#if 0
// inconsistent with linear_algebra/identity.hpp

namespace math {

// temporary hack, must go to a proper place
template <typename Element, typename MonoidOp>
struct identity {};

#if 0
template <typename Element, typename MonoidOp>
struct identity< Element, mtl::operations::update_adapter< Element, MonoidOp > >
    : struct identity< Element, MonoidOp >
{};
#endif

template < class T >
struct identity< T, mtl::operations::update_store<T> >
{
    static const T value = 0 ;
    T operator()() const { return value ; }
} ;

template < class T >
const T identity< T, mtl::operations::update_store< T > >::value ;

template < class T >
struct identity< T, mtl::operations::update_plus<T> >
{
    static const T value = 0 ;
    T operator()() const { return value ; }
} ;

template < class T >
const T identity< T, mtl::operations::update_plus< T > >::value ;

template < class T >
struct identity< T, mtl::operations::update_mult<T> >
{
    static const T value = 1 ;
    T operator()() const { return value ; }
} ;

template < class T >
const T identity< T, mtl::operations::update_mult< T > >::value ;

}
#endif

#endif // MTL_UPDATE_INCLUDE


/*  _______________________________________________________________________

    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:       ParamStudy
//- Description: Implementation code for the ParamStudy class
//- Owner:       Mike Eldred

#include "dakota_system_defs.hpp"
#include "dakota_tabular_io.hpp"
#include "ParamStudy.hpp"
#include "ProblemDescDB.hpp"
#include "ParallelLibrary.hpp"
#include "PolynomialApproximation.hpp"
#include <boost/lexical_cast.hpp>

static const char rcsId[]="@(#) $Id: ParamStudy.cpp 7024 2010-10-16 01:24:42Z mseldre $";

//#define DEBUG

namespace Dakota {

ParamStudy::ParamStudy(ProblemDescDB& problem_db, Model& model):
  PStudyDACE(problem_db, model)
{
  // use allVariables instead of default allSamples
  compactMode = false;

  // Extract specification from ProblemDescDB, perform sanity checking, and
  // compute/estimate maxEvalConcurrency.
  bool err_flag = false;
  switch (methodName) {
  case LIST_PARAMETER_STUDY: {
    const RealVector& pt_list
      = probDescDB.get_rv("method.parameter_study.list_of_points");
    if (pt_list.empty()) {
      const String& pt_fname
	= probDescDB.get_string("method.pstudy.import_file");
      unsigned short tabular_format
	= probDescDB.get_ushort("method.pstudy.import_format");
      bool active_only
	= probDescDB.get_bool("method.pstudy.import_active_only");
      if (load_distribute_points(pt_fname, tabular_format, active_only))
	err_flag = true;
    }
    else if (distribute_list_of_points(pt_list))
      err_flag = true;
    break;
  }
  case VECTOR_PARAMETER_STUDY: {
    const RealVector& step_vector
      = probDescDB.get_rv("method.parameter_study.step_vector");
    if (step_vector.empty()) { // final_point & num_steps spec.
      // check length and distribute
      if (check_final_point(
	  probDescDB.get_rv("method.parameter_study.final_point")))
	err_flag = true;
      // check value
      if (check_num_steps(
	  probDescDB.get_int("method.parameter_study.num_steps")))
	err_flag = true;
      // precompute steps (using construct-time initialPoint) and perform error
      // checks only if in check mode; else avoid additional overhead and rely
      // on run-time checks for run-time initialPoint.
      if (numSteps && parallelLib.command_line_check()) {
	initialCVPoint  = iteratedModel.continuous_variables();      // view
	initialDIVPoint = iteratedModel.discrete_int_variables();    // view
	initialDSVPoint.resize(boost::extents[numDiscreteStringVars]);
	initialDSVPoint = iteratedModel.discrete_string_variables(); // copy
	initialDRVPoint = iteratedModel.discrete_real_variables();   // view
	final_point_to_step_vector(); // covers check_ranges_sets(numSteps)
      }
    }
    else { // step_vector & num_steps spec.
      // check length and distribute
      if (check_step_vector(step_vector))
	err_flag = true;
       // check value
     if (check_num_steps(
	  probDescDB.get_int("method.parameter_study.num_steps")))
	err_flag = true;
      // discrete initial pts needed for check_sets(); reassigned in pre-run
      initialDIVPoint = iteratedModel.discrete_int_variables();    // view
      initialDSVPoint.resize(boost::extents[numDiscreteStringVars]);
      initialDSVPoint = iteratedModel.discrete_string_variables(); // copy
      initialDRVPoint = iteratedModel.discrete_real_variables();   // view
      if (check_ranges_sets(numSteps))
	err_flag = true;
    }
    break;
  }
  case CENTERED_PARAMETER_STUDY:
    if (check_step_vector(
	probDescDB.get_rv("method.parameter_study.step_vector")))
      err_flag = true;
    if (check_steps_per_variable(
	probDescDB.get_iv("method.parameter_study.steps_per_variable")))
      err_flag = true;
    initialDIVPoint = iteratedModel.discrete_int_variables();    // view
    initialDSVPoint.resize(boost::extents[numDiscreteStringVars]);
    initialDSVPoint = iteratedModel.discrete_string_variables(); // copy
    initialDRVPoint = iteratedModel.discrete_real_variables();   // view
    if (check_ranges_sets(contStepsPerVariable, discIntStepsPerVariable,
			  discStringStepsPerVariable, discRealStepsPerVariable))
      err_flag = true;
    break;
  case MULTIDIM_PARAMETER_STUDY:
    if (check_variable_partitions(probDescDB.get_usa("method.partitions")))
      err_flag = true;
    if (check_finite_bounds())
      err_flag = true;
    // precompute steps (using construct-time bounds) and perform error checks
    // only if in check mode; else avoid additional overhead and rely on
    // run-time checks for run-time bounds.
    if (parallelLib.command_line_check())
      distribute_partitions();
    break;
  default:
    Cerr << "\nError: bad methodName (" << method_enum_to_string(methodName)
	 << ") in ParamStudy constructor." << std::endl;
    err_flag = true;
  }
  if (err_flag)
    abort_handler(METHOD_ERROR);

  maxEvalConcurrency *= numEvals;
}

bool ParamStudy::resize()
{
  bool parent_reinit_comms = PStudyDACE::resize();

  // TODO:  To get resize() working, move contents of ParamStudy::pre_run()
  //        to ParamStudy::initialize_run() before Analyzer::initialize_run()
  //        is called. This populates allVariables. If the model is a
  //        RecastModel, call inverse_transform_variables() on each entry in
  //        allVariables to resize and transform to the RecastModel. This call
  //        to inverse_transform_variables() must occur after
  //        Analyzer::initialize_run(). Also in ActiveSubspaceModel,
  //        inverse_transform_variables() is not yet implemented.

  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 ParamStudy::pre_run()
{
  Analyzer::pre_run();

  // Capture any changes in initialCVPoint resulting from the strategy layer's
  // passing of best variable info between iterators.  If no such variable
  // passing has occurred, then this reassignment is merely repetitive of the
  // one in the ParamStudy constructor.  If there is a final_point
  // specification, then contStepVector and numSteps must be (re)computed.
  const Variables& vars = iteratedModel.current_variables();
  const SharedVariablesData& svd = vars.shared_data();
  if (methodName == VECTOR_PARAMETER_STUDY ||
      methodName == CENTERED_PARAMETER_STUDY) {
    copy_data(vars.continuous_variables(),      initialCVPoint);  // copy
    copy_data(vars.discrete_int_variables(),    initialDIVPoint); // copy
    initialDSVPoint.resize(boost::extents[numDiscreteStringVars]);
    initialDSVPoint = vars.discrete_string_variables();           // copy
    copy_data(vars.discrete_real_variables(),   initialDRVPoint); // copy
  }

  size_t av_size = allVariables.size();
  if (av_size != numEvals) {
    allVariables.resize(numEvals);
    for (size_t i=av_size; i<numEvals; ++i)
      allVariables[i] = Variables(svd); // use minimal data ctor
    if ( outputLevel > SILENT_OUTPUT &&
	 ( methodName == VECTOR_PARAMETER_STUDY ||
	   methodName == CENTERED_PARAMETER_STUDY ) )
      allHeaders.resize(numEvals);
  }

  switch (methodName) {
  case LIST_PARAMETER_STUDY:
    if (outputLevel > SILENT_OUTPUT)
      Cout << "\nList parameter study for " << numEvals << " samples\n\n";
    sample();
    break;
  case VECTOR_PARAMETER_STUDY:
    if (!contStepVector.empty()       || !discIntStepVector.empty() ||
	!discStringStepVector.empty() || !discRealStepVector.empty()) {
      // step_vector & num_steps
      if (outputLevel > SILENT_OUTPUT) {
	Cout << "\nVector parameter study for " << numSteps
	     << " steps starting from\n";
	write_ordered(Cout, svd.active_components_totals(), initialCVPoint,
		      initialDIVPoint, initialDSVPoint, initialDRVPoint);
	Cout << "with a step vector of\n";
	write_ordered(Cout, svd.active_components_totals(), contStepVector,
		      discIntStepVector, discStringStepVector,
		      discRealStepVector);
	Cout << '\n';
      }
    }
    else { // final_point & num_steps
      if (outputLevel > SILENT_OUTPUT) {
	Cout << "\nVector parameter study from\n";
	write_ordered(Cout, svd.active_components_totals(), initialCVPoint,
		      initialDIVPoint, initialDSVPoint, initialDRVPoint);
	Cout << "to\n";
	write_ordered(Cout, svd.active_components_totals(), finalCVPoint,
		      finalDIVPoint, finalDSVPoint, finalDRVPoint);
	Cout << "using " << numSteps << " steps\n\n";
      }
      if (numSteps) // define step vectors from initial, final, & num steps
	final_point_to_step_vector();
    }
    vector_loop();
    break;
  case CENTERED_PARAMETER_STUDY:
    if (outputLevel > SILENT_OUTPUT) {
      Cout << "\nCentered parameter study with steps per variable\n";
      write_ordered(Cout, svd.active_components_totals(), contStepsPerVariable,
		    discIntStepsPerVariable, discStringStepsPerVariable,
		    discRealStepsPerVariable);
      Cout << "and increments of\n";
      write_ordered(Cout, svd.active_components_totals(), contStepVector,
		    discIntStepVector, discStringStepVector,
		    discRealStepVector);
      Cout << "with the following center point:\n";
      write_ordered(Cout, svd.active_components_totals(), initialCVPoint,
		    initialDIVPoint, initialDSVPoint, initialDRVPoint);
      Cout << '\n';
    }
    centered_loop();
    break;
  case MULTIDIM_PARAMETER_STUDY:
    if (outputLevel > SILENT_OUTPUT) {
      Cout << "\nMultidimensional parameter study variable partitions of\n";
      write_ordered(Cout, svd.active_components_totals(), contVarPartitions,
		    discIntVarPartitions, discStringVarPartitions,
		    discRealVarPartitions);
    }
    distribute_partitions();
    multidim_loop();
    break;
  default:
    Cerr << "\nError: bad methodName (" << method_enum_to_string(methodName)
	 << ") in ParamStudy::pre_run()." << std::endl;
    abort_handler(METHOD_ERROR);
  }
}

void ParamStudy::core_run()
{
  // perform the evaluations; multidim exception
  bool log_resp_flag = (methodName == MULTIDIM_PARAMETER_STUDY)
    ? (!subIteratorFlag) : false;
  bool log_best_flag = (numObjFns || numLSqTerms); // opt or NLS data set
  evaluate_parameter_sets(iteratedModel, log_resp_flag, log_best_flag);
}

void ParamStudy::post_input()
{
  // call convenience function from Analyzer
  read_variables_responses(numEvals, numContinuousVars + numDiscreteIntVars +
			   numDiscreteStringVars + numDiscreteRealVars);
}

void ParamStudy::post_run(std::ostream& s)
{
  bool log_resp_flag = (!subIteratorFlag);
  if (methodName == MULTIDIM_PARAMETER_STUDY && log_resp_flag)
    pStudyDACESensGlobal.compute_correlations(allVariables, allResponses,
      iteratedModel.discrete_set_string_values()); // to map string variable
                                                   // values back to indices

  Analyzer::post_run(s);
}

void ParamStudy::sample()
{
  // populate allVariables
  for (size_t i=0; i<numEvals; ++i) {
    if (numContinuousVars)
      allVariables[i].continuous_variables(listCVPoints[i]);
    if (numDiscreteIntVars)
      allVariables[i].discrete_int_variables(listDIVPoints[i]);
    if (numDiscreteStringVars)
      allVariables[i].discrete_string_variables(
	listDSVPoints[boost::indices[i][idx_range(0, numDiscreteStringVars)]]);
    if (numDiscreteRealVars)
      allVariables[i].discrete_real_variables(listDRVPoints[i]);
  }
  // free up redundant memory
  listCVPoints.clear();
  listDIVPoints.clear();
  listDSVPoints.resize(boost::extents[0][0]);
  listDRVPoints.clear();
}

void ParamStudy::vector_loop()
{
  // Steps along a n-dimensional vector through numSteps additions of
  // continuous/discrete step vectors.  The step is an absolute step defining
  // magnitude & direction.  The number of fn. evaluations in the study is
  // numSteps + 1 since the initial point is also evaluated.

  const BitArray&      di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray&    dsi_values = iteratedModel.discrete_set_int_values();
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  const RealSetArray&   dsr_values = iteratedModel.discrete_set_real_values();
  size_t i, j, dsi_cntr;

  for (i=0; i<=numSteps; ++i) {
    Variables& vars = allVariables[i];

    // active continuous
    for (j=0; j<numContinuousVars; ++j)
      c_step(j, i, vars);

    // active discrete int: ranges and sets
    for (j=0, dsi_cntr=0; j<numDiscreteIntVars; ++j)
      if (di_set_bits[j]) dsi_step(j, i, dsi_values[dsi_cntr++], vars);
      else                dri_step(j, i, vars);

    // active discrete string: sets only
    for (j=0; j<numDiscreteStringVars; ++j)
      dss_step(j, i, dss_values[j], vars);

    // active discrete real: sets only
    for (j=0; j<numDiscreteRealVars; ++j)
      dsr_step(j, i, dsr_values[j], vars);

    // store each output header in allHeaders
    if (outputLevel > SILENT_OUTPUT) {
      String& h_string = allHeaders[i];
      h_string.clear();
      if (iteratedModel.asynch_flag())
	h_string += "\n\n";
      if (numSteps == 0) // Allow numSteps == 0 case
	h_string += ">>>>> Initial_point only (no steps)\n";
      h_string += ">>>>> Vector parameter study evaluation for ";
      h_string += boost::lexical_cast<std::string>(i*100./numSteps);
      h_string += "% along vector\n";
    }
  }
}

void ParamStudy::centered_loop()
{
  size_t k, cntr = 0, dsi_cntr = 0;
  String cv_str("cv"), div_str("div"), dsv_str("dsv"), drv_str("drv");

  // Always evaluate center point, even if steps_per_variable = 0
  if (outputLevel > SILENT_OUTPUT)
    allHeaders[cntr] = (iteratedModel.asynch_flag()) ?
      "\n\n>>>>> Centered parameter study evaluation for center point\n" :
      ">>>>> Centered parameter study evaluation for center point\n";
  if (numContinuousVars)
    allVariables[cntr].continuous_variables(initialCVPoint);
  if (numDiscreteIntVars)
    allVariables[cntr].discrete_int_variables(initialDIVPoint);
  if (numDiscreteStringVars)
    allVariables[cntr].discrete_string_variables(
      initialDSVPoint[boost::indices[idx_range(0, numDiscreteStringVars)]]);
  if (numDiscreteRealVars)
    allVariables[cntr].discrete_real_variables(initialDRVPoint);
  ++cntr;

  // Evaluate +/- steps for each continuous variable
  for (k=0; k<numContinuousVars; ++k) {
    int i, num_steps_k = contStepsPerVariable[k];
    for (i=-num_steps_k; i<=num_steps_k; ++i)
      if (i) {
	Variables& vars = allVariables[cntr];
	reset(vars); c_step(k, i, vars);
	if (outputLevel > SILENT_OUTPUT) centered_header(cv_str, k, i, cntr);
	++cntr;
      }
  }

  // Evaluate +/- steps for each discrete int variable
  const BitArray&   di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray& dsi_values = iteratedModel.discrete_set_int_values();
  for (k=0; k<numDiscreteIntVars; ++k) {
    int i, num_steps_k = discIntStepsPerVariable[k];
    if (di_set_bits[k]) {
      const IntSet& dsi_vals_k = dsi_values[dsi_cntr];
      for (i=-num_steps_k; i<=num_steps_k; ++i)
	if (i) {
	  Variables& vars = allVariables[cntr];
	  reset(vars); dsi_step(k, i, dsi_vals_k, vars);
	  if (outputLevel > SILENT_OUTPUT) centered_header(div_str, k, i, cntr);
	  ++cntr;
	}
      ++dsi_cntr;
    }
    else {
      for (i=-num_steps_k; i<=num_steps_k; ++i)
	if (i) {
	  Variables& vars = allVariables[cntr];
	  reset(vars); dri_step(k, i, vars);
	  if (outputLevel > SILENT_OUTPUT) centered_header(div_str, k, i, cntr);
	  ++cntr;
	}
    }
  }

  // Evaluate +/- steps for each discrete string variable
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  for (k=0; k<numDiscreteStringVars; ++k) {
    int i, num_steps_k = discStringStepsPerVariable[k];
    const StringSet& dss_vals_k = dss_values[k];
    for (i=-num_steps_k; i<=num_steps_k; ++i)
      if (i) {
	Variables& vars = allVariables[cntr];
	reset(vars); dss_step(k, i, dss_vals_k, vars);
	if (outputLevel > SILENT_OUTPUT) centered_header(dsv_str, k, i, cntr);
	++cntr;
      }
  }

  // Evaluate +/- steps for each discrete real variable
  const RealSetArray& dsr_values = iteratedModel.discrete_set_real_values();
  for (k=0; k<numDiscreteRealVars; ++k) {
    int i, num_steps_k = discRealStepsPerVariable[k];
    const RealSet& dsr_vals_k = dsr_values[k];
    for (i=-num_steps_k; i<=num_steps_k; ++i)
      if (i) {
	Variables& vars = allVariables[cntr];
	reset(vars); dsr_step(k, i, dsr_vals_k, vars);
	if (outputLevel > SILENT_OUTPUT) centered_header(drv_str, k, i, cntr);
	++cntr;
      }
  }
}

void ParamStudy::multidim_loop()
{
  // Perform a multidimensional parameter study based on the number of
  // partitions specified for each variable.

  const BitArray&      di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray&    dsi_values = iteratedModel.discrete_set_int_values();
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  const RealSetArray&   dsr_values = iteratedModel.discrete_set_real_values();
  size_t i, j, p_cntr, dsi_cntr,
    num_c_di_vars    = numContinuousVars + numDiscreteIntVars,
    num_c_di_ds_vars = num_c_di_vars + numDiscreteStringVars,
    num_vars = num_c_di_ds_vars + numDiscreteRealVars;
  UShortArray multidim_indices(num_vars, 0), partition_limits(num_vars);
  copy_data_partial(contVarPartitions, partition_limits, 0);
  copy_data_partial(discIntVarPartitions, partition_limits, numContinuousVars);
  copy_data_partial(discStringVarPartitions, partition_limits, num_c_di_vars);
  copy_data_partial(discRealVarPartitions, partition_limits, num_c_di_ds_vars);

  for (i=0; i<numEvals; ++i) {
    Variables& vars = allVariables[i];
    p_cntr = 0;
    // active continuous
    for (j=0; j<numContinuousVars; ++j, ++p_cntr)
      c_step(j, multidim_indices[p_cntr], vars);
    // active discrete int: ranges and sets
    for (j=0, dsi_cntr=0; j<numDiscreteIntVars; ++j, ++p_cntr)
      if (di_set_bits[j])
	dsi_step(j, multidim_indices[p_cntr], dsi_values[dsi_cntr++], vars);
      else
	dri_step(j, multidim_indices[p_cntr], vars);
    // active discrete string: sets only
    for (j=0; j<numDiscreteStringVars; ++j, ++p_cntr)
      dss_step(j, multidim_indices[p_cntr], dss_values[j], vars);
    // active discrete real: sets only
    for (j=0; j<numDiscreteRealVars; ++j, ++p_cntr)
      dsr_step(j, multidim_indices[p_cntr], dsr_values[j], vars);
    // increment the multidimensional index set
    Pecos::SharedPolyApproxData::increment_indices(multidim_indices,
						   partition_limits, true);
  }
}

/** Load from file and distribute points; using this function to
    manage construction of the temporary arrays.  Historically all
    data was read as a real (mixture of values and indices), but now
    points_file is valued-based (reals, integers, strings) so file
    input matches tabular data output.  Return false on success. */
bool ParamStudy::
load_distribute_points(const String& points_filename,
		       unsigned short tabular_format,
		       bool active_only)
{
  bool err = false;

  // don't know the size until the file is read, so the reader grows
  // the containers as the read takes place

  // the easiest way to read is with a variables object
  // read all variables in spec order into a temporary Variables
  Variables vars(iteratedModel.current_variables().copy());

  // then map the active data from that variables object into the
  // list*Points arrays and validate it

  // TODO: validate the read values of inactive variables

  // Could read into these dynamically or into a temporary and then allocate
  numEvals = TabularIO::
    read_data_tabular(points_filename, "List Parameter Study",
		      listCVPoints, listDIVPoints, listDSVPoints, listDRVPoints,
		      tabular_format, active_only,
		      iteratedModel.current_variables().copy());
  if (numEvals == 0) err = true;

  // validation of data: consider moving reader into this class and
  // validating while reading...
  for (size_t i=0; i<numEvals; ++i) {

    // validate continuous values read
    const RealVector& c_lb = iteratedModel.continuous_lower_bounds();
    const RealVector& c_ub = iteratedModel.continuous_upper_bounds();

    for (size_t j=0; j<numContinuousVars; ++j)
      if (listCVPoints[i][j] < c_lb[j] || listCVPoints[i][j] > c_ub[j]) {
	Cerr << "\nError: list value " << listCVPoints[i][j]
	     << " outside bounds for continuous variable " << j+1 << '.'
	     << std::endl;
	err = true;
      }

    // validate discrete integers (sets and ranges) read
    const BitArray& di_set_bits = iteratedModel.discrete_int_sets();
    const IntSetArray& dsi_vals = iteratedModel.discrete_set_int_values();
    const IntVector& di_lb = iteratedModel.discrete_int_lower_bounds();
    const IntVector& di_ub = iteratedModel.discrete_int_upper_bounds();

    for (size_t j=0, dsi_cntr=0; j<numDiscreteIntVars; ++j)
      if (di_set_bits[j]) {
	// set values
	if (set_value_to_index(listDIVPoints[i][j], dsi_vals[dsi_cntr])
	    == _NPOS) {
	  Cerr << "\nError: list value " << listDIVPoints[i][j]
	       << " not admissble for discrete int set " << dsi_cntr+1 << '.'
	       << std::endl;
	  err = true;
	}
	++dsi_cntr;
      }
      else {
	// range values: validate bounds
	if (listDIVPoints[i][j] < di_lb[j] || listDIVPoints[i][j] > di_ub[j]) {
	  Cerr << "\nError: list value " << listDIVPoints[i][j]
	       << " outside bounds for discrete int range variable " << j+1
	       << '.' << std::endl;
	  err = true;
	}
      }

    // validate discrete string sets read
    const StringSetArray& dss_vals = iteratedModel.discrete_set_string_values();
    for (size_t j=0; j<numDiscreteStringVars; ++j)
      if (set_value_to_index(listDSVPoints[i][j], dss_vals[j]) == _NPOS) {
        Cerr << "\nError: list value " << listDSVPoints[i][j]
	     << " not admissible for discrete string set " << j+1 << '.'
	     << std::endl;
        err = true;
      }

    const RealSetArray& dsr_vals = iteratedModel.discrete_set_real_values();
    for (size_t j=0; j<numDiscreteRealVars; ++j)
      if (set_value_to_index(listDRVPoints[i][j], dsr_vals[j]) == _NPOS) {
        Cerr << "\nError: list value " << listDRVPoints[i][j]
	     << " not admissible for discrete real set " << j+1 << '.'
	     << std::endl;
        err = true;
      }

  } // for each eval

  return err;

}

/** Parse list of points into typed data containers; list_of_pts will
    contain values for continuous and discrete integer range, but
    indices for all discrete set types (int, string, real) */
bool ParamStudy::distribute_list_of_points(const RealVector& list_of_pts)
{
  size_t i, j, dsi_cntr, start, len_lop = list_of_pts.length(),
    num_vars = numContinuousVars     + numDiscreteIntVars
             + numDiscreteStringVars + numDiscreteRealVars;
  if (len_lop % num_vars) {
    Cerr << "\nError: length of list_of_points ("  << len_lop
	 << ") must be evenly divisable among number of active variables ("
	 << num_vars << ")." << std::endl;
    return true;
  }
  numEvals = len_lop / num_vars;
  if (numContinuousVars)   listCVPoints.resize(numEvals);
  if (numDiscreteIntVars)  listDIVPoints.resize(numEvals);
  if (numDiscreteStringVars)
    listDSVPoints.resize(boost::extents[numEvals][numDiscreteStringVars]);
  if (numDiscreteRealVars) listDRVPoints.resize(numEvals);

  const BitArray&      di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray&    dsi_values = iteratedModel.discrete_set_int_values();
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  const RealSetArray&   dsr_values = iteratedModel.discrete_set_real_values();

  bool err = false;
  RealVector empty_rv; IntVector empty_iv; StringMultiArray empty_sa;
  for (i=0, start=0; i<numEvals; ++i) {
    RealVector& list_cv_i  = (numContinuousVars)  ? listCVPoints[i]  : empty_rv;
    IntVector&  list_div_i = (numDiscreteIntVars) ? listDIVPoints[i] : empty_iv;
    StringMultiArrayView list_dsv_i = (numDiscreteStringVars) ?
      listDSVPoints[boost::indices[i][idx_range(0, numDiscreteStringVars)]] :
      empty_sa[boost::indices[idx_range(0, 0)]];
    RealVector& list_drv_i = (numDiscreteRealVars) ?
      listDRVPoints[i] : empty_rv;
    IntVector div_combined, dsv_indices, drv_indices;

    // take a view of each sample and partition it into {c,di,dr} components
    RealVector all_sample(Teuchos::View, const_cast<Real*>(&list_of_pts[start]),
			  num_vars);
    // if list_of_pts contains range and set values:
    //distribute(all_sample, list_cv_i, list_div_i, list_dsv_i, list_drv_i);
    // if list_of_pts contains range values and set indices:
    distribute(all_sample, list_cv_i, div_combined, dsv_indices, drv_indices);
    start += num_vars;

    // Promote set indices to admissible set values
    if (numDiscreteIntVars) list_div_i.sizeUninitialized(numDiscreteIntVars);
    for (j=0, dsi_cntr=0; j<numDiscreteIntVars; ++j) {
      if (di_set_bits[j]) {
	// if set values:
	//if (set_value_to_index(list_div_i[j], dsi_values[dsi_cntr]) == _NPOS){
	//  Cerr << "\nError: list value " << list_div_i[j]<< " not admissible "
	//       << "for discrete int set " << dsi_cntr+1 << '.' << std::endl;
	//  err = true;
	//}
	// if set indices:
	list_div_i[j]
	  = set_index_to_value(div_combined[j], dsi_values[dsi_cntr]);
	++dsi_cntr;
      }
      else // range values
	list_div_i[j] = div_combined[j];
    }

    for (j=0; j<numDiscreteStringVars; ++j)
      // if set values:
      //if (set_value_to_index(list_dsv_i[j], dss_values[j]) == _NPOS) {
      //  Cerr << "\nError: list value " << list_dsv_i[j] << " not admissible "
      //       << "for discrete string set " << j+1 << '.' << std::endl;
      //  err = true;
      //}
      // if set indices:
      list_dsv_i[j] = set_index_to_value(dsv_indices[j], dss_values[j]);

    if (numDiscreteRealVars) list_drv_i.sizeUninitialized(numDiscreteRealVars);
    for (j=0; j<numDiscreteRealVars; ++j)
      // if set values:
      //if (set_value_to_index(list_drv_i[j], dsr_values[j]) == _NPOS) {
      //  Cerr << "\nError: list value " << list_drv_i[j] << " not admissible "
      //       << "for discrete real set " << j+1 << '.' << std::endl;
      //  err = true;
      //}
      // if set indices:
      list_drv_i[j] = set_index_to_value(drv_indices[j], dsr_values[j]);
  }

#ifdef DEBUG
  Cout << "distribute_list_of_points():\n";
  for (i=0; i<numEvals; ++i) {
    if (numContinuousVars) {
      Cout << "Eval " << i << " continuous:\n";
      write_data(Cout, listCVPoints[i]);
    }
    if (numDiscreteIntVars) {
      Cout << "Eval " << i << " discrete int:\n";
      write_data(Cout, listDIVPoints[i]);
    }
    if (numDiscreteStringVars) {
      Cout << "Eval " << i << " discrete string:\n";
      write_data(Cout,
	listDSVPoints[boost::indices[i][idx_range(0, numDiscreteStringVars)]]);
    }
    if (numDiscreteRealVars) {
      Cout << "Eval " << i << " discrete real:\n";
      write_data(Cout, listDRVPoints[i]);
    }
  }
#endif // DEBUG

  return err;
}

void ParamStudy::distribute_partitions()
{
  contStepVector.sizeUninitialized(numContinuousVars);
  discIntStepVector.sizeUninitialized(numDiscreteIntVars);
  discStringStepVector.sizeUninitialized(numDiscreteStringVars);
  discRealStepVector.sizeUninitialized(numDiscreteRealVars);

  initialCVPoint.sizeUninitialized(numContinuousVars);
  initialDIVPoint.sizeUninitialized(numDiscreteIntVars);
  initialDSVPoint.resize(boost::extents[numDiscreteStringVars]);
  initialDRVPoint.sizeUninitialized(numDiscreteRealVars);

  const RealVector&          c_vars = iteratedModel.continuous_variables();
  const IntVector&          di_vars = iteratedModel.discrete_int_variables();
  StringMultiArrayConstView ds_vars = iteratedModel.discrete_string_variables();
  const RealVector&         dr_vars = iteratedModel.discrete_real_variables();

  const RealVector&  c_l_bnds = iteratedModel.continuous_lower_bounds();
  const RealVector&  c_u_bnds = iteratedModel.continuous_upper_bounds();
  const IntVector&  di_l_bnds = iteratedModel.discrete_int_lower_bounds();
  const IntVector&  di_u_bnds = iteratedModel.discrete_int_upper_bounds();
  const RealVector& dr_l_bnds = iteratedModel.discrete_real_lower_bounds();
  const RealVector& dr_u_bnds = iteratedModel.discrete_real_upper_bounds();

  const BitArray&      di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray&    dsi_values = iteratedModel.discrete_set_int_values();
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  const RealSetArray&   dsr_values = iteratedModel.discrete_set_real_values();

  size_t i, dsi_cntr; unsigned short part;
  for (i=0; i<numContinuousVars; ++i) {
    part = contVarPartitions[i];
    if (part) {
      initialCVPoint[i] = c_l_bnds[i];
      contStepVector[i] = (c_u_bnds[i] - c_l_bnds[i]) / part;
    }
    else
      { initialCVPoint[i] = c_vars[i]; contStepVector[i] = 0.; }
  }
  for (i=0, dsi_cntr=0; i<numDiscreteIntVars; ++i) {
    part = discIntVarPartitions[i];
    if (part) {
      initialDIVPoint[i] = di_l_bnds[i];
      int range = (di_set_bits[i]) ? dsi_values[dsi_cntr++].size() - 1 :
	                             di_u_bnds[i] - di_l_bnds[i];
      discIntStepVector[i] = integer_step(range, part);
    }
    else
      { initialDIVPoint[i] = di_vars[i]; discIntStepVector[i] = 0; }
  }
  for (i=0; i<numDiscreteStringVars; ++i) {
    part = discStringVarPartitions[i];
    if (part) {
      const StringSet& dss_vals_i = dss_values[i];
      initialDSVPoint[i]      = *dss_vals_i.begin();
      discStringStepVector[i] = integer_step(dss_vals_i.size() - 1, part);
    }
    else
      { initialDRVPoint[i] = dr_vars[i]; discRealStepVector[i] = 0; }
  }
  for (i=0; i<numDiscreteRealVars; ++i) {
    part = discRealVarPartitions[i];
    if (part) {
      initialDRVPoint[i]    = dr_l_bnds[i];
      discRealStepVector[i] = integer_step(dsr_values[i].size() - 1, part);
    }
    else
      { initialDRVPoint[i] = dr_vars[i]; discRealStepVector[i] = 0; }
  }

#ifdef DEBUG
  Cout << "distribute_partitions():\n";
  if (numContinuousVars) {
    Cout << "c_vars:\n";             write_data(Cout, c_vars);
    Cout << "c_l_bnds:\n";           write_data(Cout, c_l_bnds);
    Cout << "c_u_bnds:\n";           write_data(Cout, c_u_bnds);
    Cout << "initialCVPoint:\n";     write_data(Cout, initialCVPoint);
    Cout << "contStepVector:\n";     write_data(Cout, contStepVector);
  }
  if (numDiscreteIntVars) {
    Cout << "di_vars:\n";            write_data(Cout, di_vars);
    Cout << "di_l_bnds:\n";          write_data(Cout, di_l_bnds);
    Cout << "di_u_bnds:\n";          write_data(Cout, di_u_bnds);
    Cout << "initialDIVPoint:\n";    write_data(Cout, initialDIVPoint);
    Cout << "discIntStepVector:\n";  write_data(Cout, discIntStepVector);
  }
  if (numDiscreteStringVars) {
    Cout << "ds_vars:\n";              write_data(Cout, ds_vars);
    Cout << "initialDSVPoint:\n";      write_data(Cout, initialDSVPoint);
    Cout << "discStringStepVector:\n"; write_data(Cout, discStringStepVector);
  }
  if (numDiscreteRealVars) {
    Cout << "dr_vars:\n";            write_data(Cout, dr_vars);
    Cout << "dr_l_bnds:\n";          write_data(Cout, dr_l_bnds);
    Cout << "dr_u_bnds:\n";          write_data(Cout, dr_u_bnds);
    Cout << "initialDRVPoint:\n";    write_data(Cout, initialDRVPoint);
    Cout << "discRealStepVector:\n"; write_data(Cout, discRealStepVector);
  }
#endif // DEBUG
}

void ParamStudy::final_point_to_step_vector()
{
  //RealVector cv_final, drv_final; IntVector div_final;
  //StringMultiArray dsv_final;
  //distribute(finalPoint, cv_final, div_final, dsv_final, drv_final);

  const BitArray&      di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray&    dsi_values = iteratedModel.discrete_set_int_values();
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  const RealSetArray&   dsr_values = iteratedModel.discrete_set_real_values();
  size_t j, dsi_cntr;

  // active continuous
  contStepVector.sizeUninitialized(numContinuousVars);
  for (j=0; j<numContinuousVars; ++j)
    contStepVector[j] = (finalCVPoint[j] - initialCVPoint[j]) / numSteps;

  // active discrete int: ranges and sets
  discIntStepVector.sizeUninitialized(numDiscreteIntVars);
  for (j=0, dsi_cntr=0; j<numDiscreteIntVars; ++j)
    if (di_set_bits[j]) {
      discIntStepVector[j] = index_step(
        set_value_to_index(initialDIVPoint[j], dsi_values[dsi_cntr]),
	// for final point defined as index:
        finalDIVPoint[j], numSteps);
	// for final point defined as admissible value:
        //set_value_to_index(div_final[j], dsi_values[dsi_cntr]), numSteps);
      ++dsi_cntr;
    }
    else
      discIntStepVector[j]
	= integer_step(finalDIVPoint[j] - initialDIVPoint[j], numSteps);

  // active discrete string: sets only
  discStringStepVector.sizeUninitialized(numDiscreteStringVars);
  for (j=0; j<numDiscreteStringVars; ++j)
    discStringStepVector[j] = index_step(
      set_value_to_index(initialDSVPoint[j], dss_values[j]),
      // for final point defined as index:
      finalDSVPoint[j], numSteps);
      // for final point defined as admissible value:
      //set_value_to_index(dsv_final[j], dss_values[j]), numSteps);

  // active discrete real: sets only
  discRealStepVector.sizeUninitialized(numDiscreteRealVars);
  for (j=0; j<numDiscreteRealVars; ++j)
    discRealStepVector[j] = index_step(
      set_value_to_index(initialDRVPoint[j], dsr_values[j]),
      // for final point defined as index:
      finalDRVPoint[j], numSteps);
      // for final point defined as admissible value:
      //set_value_to_index(drv_final[j], dsr_values[j]), numSteps);

#ifdef DEBUG
  Cout << "final_point_to_step_vector():\n";
  if (numContinuousVars) {
    Cout << "continuous step vector:\n";
    write_data(Cout, contStepVector);
  }
  if (numDiscreteIntVars) {
    Cout << "discrete int step vector:\n";
    write_data(Cout, discIntStepVector);
  }
  if (numDiscreteStringVars) {
    Cout << "discrete string step vector:\n";
    write_data(Cout, discStringStepVector);
  }
  if (numDiscreteRealVars) {
    Cout << "discrete real step vector:\n";
    write_data(Cout, discRealStepVector);
  }
#endif // DEBUG
}

bool ParamStudy::
check_sets(const IntVector& c_steps,  const IntVector& di_steps,
	   const IntVector& ds_steps, const IntVector& dr_steps)
{
  // checks for vector and centered cases: admissibility of step vectors
  // and number of steps among int/real sets
  // > check terminal set indices for out of range
  // > don't enforce that range variables remain within bounds (for now)
  // Note: this check is performed at construct time and is dependent on the
  // initial points; therefore, it is not a definitive check in the case of
  // multi-iterator execution with updated initial points.  Nonetheless,
  // verify proper set support for specified steps.

  const BitArray&      di_set_bits = iteratedModel.discrete_int_sets();
  const IntSetArray&    dsi_values = iteratedModel.discrete_set_int_values();
  const StringSetArray& dss_values = iteratedModel.discrete_set_string_values();
  const RealSetArray&   dsr_values = iteratedModel.discrete_set_real_values();
  size_t j, dsi_cntr;
  bool err = false;

  // active discrete int: ranges and sets
  for (j=0, dsi_cntr=0; j<numDiscreteIntVars; ++j)
    if (di_set_bits[j]) {
      const IntSet& dsi_vals_j = dsi_values[dsi_cntr];
      int terminal_index = set_value_to_index(initialDIVPoint[j], dsi_vals_j)
	+ discIntStepVector[j] * di_steps[j];
      if (terminal_index < 0 || terminal_index >= dsi_vals_j.size()) {
	Cerr << "\nError: ParamStudy index " << terminal_index
	     << " not admissible for discrete int set of size "
	     << dsi_vals_j.size() << '.' << std::endl;
	err = true;
      }
      ++dsi_cntr;
    }

  // active discrete string: sets only
  for (j=0; j<numDiscreteStringVars; ++j) {
    const StringSet& dss_vals_j = dss_values[j];
    int terminal_index = set_value_to_index(initialDSVPoint[j], dss_vals_j)
      + discStringStepVector[j] * ds_steps[j];
    if (terminal_index < 0 || terminal_index >= dss_vals_j.size()) {
      Cerr << "\nError: ParamStudy index " << terminal_index
	   << " not admissible for discrete string set of size "
	   << dss_vals_j.size() << '.' << std::endl;
      err = true;
    }
  }

  // active discrete real: sets only
  for (j=0; j<numDiscreteRealVars; ++j) {
    const RealSet& dsr_vals_j = dsr_values[j];
    int terminal_index = set_value_to_index(initialDRVPoint[j], dsr_vals_j)
      + discRealStepVector[j] * dr_steps[j];
    if (terminal_index < 0 || terminal_index >= dsr_vals_j.size()) {
      Cerr << "\nError: ParamStudy index " << terminal_index
	   << " not admissible for discrete real set of size "
	   << dsr_vals_j.size() << '.' << std::endl;
      err = true;
    }
  }

  return err;
}

} // namespace Dakota


// Boost.Geometry

// Copyright (c) 2017-2018 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_MERIDIAN_SEGMENT_HPP
#define BOOST_GEOMETRY_FORMULAS_MERIDIAN_SEGMENT_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

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

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Test if a segment is meridian or not.
*/

class meridian_segment
{

public :

    enum SegmentType {NonMeridian, MeridianCrossingPole, MeridianNotCrossingPole};

    template <typename T>
    static inline SegmentType is_meridian(T lon1, T lat1, T lon2, T lat2)
    {
        SegmentType res = NonMeridian;
        T diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);

        if ( meridian_not_crossing_pole(lat1, lat2, diff) )
        {
            res = MeridianNotCrossingPole;
        }
        else if ( meridian_crossing_pole(diff) )
        {
            res = MeridianCrossingPole;
        }
        return res;
    }

    template <typename T>
    static bool meridian_not_crossing_pole(T lat1, T lat2, T diff)
    {
        T half_pi = math::half_pi<T>();
        return math::equals(diff, T(0)) ||
               (math::equals(lat2, half_pi) && math::equals(lat1, -half_pi));
    }

    template <typename T>
    static bool meridian_crossing_pole(T diff)
    {
        return math::equals(math::abs(diff), math::pi<T>());
    }

};

}}} // namespace boost::geometry::formula

#endif //BOOST_GEOMETRY_FORMULAS_MERIDIAN_SEGMENT_HPP