#include "sif/pedestriancost.h"
#include "sif/costconstants.h"

#include "baldr/accessrestriction.h"
#include "midgard/constants.h"
#include "midgard/util.h"

#ifdef INLINE_TEST
#include "test/test.h"
#include <random>
#include <boost/property_tree/json_parser.hpp>
#endif

using namespace valhalla::baldr;

namespace valhalla {
namespace sif {

// Default options/values
namespace {

// Maximum route distances
constexpr uint32_t kMaxDistanceFoot        = 100000; // 100 km
constexpr uint32_t kMaxDistanceWheelchair  = 10000;  // 10 km

// Default speeds
constexpr float kDefaultSpeedFoot          = 5.1f;   // 3.16 MPH
constexpr float kDefaultSpeedWheelchair    = 4.0f;   // 2.5  MPH  TODO

// Penalty to take steps
constexpr float kDefaultStepPenaltyFoot       = 30.0f;   // 30 seconds
constexpr float kDefaultStepPenaltyWheelchair = 600.0f;  // 10 minutes

// Maximum grade30
constexpr uint32_t kDefaultMaxGradeFoot = 90;
constexpr uint32_t kDefaultMaxGradeWheelchair = 12; // Conservative for now...

// Other defaults (not dependent on type)
constexpr float kModeFactor             = 1.5f;   // Favor this mode?
constexpr float kDefaultManeuverPenalty = 5.0f;   // Seconds
constexpr float kDefaultGatePenalty     = 10.0f;  // Seconds
constexpr float kDefaultWalkwayFactor   = 0.9f;   // Slightly favor walkways
constexpr float kDefaultSideWalkFactor  = 0.95f;  // Slightly favor sidewalks
constexpr float kDefaultAlleyFactor     = 2.0f;   // Avoid alleys
constexpr float kDefaultDrivewayFactor  = 5.0f;   // Avoid driveways
constexpr float kDefaultFerryCost               = 300.0f; // Seconds
constexpr float kDefaultCountryCrossingCost     = 600.0f; // Seconds
constexpr float kDefaultCountryCrossingPenalty  = 0.0f;   // Seconds
constexpr float kDefaultUseFerry = 1.0f;

// Maximum distance at the beginning or end of a multimodal route
// that you are willing to travel for this mode.  In this case,
// it is the max walking distance.
constexpr uint32_t kTransitStartEndMaxDistance    = 2415;   // 1.5 miles

// Maximum transfer distance between stops that you are willing
// to travel for this mode.  In this case, it is the max walking
// distance you are willing to walk between transfers.
constexpr uint32_t kTransitTransferMaxDistance   = 805;   // 0.5 miles

// Avoid roundabouts
constexpr float kRoundaboutFactor = 2.0f;

// Maximum ferry penalty (when use_ferry == 0). Can't make this too large
// since a ferry is sometimes required to complete a route.
constexpr float kMaxFerryPenalty = 8.0f * 3600.0f; // 8 hours

// Minimum and maximum average pedestrian speed (to validate input).
constexpr float kMinPedestrianSpeed = 0.5f;
constexpr float kMaxPedestrianSpeed = 25.0f;

// Crossing penalties. TODO - may want to lower stop impact when
// 2 cycleways or walkways cross
constexpr uint32_t kCrossingCosts[] = { 0, 0, 1, 1, 2, 3, 5, 15 };
}

// Maximum amount of seconds that will be allowed to be passed in to influence paths
// This can't be too high because sometimes a certain kind of path is required to be taken
constexpr float kMaxSeconds = 12.0f * kSecPerHour; // 12 hours

constexpr float kMinFactor = 0.1f;
constexpr float kMaxFactor = 100000.0f;

// Valid ranges and defaults
constexpr ranged_default_t<uint32_t> kMaxDistanceWheelchairRange{0, kMaxDistanceWheelchair, kMaxDistanceFoot};
constexpr ranged_default_t<uint32_t> kMaxDistanceFootRange{0, kMaxDistanceFoot, kMaxDistanceFoot};

constexpr ranged_default_t<float> kSpeedWheelchairRange{kMinPedestrianSpeed, kDefaultSpeedWheelchair, kMaxPedestrianSpeed};
constexpr ranged_default_t<float> kSpeedFootRange{kMinPedestrianSpeed, kDefaultSpeedFoot, kMaxPedestrianSpeed};

constexpr ranged_default_t<float> kStepPenaltyWheelchairRange{0, kDefaultStepPenaltyWheelchair, kMaxSeconds};
constexpr ranged_default_t<float> kStepPenaltyFootRange{0, kDefaultStepPenaltyFoot, kMaxSeconds};

constexpr ranged_default_t<uint32_t> kMaxGradeWheelchairRange{0, kDefaultMaxGradeWheelchair, kDefaultMaxGradeFoot};
constexpr ranged_default_t<uint32_t> kMaxGradeFootRange{0, kDefaultMaxGradeFoot, kDefaultMaxGradeFoot};

// Other valid ranges and defaults (not dependent on type)
constexpr ranged_default_t<float> kModeFactorRange{kMinFactor, kModeFactor, kMaxFactor};
constexpr ranged_default_t<float> kManeuverPenaltyRange{kMinFactor, kDefaultManeuverPenalty, kMaxSeconds};
constexpr ranged_default_t<float> kGatePenaltyRange{kMinFactor, kDefaultGatePenalty, kMaxSeconds};
constexpr ranged_default_t<float> kWalkwayFactorRange{kMinFactor, kDefaultWalkwayFactor, kMaxFactor};
constexpr ranged_default_t<float> kSideWalkFactorRange{kMinFactor, kDefaultSideWalkFactor, kMaxFactor};
constexpr ranged_default_t<float> kAlleyFactorRange{kMinFactor, kDefaultAlleyFactor, kMaxFactor};
constexpr ranged_default_t<float> kDrivewayFactorRange{kMinFactor, kDefaultDrivewayFactor, kMaxFactor};
constexpr ranged_default_t<float> kFerryCostRange{0, kDefaultFerryCost, kMaxSeconds};
constexpr ranged_default_t<float> kCountryCrossingCostRange{0, kDefaultCountryCrossingCost, kMaxSeconds};
constexpr ranged_default_t<float> kCountryCrossingPenaltyRange{0, kDefaultCountryCrossingPenalty, kMaxSeconds};
constexpr ranged_default_t<uint32_t> kTransitStartEndMaxDistanceRange{0, kTransitStartEndMaxDistance,
                                                                     100000}; // Max 100k
constexpr ranged_default_t<uint32_t> kTransitTransferMaxDistanceRange{0, kTransitTransferMaxDistance,
                                                                      50000}; // Max 50k
constexpr ranged_default_t<float> kUseFerryRange{0, kDefaultUseFerry, 1.0f};

/**
 * Derived class providing dynamic edge costing for pedestrian routes.
 */
class PedestrianCost : public DynamicCost {
 public:
  /**
   * Constructor. Configuration / options for pedestrian costing are provided
   * via a property tree (JSON).
   * @param  pt  Property tree with configuration/options.
   */
  //Armin H.
  PedestrianCost(const boost::property_tree::ptree& pt, baldr::GraphReader& graphreader);
  //Armin H.

  virtual ~PedestrianCost();

  /**
   * Does the costing method allow multiple passes (with relaxed hierarchy
   * limits).
   * @return  Returns true if the costing model allows multiple passes.
   */
  virtual bool AllowMultiPass() const;

  /**
   * This method overrides the max_distance with the max_distance_mm per segment
   * distance. An example is a pure walking route may have a max distance of
   * 10000 meters (10km) but for a multi-modal route a lower limit of 5000
   * meters per segment (e.g. from origin to a transit stop or from the last
   * transit stop to the destination).
   */
  virtual void UseMaxMultiModalDistance();

  /**
   * Returns the maximum transfer distance between stops that you are willing
   * to travel for this mode.  In this case, it is the max walking
   * distance you are willing to walk between transfers.
   */
  virtual uint32_t GetMaxTransferDistanceMM();

  /**
   * This method overrides the factor for this mode.  The higher the value
   * the more the mode is favored.
   */
  virtual float GetModeFactor();

  /**
   * Get the access mode used by this costing method.
   * @return  Returns access mode.
   */
  uint32_t access_mode() const;

  /**
   * Checks if access is allowed for the provided directed edge.
   * This is generally based on mode of travel and the access modes
   * allowed on the edge. However, it can be extended to exclude access
   * based on other parameters.
   * @param  edge     Pointer to a directed edge.
   * @param  pred     Predecessor edge information.
   * @param  tile     current tile
   * @param  edgeid   edgeid that we care about
   * @return  Returns true if access is allowed, false if not.
   */
  virtual bool Allowed(const baldr::DirectedEdge* edge,
                       const EdgeLabel& pred,
                       const baldr::GraphTile*& tile,
                       const baldr::GraphId& edgeid) const;

  /**
   * Checks if access is allowed for an edge on the reverse path
   * (from destination towards origin). Both opposing edges are
   * provided.
   * @param  edge           Pointer to a directed edge.
   * @param  pred           Predecessor edge information.
   * @param  opp_edge       Pointer to the opposing directed edge.
   * @param  tile           Tile for the opposing edge (for looking
   *                        up restrictions).
   * @param  opp_edgeid     Opposing edge Id
   * @return  Returns true if access is allowed, false if not.
   */
  virtual bool AllowedReverse(const baldr::DirectedEdge* edge,
                 const EdgeLabel& pred,
                 const baldr::DirectedEdge* opp_edge,
                 const baldr::GraphTile*& tile,
                 const baldr::GraphId& opp_edgeid) const;

  /**
   * Checks if access is allowed for the provided node. Node access can
   * be restricted if bollards or gates are present.
   * @param  edge  Pointer to node information.
   * @return  Returns true if access is allowed, false if not.
   */
  virtual bool Allowed(const baldr::NodeInfo* node) const;

  /**
   * Get the cost to traverse the specified directed edge. Cost includes
   * the time (seconds) to traverse the edge.
   * @param   edge  Pointer to a directed edge.
   * @return  Returns the cost and time (seconds)
   */
  virtual Cost EdgeCost(const baldr::DirectedEdge* edge) const;

  /**
   * Returns the cost to make the transition from the predecessor edge.
   * Defaults to 0. Costing models that wish to include edge transition
   * costs (i.e., intersection/turn costs) must override this method.
   * @param  edge  Directed edge (the to edge)
   * @param  node  Node (intersection) where transition occurs.
   * @param  pred  Predecessor edge information.
   * @return  Returns the cost and time (seconds)
   */
  virtual Cost TransitionCost(const baldr::DirectedEdge* edge,
                              const baldr::NodeInfo* node,
                              const EdgeLabel& pred) const;

  /**
   * Returns the cost to make the transition from the predecessor edge
   * when using a reverse search (from destination towards the origin).
   * Defaults to 0. Costing models that wish to include edge transition
   * costs (i.e., intersection/turn costs) must override this method.
   * @param  idx   Directed edge local index
   * @param  node  Node (intersection) where transition occurs.
   * @param  pred  the opposing current edge in the reverse tree.
   * @param  edge  the opposing predecessor in the reverse tree
   * @return  Returns the cost and time (seconds)
   */
  virtual Cost TransitionCostReverse(const uint32_t idx,
                                     const baldr::NodeInfo* node,
                                     const baldr::DirectedEdge* pred,
                                     const baldr::DirectedEdge* edge) const;

  /**
   * Get the cost factor for A* heuristics. This factor is multiplied
   * with the distance to the destination to produce an estimate of the
   * minimum cost to the destination. The A* heuristic must underestimate the
   * cost to the destination. So a time based estimate based on speed should
   * assume the maximum speed is used to the destination such that the time
   * estimate is less than the least possible time along roads.
   */
  virtual float AStarCostFactor() const;

  /**
   * Get the current travel type.
   * @return  Returns the current travel type.
   */
  virtual uint8_t travel_type() const;

  /**
   * Returns a function/functor to be used in location searching which will
   * exclude and allow ranking results from the search by looking at each
   * edges attribution and suitability for use as a location by the travel
   * mode used by the costing method. Function/functor is also used to filter
   * edges not usable / inaccessible by pedestrians.
   */
   virtual const EdgeFilter GetEdgeFilter() const {
     // Throw back a lambda that checks the access for this type of costing
     auto access_mask = access_mask_;
     return [access_mask](const baldr::DirectedEdge* edge) {
       return !(edge->IsTransition() || edge->is_shortcut() ||
           edge->use() >= Use::kRail ||
          !(edge->forwardaccess() & access_mask));
     };
   }

   virtual const NodeFilter GetNodeFilter() const {
     //throw back a lambda that checks the access for this type of costing
     auto access_mask = access_mask_;
     return [access_mask](const baldr::NodeInfo* node){
       return !(node->access() & access_mask);
     };
   }

  /**
   * Returns a function/functor to be used in location searching which will
   * exclude results from the search by looking at each node's attribution
   * @return Function/functor to be used in filtering out nodes
   */

 public:
  // Type: foot (default), wheelchair, etc.
  PedestrianType type_;

  uint32_t access_mask_;

  // Maximum pedestrian distance.
  uint32_t max_distance_;

  // This is the factor for this mode.  The higher the value the more the
  // mode is favored.
  float mode_factor_;

  // Maximum pedestrian distance in meters for multimodal routes.
  // Maximum distance at the beginning or end of a multimodal route
  // that you are willing to travel for this mode.  In this case,
  // it is the max walking distance.
  uint32_t transit_start_end_max_distance_;

  // Maximum transfer, distance in meters for multimodal routes.
  // Maximum transfer distance between stops that you are willing
  // to travel for this mode.  In this case, it is the max distance
  // you are willing to walk between transfers.
  uint32_t transit_transfer_max_distance_;

  // Minimal surface type usable by the pedestrian type
  Surface minimal_allowed_surface_;

  uint32_t max_grade_;    // Maximum grade (percent).
  float speed_;           // Pedestrian speed.
  float speedfactor_;     // Speed factor for costing. Based on speed.
  float walkway_factor_;  // Factor for favoring walkways and paths.
  float sidewalk_factor_; // Factor for favoring sidewalks.
  float alley_factor_;    // Avoid alleys factor.
  float driveway_factor_; // Avoid driveways factor.
  float step_penalty_;    // Penalty applied to steps/stairs (seconds).
  float gate_penalty_;    // Penalty (seconds) to go through gate
  float maneuver_penalty_;          // Penalty (seconds) when inconsistent names
  float country_crossing_cost_;     // Cost (seconds) to go through toll booth
  float country_crossing_penalty_;  // Penalty (seconds) to go across a country border
  float ferry_cost_;                // Cost (seconds) to exit a ferry
  float ferry_penalty_;             // Penalty (seconds) to enter a ferry
  float ferry_factor_;              // Weighting to apply to ferry edges
  float use_ferry_;
};

// Constructor. Parse pedestrian options from property tree. If option is
// not present, set the default.
//Armin H.
PedestrianCost::PedestrianCost(const boost::property_tree::ptree& pt, baldr::GraphReader& graphreader)
    : DynamicCost(pt, graphreader, TravelMode::kPedestrian) {
  // Set hierarchy to allow unlimited transitions
  for (auto& h : hierarchy_limits_) {
    h.max_up_transitions = kUnlimitedTransitions;
  }

  allow_transit_connections_ = false;

  // Get the pedestrian type - enter as string and convert to enum
  std::string type = pt.get<std::string>("type", "foot");
  if (type == "wheelchair") {
    type_ = PedestrianType::kWheelchair;
  } else if (type == "segway") {
    type_ = PedestrianType::kSegway;
  } else {
    type_ = PedestrianType::kFoot;
  }

  // Set type specific defaults, override with URL inputs
  if (type == "wheelchair") {
    access_mask_ = kWheelchairAccess;
    max_distance_ = kMaxDistanceWheelchairRange(
      pt.get<uint32_t>("max_distance", kMaxDistanceWheelchair)
    );
    speed_ = kSpeedWheelchairRange(
      pt.get<float>("walking_speed", kDefaultSpeedWheelchair)
    );
    step_penalty_ = kStepPenaltyWheelchairRange(
      pt.get<float>("step_penalty", kDefaultStepPenaltyWheelchair)
    );
    max_grade_ = kMaxGradeWheelchairRange(
      pt.get<uint32_t>("max_grade", kDefaultMaxGradeWheelchair)
    );
    minimal_allowed_surface_ = Surface::kCompacted;
  } else {
    // Assume type = foot
    access_mask_ = kPedestrianAccess;
    max_distance_ = kMaxDistanceFootRange(
      pt.get<uint32_t>("max_distance", kMaxDistanceFoot)
    );
    speed_ = kSpeedFootRange(
      pt.get<float>("walking_speed", kDefaultSpeedFoot)
    );
    step_penalty_ = kStepPenaltyFootRange(
      pt.get<float>("step_penalty", kDefaultStepPenaltyFoot)
    );
    max_grade_ = kMaxGradeFootRange(
      pt.get<uint32_t>("max_grade", kDefaultMaxGradeFoot)
    );
    minimal_allowed_surface_ = Surface::kPath;
  }

  mode_factor_ = kModeFactorRange(
    pt.get<float>("mode_factor", kModeFactor)
  );
  maneuver_penalty_ = kManeuverPenaltyRange(
    pt.get<float>("maneuver_penalty", kDefaultManeuverPenalty)
  );
  gate_penalty_ = kGatePenaltyRange(
    pt.get<float>("gate_penalty", kDefaultGatePenalty)
  );
  walkway_factor_ = kWalkwayFactorRange(
    pt.get<float>("walkway_factor", kDefaultWalkwayFactor)
  );
  sidewalk_factor_ = kSideWalkFactorRange(
    pt.get<float>("sidewalk_factor", kDefaultSideWalkFactor)
  );
  alley_factor_ = kAlleyFactorRange(
    pt.get<float>("alley_factor", kDefaultAlleyFactor)
  );
  driveway_factor_ = kDrivewayFactorRange(
    pt.get<float>("driveway_factor", kDefaultDrivewayFactor)
  );
  ferry_cost_ = kFerryCostRange(
    pt.get<float>("ferry_cost", kDefaultFerryCost)
  );
  country_crossing_cost_ = kCountryCrossingCostRange(
    pt.get<float>("country_crossing_cost", kDefaultCountryCrossingCost)
  );
  country_crossing_penalty_ = kCountryCrossingPenaltyRange(
    pt.get<float>("country_crossing_penalty", kDefaultCountryCrossingPenalty)
  );
  transit_start_end_max_distance_ = kTransitStartEndMaxDistanceRange(
    pt.get<uint32_t>("transit_start_end_max_distance", kTransitStartEndMaxDistance)
  );
  transit_transfer_max_distance_  = kTransitTransferMaxDistanceRange(
    pt.get<uint32_t>("transit_transfer_max_distance", kTransitTransferMaxDistance)
  );

  // Modify ferry penalty and edge weighting based on use_ferry_ factor
  use_ferry_ = kUseFerryRange(
    pt.get<float>("use_ferry", kDefaultUseFerry)
  );
  if (use_ferry_ < 0.5f) {
    // Penalty goes from max at use_ferry_ = 0 to 0 at use_ferry_ = 0.5
    ferry_penalty_ = static_cast<uint32_t>(kMaxFerryPenalty * (1.0f - use_ferry_ * 2.0f));

    // Cost X10 at use_ferry_ == 0, slopes downwards towards 1.0 at use_ferry_ = 0.5
    ferry_factor_ = 10.0f - use_ferry_ * 18.0f;
  } else {
    // Add a ferry weighting factor to influence cost along ferries to make
    // them more favorable if desired rather than driving. No ferry penalty.
    // Half the cost at use_ferry_ == 1, progress to 1.0 at use_ferry_ = 0.5
    ferry_penalty_ = 0.0f;
    ferry_factor_  = 1.5f - use_ferry_;
  }

  // Set the speed factor (to avoid division in costing)
  speedfactor_ = (kSecPerHour * 0.001f) / speed_;
}

// Destructor
PedestrianCost::~PedestrianCost() {
}

// Allow multiple passes when ferries are on initial path.
bool PedestrianCost::AllowMultiPass() const {
  return true;
}

// This method overrides the max_distance with the max_distance_mm per segment
// distance. An example is a pure walking route may have a max distance of
// 10000 meters (10km) but for a multi-modal route a lower limit of 5000
// meters per segment (e.g. from origin to a transit stop or from the last
// transit stop to the destination).
void PedestrianCost::UseMaxMultiModalDistance() {
  max_distance_ = transit_start_end_max_distance_;
}

// Returns the maximum transfer distance between stops that you are willing
// to travel for this mode.  In this case, it is the max walking
// distance you are willing to walk between transfers.
uint32_t PedestrianCost::GetMaxTransferDistanceMM() {
  return transit_transfer_max_distance_;
}

// This method overrides the factor for this mode.  The lower the value
// the more the mode is favored.
float PedestrianCost::GetModeFactor() {
  return mode_factor_;
}

// Get the access mode used by this costing method.
uint32_t PedestrianCost::access_mode() const {
  return access_mask_;
}

// Check if access is allowed on the specified edge. Disallow if no
// access for this pedestrian type, if surface type exceeds (worse than)
// the minimum allowed surface type, or if max grade is exceeded.
// Disallow edges where max. distance will be exceeded.
bool PedestrianCost::Allowed(const baldr::DirectedEdge* edge,
                             const EdgeLabel& pred,
                             const baldr::GraphTile*& tile,
                             const baldr::GraphId& edgeid) const {
  // TODO - obtain and check the access restrictions.

  if (!(edge->forwardaccess() & access_mask_) ||
       (edge->surface() > minimal_allowed_surface_) ||
        edge->is_shortcut() || IsUserAvoidEdge(edgeid) ||
 //      (edge->max_up_slope() > max_grade_ || edge->max_down_slope() > max_grade_) ||
      ((pred.path_distance() + edge->length()) > max_distance_)) {
    return false;
  }

  // Disallow transit connections (except when set for multi-modal routes)
  if (!allow_transit_connections_ && (edge->use() == Use::kPlatformConnection ||
      edge->use() == Use::kEgressConnection ||
      edge->use() == Use::kTransitConnection)) {
    return false;
  }
  return true;
}

// Checks if access is allowed for an edge on the reverse path (from
// destination towards origin). Both opposing edges are provided.
bool PedestrianCost::AllowedReverse(const baldr::DirectedEdge* edge,
               const EdgeLabel& pred,
               const baldr::DirectedEdge* opp_edge,
               const baldr::GraphTile*& tile,
               const baldr::GraphId& opp_edgeid) const {
  // TODO - obtain and check the access restrictions.

  // Do not check max walking distance and assume we are not allowing
  // transit connections. Assume this method is never used in
  // multimodal routes).
  if (!(opp_edge->forwardaccess() & access_mask_) ||
       (opp_edge->surface() > minimal_allowed_surface_) ||
        opp_edge->is_shortcut() || IsUserAvoidEdge(opp_edgeid) ||
 //      (opp_edge->max_up_slope() > max_grade_ || opp_edge->max_down_slope() > max_grade_) ||
        opp_edge->use() == Use::kTransitConnection || opp_edge->use() == Use::kEgressConnection ||
        opp_edge->use() == Use::kPlatformConnection) {
    return false;
  }
  return true;
}

// Check if access is allowed at the specified node.
bool PedestrianCost::Allowed(const baldr::NodeInfo* node) const {
  return (node->access() & access_mask_);
}

// Returns the cost to traverse the edge and an estimate of the actual time
// (in seconds) to traverse the edge.
Cost PedestrianCost::EdgeCost(const baldr::DirectedEdge* edge) const {

  // Ferries are a special case - they use the ferry speed (stored on the edge)
  if (edge->use() == Use::kFerry) {
    float sec = edge->length() * (kSecPerHour * 0.001f) /
            static_cast<float>(edge->speed());
    return { sec * ferry_factor_, sec };
  }

  // Slightly favor walkways/paths and penalize alleys and driveways.
  float sec = edge->length() * speedfactor_;
  if (edge->use() == Use::kFootway) {
    return { sec * walkway_factor_, sec };
  } else if (edge->use() == Use::kAlley) {
    return { sec * alley_factor_, sec };
  } else if (edge->use() == Use::kDriveway) {
    return { sec * driveway_factor_, sec };
  } else if (edge->use() == Use::kSidewalk) {
    return { sec * sidewalk_factor_, sec };
  } else if (edge->roundabout()) {
    return { sec * kRoundaboutFactor, sec };
  } else {
    return { sec, sec };
  }
}

// Returns the time (in seconds) to make the transition from the predecessor
Cost PedestrianCost::TransitionCost(const baldr::DirectedEdge* edge,
                                    const baldr::NodeInfo* node,
                                    const EdgeLabel& pred) const {
  // Special cases: fixed penalty for steps/stairs
  if (edge->use() == Use::kSteps) {
    return { step_penalty_, 0.0f };
  }

  // Penalty through gates and border control.
  float seconds = 0.0f;
  float penalty = 0.0f;
  if (node->type() == NodeType::kBorderControl) {
    seconds += country_crossing_cost_;
    penalty += country_crossing_penalty_;
  } else if (node->type() == NodeType::kGate) {
    penalty += gate_penalty_;
  }

  if ((pred.use() != Use::kFerry && edge->use() == Use::kFerry)) {
    seconds += ferry_cost_;
    penalty += ferry_penalty_;
  }

  uint32_t idx = pred.opp_local_idx();
  // Ignore name inconsistency when entering a link to avoid double penalizing.
  if (!edge->link() && edge->use() != Use::kEgressConnection &&
      edge->use() != Use::kPlatformConnection &&
      !node->name_consistency(idx, edge->localedgeidx())) {
    // Slight maneuver penalty
      penalty += maneuver_penalty_;
  }

  // Costs for crossing an intersection.
  if (edge->edge_to_right(idx) && edge->edge_to_left(idx)) {
    seconds += kCrossingCosts[edge->stopimpact(idx)];
  }
  return { seconds + penalty, seconds };
}

// Returns the cost to make the transition from the predecessor edge
// when using a reverse search (from destination towards the origin).
// Defaults to 0. Costing models that wish to include edge transition
// costs (i.e., intersection/turn costs) must override this method.
Cost PedestrianCost::TransitionCostReverse(
    const uint32_t idx, const baldr::NodeInfo* node,
    const baldr::DirectedEdge* pred, const baldr::DirectedEdge* edge) const {
  // Special cases: fixed penalty for steps/stairs
  if (edge->use() == Use::kSteps) {
    return { step_penalty_, 0.0f };
  }

  // Penalty through gates and border control.
  float seconds = 0.0f;
  float penalty = 0.0f;
  if (node->type() == NodeType::kBorderControl) {
    seconds += country_crossing_cost_;
    penalty += country_crossing_penalty_;
  } else if (node->type() == NodeType::kGate) {
    penalty += gate_penalty_;
  }

  if (pred->use() != Use::kFerry && edge->use() == Use::kFerry) {
    seconds += ferry_cost_;
    penalty += ferry_penalty_;
  }

  // Ignore name inconsistency when entering a link to avoid double penalizing.
  if (!edge->link() && edge->use() != Use::kEgressConnection &&
      edge->use() != Use::kPlatformConnection &&
      !node->name_consistency(idx, edge->localedgeidx())) {
    // Slight maneuver penalty
    penalty += maneuver_penalty_;
  }

  // Costs for crossing an intersection.
  if (edge->edge_to_right(idx) && edge->edge_to_left(idx)) {
    seconds += kCrossingCosts[edge->stopimpact(idx)];
  }
  return { seconds + penalty, seconds };
}

// Get the cost factor for A* heuristics. This factor is multiplied
// with the distance to the destination to produce an estimate of the
// minimum cost to the destination. The A* heuristic must underestimate the
// cost to the destination. So a time based estimate based on speed should
// assume the maximum speed is used to the destination such that the time
// estimate is less than the least possible time along roads.
float PedestrianCost::AStarCostFactor() const {
  // On first pass use the walking speed plus a small factor to account for
  // favoring walkways, on the second pass use the the maximum ferry speed.
  if (pass_ == 0) {
    float speed = kDefaultSpeedFoot * std::min(walkway_factor_, sidewalk_factor_);
    return (kSecPerHour * 0.001f) / static_cast<float>(speed);
  } else {
    return (kSecPerHour * 0.001f) / static_cast<float>(kMaxFerrySpeedKph);
  }
}

// Returns the current travel type.
uint8_t PedestrianCost::travel_type() const {
  return static_cast<uint8_t>(type_);
}

//Armin H.
cost_ptr_t CreatePedestrianCost(const boost::property_tree::ptree& config, baldr::GraphReader& graphreader) {
  return std::make_shared<PedestrianCost>(config, graphreader);
}

}
}

/**********************************************************************************************/

#ifdef INLINE_TEST

using namespace valhalla;
using namespace sif;

namespace {

PedestrianCost* make_pedestriancost_from_json(const std::string& property, float testVal, const std::string& type) {
  std::stringstream ss;
  ss << R"({")" << property << R"(":)" << testVal << R"(,"type":")" << type << R"(")" << "}";
  boost::property_tree::ptree costing_ptree;
  boost::property_tree::read_json(ss, costing_ptree);
  //Armin H.
  boost::property_tree::ptree pt;
  pt.put("tile_dir", "test/gphrdr_test");
  GraphReader reader(pt);
  return new PedestrianCost(costing_ptree, reader);
}

std::uniform_real_distribution<float>* make_distributor_from_range (const ranged_default_t<float>& range) {
  float rangeLength = range.max - range.min;
  return new std::uniform_real_distribution<float>(range.min - rangeLength, range.max + rangeLength);
}

std::uniform_int_distribution<uint32_t>* make_distributor_from_range (const ranged_default_t<uint32_t>& range) {
  uint32_t rangeLength = range.max - range.min;
  return new std::uniform_int_distribution<uint32_t>(range.min - rangeLength, range.max + rangeLength);
}

void testPedestrianCostParams() {
  constexpr unsigned testIterations = 250;
  constexpr unsigned seed = 0;
  std::default_random_engine generator(seed);
  std::shared_ptr<std::uniform_real_distribution<float>> real_distributor;
  std::shared_ptr<std::uniform_int_distribution<uint32_t>> int_distributor;
  std::shared_ptr<PedestrianCost> ctorTester;

  // Wheelchair tests
  // max_distance_
  int_distributor.reset(make_distributor_from_range(kMaxDistanceWheelchairRange));
  for (unsigned i = 0; i < 100; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("max_distance", (*int_distributor)(generator), "wheelchair"));
    if (ctorTester->max_distance_ < kMaxDistanceWheelchairRange.min ||
        ctorTester->max_distance_ > kMaxDistanceWheelchairRange.max) {
      throw std::runtime_error ("max_distance_ with type wheelchair is not within it's range");
    }
  }

  // speed_
  real_distributor.reset(make_distributor_from_range(kSpeedWheelchairRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("walking_speed", (*real_distributor)(generator), "wheelchair"));
    if (ctorTester->speed_ < kSpeedWheelchairRange.min ||
        ctorTester->speed_ > kSpeedWheelchairRange.max) {
      throw std::runtime_error ("speed_ with type wheelchair is not within it's range");
    }
  }

  // step_penalty_
  real_distributor.reset(make_distributor_from_range(kStepPenaltyWheelchairRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("step_penalty", (*real_distributor)(generator), "wheelchair"));
    if (ctorTester->step_penalty_ < kStepPenaltyWheelchairRange.min ||
        ctorTester->step_penalty_ > kStepPenaltyWheelchairRange.max) {
      throw std::runtime_error ("step_penalty_ with type wheelchair is not within it's range");
    }
  }

  // max_grade_
  int_distributor.reset(make_distributor_from_range(kMaxGradeWheelchairRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("max_grade", (*int_distributor)(generator), "wheelchair"));
    if (ctorTester->max_grade_ < kMaxGradeWheelchairRange.min ||
        ctorTester->max_grade_ > kMaxGradeWheelchairRange.max) {
      throw std::runtime_error ("max_grade_ with type wheelchair is not within it's range");
    }
  }

  // Foot tests
  // max_distance_
  int_distributor.reset(make_distributor_from_range(kMaxDistanceFootRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("max_distance", (*int_distributor)(generator), "foot"));
    if (ctorTester->max_distance_ < kMaxDistanceFootRange.min ||
        ctorTester->max_distance_ > kMaxDistanceFootRange.max) {
      throw std::runtime_error ("max_distance_ with type foot is not within it's range");
    }
  }

  // speed_
  real_distributor.reset(make_distributor_from_range(kSpeedFootRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("walking_speed", (*real_distributor)(generator), "foot"));
    if (ctorTester->speed_ < kSpeedFootRange.min ||
        ctorTester->speed_ > kSpeedFootRange.max) {
      throw std::runtime_error ("speed_ with type foot is not within it's range");
    }
  }

  // step_penalty_
  real_distributor.reset(make_distributor_from_range(kStepPenaltyFootRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("step_penalty", (*real_distributor)(generator), "foot"));
    if (ctorTester->step_penalty_ < kStepPenaltyFootRange.min ||
        ctorTester->step_penalty_ > kStepPenaltyFootRange.max) {
      throw std::runtime_error ("step_penalty_ with type foot is not within it's range");
    }
  }

  // max_grade_
  int_distributor.reset(make_distributor_from_range(kMaxGradeFootRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("max_grade", (*int_distributor)(generator), "foot"));
    if (ctorTester->max_grade_ < kMaxGradeFootRange.min ||
        ctorTester->max_grade_ > kMaxGradeFootRange.max) {
      throw std::runtime_error ("max_grade_ with type foot is not within it's range");
    }
  }

  // Non type dependent tests
  // mode_factor_
    real_distributor.reset(make_distributor_from_range(kModeFactorRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("mode_factor", (*real_distributor)(generator), "foot"));
    if (ctorTester->mode_factor_ < kModeFactorRange.min ||
        ctorTester->mode_factor_ > kModeFactorRange.max) {
      throw std::runtime_error ("mode_factor_ is not within it's range");
    }
  }

  // maneuver_penalty_
  real_distributor.reset(make_distributor_from_range(kManeuverPenaltyRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("maneuver_penalty", (*real_distributor)(generator), "foot"));
    if (ctorTester->maneuver_penalty_ < kManeuverPenaltyRange.min ||
        ctorTester->maneuver_penalty_ > kManeuverPenaltyRange.max) {
      throw std::runtime_error ("maneuver_penalty_ is not within it's range");
    }
  }

  // gate_penalty_
  real_distributor.reset(make_distributor_from_range(kGatePenaltyRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("gate_penalty", (*real_distributor)(generator), "foot"));
    if (ctorTester->gate_penalty_ < kGatePenaltyRange.min ||
        ctorTester->gate_penalty_ > kGatePenaltyRange.max) {
      throw std::runtime_error ("gate_penalty_ is not within it's range");
    }
  }

  // walkway_factor_
  real_distributor.reset(make_distributor_from_range(kWalkwayFactorRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("walkway_factor", (*real_distributor)(generator), "foot"));
    if (ctorTester->walkway_factor_ < kWalkwayFactorRange.min ||
        ctorTester->walkway_factor_ > kWalkwayFactorRange.max) {
      throw std::runtime_error ("walkway_factor_ is not within it's range");
    }
  }

  // sidewalk_factor_
  real_distributor.reset(make_distributor_from_range(kSideWalkFactorRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("sidewalk_factor", (*real_distributor)(generator), "foot"));
    if (ctorTester->sidewalk_factor_ < kSideWalkFactorRange.min ||
        ctorTester->sidewalk_factor_ > kSideWalkFactorRange.max) {
      throw std::runtime_error ("sidewalk_factor_ is not within it's range");
    }
  }

  // alley_factor_
  real_distributor.reset(make_distributor_from_range(kAlleyFactorRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("alley_factor", (*real_distributor)(generator), "foot"));
    if (ctorTester->alley_factor_ < kAlleyFactorRange.min ||
        ctorTester->alley_factor_ > kAlleyFactorRange.max) {
      throw std::runtime_error ("alley_factor_ is not within it's range");
    }
  }

  // driveway_factor_
  real_distributor.reset(make_distributor_from_range(kDrivewayFactorRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("driveway_factor", (*real_distributor)(generator), "foot"));
    if (ctorTester->driveway_factor_ < kDrivewayFactorRange.min ||
        ctorTester->driveway_factor_ > kDrivewayFactorRange.max) {
      throw std::runtime_error ("driveway_factor_ is not within it's range");
    }
  }

  // ferry_cost_
  real_distributor.reset(make_distributor_from_range(kFerryCostRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("ferry_cost", (*real_distributor)(generator), "foot"));
    if (ctorTester->ferry_cost_ < kFerryCostRange.min ||
        ctorTester->ferry_cost_ > kFerryCostRange.max) {
      throw std::runtime_error ("ferry_cost_ is not within it's range");
    }
  }

  // country_crossing_cost_
  real_distributor.reset(make_distributor_from_range(kCountryCrossingCostRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("country_crossing_cost", (*real_distributor)(generator), "foot"));
    if (ctorTester->country_crossing_cost_ < kCountryCrossingCostRange.min ||
        ctorTester->country_crossing_cost_ > kCountryCrossingCostRange.max) {
      throw std::runtime_error ("country_crossing_cost_ is not within it's range");
    }
  }

  // country_crossing_penalty_
  real_distributor.reset(make_distributor_from_range(kCountryCrossingPenaltyRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("country_crossing_penalty", (*real_distributor)(generator), "foot"));
    if (ctorTester->country_crossing_penalty_ < kCountryCrossingPenaltyRange.min ||
        ctorTester->country_crossing_penalty_ > kCountryCrossingPenaltyRange.max) {
      throw std::runtime_error ("country_crossing_penalty_ is not within it's range");
    }
  }

  // transit_start_end_max_distance_
  int_distributor.reset(make_distributor_from_range(kTransitStartEndMaxDistanceRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("transit_start_end_max_distance",
                                                  (*int_distributor)(generator), "foot"));
    if (ctorTester->transit_start_end_max_distance_ < kTransitStartEndMaxDistanceRange.min ||
        ctorTester->transit_start_end_max_distance_ > kTransitStartEndMaxDistanceRange.max) {
      throw std::runtime_error ("transit_start_end_max_distance_ is not within it's range");
    }
  }

  // transit_transfer_max_distance_
  int_distributor.reset(make_distributor_from_range(kTransitTransferMaxDistanceRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("transit_transfer_max_distance",
                                                  (*int_distributor)(generator), "foot"));
    if (ctorTester->transit_transfer_max_distance_ < kTransitTransferMaxDistanceRange.min ||
        ctorTester->transit_transfer_max_distance_ > kTransitTransferMaxDistanceRange.max) {
      throw std::runtime_error ("transit_transfer_max_distance_ is not within it's range");
    }
  }

  // use_ferry_
  real_distributor.reset(make_distributor_from_range(kUseFerryRange));
  for (unsigned i = 0; i < testIterations; ++i) {
    ctorTester.reset(make_pedestriancost_from_json("use_ferry", (*real_distributor)(generator), "foot"));
    if (ctorTester->use_ferry_ < kUseFerryRange.min ||
        ctorTester->use_ferry_ > kUseFerryRange.max) {
      throw std::runtime_error ("use_ferry_ is not within it's range");
    }
  }
}
}

int main() {
  test::suite suite("costing");

  suite.test(TEST_CASE(testPedestrianCostParams));

  return suite.tear_down();
}

#endif


#include <data/Geometry.hpp>
#include <engine/Neighbours.hpp>
#include <engine/Vectormath.hpp>
#include <utility/Exception.hpp>

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

#include "Qhull.h"
#include "QhullFacetList.h"
#include "QhullVertexSet.h"

#include <fmt/ostream.h>

#include <random>
#include <array>

namespace Data
{
    Geometry::Geometry(std::vector<Vector3> bravais_vectors, intfield n_cells,
                        std::vector<Vector3> cell_atoms,  Basis_Cell_Composition cell_composition,
                        scalar lattice_constant, Pinning pinning, Defects defects) :
        bravais_vectors(bravais_vectors), n_cells(n_cells), n_cell_atoms(cell_atoms.size()),
        cell_atoms(cell_atoms), cell_composition(cell_composition), lattice_constant(lattice_constant),
        nos(cell_atoms.size() * n_cells[0] * n_cells[1] * n_cells[2]),
        nos_nonvacant(cell_atoms.size() * n_cells[0] * n_cells[1] * n_cells[2]),
        n_cells_total(n_cells[0] * n_cells[1] * n_cells[2]),
        pinning(pinning), defects(defects)
    {
        // Generate positions and atom types
        this->positions = vectorfield(this->nos);
        this->generatePositions();

        // Calculate some useful info
        this->calculateBounds();
        this->calculateUnitCellBounds();
        this->calculateDimensionality();

        // Calculate center of the System
        this->center = 0.5 * (this->bounds_min + this->bounds_max);

        // Generate default atom_types, mu_s and pinning masks
        this->atom_types        = intfield(this->nos, 0);
        this->mu_s              = scalarfield(this->nos, 1);
        this->mask_unpinned     = intfield(this->nos, 1);
        this->mask_pinned_cells = vectorfield(this->nos, { 0,0,0 });

        // Set atom types, mu_s
        this->applyCellComposition();

        // Apply additional pinned sites
        for( int isite=0; isite < pinning.sites.size(); ++isite )
        {
            auto& site = pinning.sites[isite];
            int ispin = site.i + Engine::Vectormath::idx_from_translations(
                        this->n_cells, this->n_cell_atoms,
                        {site.translations[0], site.translations[1], site.translations[2]} );
            this->mask_unpinned[ispin] = 0;
            this->mask_pinned_cells[ispin] = pinning.spins[isite];
        }

        // Apply additional defect sites
        for (int i = 0; i < defects.sites.size(); ++i)
        {
            auto& defect = defects.sites[i];
            int ispin = defects.sites[i].i + Engine::Vectormath::idx_from_translations(
                        this->n_cells, this->n_cell_atoms,
                        {defect.translations[0], defect.translations[1], defect.translations[2]} );
            this->atom_types[ispin] = defects.types[i];
            this->mu_s[ispin] = 0.0;
        }

        // Calculate the type of geometry
        this->calculateGeometryType();

        // For updates of triangulation and tetrahedra
        this->last_update_n_cell_step = -1;
        this->last_update_n_cells = intfield(3, -1);
    }

    void Geometry::generatePositions()
    {
        const scalar epsilon = 1e-6;

        // Check for erronous input placing two spins on the same location
        int max_a = std::min(10, n_cells[0]);
        int max_b = std::min(10, n_cells[1]);
        int max_c = std::min(10, n_cells[2]);
        Vector3 diff;
        for (int i = 0; i < n_cell_atoms; ++i)
        {
            for (int j = 0; j < n_cell_atoms; ++j)
            {
                for (int da = -max_a; da <= max_a; ++da)
                {
                    for (int db = -max_b; db <= max_b; ++db)
                    {
                        for (int dc = -max_c; dc <= max_c; ++dc)
                        {
                            // Norm is zero if translated basis atom is at position of another basis atom
                            diff = cell_atoms[i] - ( cell_atoms[j] + Vector3{scalar(da), scalar(db), scalar(dc)} );

                            if( (i != j || da != 0 || db != 0 || dc != 0) &&
                                std::abs(diff[0]) < epsilon &&
                                std::abs(diff[1]) < epsilon &&
                                std::abs(diff[2]) < epsilon )
                            {
                                Vector3 position = lattice_constant * (
                                      (da + cell_atoms[i][0]) * bravais_vectors[0]
                                    + (db + cell_atoms[i][1]) * bravais_vectors[1]
                                    + (dc + cell_atoms[i][2]) * bravais_vectors[2] );
                                std::string message = fmt::format(
                                    "Unable to initialize Spin-System, since 2 spins occupy the same space"
                                    "within a margin of {} at absolute position ({}).\n"
                                    "Index combination: i={} j={}, translations=({}, {}, {}).\n"
                                    "Please check the config file!", epsilon, position.transpose(), i, j, da, db, dc);
                                spirit_throw(Utility::Exception_Classifier::System_not_Initialized, Utility::Log_Level::Severe, message);
                            }
                        }
                    }
                }
            }
        }

        // Generate positions
        for (int dc = 0; dc < n_cells[2]; ++dc)
        {
            for (int db = 0; db < n_cells[1]; ++db)
            {
                for (int da = 0; da < n_cells[0]; ++da)
                {
                    for (int iatom = 0; iatom < n_cell_atoms; ++iatom)
                    {
                        int ispin = iatom
                            + dc * n_cell_atoms * n_cells[1] * n_cells[0]
                            + db * n_cell_atoms * n_cells[0]
                            + da * n_cell_atoms;

                        positions[ispin] = lattice_constant * (
                                  (da + cell_atoms[iatom][0]) * bravais_vectors[0]
                                + (db + cell_atoms[iatom][1]) * bravais_vectors[1]
                                + (dc + cell_atoms[iatom][2]) * bravais_vectors[2] );
                    }
                }
            }
        }
    }

    std::vector<tetrahedron_t> compute_delaunay_triangulation_3D(const std::vector<vector3_t> & points)
    try
    {
        const int ndim = 3;
        std::vector<tetrahedron_t> tetrahedra;
        tetrahedron_t tmp_tetrahedron;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        orgQhull::QhullFacetList facet_list = qhull.facetList();
        for(const auto & facet : facet_list)
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_tetrahedron[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                tetrahedra.push_back(tmp_tetrahedron);
            }
        }
        return tetrahedra;
    }
    catch( ... )
    {
        spirit_handle_exception_core( "Could not compute 3D Delaunay triangulation of the Geometry. Probably Qhull threw an exception." );
        return std::vector<tetrahedron_t>(0);
    }

    std::vector<triangle_t> compute_delaunay_triangulation_2D(const std::vector<vector2_t> & points)
    try
    {
        const int ndim = 2;
        std::vector<triangle_t> triangles;
        triangle_t tmp_triangle;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        for(const auto & facet : qhull.facetList())
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_triangle[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                triangles.push_back(tmp_triangle);
            }
        }
        return triangles;
    }
    catch( ... )
    {
        spirit_handle_exception_core( "Could not compute 2D Delaunay triangulation of the Geometry. Probably Qhull threw an exception." );
        return std::vector<triangle_t>(0);
    }

    const std::vector<triangle_t>& Geometry::triangulation(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if ( (n_cells[0]/n_cell_step < 2 && n_cells[0] > 1) ||
             (n_cells[1]/n_cell_step < 2 && n_cells[1] > 1) ||
             (n_cells[2]/n_cell_step < 2 && n_cells[2] > 1) )
        {
            _triangulation.clear();
            return _triangulation;
        }

        // 2D: triangulation
        if (this->dimensionality == 2)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                _triangulation.clear();

                std::vector<vector2_t> points;
                points.resize(positions.size());

                int icell = 0, idx;
                for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                {
                    for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                    {
                        for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                        {
                            for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                            {
                                idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                points[icell].x = double(positions[idx][0]);
                                points[icell].y = double(positions[idx][1]);
                                ++icell;
                            }
                        }
                    }
                }
                _triangulation = compute_delaunay_triangulation_2D(points);
            }
        }// endif 2D
        // 0D, 1D and 3D give no triangulation
        else
        {
            _triangulation.clear();
        }
        return _triangulation;
    }

    const std::vector<tetrahedron_t>& Geometry::tetrahedra(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if (n_cells[0]/n_cell_step < 2 || n_cells[1]/n_cell_step < 2 || n_cells[2]/n_cell_step < 2)
        {
            _tetrahedra.clear();
            return _tetrahedra;
        }

        // 3D: Tetrahedra
        if (this->dimensionality == 3)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                // If we have only one spin in the basis our lattice is a simple regular geometry
                bool is_simple_regular_geometry = n_cell_atoms == 1;

                // If we have a simple regular geometry everything can be calculated by hand
                if (is_simple_regular_geometry)
                {
                    _tetrahedra.clear();
                    int cell_indices[] = {
                        0, 1, 5, 3,
                        1, 3, 2, 5,
                        3, 2, 5, 6,
                        7, 6, 5, 3,
                        4, 7, 5, 3,
                        0, 4, 3, 5
                        };
                    int x_offset = 1;
                    int y_offset = n_cells[0]/n_cell_step;
                    int z_offset = n_cells[0]/n_cell_step*n_cells[1]/n_cell_step;
                    int offsets[] = {
                        0, x_offset, x_offset+y_offset, y_offset,
                        z_offset, x_offset+z_offset, x_offset+y_offset+z_offset, y_offset+z_offset
                        };

                    for (int ix = 0; ix < (n_cells[0]-1)/n_cell_step; ix++)
                    {
                        for (int iy = 0; iy < (n_cells[1]-1)/n_cell_step; iy++)
                        {
                            for (int iz = 0; iz < (n_cells[2]-1)/n_cell_step; iz++)
                            {
                                int base_index = ix*x_offset+iy*y_offset+iz*z_offset;
                                for (int j = 0; j < 6; j++)
                                {
                                    tetrahedron_t tetrahedron;
                                    for (int k = 0; k < 4; k++)
                                    {
                                        int index = base_index + offsets[cell_indices[j*4+k]];
                                        tetrahedron[k] = index;
                                    }
                                    _tetrahedra.push_back(tetrahedron);
                                }
                            }
                        }
                    }
                }
                // For general basis cells we calculate the Delaunay tetrahedra
                else
                {
                    std::vector<vector3_t> points;
                    points.resize(positions.size());

                    int icell = 0, idx;
                    for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                    {
                        for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                        {
                            for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                            {
                                for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                                {
                                    idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                    points[icell].x = double(positions[idx][0]);
                                    points[icell].y = double(positions[idx][1]);
                                    points[icell].z = double(positions[idx][2]);
                                    ++icell;
                                }
                            }
                        }
                    }
                    _tetrahedra = compute_delaunay_triangulation_3D(points);
                }
            }
        } // endif 3D
        // 0-2 D gives no tetrahedra
        else
        {
            _tetrahedra.clear();
        }
        return _tetrahedra;
    }

    std::vector<Vector3> Geometry::BravaisVectorsSC()
    {
        return { { scalar(1), scalar(0), scalar(0) },
                 { scalar(0), scalar(1), scalar(0) },
                 { scalar(0), scalar(0), scalar(1) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsFCC()
    {
        return { { scalar(0.5), scalar(0.0), scalar(0.5) },
                 { scalar(0.5), scalar(0.5), scalar(0.0) },
                 { scalar(0.0), scalar(0.5), scalar(0.5) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsBCC()
    {
        return { { scalar( 0.5), scalar( 0.5), scalar(-0.5) },
                 { scalar(-0.5), scalar( 0.5), scalar(-0.5) },
                 { scalar( 0.5), scalar(-0.5), scalar(-0.5) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D60()
    {
        return { { scalar(0.5*std::sqrt(3)), scalar(-0.5), scalar(0) },
                 { scalar(0.5*std::sqrt(3)), scalar( 0.5), scalar(0) },
                 { scalar(0),                scalar( 0),   scalar(1) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D120()
    {
        return { { scalar(0.5), scalar(-0.5*std::sqrt(3)), scalar(0) },
                 { scalar(0.5), scalar( 0.5*std::sqrt(3)), scalar(0) },
                 { scalar(0),   scalar( 0),                scalar(1) } };
    }

    void Geometry::applyCellComposition()
    {
        int N  = this->n_cell_atoms;
        int Na = this->n_cells[0];
        int Nb = this->n_cells[1];
        int Nc = this->n_cells[2];
        int ispin, iatom, atom_type;
        scalar concentration, rvalue;
        std::vector<bool> visited(N);

        std::mt19937 prng;
        std::uniform_real_distribution<scalar> distribution;
        if( this->cell_composition.disordered )
        {
            // TODO: the seed should be a parameter and the instance a member of this class
            prng = std::mt19937(2006);
            distribution = std::uniform_real_distribution<scalar>(0, 1);
            // In the disordered case, unvisited atoms will be vacancies
            this->atom_types = intfield(nos, -1);
        }

        for (int na = 0; na < Na; ++na)
        {
            for (int nb = 0; nb < Nb; ++nb)
            {
                for (int nc = 0; nc < Nc; ++nc)
                {
                    std::fill(visited.begin(), visited.end(), false);

                    for (int icomposition = 0; icomposition < this->cell_composition.iatom.size(); ++icomposition)
                    {
                        iatom = this->cell_composition.iatom[icomposition];

                        if( !visited[iatom] )
                        {
                            ispin = N*na + N*Na*nb + N*Na*Nb*nc + iatom;

                            // In the disordered case, we only visit an atom if the dice will it
                            if( this->cell_composition.disordered )
                            {
                                concentration = this->cell_composition.concentration[icomposition];
                                rvalue = distribution(prng);
                                if( rvalue <= concentration )
                                {
                                    this->atom_types[ispin] = this->cell_composition.atom_type[icomposition];
                                    this->mu_s[ispin]       = this->cell_composition.mu_s[icomposition];
                                    visited[iatom] = true;
                                    if( this->atom_types[ispin] < 0 )
                                        --this->nos_nonvacant;
                                }
                            }
                            // In the ordered case, we visit every atom
                            else
                            {
                                this->atom_types[ispin] = this->cell_composition.atom_type[icomposition];
                                this->mu_s[ispin]       = this->cell_composition.mu_s[icomposition];
                                visited[iatom] = true;
                                if( this->atom_types[ispin] < 0 )
                                    --this->nos_nonvacant;
                            }

                            // Pinning of boundary layers
                            if( (na < pinning.na_left || na >= Na - pinning.na_right) ||
                                (nb < pinning.nb_left || nb >= Nb - pinning.nb_right) ||
                                (nc < pinning.nc_left || nc >= Nc - pinning.nc_right) )
                            {
                                // Pinned cells
                                this->mask_unpinned[ispin] = 0;
                                this->mask_pinned_cells[ispin] = pinning.pinned_cell[iatom];
                            }
                        }
                    }
                }
            }
        }
    }

    void Geometry::calculateDimensionality()
    {
        int dims_basis = 0, dims_translations = 0;
        Vector3 test_vec_basis, test_vec_translations;

        const scalar epsilon = 1e-6;

        // ----- Find dimensionality of the basis -----
        if     ( n_cell_atoms == 1 )
            dims_basis = 0;
        else if( n_cell_atoms == 2 )
        {
            dims_basis = 1;
            test_vec_basis = positions[0] - positions[1];
        }
        else
        {
            // Get basis atoms relative to the first atom
            Vector3 v0 = positions[0];
            std::vector<Vector3> b_vectors(n_cell_atoms-1);
            for( int i = 1; i < n_cell_atoms; ++i )
                b_vectors[i-1] = (positions[i] - v0).normalized();

            // Calculate basis dimensionality
            // test vec is along line
            test_vec_basis = b_vectors[0];
            //      is it 1D?
            int n_parallel = 0;
            for( unsigned int i = 1; i < b_vectors.size(); ++i )
            {
                if( std::abs(b_vectors[i].dot(test_vec_basis)) - 1 < epsilon )
                    ++n_parallel;
                // Else n_parallel will give us the last parallel vector
                // Also the if-statement for dims_basis=1 wont be met
                else
                    break;
            }
            if( n_parallel == b_vectors.size() - 1 )
            {
                dims_basis = 1;
            }
            else
            {
                // test vec is normal to plane
                test_vec_basis = b_vectors[0].cross(b_vectors[n_parallel+1]);
                //      is it 2D?
                int n_in_plane = 0;
                for( unsigned int i = 2; i < b_vectors.size(); ++i )
                {
                    if (std::abs(b_vectors[i].dot(test_vec_basis)) < epsilon)
                        ++n_in_plane;
                }
                if( n_in_plane == b_vectors.size() - 2 )
                    dims_basis = 2;
                else
                {
                    this->dimensionality = 3;
                    return;
                }
            }
        }

        // ----- Find dimensionality of the translations -----
        //      The following are zero if the corresponding pair is parallel or antiparallel
        double t01, t02, t12;
        t01 = std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[1].normalized())) - 1.0;
        t02 = std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[2].normalized())) - 1.0;
        t12 = std::abs(bravais_vectors[1].normalized().dot(bravais_vectors[2].normalized())) - 1.0;
        //      Check if pairs are linearly independent
        int n_independent_pairs = 0;
        if( t01 < epsilon && n_cells[0] > 1 && n_cells[1] > 1 ) ++n_independent_pairs;
        if( t02 < epsilon && n_cells[0] > 1 && n_cells[2] > 1 ) ++n_independent_pairs;
        if( t12 < epsilon && n_cells[1] > 1 && n_cells[2] > 1 ) ++n_independent_pairs;
        //      Calculate translations dimensionality
        if( n_cells[0] == 1 && n_cells[1] == 1 && n_cells[2] == 1 )
        {
            dims_translations = 0;
        }
        else if( n_independent_pairs == 0 )
        {
            dims_translations = 1;
            // Test if vec is along the line
            for (int i=0; i<3; ++i) if (n_cells[i] > 1) test_vec_translations = bravais_vectors[i];
        }
        else if( n_independent_pairs < 3 )
        {
            dims_translations = 2;
            // Test if vec is normal to plane
            int n = 0;
            std::vector<Vector3> plane(2);
            for( int i = 0; i < 3; ++i )
            {
                if( n_cells[i] > 1 )
                {
                    plane[n] = bravais_vectors[i];
                    ++n;
                }
            }
            test_vec_translations = plane[0].cross(plane[1]);
        }
        else
        {
            this->dimensionality = 3;
            return;
        }

        // ----- Calculate dimensionality of system -----
        test_vec_basis.normalize();
        test_vec_translations.normalize();
        //      If one dimensionality is zero, only the other counts
        if( dims_basis == 0 )
        {
            this->dimensionality = dims_translations;
            return;
        }
        else if( dims_translations == 0 )
        {
            this->dimensionality = dims_basis;
            return;
        }
        //      If both are linear or both are planar, the test vectors should be (anti)parallel if the geometry is 1D or 2D
        else if (dims_basis == dims_translations)
        {
            if( std::abs(test_vec_basis.dot(test_vec_translations)) - 1 < epsilon )
            {
                this->dimensionality = dims_basis;
                return;
            }
            else if( dims_basis == 1 )
            {
                this->dimensionality = 2;
                return;
            }
            else if( dims_basis == 2 )
            {
                this->dimensionality = 3;
                return;
            }
        }
        //      If one is linear (1D), and the other planar (2D) then the test vectors should be orthogonal if the geometry is 2D
        else if( (dims_basis == 1 && dims_translations == 2) || (dims_basis == 2 && dims_translations == 1) )
        {
            if( std::abs(test_vec_basis.dot(test_vec_translations)) < epsilon )
            {
                this->dimensionality = 2;
                return;
            }
            else
            {
                this->dimensionality = 3;
                return;
            }
        }
    }

    void Geometry::calculateBounds()
    {
        this->bounds_max.setZero();
        this->bounds_min.setZero();
        for (int iatom = 0; iatom < nos; ++iatom)
        {
            for (int dim = 0; dim < 3; ++dim)
            {
                if (this->positions[iatom][dim] < this->bounds_min[dim]) this->bounds_min[dim] = this->positions[iatom][dim];
                if (this->positions[iatom][dim] > this->bounds_max[dim]) this->bounds_max[dim] = this->positions[iatom][dim];
            }
        }
    }

    void Geometry::calculateUnitCellBounds()
    {
        this->cell_bounds_max.setZero();
        this->cell_bounds_min.setZero();
        for (unsigned int ivec = 0; ivec < this->bravais_vectors.size(); ++ivec)
        {
            for (int iatom = 0; iatom < this->n_cell_atoms; ++iatom)
            {
                auto neighbour1 = this->positions[iatom] + this->lattice_constant * this->bravais_vectors[ivec];
                auto neighbour2 = this->positions[iatom] - this->lattice_constant * this->bravais_vectors[ivec];
                for (int dim = 0; dim < 3; ++dim)
                {
                    if (neighbour1[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour1[dim];
                    if (neighbour1[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour1[dim];
                    if (neighbour2[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour2[dim];
                    if (neighbour2[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour2[dim];
                }
            }
        }
        this->cell_bounds_min *= 0.5;
        this->cell_bounds_max *= 0.5;
    }

    void Geometry::calculateGeometryType()
    {
        const scalar epsilon = 1e-6;

        // Automatically try to determine GeometryType
        // Single-atom unit cell
        if (cell_atoms.size() == 1)
        {
            // If the basis vectors are orthogonal, it is a rectilinear lattice
            if (std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[1].normalized())) < epsilon &&
                std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[2].normalized())) < epsilon)
            {
                // If equidistant it is simple cubic
                if (bravais_vectors[0].norm() == bravais_vectors[1].norm() == bravais_vectors[2].norm())
                    this->classifier = BravaisLatticeType::SC;
                // Otherwise only rectilinear
                else
                    this->classifier = BravaisLatticeType::Rectilinear;
            }
        }
        // Regular unit cell with multiple atoms (e.g. bcc, fcc, hex)
        //else if (n_cell_atoms == 2)
        // Irregular unit cells arranged on a lattice (e.g. B20 or custom)
        /*else if (n_cells[0] > 1 || n_cells[1] > 1 || n_cells[2] > 1)
        {
            this->classifier = BravaisLatticeType::Lattice;
        }*/
        // A single irregular unit cell
        else
        {
            this->classifier = BravaisLatticeType::Irregular;
        }
    }

    void Geometry::Apply_Pinning(vectorfield & vf)
    {
        #if defined(SPIRIT_ENABLE_PINNING)
        int N  = this->n_cell_atoms;
        int Na = this->n_cells[0];
        int Nb = this->n_cells[1];
        int Nc = this->n_cells[2];
        int ispin;

        for (int iatom = 0; iatom < N; ++iatom)
        {
            for (int na = 0; na < Na; ++na)
            {
                for (int nb = 0; nb < Nb; ++nb)
                {
                    for (int nc = 0; nc < Nc; ++nc)
                    {
                        ispin = N*na + N*Na*nb + N*Na*Nb*nc + iatom;
                        if (!this->mask_unpinned[ispin])
                            vf[ispin] = this->mask_pinned_cells[ispin];
                    }
                }
            }
        }
        #endif
    }
}


#include "CRagdollCollision.h"
//#include "Water.h"
//#include "physical/physics/water/Water.h"

#include "core-ext/animation/AnimationControl.h"
#include "core-ext/animation/Skeleton.h"
#include "core-ext/types/sHitbox.h"
#include "physical/skeleton/skeletonBone.h"

#include "renderer/logic/model/RrCModel.h"

//#include "renderer/logic/model/CSkinnedModel.h"

//#include "physical/physics/motion/physRigidbody.h"
//#include "physical/physics/shapes/physBoxShape.h"

#include <list>
using std::list;

CRagdollCollision::CRagdollCollision ( const prRigidbodyCreateParams& params, RrCModel* sourceModel )
	: CMotion(params.owner, params.ownerType)
{
	hasJoints = false;
	m_model = sourceModel;

	// Set the layer
	layer = physical::layer::Hitboxes;

	hitboxEntry hb;
	std::vector<sHitbox>* mdl_hitboxes = NULL;// &m_model->m_meshGroup->m_hitboxes; // Mesh system currently doesn't load hitboxes, making this class useless.
	animation::Skeleton* skeleton = NULL; //m_model->pMyAnimation->GetSkeleton();

	// Create the model/world space of the reference pose
	std::vector<core::TransformLite> pose_model;
	pose_model.resize( skeleton->parent.size() );
	core::TransformUtility::LocalToWorld( &skeleton->parent[0], &skeleton->reference_xpose[0], &pose_model[0], (int)pose_model.size() );

#if 0
	// Add root as first hitbox/bone
	{
		//glBone* targetBone = m_skinnedmodel->GetSkeletonList()->at(0);

		physShape* tCollider;
		//tCollider = Physics::CreateBoxShape( Vector3f(0.17f,0.17f,0.17f) );
		tCollider = new physBoxShape( Vector3f(0.17f,0.17f,0.17f) );

		physRigidBodyInfo tInfo;
		tInfo.m_mass = 3.0f;
		tInfo.m_shape = tCollider;
		tInfo.m_centerOfMass = physVector4( 0,0,0 );
		tInfo.m_friction = 0.5f;
		tInfo.m_motionType = physMotion::MOTION_DYNAMIC;
		//Vector3f targetPosition = targetBone->xRagdollPoseModel.position;//targetBone->transform.position;
		Vector3f targetPosition = pose_model[0].world.position;
		tInfo.m_position = physVector4( targetPosition.x, targetPosition.y, targetPosition.z );
		//Quaternion targetRotation = targetBone->xRagdollPoseModel.rotation;//targetBone->transform.rotation.getQuaternion();
		Quaternion targetRotation = pose_model[0].world.rotation;
		tInfo.m_rotation = physQuaternion( targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w );
		tInfo.m_linearDamping = 0.1f;
		tInfo.m_angularDamping = 0.2f;

		tInfo.m_collisionFilterInfo = CPhysics::GetCollisionFilter( Layers::PhysicsTypes::PHYS_HITBOX );

		hitboxEntry thb;
		thb.rigidbody = new physRigidBody( &tInfo );
		thb.rigidbody->setUserData( GetId() ); // Give the rigidbody this ragoll's ID.

		thb.impulse = Vector3f(0,0,0);
		thb.boneIndex = 0;//targetBone->index;
		thb.hitboxIndex = 0;
		thb.name = "Root";
		thb.constraint = NULL;

		m_hitboxList.push_back( thb );
		delete_safe(tCollider);
	}
	// Loop through model hitboxes
	for ( uint i = 0; i < mdl_hitboxes->size(); ++i )
	{
		//glBone* targetBone = m_skinnedmodel->GetSkeletonList()->at(mdl_hitboxes->at(i).indexLink);
		int boneIndex = mdl_hitboxes->at(i).indexLink;

		physShape* tCollider;
		//tCollider = Physics::CreateBoxShape( mdl_hitboxes->at(i).extents*1.3f, mdl_hitboxes->at(i).center );
		tCollider = new physBoxShape( mdl_hitboxes->at(i).extents*1.3f, mdl_hitboxes->at(i).center );

		physRigidBodyInfo tInfo;
		tInfo.m_mass = 3.0f;
		tInfo.m_shape = tCollider;
		tInfo.m_centerOfMass = physVector4( 0,0,0 );
		tInfo.m_friction = 0.5f;
		tInfo.m_motionType = physMotion::MOTION_DYNAMIC;
		//Vector3f targetPosition = targetBone->xRagdollPoseModel.position;//targetBone->transform.position;
		Vector3f targetPosition = pose_model[boneIndex].world.position;
		tInfo.m_position = physVector4( targetPosition.x, targetPosition.y, targetPosition.z );
		//Quaternion targetRotation = targetBone->xRagdollPoseModel.rotation;// targetBone->transform.rotation.getQuaternion();
		Quaternion targetRotation = pose_model[boneIndex].world.rotation;
		tInfo.m_rotation = physQuaternion( targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w );
		tInfo.m_linearDamping = 0.1f;
		tInfo.m_angularDamping = 0.3f;

		tInfo.m_collisionFilterInfo = CPhysics::GetCollisionFilter( Layers::PhysicsTypes::PHYS_HITBOX );

		hitboxEntry thb;
		thb.rigidbody = new physRigidBody( &tInfo );
		thb.rigidbody->setUserData( GetId() ); // Give the rigidbody this ragoll's ID.

		thb.impulse = Vector3f(0,0,0);
		thb.boneIndex = boneIndex;
		thb.hitboxIndex = m_hitboxList.size();
		thb.name = mdl_hitboxes->at(i).name;
		thb.constraint = NULL;

		m_hitboxList.push_back( thb );
		delete_safe(tCollider);
	}
#endif

	CreateJoints(pose_model);
	CreateMapping(pose_model);

	// Now, change inertias so ragdolls stop exploding
	/*{
		std::vector<hkpConstraintInstance*> constraints;
		for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb )
		{
			if ( hb->constraint_instance ) {
				constraints.push_back( hb->constraint_instance );
			}
		}
		hkpInertiaTensorComputer::optimizeInertiasOfConstraintTree( &(constraints[0]), constraints.size(), m_hitboxList[0].rigidbody, 4 );
	}*/
}

void CRagdollCollision::CreateJoints ( std::vector<core::TransformLite>& pose_model )
{
	std::vector<sHitbox>* mdl_hitboxes = NULL;//m_skinnedmodel->GetHitboxes();
	animation::Skeleton* skeleton = NULL;//m_skinnedmodel->GetSkeleton();

	// Loop through model skeleton, child-first order, to create joints
	//Transform* tr_root = m_skinnedmodel->GetSkeletonRoot();
	//std::list<Transform*> tr_bonelist;
	//std::list<Transform*> tr_heirarchylist;
	//tr_bonelist.push_back( tr_root );
	//// First add all the children to the list in child-first order
	//while ( !tr_bonelist.empty() ) {
	//	Transform* tr_current = tr_bonelist.front();
	//	tr_bonelist.pop_front();
	//	tr_heirarchylist.push_front( tr_current ); // Add in child-first order
	//	for ( uint i = 0; i < tr_current->children.size(); ++i )
	//	{
	//		tr_bonelist.push_back( tr_current->children[i] );
	//	}
	//}
	//// Loop through the children and generate joints as needed
	///*while ( !tr_heirarchylist.empty() ) {
	//	Transform* tr_current = tr_heirarchylist.front();
	//	tr_heirarchylist.pop_front();
	//	// Skip root
	//	if ( tr_current == m_skinnedmodel->GetSkeletonRoot() ) {
	//		continue;
	//	}
	//	glBone* targetBone = (glBone*)tr_current->pOwnerRenderer;
	//	// Search the hitboxes for this bone
	//	uint hitboxIndex = uint(-1);
	//	for ( uint i = 0; i < m_hitboxList.size(); ++i )
	//	{
	//		if ( m_hitboxList[i].boneIndex == targetBone->index ) {
	//			hitboxIndex = i; // Save hitbox index
	//			// Break out of loop
	//			i = m_hitboxList.size() + 1;
	//		}
	//	}
	//	// Skip if no hitbox
	//	if ( hitboxIndex == uint(-1) ) {
	//		continue;
	//	}*/
#if 0
	for ( uint i = 0; i < m_hitboxList.size(); ++i )
	{
		//Transform* tr_current = &(m_skinnedmodel->GetSkeletonList()->at(mdl_hitboxes->at(i).indexLink)->transform);
		//Transform* tr_current = &(m_skinnedmodel->GetSkeletonList()->at(m_hitboxList[i].boneIndex)->transform);
		uint hitboxIndex = i;
		//glBone* targetBone = (glBone*)tr_current->owner;
		int boneIndex = m_hitboxList[i].boneIndex;

		// Has a hitbox, so find the next parent with a hitbox (search up the list)
		int parentHitboxIndex = -1;
		// Search up the parents
		//CTransform* tr_next = tr_current->GetParent();
		int tr_next = skeleton->parent[boneIndex];
		//glBone* parentBone;
		int parentBone;
		//while ( tr_next != &CTransform::root )
		while ( tr_next >= 0 )
		{
			//parentBone = (glBone*)tr_next->owner;
			parentBone = tr_next;
			for ( uint i = 0; i < m_hitboxList.size(); ++i )
			{
				if ( m_hitboxList[i].boneIndex == tr_next )
				{
					parentHitboxIndex = i; // Save hitbox index
					// Break out of loop
					i = m_hitboxList.size() + 1;
				}
			}
			if ( parentHitboxIndex != -1 )
			{
				break;
			}
			//tr_next = tr_next->GetParent();
			 tr_next = skeleton->parent[tr_next];
		}
		// Skip if no parent hitbox
		if ( parentHitboxIndex == -1 )
		{
			m_hitboxList[i].constraint = NULL;
			m_hitboxList[i].constraint_instance = NULL;
			continue;
		}
		//cout << "PARENT HITBOX: " << hitboxIndex << " to " << parentHitboxIndex << endl;
		// Have a parent, so make a joint between the two rigidbodies at the position of the current bone
		/*{
			hkpConstraintData* t_cst_data;
			hkpConstraintInstance* t_cst_inst;
			if ( (targetBone->name.find( "Calf" ) != string::npos) || (targetBone->name.find( "Forearm" ) != string::npos) )
			{
				//targetBone->transform.Up();
				Vector3f axisUp = targetBone->xRagdollPoseModel.rotation * Vector3f::up;

				hkpLimitedHingeConstraintData* hc;
				hc = new hkpLimitedHingeConstraintData();
				hc->setInWorldSpace(
					m_hitboxList[hitboxIndex].rigidbody->getTransform(),
					m_hitboxList[parentHitboxIndex].rigidbody->getTransform(),
					m_hitboxList[hitboxIndex].rigidbody->getPosition(),
					hkVector4( axisUp.x, axisUp.y, axisUp.z ) );
				hc->setMinAngularLimit( -HK_REAL_PI * 0.9f );
				hc->setMaxAngularLimit( HK_REAL_PI * 0.1f );

				t_cst_data = hc;
			}
			else if ( (m_hitboxList[i].name.compare( "Hips" )) )
			{
				hkpBallAndSocketConstraintData* bs;
				bs = new hkpBallAndSocketConstraintData();
				bs->setInWorldSpace(
					m_hitboxList[hitboxIndex].rigidbody->getTransform(),
					m_hitboxList[parentHitboxIndex].rigidbody->getTransform(),
					m_hitboxList[hitboxIndex].rigidbody->getPosition() );

				t_cst_data = bs;
			}
			else
			{
				Vector3f axisUp = targetBone->xRagdollPoseModel.rotation * Vector3f::up;
				Vector3f axisSide = targetBone->xRagdollPoseModel.rotation * Vector3f::left;
				Vector3f axisForward = targetBone->xRagdollPoseModel.rotation * Vector3f::forward;

				hkpRagdollConstraintData* rdc;

				rdc = new hkpRagdollConstraintData();

				hkVector4 twistAxisA( axisForward.x, axisForward.y, axisForward.z );
				hkVector4 planeAxisA( axisUp.x, axisUp.y, axisUp.z );

				rdc->setInWorldSpace(
					m_hitboxList[hitboxIndex].rigidbody->getTransform(),
					m_hitboxList[parentHitboxIndex].rigidbody->getTransform(),
					m_hitboxList[hitboxIndex].rigidbody->getPosition(),
					twistAxisA, planeAxisA );

				rdc->setTwistMinAngularLimit( -HK_REAL_PI * 0.45f );
				rdc->setTwistMaxAngularLimit( +HK_REAL_PI * 0.45f );
				rdc->setPlaneMinAngularLimit( -HK_REAL_PI * 0.45f );
				rdc->setPlaneMaxAngularLimit( +HK_REAL_PI * 0.45f );

				rdc->setAsymmetricConeAngle( -HK_REAL_PI * 0.45f, +HK_REAL_PI * 0.45f );

				t_cst_data = rdc;
			}

			// Wrap constraint in malleable constraint data
			m_hitboxList[i].constraint = new hkpMalleableConstraintData ( t_cst_data );
			m_hitboxList[i].constraint->m_strength = 1;

			t_cst_inst = new hkpConstraintInstance(
				m_hitboxList[hitboxIndex].rigidbody,
				m_hitboxList[parentHitboxIndex].rigidbody,
				m_hitboxList[i].constraint);

			CPhysics::AddConstraint( t_cst_inst );

			m_hitboxList[i].constraint_instance = t_cst_inst;

			t_cst_data->removeReference();
			t_cst_inst->removeReference();
		}*/
	}
#endif
}

//#include <boost/algorithm/string.hpp>
#include "core/utils/string.h"
int boneNameCompare ( const char * bone1, const char * bone2 )
{
	string sbone1 (bone1);
	string sbone2 (bone2);
	sbone1 = core::utils::string::GetLower(sbone1);
	sbone2 = core::utils::string::GetLower(sbone2);
	if ( sbone1.find(sbone2) != string::npos ) {
		return 0;
	}
	else if ( sbone2.find(sbone1) != string::npos ) {
		return 0;
	}
	else {
		return sbone1.compare(sbone2);
	}
}

void CRagdollCollision::CreateMapping ( std::vector<core::TransformLite>& pose_model )
{
	// First, create the skeletons
	/*skeletonModel = new hkaSkeleton();
	skeletonRagdoll = new hkaSkeleton();

	// Loop through model bones and create the skeleton
	skeletonModel->m_referencePose.setSize( m_skinnedmodel->GetSkeletonList()->size() );
	skeletonModel->m_parentIndices.setSize( m_skinnedmodel->GetSkeletonList()->size() );
	skeletonModel->m_bones.setSize( m_skinnedmodel->GetSkeletonList()->size() );
	XTransform* temp;
	for ( int i = 0; i < skeletonModel->m_bones.getSize(); ++i )
	{
		glBone* currentBone = m_skinnedmodel->GetSkeletonList()->at(i);
		temp = &(currentBone->xBindPose);
		skeletonModel->m_referencePose[i] = hkQsTransform(
			hkVector4( temp->position.x, temp->position.y, temp->position.z ),
			hkQuaternion( temp->rotation.x, temp->rotation.y, temp->rotation.z, temp->rotation.w ),
			hkVector4( temp->scale.x, temp->scale.y, temp->scale.z ) );
		skeletonModel->m_bones[i].m_name = currentBone->name.c_str();
		skeletonModel->m_bones[i].m_lockTranslation = false;

		// Search for the parent index
		skeletonModel->m_parentIndices[i] = -1;
		for ( int j = 0; j < skeletonModel->m_bones.getSize(); ++j ) {
			glBone* parentBone = m_skinnedmodel->GetSkeletonList()->at(j);
			if ( currentBone->transform.GetParent() == (&(parentBone->transform)) ) {
				skeletonModel->m_parentIndices[i] = j;
				j = skeletonModel->m_bones.getSize();
			}
		}
	}

	// Loop through hitboxes and create another skeleton
	skeletonRagdoll->m_referencePose.setSize( m_hitboxList.size() );
	skeletonRagdoll->m_parentIndices.setSize( m_hitboxList.size() );
	skeletonRagdoll->m_bones.setSize( m_hitboxList.size() );
	for ( int i = 0; i < m_hitboxList.size(); ++i )
	{
		glBone* currentBone = m_skinnedmodel->GetSkeletonList()->at(m_hitboxList[i].boneIndex);
		temp = &(currentBone->xBindPose);
		skeletonRagdoll->m_referencePose[i] = hkQsTransform(
			hkVector4( temp->position.x, temp->position.y, temp->position.z ),
			hkQuaternion( temp->rotation.x, temp->rotation.y, temp->rotation.z, temp->rotation.w ),
			hkVector4( temp->scale.x, temp->scale.y, temp->scale.z ) );
		skeletonRagdoll->m_bones[i].m_name = currentBone->name.c_str();
		skeletonRagdoll->m_bones[i].m_lockTranslation = false;

		// Search up the parents
		CTransform* tr_next = currentBone->transform.GetParent();
		skeletonRagdoll->m_parentIndices[i] = -1;
		while ( tr_next != &CTransform::root ) {
			glBone* parentBone = (glBone*)tr_next->pOwnerRenderer;
			for ( uint j = 0; j < m_hitboxList.size(); ++j )
			{
				if ( m_hitboxList[j].boneIndex == parentBone->index ) {
					skeletonRagdoll->m_parentIndices[i] = j; // Save hitbox index
		cout << "PARENT HITBOX: " << i << " to " << j << endl;
					// Break out of loop
					j = m_hitboxList.size() + 1;
				}
			}
			if ( skeletonRagdoll->m_parentIndices[i] != -1 ) {
				break;
			}
			tr_next = tr_next->GetParent();
		}
	}

	///
	hkaSkeletonMapperUtils::Params param;
	param.m_skeletonA = skeletonModel;
	param.m_skeletonB = skeletonRagdoll;
	param.m_autodetectChains = false;
	param.m_compareNames = boneNameCompare;
	hkaSkeletonMapperUtils::UserChain userchain;
	userchain.m_start = "Spine";
	userchain.m_end = "Spine3";
	param.m_userChains.pushBack( userchain );

	hkaSkeletonMapperData* mdRagdollToAnim = new hkaSkeletonMapperData();
	hkaSkeletonMapperData* mdAnimToRagdoll = new hkaSkeletonMapperData();
	hkaSkeletonMapperUtils::createMapping( param, *mdAnimToRagdoll, *mdRagdollToAnim );

	mMapperRagdollToAnim = new hkaSkeletonMapper( *mdRagdollToAnim );
	mMapperAnimToRagdoll = new hkaSkeletonMapper( *mdAnimToRagdoll );
	*/
}

CRagdollCollision::~CRagdollCollision ( void )
{
	//for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb ) {
	//	//Physics::FreeRigidBody( hb->rigidbody );
	//	delete_safe( hb->rigidbody );
	//	if ( hb->constraint ) {
	//		Physics::RemoveReference(hb->constraint);
	//		hb->constraint = NULL;
	//	}
	//}
	//m_hitboxList.clear();
}

void CRagdollCollision::Update ( void )
{
	//bool inWater = WaterTester::Get()->PositionInside( m_hitboxList[0].start.position );
	//auto skeleton = m_skinnedmodel->GetSkeleton();
	//for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb )
	//{
	//	//glBone* targetBone = m_skinnedmodel->GetSkeletonList()->at(hb->boneIndex);
	//	if ( hb->constraint )
	//	{
	//		//glBone* parentBone = (glBone*)(targetBone->transform.owner);
	//		//hb->constraint->m_strength = std::max<Real>(targetBone->ragdollStrength,parentBone->ragdollStrength); //* 0.7f;
	//		if ( skeleton->parent[hb->boneIndex] >= 0 )
	//		{
	//			hb->constraint->m_strength = std::max<Real>(
	//				skeleton->ext_physics_strength[hb->boneIndex],
	//				skeleton->ext_physics_strength[skeleton->parent[hb->boneIndex]]);
	//		}
	//	}
	//	if ( hb->rigidbody )
	//	{
	//		if ( !inWater ) {
	//			//hb->rigidbody->setGravityFactor( targetBone->ragdollStrength );
	//			hb->rigidbody->setGravityFactor( skeleton->ext_physics_strength[hb->boneIndex] );
	//			hb->rigidbody->setLinearDamping( 0.1f );
	//		}
	//		else {
	//			hb->rigidbody->setGravityFactor( 0.05f );
	//			hb->rigidbody->setLinearDamping( 0.6f );
	//		}
	//	}
	//}
}

void CRagdollCollision::LateUpdate ( void )
{
	//// Set the current hitbox rigidbody position, so can create proper velocities later
	//for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb )
	//{
	//	hb->start.position = hb->rigidbody->getPosition();
	//	hb->start.rotation = hb->rigidbody->getRotation();
	//}
}

void CRagdollCollision::PostUpdate ( void )
{

}

void CRagdollCollision::PostFixedUpdate ( void )
{

}

void CRagdollCollision::RigidbodyUpdate ( Real interpolation )
{
	animation::Skeleton* skeleton = NULL;//m_skinnedmodel->GetSkeleton();
	// Need a Ragdoll to Animation conversion here
	for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb )
	{	// TODO: apply impulse and continue
		hb->impulse = Vector3f(0,0,0);

		// If the ragdoll strength is high with this one, take the output from the physics
		/*glBone* targetBone = m_skinnedmodel->GetSkeletonList()->at(hb->boneIndex);
		if ( targetBone->ragdollStrength > FTYPE_PRECISION )
		{
			Vector3f nextPosition = hb->rigidbody->getPosition();
			nextPosition -= m_skinnedmodel->transform.position;
			nextPosition = m_skinnedmodel->transform.rotation.inverse() * nextPosition;
			targetBone->transform.position = targetBone->transform.position.lerp(nextPosition,targetBone->ragdollStrength);

			Quaternion nextRotation = hb->rigidbody->getRotation();
			Rotator nextRotator = m_skinnedmodel->transform.rotation.inverse() * Rotator(nextRotation);
			targetBone->transform.rotation = targetBone->transform.rotation.LerpTo(nextRotator,targetBone->ragdollStrength);
		}*/
		if ( skeleton->ext_physics_strength[hb->boneIndex] > FTYPE_PRECISION)
		{
			throw core::NotYetImplementedException();
		}
	}
}

void CRagdollCollision::FixedUpdate ( void )
{
	animation::Skeleton* skeleton = NULL;//m_skinnedmodel->GetSkeleton();
	// TODO: Ensure lite transform is updated
#if 0

	// Need a Animation to Ragdoll conversion here
	for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb )
	{
		//glBone* targetBone = m_skinnedmodel->GetSkeletonList()->at(hb->boneIndex);
		core::TransformLite* bone_transform = &skeleton->current_transform[hb->boneIndex];
		Real bone_strength = skeleton->ext_physics_strength[hb->boneIndex];

		Vector3f targetPosition = m_skinnedmodel->transform.rotation * bone_transform->world.position + m_skinnedmodel->transform.position;
		Quaternion targetRotation = ( m_skinnedmodel->transform.rotation * bone_transform->world.rotation ).getQuaternion();

		Vector3f linearVelocity = hb->rigidbody->getLinearVelocity();
		Vector4f angularVelocity = hb->rigidbody->getAngularVelocity();
		linearVelocity *= bone_strength;
		angularVelocity *= bone_strength;
		Vector3f currentPosition = hb->rigidbody->getPosition();
		Vector3f deltaPosition = targetPosition - currentPosition;
		//currentPosition.setInterpolate( hkVector4(targetPosition.x,targetPosition.y,targetPosition.z), currentPosition, targetBone->ragdollStrength );
		currentPosition = targetPosition.lerp( currentPosition, bone_strength );
		Quaternion currentRotation = hb->rigidbody->getRotation();
		Quaternion deltaRotation = targetRotation;
		Quaternion invRotation = currentRotation;
//		invRotation.setInverse( currentRotation );
		invRotation.Invert();
		//deltaRotation.mul( invRotation );
		deltaRotation = deltaRotation * invRotation;
		//currentRotation.setSlerp( hkQuaternion(targetRotation.x,targetRotation.y,targetRotation.z,targetRotation.w), currentRotation, targetBone->ragdollStrength );
		currentRotation = targetRotation.Slerp( currentRotation, bone_strength );

		// now, motorize the rigidbody
		deltaPosition = targetPosition;//hkVector4( targetPosition.x, targetPosition.y, targetPosition.z );
		//deltaPosition.sub( hkVector4( hb->start.position.x,hb->start.position.y,hb->start.position.z ) );
		deltaPosition -= hb->start.position;

		//deltaPosition.mul( (1 - targetBone->ragdollStrength)*30 );
		deltaPosition *= (1.0F - bone_strength)*30.0F;
		linearVelocity += deltaPosition;
		//deltaRotation.setSlerp( deltaRotation, hkQuaternion::getIdentity(), targetBone->ragdollStrength );
		deltaRotation = deltaRotation.Slerp( Quaternion(), bone_strength );
		angularVelocity += Vector4f( &deltaRotation.x );

		// set velocities and position
		hb->rigidbody->setLinearVelocity( linearVelocity );
		hb->rigidbody->setAngularVelocity( angularVelocity );
		hb->rigidbody->setPositionAndRotation( currentPosition, currentRotation );

	}
#endif
	/*hkaPose modelPose ( skeletonModel );
	hkaPose ragdollPose ( skeletonRagdoll );

	// Get current anim pose
	hkArray<hkQsTransform>& tempTransforms = modelPose.accessUnsyncedPoseModelSpace();
	for ( uint i = 0; i < skeletonModel->m_bones.getSize(); ++i )
	{
		glBone* bone = m_skinnedmodel->GetSkeletonList()->at(i);
		Vector3f position = bone->transform.position;
		tempTransforms[i].m_translation = hkVector4( position.x, position.y, position.z );
		Quaternion rotation = bone->transform.rotation;
		tempTransforms[i].m_rotation = hkQuaternion( rotation.x, rotation.y, rotation.z, rotation.w );
		Quaternion scale = bone->transform.scale;
		tempTransforms[i].m_scale = hkVector4( scale.x, scale.y, scale.z );
	}

	ragdollPose.setToReferencePose();

	mMapperAnimToRagdoll->mapPose( modelPose, ragdollPose, hkaSkeletonMapper::CURRENT_POSE );
	ragdollPose.syncModelSpace();

	// Apply the animation to the ragdoll
	for ( auto hb = m_hitboxList.begin(); hb != m_hitboxList.end(); ++hb )
	{
		glBone* targetBone = m_skinnedmodel->GetSkeletonList()->at(hb->boneIndex);
		hkQsTransform targetTransform = ragdollPose.getBoneModelSpace(hb->hitboxIndex);

		hkVector4 linearVelocity = hb->rigidbody->getLinearVelocity();
		hkVector4 angularVelocity = hb->rigidbody->getAngularVelocity();
		linearVelocity.mul( targetBone->ragdollStrength );
		angularVelocity.mul( targetBone->ragdollStrength );

		hkVector4 currentPosition = hb->rigidbody->getPosition();
		currentPosition.setInterpolate( targetTransform.getTranslation(), currentPosition, targetBone->ragdollStrength );
		hkQuaternion currentRotation = hb->rigidbody->getRotation();
		currentRotation.setSlerp( targetTransform.getRotation(), currentRotation, targetBone->ragdollStrength );

		hb->rigidbody->setLinearVelocity( linearVelocity );
		hb->rigidbody->setAngularVelocity( angularVelocity );
		hb->rigidbody->setPositionAndRotation( currentPosition, currentRotation );
	}*/
}

void CRagdollCollision::SetActor ( CActor* n_actor )
{
	m_owning_actor = n_actor;
	owner = (CGameBehavior*)n_actor;
}

CActor* CRagdollCollision::GetActor ( void )
{
	return m_owning_actor;
}
Real	CRagdollCollision::GetMultiplier ( btRigidBody* n_hitBody )
{
	// Todo: give certain rigidbodies different damage boosters
	return 1;
}


#include <iostream>
#include <sstream>

#include <armadillo>

#include <cpp-argparse/OptionParser.h>
#include <besiq/io/covariates.hpp>

#include <plink/plink_file.hpp>

#include "gene_environment.hpp"

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-predict [OPTIONS] plink_file beta_file";
const std::string DESCRIPTION = "Predicts phenotypes given a list of betas.";
const std::string VERSION = "besiq 0.0.1";
const std::string EPILOG = "";

arma::vec
parse_beta(std::istream &stream, std::vector<std::string> &labels)
{
    std::string line;
    std::vector<double> beta_list;
    while( std::getline( stream, line ) )
    {
        std::istringstream line_stream( line );
        std::string rs_name;
        double beta;

        line_stream >> rs_name >> beta;

        labels.push_back( rs_name );
        beta_list.push_back( beta );
    }

    arma::vec b = arma::zeros<arma::vec>( beta_list.size( ) );
    for(int i = 0; i < beta_list.size( ); i++)
    {
        b[ i ] = beta_list[ i ];
    }

    return b;
}

arma::uvec get_causal_indices(const std::vector<std::string> variable_order, const std::vector<std::string> &labels)
{
    std::vector<size_t> indices;
    std::map<std::string, int> variable_to_index;
    for(int i = 0; i < variable_order.size( ); i++)
    {
        variable_to_index[ variable_order[ i ] ] = i;
    }

    for(int i = 0; i < labels.size( ); i++)
    {
        if( variable_to_index.count( labels[ i ] ) > 0 )
        {
            indices.push_back( variable_to_index[ labels[ i ] ] );
        }
    }

    arma::uvec result = arma::zeros<arma::uvec>( indices.size( ) );
    for(int i = 0; i < indices.size( ); i++)
    {
        result[ i ] = indices[ i ];
    }

    return result;
}

int
main(int argc, char *argv[])
{
    OptionParser parser = OptionParser( ).usage( USAGE )
                                         .version( VERSION )
                                         .description( DESCRIPTION )
                                         .epilog( EPILOG );

    parser.add_option( "-c", "--cov" ).action( "store" ).type( "string" ).metavar( "filename" ).help( "Performs the analysis by including the covariates in this file." );
    parser.add_option( "-o", "--out" ).help( "The output file that will contain the results (binary)." );
    parser.add_option( "--standardize" ).help( "Use standardized genotypes and covariates." ).action( "store_true" );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 2 )
    {
        parser.print_help( );
        exit( 1 );
    }

    std::ios_base::sync_with_stdio( false );

    /* Read all genotypes */
    plink_file_ptr genotype_file = open_plink_file( parser.args( )[ 0 ] );
    genotype_matrix_ptr genotypes = create_genotype_matrix( genotype_file );
    std::vector<std::string> order = genotype_file->get_sample_iids( );
    std::vector< std::pair<std::string, std::string> > fid_iid = genotype_file->get_sample_fid_iid( );

    std::vector<std::string> labels;
    std::ifstream beta_file( parser.args( )[ 1 ].c_str( ) );
    arma::vec beta = parse_beta( beta_file, labels );

    /* Make error streams separate from stdout */
    arma::set_stream_err1( std::cerr );
    arma::set_stream_err2( std::cerr );

    /* Parse phenotypes */
    arma::uvec cov_missing = arma::zeros<arma::uvec>( genotype_file->get_samples( ).size( ) );
    arma::mat cov;
    std::vector<std::string> cov_names;
    if( options.is_set( "cov" ) )
    {
        std::ifstream covariate_file( options[ "cov" ].c_str( ) );
        cov = parse_covariate_matrix( covariate_file, cov_missing, order, &cov_names );
    }

    /* Open output stream */
    std::ofstream output_file;
    if( options.is_set( "out" ) )
    {
        output_file.open( options[ "out" ].c_str( ) );
    }
    std::ostream &out = options.is_set( "out" ) ? output_file : std::cout;

    arma::vec phenotype = arma::zeros<arma::vec>( fid_iid.size( ) );
    gene_environment ge( genotypes, cov, phenotype, cov_names, true );
    ge.impute_missing( );

    arma::uvec causal = get_causal_indices( ge.get_names( ), labels );
    if( causal.n_elem != labels.size( ) )
    {
        std::cout << "besiq-predict: Error could not find all genotype or covariate variables.\n";
        return 1;
    }

    bool standardize = options.is_set( "only_pvalues" );

    arma::mat X;
    if( standardize )
    {
        X = ge.get_active( causal );
    }
    else
    {
        X = ge.get_active_raw( causal );
    }
    arma::vec y = X * beta;
    arma::vec y_std = ( y - arma::mean( y ) ) / arma::stddev( y );

    out << "FID\tIID\tPhenotype\n";
    for(int i = 0; i < fid_iid.size( ); i++)
    {
        out << fid_iid[ i ].first << "\t" << fid_iid[ i ].second << "\t" << y_std[ i ] << "\n";
    }

    return 0;
}