#ifndef DEEPQCACLA_HPP
#define DEEPQCACLA_HPP

#include <vector>
#include <string>
#include <boost/serialization/list.hpp>
#include <boost/serialization/set.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/deque.hpp>

#include "nn/MLP.hpp"
#include "nn/DevMLP.hpp"
#include "nn/DODevMLP.hpp"
#include "arch/AACAgent.hpp"
#include "bib/Seed.hpp"
#include "bib/Utils.hpp"
#include <bib/MetropolisHasting.hpp>
#include <bib/XMLEngine.hpp>

#define DOUBLE_COMPARE_PRECISION 1e-9

typedef struct _sample {
  std::vector<double> s;
  std::vector<double> pure_a;
  std::vector<double> a;
  std::vector<double> next_s;
  double r;
  bool goal_reached;

  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive& ar, const unsigned int) {
    ar& BOOST_SERIALIZATION_NVP(s);
    ar& BOOST_SERIALIZATION_NVP(pure_a);
    ar& BOOST_SERIALIZATION_NVP(a);
    ar& BOOST_SERIALIZATION_NVP(next_s);
    ar& BOOST_SERIALIZATION_NVP(r);
    ar& BOOST_SERIALIZATION_NVP(goal_reached);
  }

  //Used to store all sample into a tree, might be stochastic
  //only pure_a is negligate
  bool operator< (const _sample& b) const {
    for (uint i = 0; i < s.size(); i++) {
      if(fabs(s[i] - b.s[i])>=DOUBLE_COMPARE_PRECISION)
        return s[i] < b.s[i];
    }

    for (uint i = 0; i < a.size(); i++) {
      if(fabs(a[i] - b.a[i])>=DOUBLE_COMPARE_PRECISION)
        return a[i] < b.a[i];
    }

    for (uint i = 0; i < next_s.size(); i++) {
      if(fabs(next_s[i] - b.next_s[i])>=DOUBLE_COMPARE_PRECISION)
        return next_s[i] < b.next_s[i];
    }

    if(fabs(r - b.r)>=DOUBLE_COMPARE_PRECISION)
        return r < b.r;

    return goal_reached < b.goal_reached;
  }

  typedef double value_type;

  inline double operator[](size_t const N) const{
        return s[N];
  }

  bool same_state(const _sample& b) const{
    for (uint i = 0; i < s.size(); i++) {
      if(fabs(s[i] - b.s[i])>=DOUBLE_COMPARE_PRECISION)
        return false;
    }

    return true;
  }

} sample;

template<typename NN = MLP>
class DeepQCaclaAg : public arch::AACAgent<MLP, arch::AgentGPUProgOptions> {
 public:
  typedef MLP PolicyImpl;

  DeepQCaclaAg(unsigned int _nb_motors, unsigned int _nb_sensors)
    : arch::AACAgent<MLP, arch::AgentGPUProgOptions>(_nb_motors, _nb_sensors), nb_sensors(_nb_sensors) {

  }

  virtual ~DeepQCaclaAg() {
    delete qnn;
    delete ann;

    delete qnn_target;
    delete ann_target;

    delete ann_testing;

    delete hidden_unit_q;
    delete hidden_unit_a;
  }

  const std::vector<double>& _run(double reward, const std::vector<double>& sensors,
                                 bool learning, bool goal_reached, bool) override {

    // protect batch norm from testing data and poor data
    double* weights = new double[ann->number_of_parameters(false)];
    ann->copyWeightsTo(weights, false);
    ann_testing->copyWeightsFrom(weights, false);
    delete[] weights;
    vector<double>* next_action = ann_testing->computeOut(sensors);

    if (last_action.get() != nullptr && learning){
      sample sa = {last_state, *last_pure_action, *last_action, sensors, reward, goal_reached};
      insertSample(sa);
    }

    last_pure_action.reset(new vector<double>(*next_action));
    if(learning) {
      if(gaussian_policy){
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*next_action, noise);
        delete next_action;
        next_action = randomized_action;
      } else if(bib::Utils::rand01() < noise){ //e-greedy
        for (uint i = 0; i < next_action->size(); i++)
          next_action->at(i) = bib::Utils::randin(-1.f, 1.f);
      }
    }
    last_action.reset(next_action);

    last_state.clear();
    for (uint i = 0; i < sensors.size(); i++)
      last_state.push_back(sensors[i]);

    last_trajectory_a.push_back(*next_action);

    return *next_action;
  }

  void insertSample(const sample& sa){
    if(trajectory.size() >= replay_memory)
      trajectory.pop_front();
    trajectory.push_back(sa);

    end_episode(true);
  }

  void _unique_invoke(boost::property_tree::ptree* pt, boost::program_options::variables_map* command_args) override {
    hidden_unit_q           = bib::to_array<uint>(pt->get<std::string>("agent.hidden_unit_q"));
    hidden_unit_a           = bib::to_array<uint>(pt->get<std::string>("agent.hidden_unit_a"));
    noise                   = pt->get<double>("agent.noise");
    gaussian_policy         = pt->get<bool>("agent.gaussian_policy");
    kMinibatchSize          = pt->get<uint>("agent.mini_batch_size");
    replay_memory           = pt->get<uint>("agent.replay_memory");
    inverting_grad          = pt->get<bool>("agent.inverting_grad");
    force_more_update       = pt->get<uint>("agent.force_more_update");
    tau_soft_update         = pt->get<double>("agent.tau_soft_update");
    alpha_a                 = pt->get<double>("agent.alpha_a");
    alpha_v                 = pt->get<double>("agent.alpha_v");
    decay_v                 = pt->get<double>("agent.decay_v");
    batch_norm_critic       = pt->get<uint>("agent.batch_norm_critic");
    batch_norm_actor        = pt->get<uint>("agent.batch_norm_actor");
    actor_output_layer_type = pt->get<uint>("agent.actor_output_layer_type");
    hidden_layer_type       = pt->get<uint>("agent.hidden_layer_type");
    bool test_net           = pt->get<bool>("agent.test_net");
    bn_adapt                = pt->get<bool>("agent.bn_adapt");
    uint momentum           = pt->get<uint>("agent.momentum");
    qac_target              = pt->get<bool>("agent.qac_target");
    aac_target              = pt->get<bool>("agent.aac_target");
    qac_sample              = pt->get<uint>("agent.qac_sample");
    qnextac_sample          = pt->get<uint>("agent.qnextac_sample");
    recompute_next_ac       = pt->get<bool>("agent.recompute_next_ac");
    onpolac                 = pt->get<bool>("agent.onpolac");
    qmu                     = pt->get<bool>("agent.qmu");

    if(recompute_next_ac && aac_target) {
      LOG_DEBUG("recompute_next_ac useless");
      exit(1);
    }

#ifdef CAFFE_CPU_ONLY
    LOG_INFO("CPU mode");
    (void) command_args;
#else
    if(command_args->count("gpu") == 0 || command_args->count("cpu") > 0){
      caffe::Caffe::set_mode(caffe::Caffe::Brew::CPU);
      LOG_INFO("CPU mode");
    } else {
      caffe::Caffe::set_mode(caffe::Caffe::Brew::GPU);
      caffe::Caffe::SetDevice(0);
      LOG_INFO("GPU mode");
    }
#endif

    ann = new NN(nb_sensors, *hidden_unit_a, nb_motors, alpha_a, kMinibatchSize,
                 hidden_layer_type, actor_output_layer_type, batch_norm_actor, false, momentum);
    if(std::is_same<NN, DevMLP>::value)
      ann->exploit(pt, static_cast<DeepQCaclaAg *>(old_ag)->ann);
    else if(std::is_same<NN, DODevMLP>::value)
      ann->exploit(pt, nullptr);

    if(test_net)
      ann_target = new NN(*ann, false, ::caffe::Phase::TEST);
    else
      ann_target = new NN(*ann, false);

    ann_testing = new NN(*ann, false, ::caffe::Phase::TEST);
    ann_testing->increase_batchsize(1);

    qnn = new NN(nb_sensors + nb_motors, nb_sensors, *hidden_unit_q, alpha_v, kMinibatchSize,
                 decay_v, hidden_layer_type, batch_norm_critic, false, momentum);
    if(std::is_same<NN, DevMLP>::value)
      qnn->exploit(pt, static_cast<DeepQCaclaAg *>(old_ag)->qnn);
    else if(std::is_same<NN, DODevMLP>::value)
      qnn->exploit(pt, ann);

    if(test_net)
      qnn_target = new NN(*qnn, false, ::caffe::Phase::TEST);
    else
      qnn_target = new NN(*qnn, false);
  }

  void _start_episode(const std::vector<double>& sensors, bool) override {
    last_state.clear();
    for (uint i = 0; i < sensors.size(); i++)
      last_state.push_back(sensors[i]);

    last_action = nullptr;
    last_pure_action = nullptr;

    last_trajectory_a.clear();

    valid_advantage = 0;
    invalid_advantage = 0;
  }

  void sample_transition(std::vector<sample>& traj, const std::deque<sample>& from){
     for(uint i=0;i<traj.size();i++){
       int r = std::uniform_int_distribution<int>(0, from.size() - 1)(*bib::Seed::random_engine());
       traj[i] = from[r];
     }
  }

  void end_episode(bool learning) override {

    if(!learning || trajectory.size() < 250 || trajectory.size() < kMinibatchSize)
      return;

    for(uint fupd=0;fupd<1+force_more_update;fupd++) {
      std::vector<sample> traj(kMinibatchSize);
      sample_transition(traj, trajectory);

      //compute \pi(s_{t+1})
      std::vector<double> all_next_states(traj.size() * nb_sensors);
      std::vector<double> all_states(traj.size() * nb_sensors);
      std::vector<double> all_actions(traj.size() * nb_motors);
      std::vector<bool> disable_back(traj.size() * this->nb_motors, false);
      const std::vector<bool> disable_back_ac(this->nb_motors, true);
      std::vector<double> deltas(traj.size());
      uint i=0;
      for (auto it : traj){
        std::copy(it.next_s.begin(), it.next_s.end(), all_next_states.begin() + i * nb_sensors);
        std::copy(it.s.begin(), it.s.end(), all_states.begin() + i * nb_sensors);
        std::copy(it.a.begin(), it.a.end(), all_actions.begin() + i * nb_motors);
        i++;
      }

      auto all_next_actions = ann_target->computeOutBatch(all_next_states);

      //compute next q
      std::vector<double>* q_targets;
      if(!qmu)
        q_targets = qnn_target->computeOutVFBatch(all_next_states, *all_next_actions);
      else {
        q_targets = new std::vector<double>(traj.size(), 0.f);
        for(uint j=0;j<7;j++){
          vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*all_next_actions, noise);
          auto local_all_nextV = qnn_target->computeOutVFBatch(all_next_states, *randomized_action);
          std::transform(q_targets->begin(), q_targets->end(), local_all_nextV->begin(), q_targets->begin(), std::plus<double>());
          delete local_all_nextV;
          delete randomized_action;
        }
        double factor_ = 1.f/((double)7.);
        std::transform(q_targets->begin(), q_targets->end(), q_targets->begin(), std::bind1st(std::multiplies<double>(), factor_));
      }

      //adjust q_targets
      i=0;
      for (auto it : traj){
        if(it.goal_reached)
          q_targets->at(i) = it.r;
        else
          q_targets->at(i) = it.r + gamma * q_targets->at(i);

        i++;
      }

      //Update critic
      qnn->learn_batch(all_states, all_actions, *q_targets, 1);

      //Update actor
      qnn->ZeroGradParameters();
      ann->ZeroGradParameters();

      auto all_actions_outputs = ann->computeOutBatch(all_states);
      if(batch_norm_actor != 0 && bn_adapt){
        delete all_actions_outputs;
        ann_testing->increase_batchsize(kMinibatchSize);
        all_actions_outputs = ann_testing->computeOutBatch(all_states);
        ann_testing->increase_batchsize(1);
      }

      MLP* qnn_worker = qnn;
      if(qac_target)
        qnn_worker = qnn_target;

      MLP* ann_worker = ann;
      if(aac_target)
        ann_worker = ann_target;

      // pre-compute delta
      std::vector<double>* all_mine, *all_nextV;

      std::vector<double>* current_all_actions = all_actions_outputs;
      if(onpolac)
        current_all_actions = &all_actions;

      if(qac_sample <= 1)
        all_mine = qnn->computeOutVFBatch(all_states, *current_all_actions);
      else {
        all_mine = new std::vector<double>(traj.size(), 0.f);
        for(uint j=0;j<qac_sample;j++){
          vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*current_all_actions, noise);
          auto local_all_mine = qnn_worker->computeOutVFBatch(all_states, *randomized_action);
          std::transform(all_mine->begin(), all_mine->end(), local_all_mine->begin(), all_mine->begin(), std::plus<double>());
          delete local_all_mine;
          delete randomized_action;
        }
        double factor_ = 1.f/((double)qac_sample);
        std::transform(all_mine->begin(), all_mine->end(), all_mine->begin(), std::bind1st(std::multiplies<double>(), factor_));
      }

      std::vector<double>* current_all_next_actions = all_next_actions;
      if(recompute_next_ac)
        current_all_next_actions = ann_worker->computeOutBatch(all_next_states);

      if(qnextac_sample <= 1){
        all_nextV = qnn_target->computeOutVFBatch(all_next_states, *current_all_next_actions);
      } else {
        all_nextV = new std::vector<double>(traj.size(), 0.f);
        for(uint j=0;j<qnextac_sample;j++){
          vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*current_all_next_actions, noise);
          auto local_all_nextV = qnn_worker->computeOutVFBatch(all_next_states, *randomized_action);
          std::transform(all_nextV->begin(), all_nextV->end(), local_all_nextV->begin(), all_nextV->begin(), std::plus<double>());
          delete local_all_nextV;
          delete randomized_action;
        }
        double factor_ = 1.f/((double)qnextac_sample);
        std::transform(all_nextV->begin(), all_nextV->end(), all_nextV->begin(), std::bind1st(std::multiplies<double>(), factor_));
      }

      if(recompute_next_ac)
        delete current_all_next_actions;

      // compute delta
      i=0;
      for (auto it : traj) {
        double v_target = it.r;
        if (!it.goal_reached) {
          double nextV = all_nextV->at(i);
          v_target += this->gamma * nextV;
        }

        deltas[i] = v_target - all_mine->at(i);
        ++i;
      }

      // compute disable_back
      i=0;
      for(auto it : traj) {
        if(deltas[i] <= 0.0000000000f){
          std::copy(disable_back_ac.begin(), disable_back_ac.end(), disable_back.begin() + i * this->nb_motors);
          invalid_advantage++;
        } else
          valid_advantage++;
        i++;
      }

      // unless deter update
      qnn->ZeroGradParameters();
      ann->ZeroGradParameters();

//       delete qnn->computeOutVFBatch(all_states, *all_actions_outputs);
//       const auto q_values_blob = qnn->getNN()->blob_by_name(MLP::q_values_blob_name);
//       double* q_values_diff = q_values_blob->mutable_cpu_diff();
//       i=0;
//       for (auto it : traj)
//         q_values_diff[q_values_blob->offset(i++,0,0,0)] = -1.0f;
//       qnn->critic_backward();
//       const auto critic_action_blob = qnn->getNN()->blob_by_name(MLP::actions_blob_name);
//       const double* critic_action_diff = critic_action_blob->cpu_diff();

      const auto actor_actions_blob = ann->getNN()->blob_by_name(MLP::actions_blob_name);
      auto ac_diff = actor_actions_blob->mutable_cpu_diff();
      for(i=0; i<(uint) actor_actions_blob->count(); i++) {
        if(disable_back[i]) {
//           ac_diff[i] = critic_action_diff[i];
          ac_diff[i] = 0.f;
        } else {
          double x = all_actions[i] - all_actions_outputs->at(i);
          ac_diff[i] = -x;
        }
      }

      if(inverting_grad){

        for (uint n = 0; n < traj.size(); ++n) {
          for (uint h = 0; h < nb_motors; ++h) {
            int offset = actor_actions_blob->offset(n,h,0,0);
            double diff = ac_diff[offset];
            double output = all_actions_outputs->at(offset);
            double min = -1.0;
            double max = 1.0;
            if (diff < 0) {
              diff *= (max - output) / (max - min);
            } else if (diff > 0) {
              diff *= (output - min) / (max - min);
            }
            ac_diff[offset] = diff;
          }
        }
      }

      ann->actor_backward();
      ann->getSolver()->ApplyUpdate();
      ann->getSolver()->set_iter(ann->getSolver()->iter() + 1);

      // Soft update of targets networks
      qnn_target->soft_update(*qnn, tau_soft_update);
      ann_target->soft_update(*ann, tau_soft_update);

      delete q_targets;
      delete all_actions_outputs;
      delete all_next_actions;

      delete all_nextV;
      delete all_mine;
    }

  }

  double criticEval(const std::vector<double>& perceptions, const std::vector<double>& actions) override {
      return qnn->computeOutVF(perceptions, actions);
  }

  arch::Policy<MLP>* getCopyCurrentPolicy() override {
    return new arch::Policy<MLP>(new MLP(*ann, true) , gaussian_policy ? arch::policy_type::GAUSSIAN : arch::policy_type::GREEDY, noise, decision_each);
  }

  void save(const std::string& path, bool, bool) override {
//     if(save_best && best_population.size() > 0){
//       auto it = best_population.begin();
//       bib::XMLEngine::save(it->trajectory_a, "trajectory_a", "best_trajectory_a.data");
//     } else {
      ann->save(path+".actor");
      qnn->save(path+".critic");
//     }
  }

  void load(const std::string& path) override {
    ann->load(path+".actor");
    qnn->load(path+".critic");

    delete qnn_target;
    delete ann_target;

    qnn_target = new MLP(*qnn, false, ::caffe::Phase::TEST);
    ann_target = new MLP(*ann, false, ::caffe::Phase::TEST);
  }

 protected:
  void _display(std::ostream& out) const override {
    out << std::setw(12) << std::fixed << std::setprecision(10) << sum_weighted_reward
    << " " << std::setprecision(3) << ((float)(valid_advantage)/((float)(valid_advantage+invalid_advantage)))
//     #ifndef NDEBUG
//     << " " << std::setw(8) << std::fixed << std::setprecision(5) << noise
//     << " " << trajectory.size()
//     << " " << ann->weight_l1_norm()
//     << " " << std::fixed << std::setprecision(7) << qnn->error()
//     << " " << qnn->weight_l1_norm()
//     #endif
    ;
  }

  void _dump(std::ostream& out) const override {
    out << std::setw(25) << std::fixed << std::setprecision(22) <<
        sum_weighted_reward << " " << std::setw(8) << std::fixed <<
        std::setprecision(5) << trajectory.size() << " " << std::setprecision(3) <<
        ((float)(valid_advantage)/((float)(valid_advantage+invalid_advantage)));
  }

 private:
  uint nb_sensors;

  double noise;
  double tau_soft_update;
  double alpha_a;
  double alpha_v;
  double decay_v;

  bool gaussian_policy;
  std::vector<uint>* hidden_unit_q;
  std::vector<uint>* hidden_unit_a;
  uint kMinibatchSize;
  uint replay_memory;
  uint force_more_update;
  uint batch_norm_actor, batch_norm_critic;
  uint actor_output_layer_type, hidden_layer_type;

  bool inverting_grad, bn_adapt;

  std::shared_ptr<std::vector<double>> last_action;
  std::shared_ptr<std::vector<double>> last_pure_action;
  std::vector<double> last_state;

  std::deque<sample> trajectory;
  std::deque<std::vector<double>> last_trajectory_a;

  bool qac_target, aac_target, recompute_next_ac, onpolac, qmu;
  uint qac_sample, qnextac_sample;

  uint episode = 0;
  uint valid_advantage, invalid_advantage;

  struct my_pol_dpmt{
    MLP* ann;
    MLP* qnn;
    double J;
    std::deque<std::vector<double>> trajectory_a;

    bool operator< (const my_pol_dpmt& b) const {
      return J > b.J;
    }
  };

  MLP* ann, *ann_target;
  MLP* qnn, *qnn_target;
  MLP* ann_testing;

  struct algo_state {
    uint episode;
    double J;
    std::deque<std::vector<double>> best_trajectory_a;

    friend class boost::serialization::access;
    template <typename Archive>
    void serialize(Archive& ar, const unsigned int) {
      ar& BOOST_SERIALIZATION_NVP(episode);
      ar& BOOST_SERIALIZATION_NVP(J);
      ar& BOOST_SERIALIZATION_NVP(best_trajectory_a);
    }
  };
};

#endif


/******************************************************************************
Copyright (c) 2021, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

 * Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include <iostream>
#include <string>

#include <pinocchio/fwd.hpp>  // forward declarations must be included first.

#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>

#include "ocs2_legged_robot/LeggedRobotInterface.h"

#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <ocs2_centroidal_model/ModelHelperFunctions.h>
#include <ocs2_core/misc/Display.h>
#include <ocs2_core/soft_constraint/StateInputSoftConstraint.h>
#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>

#include "ocs2_legged_robot/LeggedRobotPreComputation.h"
#include "ocs2_legged_robot/constraint/FrictionConeConstraint.h"
#include "ocs2_legged_robot/constraint/NormalVelocityConstraintCppAd.h"
#include "ocs2_legged_robot/constraint/ZeroForceConstraint.h"
#include "ocs2_legged_robot/constraint/ZeroVelocityConstraintCppAd.h"
#include "ocs2_legged_robot/cost/LeggedRobotStateInputQuadraticCost.h"
#include "ocs2_legged_robot/dynamics/LeggedRobotDynamicsAD.h"

// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

namespace ocs2 {
namespace legged_robot {

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile) {
  // check that task file exists
  boost::filesystem::path taskFilePath(taskFile);
  if (boost::filesystem::exists(taskFilePath)) {
    std::cerr << "[LeggedRobotInterface] Loading task file: " << taskFilePath << std::endl;
  } else {
    throw std::invalid_argument("[LeggedRobotInterface] Task file not found: " + taskFilePath.string());
  }
  // check that urdf file exists
  boost::filesystem::path urdfFilePath(urdfFile);
  if (boost::filesystem::exists(urdfFilePath)) {
    std::cerr << "[LeggedRobotInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl;
  } else {
    throw std::invalid_argument("[LeggedRobotInterface] URDF file not found: " + urdfFilePath.string());
  }
  // check that targetCommand file exists
  boost::filesystem::path referenceFilePath(referenceFile);
  if (boost::filesystem::exists(referenceFilePath)) {
    std::cerr << "[LeggedRobotInterface] Loading target command settings from: " << referenceFilePath << std::endl;
  } else {
    throw std::invalid_argument("[LeggedRobotInterface] targetCommand file not found: " + referenceFilePath.string());
  }

  bool verbose;
  loadData::loadCppDataType(taskFile, "legged_robot_interface.verbose", verbose);

  // load setting from loading file
  modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose);
  ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
  mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
  rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
  sqpSettings_ = multiple_shooting::loadSettings(taskFile, "multiple_shooting", verbose);

  // OptimalConrolProblem
  setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose);

  // initial state
  initialState_.setZero(centroidalModelInfo_.stateDim);
  loadData::loadEigenMatrix(taskFile, "initialState", initialState_);
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile, const std::string& urdfFile,
                                                     const std::string& referenceFile, bool verbose) {
  // PinocchioInterface
  pinocchioInterfacePtr_.reset(new PinocchioInterface(centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames)));

  // CentroidalModelInfo
  centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo(
      *pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile),
      centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile), modelSettings_.contactNames3DoF,
      modelSettings_.contactNames6DoF);

  // Swing trajectory planner
  std::unique_ptr<SwingTrajectoryPlanner> swingTrajectoryPlanner(
      new SwingTrajectoryPlanner(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4));

  // Mode schedule manager
  referenceManagerPtr_ =
      std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose), std::move(swingTrajectoryPlanner));

  // Optimal control problem
  problemPtr_.reset(new OptimalControlProblem);

  // Dynamics
  bool useAnalyticalGradientsDynamics = false;
  loadData::loadCppDataType(taskFile, "legged_robot_interface.useAnalyticalGradientsDynamics", useAnalyticalGradientsDynamics);
  std::unique_ptr<SystemDynamicsBase> dynamicsPtr;
  if (useAnalyticalGradientsDynamics) {
    throw std::runtime_error("[LeggedRobotInterface::setupOptimalConrolProblem] The analytical dynamics class is not yet implemented!");
  } else {
    const std::string modelName = "dynamics";
    dynamicsPtr.reset(new LeggedRobotDynamicsAD(*pinocchioInterfacePtr_, centroidalModelInfo_, modelName, modelSettings_));
  }

  problemPtr_->dynamicsPtr = std::move(dynamicsPtr);

  // Cost terms
  problemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, false));

  // Constraint terms
  // friction cone settings
  scalar_t frictionCoefficient = 0.7;
  RelaxedBarrierPenalty::Config barrierPenaltyConfig;
  std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(taskFile, verbose);

  bool useAnalyticalGradientsConstraints = false;
  loadData::loadCppDataType(taskFile, "legged_robot_interface.useAnalyticalGradientsConstraints", useAnalyticalGradientsConstraints);
  for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) {
    const std::string& footName = modelSettings_.contactNames3DoF[i];

    std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr;
    if (useAnalyticalGradientsConstraints) {
      throw std::runtime_error(
          "[LeggedRobotInterface::setupOptimalConrolProblem] The analytical end-effector linear constraint is not implemented!");
    } else {
      const auto infoCppAd = centroidalModelInfo_.toCppAd();
      const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd);
      auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t& state, PinocchioInterfaceCppAd& pinocchioInterfaceAd) {
        const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd);
        updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q);
      };
      eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, {footName},
                                                                    centroidalModelInfo_.stateDim, centroidalModelInfo_.inputDim,
                                                                    velocityUpdateCallback, footName, modelSettings_.modelFolderCppAd,
                                                                    modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));
    }

    problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
                                        getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig));
    problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i));
    problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
                                            getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));
    problemPtr_->equalityConstraintPtr->add(footName + "_normalVelocity",
                                            getNormalVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));
  }

  // Pre-computation
  problemPtr_->preComputationPtr.reset(new LeggedRobotPreComputation(*pinocchioInterfacePtr_, centroidalModelInfo_,
                                                                     *referenceManagerPtr_->getSwingTrajectoryPlanner(), modelSettings_));

  // Rollout
  rolloutPtr_.reset(new TimeTriggeredRollout(*problemPtr_->dynamicsPtr, rolloutSettings_));

  // Initialization
  constexpr bool extendNormalizedMomentum = true;
  initializerPtr_.reset(new LeggedRobotInitializer(centroidalModelInfo_, *referenceManagerPtr_, extendNormalizedMomentum));
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::shared_ptr<GaitSchedule> LeggedRobotInterface::loadGaitSchedule(const std::string& file, bool verbose) const {
  const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false);
  const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false);

  const auto defaultGait = [&] {
    Gait gait{};
    gait.duration = defaultModeSequenceTemplate.switchingTimes.back();
    // Events: from time -> phase
    std::for_each(defaultModeSequenceTemplate.switchingTimes.begin() + 1, defaultModeSequenceTemplate.switchingTimes.end() - 1,
                  [&](double eventTime) { gait.eventPhases.push_back(eventTime / gait.duration); });
    // Modes:
    gait.modeSequence = defaultModeSequenceTemplate.modeSequence;
    return gait;
  }();

  // display
  if (verbose) {
    std::cerr << "\n#### Modes Schedule: ";
    std::cerr << "\n#### =============================================================================\n";
    std::cerr << "Initial Modes Schedule: \n" << initModeSchedule;
    std::cerr << "Default Modes Sequence Template: \n" << defaultModeSequenceTemplate;
    std::cerr << "#### =============================================================================\n";
  }

  return std::make_shared<GaitSchedule>(initModeSchedule, defaultModeSequenceTemplate, modelSettings_.phaseTransitionStanceTime);
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
matrix_t LeggedRobotInterface::initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info) {
  const size_t totalContactDim = 3 * info.numThreeDofContacts;

  vector_t initialState(centroidalModelInfo_.stateDim);
  loadData::loadEigenMatrix(taskFile, "initialState", initialState);

  const auto& model = pinocchioInterfacePtr_->getModel();
  auto& data = pinocchioInterfacePtr_->getData();
  const auto q = centroidal_model::getGeneralizedCoordinates(initialState, centroidalModelInfo_);
  pinocchio::computeJointJacobians(model, data, q);
  pinocchio::updateFramePlacements(model, data);

  matrix_t baseToFeetJacobians(totalContactDim, info.actuatedDofNum);
  for (size_t i = 0; i < info.numThreeDofContacts; i++) {
    matrix_t jacobianWorldToContactPointInWorldFrame = matrix_t::Zero(6, info.generalizedCoordinatesNum);
    pinocchio::getFrameJacobian(model, data, model.getBodyId(modelSettings_.contactNames3DoF[i]), pinocchio::LOCAL_WORLD_ALIGNED,
                                jacobianWorldToContactPointInWorldFrame);

    baseToFeetJacobians.block(3 * i, 0, 3, info.actuatedDofNum) =
        jacobianWorldToContactPointInWorldFrame.block(0, 6, 3, info.actuatedDofNum);
  }

  matrix_t R_taskspace(totalContactDim + totalContactDim, totalContactDim + totalContactDim);
  loadData::loadEigenMatrix(taskFile, "R", R_taskspace);

  matrix_t R = matrix_t::Zero(info.inputDim, info.inputDim);
  // Contact Forces
  R.topLeftCorner(totalContactDim, totalContactDim) = R_taskspace.topLeftCorner(totalContactDim, totalContactDim);
  // Joint velocities
  R.bottomRightCorner(info.actuatedDofNum, info.actuatedDofNum) =
      baseToFeetJacobians.transpose() * R_taskspace.bottomRightCorner(totalContactDim, totalContactDim) * baseToFeetJacobians;
  return R;
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputCost> LeggedRobotInterface::getBaseTrackingCost(const std::string& taskFile, const CentroidalModelInfo& info,
                                                                          bool verbose) {
  matrix_t Q(info.stateDim, info.stateDim);
  loadData::loadEigenMatrix(taskFile, "Q", Q);
  matrix_t R = initializeInputCostWeight(taskFile, info);

  if (verbose) {
    std::cerr << "\n #### Base Tracking Cost Coefficients: ";
    std::cerr << "\n #### =============================================================================\n";
    std::cerr << "Q:\n" << Q << "\n";
    std::cerr << "R:\n" << R << "\n";
    std::cerr << " #### =============================================================================\n";
  }

  return std::unique_ptr<StateInputCost>(new LeggedRobotStateInputQuadraticCost(std::move(Q), std::move(R), info, *referenceManagerPtr_));
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedRobotInterface::loadFrictionConeSettings(const std::string& taskFile,
                                                                                bool verbose) const {
  boost::property_tree::ptree pt;
  boost::property_tree::read_info(taskFile, pt);
  const std::string prefix = "frictionConeSoftConstraint.";

  scalar_t frictionCoefficient = 1.0;
  RelaxedBarrierPenalty::Config barrierPenaltyConfig;
  if (verbose) {
    std::cerr << "\n #### Friction Cone Settings: ";
    std::cerr << "\n #### =============================================================================\n";
  }
  loadData::loadPtreeValue(pt, frictionCoefficient, prefix + "frictionCoefficient", verbose);
  loadData::loadPtreeValue(pt, barrierPenaltyConfig.mu, prefix + "mu", verbose);
  loadData::loadPtreeValue(pt, barrierPenaltyConfig.delta, prefix + "delta", verbose);
  if (verbose) {
    std::cerr << " #### =============================================================================\n";
  }

  return {frictionCoefficient, std::move(barrierPenaltyConfig)};
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
                                                                                const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) {
  FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
  std::unique_ptr<FrictionConeConstraint> frictionConeConstraintPtr(
      new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_));

  std::unique_ptr<PenaltyBase> penalty(new RelaxedBarrierPenalty(barrierPenaltyConfig));

  return std::unique_ptr<StateInputCost>(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty)));
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getZeroForceConstraint(size_t contactPointIndex) {
  return std::unique_ptr<StateInputConstraint>(new ZeroForceConstraint(*referenceManagerPtr_, contactPointIndex, centroidalModelInfo_));
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
                                                                                size_t contactPointIndex,
                                                                                bool useAnalyticalGradients) {
  auto eeZeroVelConConfig = [](scalar_t positionErrorGain) {
    EndEffectorLinearConstraint::Config config;
    config.b.setZero(3);
    config.Av.setIdentity(3, 3);
    if (!numerics::almost_eq(positionErrorGain, 0.0)) {
      config.Ax.setZero(3, 3);
      config.Ax(2, 2) = positionErrorGain;
    }
    return config;
  };

  if (useAnalyticalGradients) {
    throw std::runtime_error(
        "[LeggedRobotInterface::getZeroVelocityConstraint] The analytical end-effector zero velocity constraint is not implemented!");
  } else {
    return std::unique_ptr<StateInputConstraint>(new ZeroVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex,
                                                                                eeZeroVelConConfig(modelSettings_.positionErrorGain)));
  }
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getNormalVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
                                                                                size_t contactPointIndex,
                                                                                bool useAnalyticalGradients) {
  if (useAnalyticalGradients) {
    throw std::runtime_error(
        "[LeggedRobotInterface::getNormalVelocityConstraint] The analytical end-effector normal velocity constraint is not implemented!");
  } else {
    return std::unique_ptr<StateInputConstraint>(new NormalVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex));
  }
}

}  // namespace legged_robot
}  // namespace ocs2


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include "reference.hpp"

using namespace boost::simd;

STF_CASE_TPL( "Cardinal 4 shuffles resolving as AVX permute2f"
            , (double)(std::int64_t)(std::uint64_t)
            )
{
  bs::pack<T,4> a{ Valmax<T>(), T(42), T(13), Valmin<T>() };
  bs::pack<T,4> b{ T(69), Valmin<T>(), T(37), T(0) };

  STF_ALL_EQUAL( (shuffle<2, 3, 0, 1>(a))  , ( unary_ref<2, 3, 0, 1>(a)  )  );

  STF_ALL_EQUAL( (shuffle<2, 3, 0, 1>(a,b)), (binary_ref<2, 3, 0, 1>(a,b))  );
  STF_ALL_EQUAL( (shuffle<4, 5, 0, 1>(a,b)), (binary_ref<4, 5, 0, 1>(a,b))  );
  STF_ALL_EQUAL( (shuffle<6, 7, 0, 1>(a,b)), (binary_ref<6, 7, 0, 1>(a,b))  );
  STF_ALL_EQUAL( (shuffle<6, 7, 2, 3>(a,b)), (binary_ref<6, 7, 2, 3>(a,b))  );
  STF_ALL_EQUAL( (shuffle<0, 1, 4, 5>(a,b)), (binary_ref<0, 1, 4, 5>(a,b))  );
  STF_ALL_EQUAL( (shuffle<2, 3, 4, 5>(a,b)), (binary_ref<2, 3, 4, 5>(a,b))  );
  STF_ALL_EQUAL( (shuffle<6, 7, 4, 5>(a,b)), (binary_ref<6, 7, 4, 5>(a,b))  );
  STF_ALL_EQUAL( (shuffle<2, 3, 6, 7>(a,b)), (binary_ref<2, 3, 6, 7>(a,b))  );
}


#pragma once
#include <boost/rational.hpp>
#include <cstddef>
#include <memory>
#include <string>

#include "ear/metadata.hpp"

namespace bear {

struct ConfigImpl;
struct ListenerImpl;
class RendererImpl;

/// configuration for the renderer
class Config {
 public:
  Config();
  Config(const Config &c);
  ~Config();

  Config &operator=(const Config &c);

  /// set the number of Objects channels (default 0)
  void set_num_objects_channels(size_t num_channels);
  size_t get_num_objects_channels() const;

  /// set the number of DirectSpeakers channels (default 0)
  void set_num_direct_speakers_channels(size_t num_channels);
  size_t get_num_direct_speakers_channels() const;

  /// set the number of HOA channels (default 0)
  void set_num_hoa_channels(size_t num_channels);
  size_t get_num_hoa_channels() const;

  /// set the number of samples in a period (no default, must be set)
  void set_period_size(size_t num_samples);
  size_t get_period_size() const;

  /// set the sample rate (default: 48kHz)
  void set_sample_rate(size_t sample_rate);
  size_t get_sample_rate() const;

  /// set the path to the data file (no default, must be set)
  void set_data_path(const std::string &path);
  const std::string &get_data_path() const;

  /// set the FFT implementation to use (default: a built-in implementation)
  void set_fft_implementation(const std::string &fft_implementation);
  const std::string &get_fft_implementation() const;

  /// check that the configuration is valid; raises exceptions for missing or
  /// incorrect values
  void validate() const;

  ConfigImpl &get_impl();
  const ConfigImpl &get_impl() const;

 private:
  std::unique_ptr<ConfigImpl> impl;
};

using Sample = float;
// time in seconds
using Time = boost::rational<int64_t>;

/// interface for specifying distance behaviour.
class DistanceBehaviour {
 public:
  virtual ~DistanceBehaviour() {}
  /// given the distance to an object and the absoluteDistance parameter, get a
  /// linear gain to be applied
  ///
  /// if absoluteDistance is set, distance is the distance parameter multiplied
  /// by the absoluteDistance in metres, accounting for the listener head
  /// position
  ///
  /// if it's not set, distance is just the distance parameter
  ///
  /// For standard behaviour, this should return 1.0 (or a constant value) if
  /// absoluteDistance is not provided, as BS.2076-2 says:
  ///
  ///   If absoluteDistance is negative or undefined, distance based binaural
  ///   rendering is not intended.
  virtual double get_gain(double distance, boost::optional<double> absoluteDistance) = 0;
};

struct AudioPackFormatData {
  boost::optional<double> absoluteDistance;
};

struct MetadataInput {
  boost::optional<Time> rtime;
  boost::optional<Time> duration;
  AudioPackFormatData audioPackFormat_data;
};

struct ObjectsInput : public MetadataInput {
  ear::ObjectsTypeMetadata type_metadata;
  boost::optional<Time> interpolationLength;
  /// distance behaviour; there is no distance-dependant gain if null
  std::shared_ptr<DistanceBehaviour> distance_behaviour = nullptr;
};

struct DirectSpeakersInput : public MetadataInput {
  ear::DirectSpeakersTypeMetadata type_metadata;
};

struct HOAInput : public MetadataInput {
  ear::HOATypeMetadata type_metadata;
  std::vector<size_t> channels;
};

/// Representation of the listener position and head orientation.
///
/// The ADM Cartesian coordinate system is used, as defined in section 8 of
/// BS.2076-2.
class Listener {
 public:
  Listener();
  ~Listener();
  Listener(const Listener &);

  /// ADM-format Cartesian position of the listener relative to the origin in
  /// metres
  void set_position_cart(std::array<double, 3>);
  std::array<double, 3> get_position_cart() const;

  /// orientation of the listener; this translates from world coordinates to
  /// listener coordinates, so:
  ///     position_rel_listener = orientation * position
  ///     position = orientation' * position_rel_listener
  /// quaternion in order {w, x, y, z}, with the coordinate axes from the ADM
  /// Cartesian system
  void set_orientation_quaternion(std::array<double, 4>);
  std::array<double, 4> get_orientation_quaternion() const;

  Listener &operator=(const Listener &other);

  /// methods intended for testing rotation mappings

  /// the ADM-format Cartesian vector that the listener is looking along
  std::array<double, 3> look() const;
  /// the ADM-format Cartesian vector to the right of the listener
  std::array<double, 3> right() const;
  /// the ADM-format Cartesian vector pointing up from the listener
  std::array<double, 3> up() const;

  const ListenerImpl &get_impl() const;

 private:
  std::unique_ptr<ListenerImpl> impl;
};

class Renderer {
 public:
  Renderer(const Config &config);
  Renderer(Renderer &&r);
  Renderer();
  ~Renderer();

  Renderer &operator=(Renderer &&r);

  /// process period_size samples
  ///
  /// @param objects_input num_objects_channels pointers to period_size samples
  ///     of objects input audio
  /// @param direct_speakers_input num_direct_speakers_channels pointers to
  ///     period_size samples of direct speakers input audio
  /// @param hoa_input num_hoa_channels pointers to period_size samples of HOA
  ///     input audio
  /// @param output 2 pointers to period_size samples to store the output audio
  void process(const Sample *const *objects_input,
               const Sample *const *direct_speakers_input,
               const Sample *const *hoa_input,
               Sample *const *output);

  /// Add some objects metadata to be rendered. This returns true if the
  /// metadata was stored, or false if it was added too early and should be
  /// pushed after processing more samples.
  ///
  /// timing behaviour:
  /// - If metadata contains an rtime equal to the rtime+duration of the
  ///   previous block, there will be interpolation from the previous block
  ///   over the interpolationLength (or the full block if that is not
  ///   specified), followed by constant rendering for the rest of the block.
  /// - If metadata contains an rtime greater than to the rtime+duration of the
  ///   previous block (or it is the first block), the rendering will be
  ///   constant over the length of the block. Between the end of the previous
  ///   block (or the start of the audio processing) and the start of this
  ///   block, there will be silence.
  /// - If the rtime is less than the rtime+duration of the previous block, and
  ///   the rtime is greater than or equal to block_start_time, then the
  ///   samples during the previous block will not be affected, but the
  ///   interpolation during the current block will be smooth. this means that
  ///   this sequence:
  ///
  ///       add(a, 0..100)
  ///       add(b, 100..200)
  ///       add(c, 150..200)
  ///
  ///   results in these interpolation points:
  ///
  ///       time   value
  ///       0      a
  ///       100    a
  ///       150    (a + b) / 2
  ///       200    c
  ///
  ///   If the rtime is less than block_start_time, the result is not
  ///   specified, except that the interpolation at the end of the block uses
  ///   the specified metadata; this can be used to reset the renderer to a
  ///   known state, for example to start rendering half way through a block
  ///   after seeking or when rendering a block which has been changed.
  /// @param channel channel in objects_input that this metadata corresponds to
  bool add_objects_block(size_t channel, ObjectsInput metadata);

  /// Add direct speakers metadata; the semantics are the same as
  /// add_objects_block, except that no interpolation occurs.
  ///
  /// @param channel channel in direct_speakers_input that this metadata
  ///     applies to
  bool add_direct_speakers_block(size_t channel, DirectSpeakersInput metadata);

  /// Add HOA metadata; the semantics are the same as
  /// add_direct_speakers_block, except because HOA metadata refers to multiple
  /// channels, a stream identifier is used rather than a channel number.
  /// @param stream arbitrary value used to identify which HOA stream this
  ///     metadata applies to
  bool add_hoa_block(size_t stream, HOAInput metadata);

  /// start time of the first sample in the next block
  Time get_block_start_time() const;

  /// set the start time of the first sample in the next block
  ///
  /// note that this is purely convenience to apply an offset to the rtimes of
  /// the added blocks at the time they are pushed; the specified timing
  /// behaviour still applies
  void set_block_start_time(const Time &time);

  /// get the delay added by the renderer
  Time get_delay() const;

  /// set the listener position and orientation in the next frame. If
  /// interpolation_time is specified, then the change will possibly be
  /// interpolated over more than one frame; this may be useful if the head
  /// position update rate is slower than the frame rate.
  ///
  /// note that we don't keep a reference to Listener; you should call this
  /// every frame (or so).
  void set_listener(const Listener &l, const boost::optional<Time> &interpolation_time = {});

 private:
  std::unique_ptr<RendererImpl> impl;
};

};  // namespace bear


/*!
 * Copyright (c) 2021 Microsoft Corporation. All rights reserved.
 * Licensed under the MIT License. See LICENSE file in the project root for
 * license information.
 */
#include "RangeReduceExpression.h"

#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#include <sstream>

#include "BinaryOperator.h"
#include "Conditional.h"
#include "FreeForm2Assert.h"
#include "LiteralExpression.h"
#include "OperatorExpression.h"
#include "RefExpression.h"
#include "SimpleExpressionOwner.h"
#include "TypeUtil.h"
#include "UnaryOperator.h"
#include "Visitor.h"

namespace {
// This method creates the following precondition expression:
//    (max(low, high) - abs(step) < max(low, high))
// && (min(low, high) + abs(step) > min(low, high))
// && ((step > 0) == (high > low))
// && step != 0)
// and the following loop condition:
// (step > 0 ? loopVar <= high - step
//           : loopVar >= high - step).

boost::tuple<const FreeForm2::Expression *, const FreeForm2::Expression *>
CreateGenericLoopConditions(
    const std::pair<const FreeForm2::Expression *,
                    const FreeForm2::Expression *> &p_range,
    const FreeForm2::Expression &p_step, const FreeForm2::Expression &p_loopVar,
    FreeForm2::SimpleExpressionOwner &p_owner,
    FreeForm2::TypeManager &p_typeManager) {
  using namespace FreeForm2;

  // Common expressions.
  auto zero =
      boost::make_shared<LiteralInt32Expression>(p_loopVar.GetAnnotations(), 0);
  p_owner.AddExpression(zero);
  auto stepSign =
      BinaryOperatorExpression::Alloc(p_loopVar.GetAnnotations(), p_step, *zero,
                                      BinaryOperator::gt, p_typeManager);
  p_owner.AddExpression(stepSign);

  // Create the precondition
  const Expression *precondition = nullptr;
  {
    auto high = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *p_range.first, *p_range.second,
        BinaryOperator::max, p_typeManager);
    p_owner.AddExpression(high);
    auto low = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *p_range.first, *p_range.second,
        BinaryOperator::min, p_typeManager);
    p_owner.AddExpression(low);
    auto step = boost::make_shared<UnaryOperatorExpression>(
        p_loopVar.GetAnnotations(), p_step, UnaryOperator::abs);
    p_owner.AddExpression(step);
    auto highMinusStep = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *high, *step, BinaryOperator::minus,
        p_typeManager);
    p_owner.AddExpression(highMinusStep);
    auto lowPlusStep =
        BinaryOperatorExpression::Alloc(p_loopVar.GetAnnotations(), *low, *step,
                                        BinaryOperator::plus, p_typeManager);
    p_owner.AddExpression(lowPlusStep);
    auto underflowCheck = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *highMinusStep, *high, BinaryOperator::lt,
        p_typeManager);
    p_owner.AddExpression(underflowCheck);
    auto overflowCheck = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *lowPlusStep, *low, BinaryOperator::gt,
        p_typeManager);
    p_owner.AddExpression(overflowCheck);
    auto stepMoving = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), p_step, *zero, BinaryOperator::neq,
        p_typeManager);
    p_owner.AddExpression(stepMoving);
    auto rangeSign = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *p_range.second, *p_range.first,
        BinaryOperator::gt, p_typeManager);
    p_owner.AddExpression(rangeSign);
    auto rangeCheck = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *stepSign, *rangeSign, BinaryOperator::eq,
        p_typeManager);
    p_owner.AddExpression(rangeCheck);
    auto and1 = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *underflowCheck, *overflowCheck,
        BinaryOperator::_and, p_typeManager);
    p_owner.AddExpression(and1);
    auto and2 = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *and1, *rangeCheck, BinaryOperator::_and,
        p_typeManager);
    p_owner.AddExpression(and2);
    auto and3 = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *and2, *stepMoving, BinaryOperator::_and,
        p_typeManager);
    p_owner.AddExpression(and3);
    precondition = and3.get();
  }

  // Create the loop condition.
  const Expression *condition = nullptr;
  {
    auto endMinusStep = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *p_range.second, p_step,
        BinaryOperator::minus, p_typeManager);
    p_owner.AddExpression(endMinusStep);
    auto incRange = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), p_loopVar, *endMinusStep,
        BinaryOperator::lte, p_typeManager);
    p_owner.AddExpression(incRange);
    auto decRange = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), p_loopVar, *endMinusStep,
        BinaryOperator::gte, p_typeManager);
    p_owner.AddExpression(decRange);
    auto cond = boost::make_shared<ConditionalExpression>(
        p_loopVar.GetAnnotations(), *stepSign, *incRange, *decRange);
    p_owner.AddExpression(cond);
    condition = cond.get();
  }

  return boost::make_tuple(precondition, condition);
}

boost::tuple<const FreeForm2::Expression *, const FreeForm2::Expression *>
CreateConditionsForKnownStep(
    const std::pair<const FreeForm2::Expression *,
                    const FreeForm2::Expression *> &p_range,
    const FreeForm2::Expression &p_step, const FreeForm2::Expression &p_loopVar,
    FreeForm2::SimpleExpressionOwner &p_owner,
    FreeForm2::TypeManager &p_typeManager) {
  using namespace FreeForm2;
  FF2_ASSERT(p_step.IsConstant() &&
             p_step.GetConstantValue().GetInt(p_step.GetType()) != 0);
  const Result::IntType stepVal =
      p_step.GetConstantValue().GetInt(p_step.GetType());
  const bool isIncreasing = stepVal > 0;

  // For increasing ranges, create the expression:
  // ((high - step < high) && (low + step > low) && high > low)
  // For decreasing ranges, create the expression:
  // ((high - step > high) && (low + step < low) && high < low)
  const Expression *precondition = nullptr;
  {
    const Expression &low = *p_range.first;
    const Expression &high = *p_range.second;
    auto highMinusStep = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), high, p_step, BinaryOperator::minus,
        p_typeManager);
    p_owner.AddExpression(highMinusStep);
    auto lowPlusStep =
        BinaryOperatorExpression::Alloc(p_loopVar.GetAnnotations(), low, p_step,
                                        BinaryOperator::plus, p_typeManager);
    p_owner.AddExpression(lowPlusStep);
    const BinaryOperator::Operation check1Op =
        isIncreasing ? BinaryOperator::lt : BinaryOperator::gt;
    auto check1 = BinaryOperatorExpression::Alloc(p_loopVar.GetAnnotations(),
                                                  *highMinusStep, high,
                                                  check1Op, p_typeManager);
    p_owner.AddExpression(check1);
    const BinaryOperator::Operation check2Op =
        isIncreasing ? BinaryOperator::gt : BinaryOperator::lt;
    auto check2 = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *lowPlusStep, low, check2Op, p_typeManager);
    p_owner.AddExpression(check2);
    const BinaryOperator::Operation rangeOp =
        isIncreasing ? BinaryOperator::gt : BinaryOperator::lt;
    auto rangeCheck = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), high, low, rangeOp, p_typeManager);
    p_owner.AddExpression(rangeCheck);
    auto and1 = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *check1, *check2, BinaryOperator::_and,
        p_typeManager);
    p_owner.AddExpression(and1);
    auto and2 = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), *and1, *rangeCheck, BinaryOperator::_and,
        p_typeManager);
    p_owner.AddExpression(and2);
    precondition = and2.get();
  }

  // For increasing ranges, create the expression: loopVar <= high - step.
  // For decreasing ranges, create the expression: loopVar >= high - step.
  const Expression *condition = nullptr;
  {
    const Expression &high = *p_range.second;
    auto highMinusStep = BinaryOperatorExpression::Alloc(
        p_loopVar.GetAnnotations(), high, p_step, BinaryOperator::minus,
        p_typeManager);
    p_owner.AddExpression(highMinusStep);
    const BinaryOperator::Operation compareOp =
        isIncreasing ? BinaryOperator::lte : BinaryOperator::gte;
    auto compare = BinaryOperatorExpression::Alloc(p_loopVar.GetAnnotations(),
                                                   p_loopVar, *highMinusStep,
                                                   compareOp, p_typeManager);
    p_owner.AddExpression(compare);
    condition = compare.get();
  }

  return boost::make_tuple(precondition, condition);
}
}  // namespace

FreeForm2::RangeReduceExpression::RangeReduceExpression(
    const Annotations &p_annotations, const Expression &p_low,
    const Expression &p_high, const Expression &p_initial,
    const Expression &p_reduce, VariableID p_stepId, VariableID p_reduceId)
    : Expression(p_annotations),
      m_low(p_low),
      m_high(p_high),
      m_initial(p_initial),
      m_reduce(p_reduce),
      m_stepId(p_stepId),
      m_reduceId(p_reduceId),
      m_type(InferType()) {}

const FreeForm2::TypeImpl &FreeForm2::RangeReduceExpression::InferType() const {
  if (!m_low.GetType().IsIntegerType() || !m_high.GetType().IsIntegerType()) {
    std::ostringstream err;
    err << "Expected low range and high range arguments to be of compatible "
           "integer types;"
        << " got " << m_low.GetType() << ", " << m_high.GetType()
        << " respectively.";
    throw ParseError(err.str(), GetSourceLocation());
  }

  if (!(m_initial.GetType().IsSameAs(m_reduce.GetType(), true))) {
    std::ostringstream err;
    err << "Expected initial reduction argument to range-reduce to "
           "be of the same type as the reduction expression.  Got "
        << m_initial.GetType() << " and " << m_reduce.GetType()
        << " respectively.";
    throw ParseError(err.str(), GetSourceLocation());
  }

  if (m_reduce.GetType().Primitive() == Type::Array) {
    std::ostringstream err;
    err << "An array cannot be the result of a looping expression, "
           "such as range-reduce, as our array representation "
           "relies on reusing array space (and thus uses constant "
           "space).  If arrays were the result of loops using this "
           "representation, dangling pointers would result.";
    throw ParseError(err.str(), GetSourceLocation());
  }
  return m_reduce.GetType().AsConstType();
}

size_t FreeForm2::RangeReduceExpression::GetNumChildren() const { return 4; }

void FreeForm2::RangeReduceExpression::Accept(Visitor &p_visitor) const {
  size_t stackSize = p_visitor.StackSize();

  if (!p_visitor.AlternativeVisit(*this)) {
    m_initial.Accept(p_visitor);
    m_high.Accept(p_visitor);
    m_low.Accept(p_visitor);
    m_reduce.Accept(p_visitor);

    p_visitor.Visit(*this);
  }

  FF2_ASSERT(p_visitor.StackSize() == stackSize + p_visitor.StackIncrement());
}

const FreeForm2::TypeImpl &FreeForm2::RangeReduceExpression::GetType() const {
  return m_type;
}

const FreeForm2::Expression &FreeForm2::RangeReduceExpression::GetLow() const {
  return m_low;
}

const FreeForm2::Expression &FreeForm2::RangeReduceExpression::GetHigh() const {
  return m_high;
}

const FreeForm2::Expression &FreeForm2::RangeReduceExpression::GetInitial()
    const {
  return m_initial;
}

const FreeForm2::Expression &
FreeForm2::RangeReduceExpression::GetReduceExpression() const {
  return m_reduce;
}

FreeForm2::VariableID FreeForm2::RangeReduceExpression::GetReduceId() const {
  return m_reduceId;
}

FreeForm2::VariableID FreeForm2::RangeReduceExpression::GetStepId() const {
  return m_stepId;
}

FreeForm2::ForEachLoopExpression::ForEachLoopExpression(
    const Annotations &p_annotations,
    const std::pair<const Expression *, const Expression *> &p_bounds,
    const Expression &p_next, const Expression &p_body, VariableID p_iteratorId,
    size_t p_version, LoopHint p_hint, TypeManager &p_typeManager)
    : Expression(p_annotations),
      m_begin(*p_bounds.first),
      m_end(*p_bounds.second),
      m_next(p_next),
      m_body(p_body),
      m_iteratorType(nullptr),
      m_iteratorId(p_iteratorId),
      m_version(p_version),
      m_hint(p_hint) {
  FF2_ASSERT(p_bounds.first && p_bounds.second);
  m_iteratorType = &TypeUtil::Unify(m_begin.GetType(), m_end.GetType(),
                                    p_typeManager, false, true);
  m_iteratorType = &TypeUtil::Unify(m_next.GetType(), *m_iteratorType,
                                    p_typeManager, false, true);

  if (!m_iteratorType->IsValid()) {
    std::ostringstream err;
    err << "For-each bounds must have unifiable types. Got "
        << m_begin.GetType() << ", " << m_end.GetType() << ", and "
        << m_next.GetType()
        << " for beginning, ending, and step values respectively.";
    throw ParseError(err.str(), GetSourceLocation());
  }
}

size_t FreeForm2::ForEachLoopExpression::GetNumChildren() const { return 4; }

void FreeForm2::ForEachLoopExpression::Accept(Visitor &p_visitor) const {
  size_t stackSize = p_visitor.StackSize();

  if (!p_visitor.AlternativeVisit(*this)) {
    m_begin.Accept(p_visitor);
    m_end.Accept(p_visitor);
    m_next.Accept(p_visitor);
    m_body.Accept(p_visitor);

    p_visitor.Visit(*this);
  }

  FF2_ASSERT(p_visitor.StackSize() == stackSize + p_visitor.StackIncrement());
}

const FreeForm2::TypeImpl &FreeForm2::ForEachLoopExpression::GetType() const {
  return TypeImpl::GetVoidInstance();
}

const FreeForm2::Expression &FreeForm2::ForEachLoopExpression::GetBegin()
    const {
  return m_begin;
}

const FreeForm2::Expression &FreeForm2::ForEachLoopExpression::GetEnd() const {
  return m_end;
}

const FreeForm2::Expression &FreeForm2::ForEachLoopExpression::GetNext() const {
  return m_next;
}

const FreeForm2::Expression &FreeForm2::ForEachLoopExpression::GetBody() const {
  return m_body;
}

const FreeForm2::TypeImpl &FreeForm2::ForEachLoopExpression::GetIteratorType()
    const {
  return *m_iteratorType;
}

FreeForm2::VariableID FreeForm2::ForEachLoopExpression::GetIteratorId() const {
  return m_iteratorId;
}

size_t FreeForm2::ForEachLoopExpression::GetVersion() const {
  return m_version;
}

FreeForm2::ForEachLoopExpression::LoopHint
FreeForm2::ForEachLoopExpression::GetHint() const {
  return m_hint;
}

FreeForm2::ComplexRangeLoopExpression::ComplexRangeLoopExpression(
    const Annotations &p_annotations,
    const std::pair<const Expression *, const Expression *> &p_range,
    const Expression &p_step, const Expression &p_body,
    const Expression &p_precondition, const Expression &p_loopCondition,
    const TypeImpl &p_stepType, VariableID p_stepId, size_t p_version)
    : Expression(p_annotations),
      m_low(*p_range.first),
      m_high(*p_range.second),
      m_step(p_step),
      m_body(p_body),
      m_precondition(p_precondition),
      m_loopCondition(p_loopCondition),
      m_stepType(p_stepType),
      m_stepId(p_stepId),
      m_version(p_version) {
  FF2_ASSERT(p_range.first && p_range.second);
  FF2_ASSERT(p_stepId != VariableID::c_invalidID);
  if (!p_range.first->GetType().IsIntegerType() ||
      !p_range.second->GetType().IsIntegerType() ||
      !p_step.GetType().IsIntegerType()) {
    std::ostringstream err;
    err << "Range bounds and step value must all be integral types. Got "
        << p_range.first->GetType() << ", " << p_range.second->GetType()
        << ", and " << p_step.GetType()
        << " for low, high, and step respectively.";
    throw ParseError(err.str(), GetSourceLocation());
  }

  if (p_precondition.GetType().Primitive() != Type::Bool ||
      p_loopCondition.GetType().Primitive() != Type::Bool) {
    std::ostringstream err;
    err << "Loop conditions must evaluate to boolean types. Got "
        << p_precondition.GetType() << " and " << p_loopCondition.GetType()
        << " for the precondition and loop condition respectively.";
    throw ParseError(err.str(), GetSourceLocation());
  }
}

const FreeForm2::ComplexRangeLoopExpression &
FreeForm2::ComplexRangeLoopExpression::Create(
    const Annotations &p_annotations,
    const std::pair<const Expression *, const Expression *> &p_range,
    const Expression &p_step, const Expression &p_body,
    const Expression &p_loopVar, VariableID p_stepId, size_t p_version,
    SimpleExpressionOwner &p_owner, TypeManager &p_typeManager) {
  FF2_ASSERT(p_range.first && p_range.second);
  const TypeImpl *stepType =
      &TypeUtil::Unify(p_range.first->GetType(), p_range.second->GetType(),
                       p_typeManager, false, true);
  stepType =
      &TypeUtil::Unify(*stepType, p_step.GetType(), p_typeManager, false, true);

  if (!p_range.first->GetType().IsIntegerType() ||
      !p_range.second->GetType().IsIntegerType() ||
      !p_step.GetType().IsIntegerType() ||
      !p_loopVar.GetType().IsIntegerType() ||
      !TypeUtil::IsAssignable(p_loopVar.GetType(), *stepType)) {
    std::ostringstream err;
    err << "Range bounds, step value, and loop variable must all be integral "
           "types. Got "
        << p_range.first->GetType() << ", " << p_range.second->GetType() << ", "
        << p_step.GetType() << ", and " << p_loopVar.GetType()
        << " for low, high, and step respectively.";
    throw ParseError(err.str(), p_annotations.m_sourceLocation);
  }

  const Expression *precondition = nullptr;
  const Expression *condition = nullptr;
  if (p_step.IsConstant()) {
    boost::tie(precondition, condition) = CreateConditionsForKnownStep(
        p_range, p_step, p_loopVar, p_owner, p_typeManager);
  } else {
    boost::tie(precondition, condition) = CreateGenericLoopConditions(
        p_range, p_step, p_loopVar, p_owner, p_typeManager);
  }

  FF2_ASSERT(precondition && condition);
  auto loop = boost::make_shared<ComplexRangeLoopExpression>(
      p_annotations, p_range, p_step, p_body, *precondition, *condition,
      p_loopVar.GetType(), p_stepId, p_version);
  p_owner.AddExpression(loop);
  return *loop;
}

size_t FreeForm2::ComplexRangeLoopExpression::GetNumChildren() const {
  return 6;
}

void FreeForm2::ComplexRangeLoopExpression::Accept(Visitor &p_visitor) const {
  size_t stackSize = p_visitor.StackSize();

  if (!p_visitor.AlternativeVisit(*this)) {
    m_precondition.Accept(p_visitor);
    m_low.Accept(p_visitor);
    m_high.Accept(p_visitor);
    m_step.Accept(p_visitor);
    m_body.Accept(p_visitor);
    m_loopCondition.Accept(p_visitor);

    p_visitor.Visit(*this);
  }

  FF2_ASSERT(p_visitor.StackSize() == stackSize + p_visitor.StackIncrement());
}

const FreeForm2::TypeImpl &FreeForm2::ComplexRangeLoopExpression::GetType()
    const {
  return TypeImpl::GetVoidInstance();
}

const FreeForm2::Expression &
FreeForm2::ComplexRangeLoopExpression::GetPrecondition() const {
  return m_precondition;
}

const FreeForm2::Expression &FreeForm2::ComplexRangeLoopExpression::GetLow()
    const {
  return m_low;
}

const FreeForm2::Expression &FreeForm2::ComplexRangeLoopExpression::GetHigh()
    const {
  return m_high;
}

const FreeForm2::Expression &FreeForm2::ComplexRangeLoopExpression::GetStep()
    const {
  return m_step;
}

const FreeForm2::Expression &FreeForm2::ComplexRangeLoopExpression::GetBody()
    const {
  return m_body;
}

const FreeForm2::Expression &
FreeForm2::ComplexRangeLoopExpression::GetLoopCondition() const {
  return m_loopCondition;
}

const FreeForm2::TypeImpl &FreeForm2::ComplexRangeLoopExpression::GetStepType()
    const {
  return m_stepType;
}

FreeForm2::VariableID FreeForm2::ComplexRangeLoopExpression::GetStepId() const {
  return m_stepId;
}

size_t FreeForm2::ComplexRangeLoopExpression::GetVersion() const {
  return m_version;
}


// THIDFaceDeepFeat.cpp : Defines the exported functions for the DLL application.
//

#include <cstring>
#include <map>
#include <string>
#include <vector>
#include <memory>
#include <fstream>

#define GLOG_NO_ABBREVIATED_SEVERITIES

#include "glog/logging.h"
#include <boost/algorithm/string.hpp>
#include "caffe/caffe.hpp"
#include "caffe/util/upgrade_proto.hpp"
#include <boost/algorithm/string.hpp>
#include "autoarray.h"
#include "caffe/face_recognition/face_recognition.hpp"
#include "AlgorithmUtils.h"

using caffe::Blob;
using caffe::Caffe;
using caffe::Net;
using caffe::Layer;
using caffe::shared_ptr;
using caffe::Timer;
using caffe::vector;

#ifndef _WIN32
#define _MAX_PATH 260
#endif

namespace
{
    char g_szFaceRecognitionSDKPath[_MAX_PATH] = { 0 };
}

int __stdcall InnerFaceRecognition(RecognitionHandle handle, const unsigned char *pNormImage, int batchSize, int channels,
    int imageHeight, int imageWidth, float *pFeatures)
{
    int nRet = 0;
    try
    {
        Net<float> *pCaffeNet = reinterpret_cast<Net<float> *>(handle);
        int length = batchSize * channels * imageHeight * imageWidth;
        AutoArray<float> normRealImage(length);
        for (int i = 0; i < batchSize; ++i)
        {
            for (int j = 0; j < channels; ++j)
            {
                for (int h = 0; h < imageHeight; ++h)
                {
                    for (int w = 0; w < imageWidth; ++w)
                    {
                        int index = i * channels * imageHeight * imageWidth + j * imageHeight * imageWidth + h * imageWidth + w;
                        int ori_idx = i * channels * imageHeight * imageWidth + h * imageWidth * channels + w * channels + j;
                        normRealImage[index] = (static_cast<float>(pNormImage[ori_idx]) - 127.5) * 0.0078125;
                    }

                }
            }
        }

        std::vector<caffe::Blob<float>*> bottom_vec;
        bottom_vec.push_back(new caffe::Blob<float>);
        bottom_vec[0]->Reshape(batchSize, channels, imageHeight, imageWidth);
        bottom_vec[0]->set_cpu_data(normRealImage);

        float iter_loss;
        const vector<Blob<float>*>& result = pCaffeNet->Forward(bottom_vec, &iter_loss);

        for (int i = 0; i < result[0]->count(); ++i)
        {
            pFeatures[i] = result[0]->cpu_data()[i];
        }

        delete bottom_vec[0];
    }
    catch (const std::bad_alloc &)
    {
        nRet = -2;
    }
    catch (const int &errCode)
    {
        nRet = errCode;
    }
    catch (...)
    {
        nRet = -3;
    }

    return nRet;
}

int __stdcall SetFaceRecognitionLibPath(const char *szLibPath)
{
	if (szLibPath == NULL)
		return -1;

#ifdef _WIN32
	strcpy_s(g_szFaceRecognitionSDKPath, _MAX_PATH, szLibPath);
#else
    strncpy(g_szFaceRecognitionSDKPath, szLibPath, _MAX_PATH);
#endif

	size_t len = strlen(g_szFaceRecognitionSDKPath);
	if (len != 0)
	{
	#ifdef _WIN32
		if (g_szFaceRecognitionSDKPath[len - 1] != '\\')
			strcat_s(g_szFaceRecognitionSDKPath, "\\");
	#else
	    if (g_szFaceRecognitionSDKPath[len - 1] != '/')
	        strncat(g_szFaceRecognitionSDKPath, "/", _MAX_PATH);
	#endif
	}

	return 0;
}

int __stdcall InitFaceRecognition(const char *szResName, int gpuID,
    RecognitionHandle *pHandle)
{
	if (pHandle == NULL)
		return -1;

	// initialize deep face network
	*pHandle = NULL;
	std::locale::global(std::locale(""));

	int retValue = 0;

#ifndef _WIN32
	if (strlen(g_szFaceRecognitionSDKPath) == 0)
		strncpy(g_szFaceRecognitionSDKPath, "./", _MAX_PATH);
#endif

	try
	{
		std::string strDllPath;
		strDllPath = g_szFaceRecognitionSDKPath;
        strDllPath += szResName;

        std::fstream fileModel;
        fileModel.open(strDllPath.c_str(), std::fstream::in | std::fstream::binary);
        if (false == fileModel.is_open())
          return 1;

        fileModel.seekg(0, std::fstream::end);
        int dataSize = int(fileModel.tellg());
        fileModel.seekg(0, std::fstream::beg);

		//CMyFile fileModel(strDllPath.c_str(), CMyFile::modeRead);
		//int dataSize = static_cast<int>(fileModel.GetLength());
		AutoArray<char> encryptedData(dataSize);
		//fileModel.Read(encryptedData, dataSize);
		fileModel.read(encryptedData, dataSize);
		//fileModel.Close();
		fileModel.close();

		int *pBuffer = reinterpret_cast<int *>(encryptedData.begin());
		// encrypt data by shift left
		int numOfData = dataSize / sizeof(pBuffer[0]);
		for (int i = 0; i < numOfData; ++i)
		{
			int tempData = pBuffer[i];
			pBuffer[i] = hzx::ror(static_cast<unsigned int>(tempData),
                hzx::g_shiftBits);
		}

        const int modelnumber = pBuffer[0];
        std::vector<int> protoTxtLen, modelSize;
        for (int i = 0; i < modelnumber; ++i)
        {
            protoTxtLen.push_back(pBuffer[2 * i + 1]);
            modelSize.push_back(pBuffer[2 * i + 2]);
        }
        unsigned char *pDataBuf = reinterpret_cast<unsigned char *>(encryptedData.begin()) + sizeof(int) * (2 * modelnumber + 1);

		FLAGS_minloglevel = 2;	// INFO(=0)<WARNING(=1)<ERROR(=2)<FATAL(=3)

		// initialize network structure
#ifdef CPU_ONLY
        Caffe::set_mode(Caffe::CPU);
#else
        if (gpuID < 0)
            Caffe::set_mode(Caffe::CPU);
        else {
            Caffe::set_mode(Caffe::GPU);
            Caffe::SetDevice(gpuID);
        }
#endif
		caffe::NetParameter net_param;
        caffe::NetParameter weight_param;

		retValue = caffe::ReatNetParamsFromBuffer(
            pDataBuf, protoTxtLen[0], &net_param);
        CHECK_EQ(retValue, 0) << "Read net structure from buffer error, code: " << retValue;
        CHECK(caffe::UpgradeNetAsNeeded("<memory>", &net_param));
        retValue = caffe::ReatNetParamsFromBuffer(
            pDataBuf + protoTxtLen[0], modelSize[0], &weight_param);
        CHECK_EQ(retValue, 0) << "Read net structure from buffer error, code: " << retValue;
        CHECK(caffe::UpgradeNetAsNeeded("<memory>", &weight_param));

        net_param.mutable_state()->set_phase(caffe::TEST);
		Net<float> *pCaffeNet = new Net<float>(net_param);

		// initialize network parameters
		pCaffeNet->CopyTrainedLayersFrom(weight_param);
		*pHandle = reinterpret_cast<RecognitionHandle>(pCaffeNet);
	}
	catch (const std::bad_alloc &)
	{
		retValue = -2;
	}
	catch (const int &errCode)
	{
		retValue = errCode;
	}
	catch (...)
	{
		retValue = -3;
	}

	return retValue;
}

int __stdcall UninitFaceRecognition(RecognitionHandle handle)
{
	Net<float> *pCaffeNet = reinterpret_cast<Net<float> *>(handle);
	delete pCaffeNet;

	return 0;
}

int __stdcall GetFaceRecognitionSize(RecognitionHandle handle)
{
	Net<float> *pCaffeNet = reinterpret_cast<Net<float> *>(handle);
	const vector<Blob<float>*>& result = pCaffeNet->output_blobs();
	int len = result[0]->count() * sizeof(float);
	return len;
}


//
// Created by fjh on 17-11-3. Copyright (c) 2017 ThinkForce, Inc. All Rights Reserved
//

#include "layer/DetectionOutput_layer.h"
#include <boost/shared_ptr.hpp>
namespace mdl{
    DetectionOutput_layer::DetectionOutput_layer(const Json& config):Layer(config){
        auto &param=config["param"];
        num_classes_ = param["num_classes"].number_value();
        share_location_ = true;
        num_loc_classes_ = share_location_ ? 1 : num_classes_;
        background_label_id_ = param["background_label_id"].int_value();
        code_type_ = CodeType::PriorBoxParameter_CodeType_CENTER_SIZE;
        variance_encoded_in_target_ =
                param["variance_encoded_in_target"].bool_value();
        keep_top_k_ = param["keep_top_k"].int_value();
        confidence_threshold_ = param["confidence_threshold"].is_null() ?
                                -FLT_MAX:param["confidence_threshold"].number_value() ;
        // Parameters used in nms.
        nms_threshold_ = 0.45;//param["nms_threshold"].number_value();
        CHECK_GE(nms_threshold_, 0.) << "nms_threshold must be non negative.";
        eta_ = 1.0;
        //CHECK_GT(eta_, 0.);
        //CHECK_LE(eta_, 1.);
        top_k_ = -1;
        if (!param["top_k"].is_null()) {
            top_k_ = 400;//param["top_k"].number_value();
        }
        top_k_ = 400;

    }
    template <typename T>
    bool SortScorePairDescend(const std::pair<float, T>& pair1,
                              const std::pair<float, T>& pair2) {
        return pair1.first > pair2.first;
    }

// Explicit initialization.
    template bool SortScorePairDescend(const std::pair<float, int>& pair1,
                                       const std::pair<float, int>& pair2);
    template bool SortScorePairDescend(const std::pair<float, std::pair<int, int> >& pair1,
                                       const std::pair<float, std::pair<int, int> >& pair2);
    DetectionOutput_layer::~DetectionOutput_layer() {}
    void DetectionOutput_layer::forward(int numthread) {
        const Mtype* loc_data = _input[0]->get_data();
        const Mtype* conf_data = _input[1]->get_data();
        const Mtype* prior_data = _input[2]->get_data();
        const int num = _output[0]->dimension(0);
        num_priors_ = (_input[2]->dimension(2)) / 4;
        // Retrieve all location predictions.
        vector<LabelBBox> all_loc_preds;
        GetLocPredictions(loc_data, num, num_priors_, num_loc_classes_,
                          share_location_, &all_loc_preds);

        // Retrieve all confidences.
        std::vector<std::map<int, vector<float> > > all_conf_scores;
        GetConfidenceScores(conf_data, num, num_priors_, num_classes_,
                            &all_conf_scores);

        // Retrieve all prior bboxes. It is same within a batch since we assume all
        // images in a batch are of same dimension.
        vector<NormalizedBBox> prior_bboxes;
        vector<vector<float> > prior_variances;
        GetPriorBBoxes(prior_data, num_priors_, &prior_bboxes, &prior_variances);

        // Decode all loc predictions to bboxes.
        vector<LabelBBox> all_decode_bboxes;
        const bool clip_bbox = false;
        DecodeBBoxesAll(all_loc_preds, prior_bboxes, prior_variances, num,
                        share_location_, num_loc_classes_, background_label_id_,
                        code_type_, variance_encoded_in_target_, clip_bbox,
                        &all_decode_bboxes);

        int num_kept = 0;
        vector<map<int, vector<int> > > all_indices;
        for (int i = 0; i < num; ++i) {
            const LabelBBox& decode_bboxes = all_decode_bboxes[i];
            const map<int, vector<float> >& conf_scores = all_conf_scores[i];
            map<int, vector<int> > indices;
            int num_det = 0;
            for (int c = 0; c < num_classes_; ++c) {
                if (c == background_label_id_) {
                    // Ignore background class.
                    continue;
                }
                if (conf_scores.find(c) == conf_scores.end()) {
                    // Something bad happened if there are no predictions for current label.
                    LOG(FATAL) << "Could not find confidence predictions for label " << c;
                }
                const vector<float>& scores = conf_scores.find(c)->second;
                int label = share_location_ ? -1 : c;
                if (decode_bboxes.find(label) == decode_bboxes.end()) {
                    // Something bad happened if there are no predictions for current label.
                    LOG(FATAL) << "Could not find location predictions for label " << label;
                    continue;
                }
                const vector<NormalizedBBox>& bboxes = decode_bboxes.find(label)->second;
                ApplyNMSFast(bboxes, scores, confidence_threshold_, nms_threshold_, eta_,
                             top_k_, &(indices[c]));
                num_det += indices[c].size();
            }
            if (keep_top_k_ > -1 && num_det > keep_top_k_) {
                std::vector<std::pair<float, std::pair<int, int> > > score_index_pairs;
                for (map<int, vector<int> >::iterator it = indices.begin();
                     it != indices.end(); ++it) {
                    int label = it->first;
                    const vector<int>& label_indices = it->second;
                    if (conf_scores.find(label) == conf_scores.end()) {
                        // Something bad happened for current label.
                        LOG(FATAL) << "Could not find location predictions for " << label;
                        continue;
                    }
                    const vector<float>& scores = conf_scores.find(label)->second;
                    for (int j = 0; j < label_indices.size(); ++j) {
                        int idx = label_indices[j];
                        CHECK_LT(idx, scores.size());
                        score_index_pairs.push_back(std::make_pair(
                                scores[idx], std::make_pair(label, idx)));
                    }
                }
                // Keep top k results per image.
                std::sort(score_index_pairs.begin(), score_index_pairs.end(),
                          SortScorePairDescend<std::pair<int, int> >);
                score_index_pairs.resize(keep_top_k_);
                // Store the new indices.
                map<int, vector<int> > new_indices;
                for (int j = 0; j < score_index_pairs.size(); ++j) {
                    int label = score_index_pairs[j].second.first;
                    int idx = score_index_pairs[j].second.second;
                    new_indices[label].push_back(idx);
                }
                all_indices.push_back(new_indices);
                num_kept += keep_top_k_;
            } else {
                all_indices.push_back(indices);
                num_kept += num_det;
            }
        }

        vector<int> top_shape(2, 1);
        top_shape.push_back(num_kept);
        top_shape.push_back(7);
        Mtype* top_data;
        if (num_kept == 0) {
            LOG(INFO) << "Couldn't find any detections";
            top_shape[2] = num;
            _output[0]->resize(top_shape);
            top_data = _output[0]->get_data();
            for(int i=0;i<_output[0]->count(0);i++){
                top_data[i]=-1;
            }
            // Generate fake results per image.
            for (int i = 0; i < num; ++i) {
                top_data[0] = i;
                top_data += 7;
            }
        } else {
            _output[0]->resize(top_shape);
            top_data = _output[0]->get_data();
        }

        int count = 0;
        for (int i = 0; i < num; ++i) {
            const map<int, vector<float> >& conf_scores = all_conf_scores[i];
            const LabelBBox& decode_bboxes = all_decode_bboxes[i];
            for (map<int, vector<int> >::iterator it = all_indices[i].begin();
                 it != all_indices[i].end(); ++it) {
                int label = it->first;
                if (conf_scores.find(label) == conf_scores.end()) {
                    // Something bad happened if there are no predictions for current label.
                    LOG(FATAL) << "Could not find confidence predictions for " << label;
                    continue;
                }
                const vector<float>& scores = conf_scores.find(label)->second;
                int loc_label = share_location_ ? -1 : label;
                if (decode_bboxes.find(loc_label) == decode_bboxes.end()) {
                    // Something bad happened if there are no predictions for current label.
                    LOG(FATAL) << "Could not find location predictions for " << loc_label;
                    continue;
                }
                const vector<NormalizedBBox>& bboxes =
                        decode_bboxes.find(loc_label)->second;
                vector<int>& indices = it->second;
                for (int j = 0; j < indices.size(); ++j) {
                    int idx = indices[j];
                    top_data[count * 7] = i;
                    top_data[count * 7 + 1] = label;
                    top_data[count * 7 + 2] = scores[idx];
                    const NormalizedBBox& bbox = bboxes[idx];
                    top_data[count * 7 + 3] = bbox.xmin();
                    top_data[count * 7 + 4] = bbox.ymin();
                    top_data[count * 7 + 5] = bbox.xmax();
                    top_data[count * 7 + 6] = bbox.ymax();

                    ++count;
                }
            }

        }

    }
}


#pragma once
#include "field.hpp"
#include <boost/hana/tuple.hpp>

namespace reg
{
    template<std::uint8_t count, std::uint8_t stride> struct RepMask {};
    template<std::uint8_t count, std::uint8_t stride> struct RepLocation {};

    template<class _Location, class _Mask, class _RWPolicy, class _RepeatMask, class _RepeatLocation, class ValueType>
    struct MultiField;

    template<class T, class _Owner, class TOffset, TOffset baseOffset,
        T maskShift, std::uint8_t maskSize, class _RWPolicy,
        std::uint8_t maskCount, std::uint8_t maskStride,
        std::uint8_t locationCount, std::uint8_t locationStride, class ValueType>
    struct MultiField<
        FieldLocation<T, _Owner, FieldOffset<TOffset, baseOffset>>,
        BitMask<T, maskShift, maskSize>,
        _RWPolicy,
        RepMask<maskCount, maskStride>,
        RepLocation<locationCount, locationStride>,
        ValueType>
    {
        constexpr MultiField() = default;

        template<class U, U indx>
        constexpr auto operator[](hana::integral_constant<U, indx>) const
        {
            static_assert(indx < (maskCount*locationCount), "Bit mask index out of range");

            constexpr auto offset = integral_c<TOffset, baseOffset> + integral_c<TOffset, (indx / maskCount)> * integral_c<TOffset, locationStride>;
            constexpr auto mask = integral_c<T, maskShift> + integral_c<T, (indx % maskCount)> * integral_c<T, maskStride>;

            using Loc = FieldLocation<T, _Owner, FieldOffset<TOffset, hana::value(offset)>>;
            using Mask = BitMask<T, hana::value(mask), maskSize>;

            return Field<Loc, Mask, _RWPolicy, ValueType>{};
        }
    };

    template<class _Location, class _Mask, class _RepMask, class _RepLoc, class Type = typename _Location::Value>
    using RWMultiField = MultiField<_Location, _Mask, detail::RWPolicy, _RepMask, _RepLoc, Type>;

    template<class _Location, class _Mask, class _RepMask, class _RepLoc, class Type = typename _Location::Value>
    using ROMultiField = MultiField<_Location, _Mask, detail::ROPolicy, _RepMask, _RepLoc, Type>;

    template<class _Location, class _Mask, class _RepMask, class _RepLoc, class Type = typename _Location::Value>
    using WOMultiField = MultiField<_Location, _Mask, detail::WOPolicy, _RepMask, _RepLoc, Type>;
}


/****
   Copyright 2005-2007, Moshe Looks

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****/

#include "enf.h"
#include <boost/iterator/transform_iterator.hpp>

namespace rewrite {
  namespace enf {
    using namespace std;
    using namespace boost;
    using namespace id;

    bool consistent(const Set& s) {
      Set::const_iterator pos=find_if(s.begin(),s.end(),
				      bind(greater<Constraint>(),_1,0));
      return has_empty_intersection
	(make_transform_iterator(Set::reverse_iterator(pos),
				 negate<Constraint>()),
	 make_transform_iterator(s.rend(),negate<Constraint>()),pos,s.end());
    }

    bool reduce_to_elegance::and_cut(sib_it child) {
      bool adopted=false;
      for (sib_it gchild=child.begin();gchild!=child.end();)
	if (gchild.number_of_children()==1 && guard_set(*gchild).empty()) {
	  _tr.erase(_tr.flatten(gchild.begin()));
	  if (!adopted) //is child adopting a terminal 1-constrant AND, x?
	    for (sib_it x=gchild.begin();x!=gchild.end();++x)
	      if (x.begin()==x.end() && guard_set(*x).size()==1) {
		adopted=true;
		break;
	      }
	  gchild=_tr.erase(_tr.flatten(gchild));
	} else {
	  ++gchild;
	}
      return adopted;
    }
    void reduce_to_elegance::or_cut(sib_it current) {
      for (sib_it child=current.begin();child!=current.end();)
	if (child.number_of_children()==1) {
	  union2(guard_set(*current),guard_set(*child.begin()));
	  _tr.erase(_tr.flatten(child.begin()));
	  child=_tr.erase(_tr.flatten(child));
	} else {
	  ++child;
	}
    }

    Result reduce_to_elegance::reduce(pre_it current,
				      const Set& dominant,const Set& command) {
      if (is_and(*current)) {
	difference2(guard_set(*current),dominant);

	set_difference
	  (guard_set(*current),
	   make_transform_iterator(command.rbegin(),negate<Constraint>()),
	   make_transform_iterator(command.rend(),negate<Constraint>()));

	if (current.begin()==current.end() && guard_set(*current).empty())
	  return Disconnect; //0subsume

	if (!has_empty_intersection(guard_set(*current).begin(),
				    guard_set(*current).end(),
				    command.begin(),command.end()))
	  return Delete;     //1subsume

	Set prev_guard_set;
	do {
	  prev_guard_set=guard_set(*current);

	  Set handle_set;
	  set_union(dominant.begin(),dominant.end(),
		    guard_set(*current).begin(),guard_set(*current).end(),
		    inserter(handle_set,handle_set.begin()));

	  if (!consistent(handle_set))
	    return Delete;

	  for (sib_it child=current.begin();child!=current.end();) {
	    switch(reduce(child,handle_set,command)) {
	    case Delete:
	      return Delete; //since current is in all selections with child
	    case Disconnect:
	      child=_tr.erase(child);
	      break;
	    case Keep:
	      assert(child.begin()!=child.end()); //not sure if this is ok..
	      //make result_set the union of child's children's guard sets
	      Set result_set(guard_set(*child.begin()));
	      for_each
		(++child.begin(),child.end(),
		 bind(intersection2<Set>,ref(result_set),bind(guard_set,_1)));
	      if (!result_set.empty()) {
		union2(guard_set(*current),result_set);
		for_each
		  (child.begin(),child.end(),
		   bind(difference2<Set>,bind(guard_set,_1),result_set));
		//try to apply and-cut to child's children
		and_cut(child.begin());
		//guard_set has been enlarged, so we need to reprocess all kids
		child=current.begin();
	      } else {
		if (!and_cut(child)) //if child is adopting a terminal
		  ++child;           //1-constrant AND, need to reprocess it
	      }
	    }
	  }
	  //try to apply or-cut to current's children
	  or_cut(current);
	} while (prev_guard_set!=guard_set(*current));
	if (current.begin()==current.end() && guard_set(*current).empty())
	  return Disconnect; //0-subsumption
      } else {
	assert(is_or(*current));

	for (sib_it child=current.begin();child!=current.end();) {
	  Set child_command(command);
	  for (sib_it sib=current.begin();sib!=current.end();++sib)
	    if (sib!=child && sib.begin()==sib.end() &&
		guard_set(*sib).size()==1)
	      child_command.insert(*guard_set(*sib).begin());
	  switch(reduce(child,dominant,child_command)) {
	  case Delete:
	    if (current.number_of_children()>1)
	      child=_tr.erase(child);
	    else
	      return Delete;
	    break;
	  case Disconnect:
	    return Disconnect;
	  case Keep:
	    ++child;
	  }
	}
      }
      return Keep;
    }
  } //~namespace enf
} //~namespace rewrite

std::ostream& operator<<(std::ostream& out,const rewrite::enf::Node& x) {
  if (x.first) {
    out << "[ ";
    for (rewrite::enf::Set::const_iterator it=x.second.begin();
	 it!=x.second.end();++it)
      out << *it << " ";
    out << "]";
  } else {
    out << "o";
  }
  return out;
}