#pragma once

#include <bunsan/curl/options/traits.hpp>

#include <curl/curl.h>

#include <boost/cast.hpp>

#include <chrono>

namespace bunsan::curl::options::wrapper {

template <typename Duration>
class duration;

template <typename Rep, typename Period>
class duration<std::chrono::duration<Rep, Period>> {
 public:
  using retention_policy = retention_policy::by_curl;

  using duration_type = std::chrono::duration<Rep, Period>;

  template <typename Rep_, typename Period_>
  explicit duration(const std::chrono::duration<Rep_, Period_> &duration)
      : m_data(std::chrono::duration_cast<duration_type>(duration)) {}

  explicit duration(const duration_type &duration) : m_data(duration) {}

  long data() const { return boost::numeric_cast<long>(m_data.count()); }

 private:
  duration_type m_data;
};

using seconds = duration<std::chrono::seconds>;
using milliseconds = duration<std::chrono::milliseconds>;

}  // namespace bunsan::curl::options::wrapper


//  Copyright (c) 2007-2016 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(HPX_PARALLEL_DATAPAR_BOOST_SIMD_COUNT_BITS_SEP_22_2016_0220PM)
#define HPX_PARALLEL_DATAPAR_BOOST_SIMD_COUNT_BITS_SEP_22_2016_0220PM

#include <hpx/config.hpp>

#if defined(HPX_HAVE_DATAPAR_BOOST_SIMD)
#include <cstddef>

#include <boost/simd.hpp>
#include <boost/simd/function/sum.hpp>

namespace hpx { namespace parallel { namespace traits
{
    ///////////////////////////////////////////////////////////////////////////
    template <typename T, std::size_t N, typename Abi>
    HPX_HOST_DEVICE HPX_FORCEINLINE std::size_t
    count_bits(boost::simd::pack<boost::simd::logical<T>, N, Abi> const& mask)
    {
        return boost::simd::sum(mask);
    }
}}}

#endif
#endif


#include <boost/test/included/unit_test.hpp>

#include <wuline.hpp>

using boost::unit_test::test_suite;
using namespace boost::gil;

//g++ wuline.tests.cpp -I /home/scott/project/boost_1_36_0 -I .

struct test1
{
	void operator()()
	{
		int width = 200;
		int height = 200;
		unsigned char* buffer = new unsigned char[width * height * 3];
		memset(buffer, 255, width * height * 3);
		rgb8_view_t view = interleaved_view(width,height,(rgb8_pixel_t*)buffer,width*3);

		double phase = 0;
		for (double theta = phase; theta < 360 + phase; theta += 10)
		{
			int x = (100.0*cos(theta*3.14/180.0)+100);
			int y = (-100*sin(theta*3.14/180.0)+100);
			wuline(view,rgb8_pixel_t(0,0,0),x,y,100,100);
		}

		FILE* fd = fopen("wuline.ppm", "wb");
		fprintf(fd, "P6\n# CREATOR: reportbase\n%d %d\n255\n", width, height);
		fwrite(buffer, 1, width*height*3, fd);
		fclose(fd);
		delete [] buffer;
	}
};

test_suite* init_unit_test_suite( int argc, char** argv)
{
	test_suite* test= BOOST_TEST_SUITE( "wuline tests" );
	test->add( BOOST_TEST_CASE(test1()), 0);
	return test;
}


#ifndef __VSIM_FRAME_HPP__
#define __VSIM_FRAME_HPP__

#include <Eigen/Geometry>

#include <vsim/env/pose.hpp>

namespace vsim {

struct Frame ;
typedef std::shared_ptr<Frame> FramePtr ;

struct Frame {
public:

    Frame() ;

    Eigen::Matrix4f transform() const {
        return pose_.absolute() ;
    }

    Pose pose_ ;
};

}
#endif


#include <boost/geometry/strategies/spherical/intersection.hpp>


// © 2016 PORT INC.

#ifndef W2V__H
#define W2V__H

#include <string>
#include <vector>
#include <map>
#include <boost/numeric/ublas/vector.hpp>

namespace W2V {

    using namespace boost::numeric::ublas;
    typedef boost::numeric::ublas::vector<double> vector;

 	class Matrix_ {
	public:
		Matrix_();
        virtual ~Matrix_();
		void read(const std::string& file);
		long long w2i(const std::string& w);
		const vector& i2v(int i);
		long long getNumWords() { return words; }
		long long getSize() { return size; }
    private:
		long long words;
		long long size;
        std::map<std::string, long long> w2i_;
        std::map<long long, std::string> i2w_;
        std::vector<vector> M;
	};

	typedef std::shared_ptr<Matrix_> Matrix;
}

#endif // W2V__H


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_IS_GEZ_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_IS_GEZ_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_predicates
 * \defgroup boost_simd_predicates_is_gez is_gez
 *
 * \par Description
 * Returns True<result_type>() or False<result_type>() according a0 is greater or equal to zero or not.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/is_gez.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::is_gez_(A0)>::type
 *     is_gez(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of is_gez
 *
 * \return an integer value
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 * This is a predicate operation. Such operations return bool in scalar mode,
 * but not in SIMD mode.
 * \par
 * The return type in SIMD mode is the common type of the parameters and is
 * a 'signed boolean' type. This means that in this case True has all its bits
 * sets to one. This is to facilitate masking operations. You are invited to
 * consult the rationale.
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag is_gez_ of functor is_gez
     *        in namespace boost::simd::tag for toolbox boost.simd.predicates
    **/
    struct is_gez_ : ext::elementwise_<is_gez_> { typedef ext::elementwise_<is_gez_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::is_gez_, is_gez, 1)
} }

#endif

// modified by jt the 25/12/2010


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file mpc_solver_XUopt.cpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#include <Eigen/SVD>

#include <vtr_path_tracker/robust_mpc/optimization/mpc_solver_XUopt.hpp>

#define COMPUTE_DW_EXTERNALLY
const bool USE_LOG_BARRIER_FUNC = false;
const bool USE_LIN_BARRIER_FUNC = true;
const bool USE_EXP_BARRIER_FUNC = false;

namespace vtr {
namespace path_tracker {

void print_mtx_str(Eigen::MatrixXf &mtx_in, int row_start, int col_start,
                   int rows, int cols, bool transpose) {
  CLOG(INFO, "path_tracker") << "Printing mtx (XUopt_impl): ";

  rows = std::min((int)mtx_in.rows(), row_start + rows);
  cols = std::min((int)mtx_in.cols(), col_start + cols);

  std::vector<float> failed_nums;

  if (!transpose) {
    for (int row = row_start; row < row_start + rows; row++) {
      for (int col = col_start; col < col_start + cols; col++) {
        char buffer[20];
        int length = std::snprintf(buffer, 20, "%.2f", mtx_in(row, col));
        if (length >= 19) {
          failed_nums.push_back(mtx_in(row, col));
          std::cout << "abc"
                    << ", ";
        } else {
          std::cout << buffer << ", ";
        }
      }
      std::cout << "\n";
    }
  } else {
    for (int col = col_start; col < col_start + cols; col++) {
      for (int row = row_start; row < row_start + rows; row++) {
        char buffer[20];
        int length = std::snprintf(buffer, 20, "%.2f", mtx_in(row, col));
        if (length >= 19) {
          failed_nums.push_back(mtx_in(row, col));
          std::cout << "abc"
                    << ", ";
        } else {
          std::cout << buffer << ", ";
        }
      }
      std::cout << "\n";
    }
  }

  if (!failed_nums.empty()) {
    CLOG(WARNING, "path_tracker")
        << "path tracker XU_opt_implementation:  Print mtx has elements that "
           "exceed the buffer size.";
    for (float &failed_num : failed_nums) {
      CLOG(INFO, "path_tracker") << failed_num;
    }
  }
}

float MpcSolverXUopt::getSign(float number) {
  return (float)((0.0 < number) - (number < 0.0));
}

MpcSolverXUopt::MpcSolverXUopt() { result_flgs.num_failed_opt_results = 0; }

MpcSolverXUopt::~MpcSolverXUopt() {}

void MpcSolverXUopt::set_cmd_km1(const float &u_km1_in, const float &v_km1_in) {
  u_km1 = u_km1_in;
  v_km1 = v_km1_in;
}

void MpcSolverXUopt::set_exp_consts(float c_in_new, float c_out_new) {
  c_in = c_in_new;
  c_out = c_out_new;
}

void MpcSolverXUopt::reset_solver_specific() {
  compute_mtx_offsets_and_sizes();

  reset_solver_variables();

  compute_weight_mtxs();

  x_lb_lims.setOnes(2, lookahead);
  x_ub_lims.setOnes(2, lookahead);
  x_lb_lims *= -50;
  x_ub_lims *= 50;

  step_size_ = 0.95;

  grad_L_im1.setZero(1, 1);
  ds_opt_im1.setZero(1, 1);
  slope_step.clear();
  slope_step1.clear();
  slope_step2.clear();

  iterations_small_step = 0;
}

void MpcSolverXUopt::compute_mtx_offsets_and_sizes() {
  // Compute the respective sizes
  size_x_vec = lookahead * size_x;
  size_u_vec = lookahead * size_u;
  size_v_vec = lookahead * size_v;
  size_y_vec = size_x_vec;

  // Compute the respective offset indices
  x_offset_index = 0;
  u_offset_index = x_offset_index + size_x_vec;
  v_offset_index = u_offset_index + size_u_vec;
  y_offset_index = v_offset_index + size_v_vec;

  // Compute the sizes and offset indices for variables involved in inequality
  // constraints
  if (opt_params.flg_en_mpcConstraints) {
    size_z_vec = 2 * lookahead * (size_x - 1) + 2;
    size_w_vec = size_z_vec;
    z_lb_offset_index = y_offset_index + size_y_vec;
    z_ub_offset_index = z_lb_offset_index + size_z_vec / 2;
    w_lb_offset_index = z_lb_offset_index + size_z_vec;
    w_ub_offset_index = w_lb_offset_index + size_w_vec / 2;
    mu_value = 1;
    mu_index = 0;

  } else {
    size_z_vec = 0;
    size_w_vec = 0;
    z_lb_offset_index = 0;
    z_ub_offset_index = 0;
    w_lb_offset_index = 0;
    w_ub_offset_index = 0;
    mu_value = 1;
    mu_index = 0;
  }
}

void MpcSolverXUopt::reset_solver_variables() {
  // Initialize desired and target vectors
  // target: the current sequence of states produced by the current sequence of
  // control inputs desired: the desired sequence of states and control inputs
  // (x_d, v_d)
  x_target.setZero(size_x_vec, 1);
  x_desired.setZero(size_x_vec, 1);
  v_desired.setZero(size_v_vec, 1);

  solution_result = SOLN_OK;

  // Initialize s_bar and grad_L (gradient of the Lagrangian w.r.t. s)
  int size_s_bar = size_x_vec + size_u_vec + size_v_vec + size_y_vec +
                   size_z_vec + size_w_vec;
  s_bar = Eigen::MatrixXf::Zero(size_s_bar, 1);

  // s_bar.block(u_offset_index,0,size_u_vec,1) =
  // 0.5*Eigen::MatrixXf::Ones(size_u_vec,1);
  grad_L = Eigen::MatrixXf::Zero(size_s_bar, 1);

  // Initialize Lagrange variables y and z)
  s_bar.block(y_offset_index, 0, size_y_vec + size_z_vec, 1) =
      0.1 * Eigen::MatrixXf::Ones(size_y_vec + size_z_vec, 1);

  // Initialize the slack variables w
  if (opt_params.flg_en_mpcConstraints) {
    s_bar.block(w_lb_offset_index, 0, size_w_vec, 1) =
        Eigen::MatrixXf::Ones(size_w_vec, 1);
  }

  x_minus_fx_ = Eigen::MatrixXf::Zero(size_y_vec, 1);
  gx_minus_w_ = Eigen::MatrixXf::Zero(size_w_vec, 1);

  // Initialize the variables used to track the creation of the sparse Jacobian
  // matrix
  J_grad_L_mtx_indices.clear();
  J_grad_L_mtx_entries_flg.clear();
  J_grad_L_mtx_indices.resize(size_s_bar * size_s_bar);
  J_grad_L_mtx_entries_flg.resize(size_s_bar * size_s_bar);
  for (unsigned i = 0; i < J_grad_L_mtx_entries_flg.size(); i++) {
    J_grad_L_mtx_entries_flg[i] = false;
  }

  norm_ds_opt.clear();
  norm_ds.clear();
  norm_grad_L_ds_opt_vec.clear();
}

void MpcSolverXUopt::compute_weight_mtxs() {
  int x_index, u_index, v_index;
  float wl = 2 * opt_params.weight_lat;
  float wlf = 2 * opt_params.weight_lat_final;
  float wh = 2 * opt_params.weight_head;
  float whf = 2 * opt_params.weight_head_final;
  float wu = 2 * opt_params.weight_u;
  float wv = 2 * opt_params.weight_v;

  weight_Q_triplet.clear();
  weight_Ru_triplet.clear();
  weight_Rv_triplet.clear();

  if (size_u_vec != size_v_vec) {
    CLOG(WARNING, "path_tracker")
        << "Expecting u and v vec of the same length "
           "(path_tracker_mpc_solver_XUopt_implementation.hpp).";
  }

  Eigen::MatrixXf dI_uv = Eigen::MatrixXf::Zero(size_u_vec, size_u_vec);
  Eigen::MatrixXf weight_du_vec(size_u_vec, 1);
  Eigen::MatrixXf weight_dv_vec(size_v_vec, 1);

  weight_Ru_diag.resize(size_u_vec, 1);
  weight_Rv_diag.resize(size_v_vec, 1);

  for (int index = 0; index < lookahead; index++) {
    x_index = size_x * index;
    u_index = size_u * index;
    v_index = size_v * index;

    if (index == lookahead - 1) {
      weight_Q_triplet.push_back(mtx_triplet(x_index, x_index, 1));
      weight_Q_triplet.push_back(mtx_triplet(x_index + 1, x_index + 1, wlf));
      weight_Q_triplet.push_back(mtx_triplet(x_index + 2, x_index + 2, whf));
    } else {
      weight_Q_triplet.push_back(mtx_triplet(x_index, x_index, 1));
      weight_Q_triplet.push_back(mtx_triplet(x_index + 1, x_index + 1, wl));
      weight_Q_triplet.push_back(mtx_triplet(x_index + 2, x_index + 2, wh));
    }

    weight_Ru_triplet.push_back(mtx_triplet(u_index, u_index, wu));
    weight_Rv_triplet.push_back(mtx_triplet(v_index, v_index, wv));

    weight_Ru_diag(index, 0) = wu;
    weight_Rv_diag(index, 0) = wv;

    /** Weighting to smooth inputs over time **/
    if (index == 0) {
      dI_uv(index, index) = 1;
    } else {
      dI_uv(index, index) = 1;
      dI_uv(index, index - 1) = -1;
    }
    weight_du_vec(index, 0) = opt_params.weight_du;
    weight_dv_vec(index, 0) = opt_params.weight_dv;
  }

  Eigen::MatrixXf u_0 = Eigen::MatrixXf::Zero(size_u_vec, 1);
  Eigen::MatrixXf v_0 = Eigen::MatrixXf::Zero(size_v_vec, 1);

  u_0(0, 0) = u_km1;
  v_0(0, 0) = v_km1;

  weight_u0 = -2 * dI_uv.transpose() * weight_du_vec.asDiagonal() * u_0;
  weight_v0 = -2 * dI_uv.transpose() * weight_dv_vec.asDiagonal() * v_0;

  // print_mtx_str(weight_u0,0,0,lookahead,1,true);

  dI_t_Rdu_dI = 2 * dI_uv.transpose() * weight_du_vec.asDiagonal() * dI_uv;
  dI_t_Rdv_dI = 2 * dI_uv.transpose() * weight_dv_vec.asDiagonal() * dI_uv;

  weight_Q.resize(size_x_vec, size_x_vec);
  populate_mtx(weight_Q, weight_Q_triplet);
}

void MpcSolverXUopt::compute_solver_update(const local_path_t &local_path,
                                           int iteration) {
  // Extract x_target, x_desired, x_lb, and x_ub from x_pred and local_path
  // Note: x_opt is constrained to be equal to x_target (dynamics) and weighted
  // to be close to x_desired
  extract_x(local_path);

  if (opt_params.flg_en_mpcConstraints) {
    compute_constraints_V2(local_path);
  }

  // Compose the gradient of the Lagrangian
  extract_grad_L();

  // Check progress relative to iteration i-1
  int size_grad_L = grad_L.rows();
  int size_grad_L_im1 = grad_L_im1.rows();

  if (size_grad_L == size_grad_L_im1) {
    // Compute change in slope
    Eigen::MatrixXf dx_im1 = grad_L_im1.transpose() * ds_opt_im1;
    Eigen::MatrixXf dx_i = grad_L.transpose() * ds_opt_im1;

    if (dx_i.rows() != 1 || dx_i.cols() != 1) {
      CLOG(WARNING, "path_tracker") << "Not singleton.";
    }

    float f_dx_im1 = fabs(dx_im1(0, 0));
    float f_dx_i = fabs(dx_i(0, 0));
    slope_step1.push_back(f_dx_i);
    slope_step2.push_back(f_dx_im1);

    slope_step.push_back(f_dx_i / f_dx_im1);

    int steps_for_done = 6;
    if (iterations_small_step < steps_for_done) {
      if (f_dx_im1 < 1) {
        iterations_small_step = iterations_small_step + 1;
      } else {
        iterations_small_step = 0;
      }
    } else {
      iterations_small_step = iterations_small_step + 1;
    }
  }

  if (iterations_small_step < 3) {
    // Compose the Jacobian of the gradient of the Lagrangian
    extract_J_grad_L(iteration);

    // Update the value of mu (used in constrained optimization)
    update_mu_value(mu_value);
  }
}

void MpcSolverXUopt::populate_mtx(Eigen::SparseMatrix<float, 0> &mtx_sm,
                                  mtx_triplet_list_t &triplet_list) {
  int num_elements = triplet_list.size();
  mtx_sm.reserve(num_elements);

  for (int element = 0; element < num_elements; element++) {
    if (triplet_list[element].i >= mtx_sm.rows() ||
        triplet_list[element].j >= mtx_sm.cols()) {
      CLOG(INFO, "path_tracker") << "Solver.populate_mtx:  Trying to add mtx "
                                    "triplet outside size of mtx.";
    } else {
      mtx_sm.insert(triplet_list[element].i, triplet_list[element].j) =
          triplet_list[element].v_ij;
    }
  }
}

MpcNominalModel::model_state_t *MpcSolverXUopt::select_x_pred(int index) {
  MpcNominalModel::model_state_t *x_pred_curr_index = &x_opt[index];
  return x_pred_curr_index;
}

MpcNominalModel::model_trajectory_t *MpcSolverXUopt::select_x_pred() {
  MpcNominalModel::model_trajectory_t *x_pred_ptr = &x_opt;
  return x_pred_ptr;
}

void MpcSolverXUopt::set_desired_speed_ctrl(int &index, const double &speed,
                                            const double &ctrl) {
  if (fabs(s_bar(v_offset_index + index, 0)) > 0 ||
      fabs(s_bar(v_offset_index + index, 0)) > 0) {
    CLOG(WARNING, "path_tracker")
        << "Overwriting initial (v,w) commands in MPC Solver.";
  }

  // Overwrite speed
  v_desired(index, 0) = (float)speed;
  s_bar(v_offset_index + index, 0) = (float)speed;
  x_opt[index].command_k[0] = s_bar(v_offset_index + index, 0);

  // Overwrite ctrl
  s_bar(u_offset_index + index, 0) = (float)ctrl;
  x_opt[index].command_k[1] = s_bar(u_offset_index + index, 0);
}

void MpcSolverXUopt::set_desired_speed(int &index, const double &speed) {
  v_desired(index, 0) = (float)speed;
  x_opt[index].command_k[0] = s_bar(v_offset_index + index, 0);
}

void MpcSolverXUopt::get_desired_ctrl(int &index) {
  x_opt[index].command_k[1] = s_bar(u_offset_index + index, 0);
}

void MpcSolverXUopt::post_process_x_pred(int &index) {
  x_opt[index + 1].var_x_k = x_pred[index + 1].var_x_k;
}

void MpcSolverXUopt::extract_x(const local_path_t &local_path) {
  int state_index;

  for (int index = 0; index < lookahead; index++) {
    state_index = size_x * index;
    x_target.block(state_index, 0, size_x, 1) = x_pred[index + 1].x_k;
    x_desired.block(state_index, 0, size_x, 1) =
        local_path.x_des_interp.block(0, index + 1, size_x, 1);
    if (opt_params.flg_en_mpcConstraints) {
      x_lb_lims.block(0, index, 2, 1) =
          local_path.x_lb_interp.block(0, index + 1, 2, 1);
      x_ub_lims.block(0, index, 2, 1) =
          local_path.x_ub_interp.block(0, index + 1, 2, 1);
    }
  }
}

void compute_C_0_k(Eigen::Matrix2f &C_0_k, const float &th_des) {
  /** Define the rotation mtx **/
  C_0_k << cos(th_des), -sin(th_des), sin(th_des), cos(th_des);
}

Eigen::MatrixXf compute_linear_abc(const Eigen::MatrixXf &point_1,
                                   const Eigen::MatrixXf &point_2,
                                   const Eigen::MatrixXf &des_point,
                                   const float &norm_dist) {
  Eigen::MatrixXf abc(3, 1), abc_temp(3, 1);
  /** a*x + b*y + c = 0 **/

  /** Compute the constants describing the line **/
  abc_temp << (point_2(1, 0) - point_1(1, 0)), (point_1(0, 0) - point_2(0, 0)),
      (point_2(0, 0) * point_1(1, 0) - point_1(0, 0) * point_2(1, 0));

  float test_out = abc_temp(0, 0) * des_point(0, 0) +
                   abc_temp(1, 0) * des_point(1, 0) + abc_temp(2, 0);
  bool test_out_gt_zero = test_out > 0.0f;

  if (!test_out_gt_zero) {
    abc = -abc_temp;
  } else {
    abc = abc_temp;
  }
  abc_temp = abc;

  /** Normalize the constants such that at norm_dist from the line, ax+by+c = 1
   * **/
  // 1) Compute a point norm_dist perpendicular from the line connecting point1
  // and point2
  float th_line =
      std::atan2(point_2(1, 0) - point_1(1, 0), point_2(0, 0) - point_1(0, 0));
  Eigen::MatrixXf C_line(2, 2);
  C_line << cos(th_line), -sin(th_line), sin(th_line), cos(th_line);
  Eigen::MatrixXf norm_point_des(2, 1);
  norm_point_des << 0, fabs(norm_dist);
  Eigen::MatrixXf norm_point_lineFrame = point_1 + C_line * norm_point_des;

  // 2) Compute the value of abc(norm_point_lineFrame), then divide the abc
  // constants
  if (fabs(norm_dist) > 0) {
    float norm_value = fabs(abc(0, 0) * norm_point_lineFrame(0, 0) +
                            abc(1, 0) * norm_point_lineFrame(1, 0) + abc(2, 0));
    abc = abc / fabs(norm_value);
  } else {
    CLOG(WARNING, "path_tracker")
        << "Path tracker XU Solver: Cannot normalize at dist 0.";
  }

  if (fabs(abc(0, 0)) > 5000 || fabs(abc(1, 0)) > 5000 ||
      fabs(abc(2, 0)) > 5000) {
    CLOG(WARNING, "path_tracker")
        << "Computation of abc results in large values (p_1,p_2,abc_temp/abc)";
    Eigen::MatrixXf test_out(2, 6);
    test_out << point_1, point_2, norm_point_lineFrame;
    test_out.block(0, 3, 1, 3) = abc_temp.transpose();
    test_out.block(1, 3, 1, 3) = abc.transpose();
    // print_mtx_str(test_out,0,0,2,6,false);
  }
  return abc;
}

Eigen::MatrixXf compute_bound_point(Eigen::MatrixXf &p_in, float &y_offset) {
  // Point (x,y coords) in frame of p_in
  Eigen::Matrix2f C_0_k;
  compute_C_0_k(C_0_k, p_in(2, 0));
  Eigen::MatrixXf p_offset(2, 1);
  p_offset << 0, y_offset;
  Eigen::MatrixXf p_out = p_in.block(0, 0, 2, 1) + C_0_k * p_offset;
  return p_out;
}

void MpcSolverXUopt::compute_constraints_V2(const local_path_t &local_path) {
  /** Prepare constraint coefficients
   * Lateral:
   *   a_lb*x + b_lb*y + c_lb > 0
   *   a_ub*x + b_ub*y + c_ub > 0
   *
   * Heading:
   *   (th_des - th) - eh_lb > 0
   *  -(th_des - th) + eh_ub > 0
   *
   **/

  // Initialize the constraints
  x_ub_abc.resize(5, lookahead);
  x_lb_abc.resize(5, lookahead);

  // Set to inf = Valgrind, Texas-style
  x_ub_abc = Eigen::MatrixXf::Ones(5, lookahead) / 0.0f;
  x_lb_abc = Eigen::MatrixXf::Ones(5, lookahead) / 0.0f;

  float c_mult = 1.0f;
  if (!opt_params.flg_en_robustMpcConstraints) {
    c_mult = 0.0f;
  }

  /** Check if nom/uncert pose meets constraints **/
  result_flgs.flg_nominal_pose_grossly_fails_constraints = false;
  if (x_ub_lims(0, 0) - x_opt[0].x_k[1] < -0.05 ||
      x_opt[0].x_k[1] - x_lb_lims(0, 0) < -0.05) {
    result_flgs.flg_nominal_pose_grossly_fails_constraints = true;
  }

  result_flgs.flg_nominal_pose_fails_constraints = false;
  if (x_ub_lims(0, 0) - x_opt[0].x_k[1] < 0 ||
      x_opt[0].x_k[1] - x_lb_lims(0, 0) < 0) {
    result_flgs.flg_nominal_pose_fails_constraints = true;
  }

  float ub_lim = x_ub_lims(0, 0) - c_mult * x_opt[0].lateral_uncertainty;
  float lb_lim = x_lb_lims(0, 0) + c_mult * x_opt[0].lateral_uncertainty;

  result_flgs.flg_uncertain_pose_fails_constraints = false;
  if (ub_lim - x_opt[0].x_k[1] < 0 || x_opt[0].x_k[1] - lb_lim < 0) {
    result_flgs.flg_uncertain_pose_fails_constraints = true;
  }

  /** Initialize temporary variables **/
  int k_i = 0;  // index variable
  // float distance = 0;
  float d_max = 1.2;  // max distance between constraint vertices, m
  Eigen::MatrixXf pose_1(3, 1),
      pose_2(3, 1);  // init varibles representing constraint vertices

  /** Compute point 1 and offsets from point 1 (ub, lb, mid) **/
  pose_1 << -getSign(v_desired(0, 0)) * (d_max + 0.001), 0,
      0;  // initialize pose_1 behind the robot
  float c_ub = x_ub_lims(0, 0) - c_mult * x_opt[0].lateral_uncertainty;
  float c_lb = x_lb_lims(0, 0) + c_mult * x_opt[0].lateral_uncertainty;
  float c_mid_1 = (c_ub + c_lb) / 2.0f;
  float c_mid_2 = c_mid_1;

  /** Ensure constraints at point 1 are a minimum of 0.05m apart **/
  float c_min = 0.05f;
  if (c_ub - c_lb < c_min) {
    c_ub = c_mid_1 + c_min / 2.0f;
    c_lb = c_mid_1 - c_min / 2.0f;
  }

  /** Compute constraint points **/
  Eigen::MatrixXf p_1_ub(2, 1), p_1_lb(2, 1), p_1_in(2, 1), p_2_ub(2, 1),
      p_2_lb(2, 1);
  p_1_ub = compute_bound_point(pose_1, c_ub);
  p_1_lb = compute_bound_point(pose_1, c_lb);
  p_1_in = compute_bound_point(pose_1, c_mid_1);

  test_out_ = Eigen::MatrixXf::Zero(8, lookahead + 1);
  test_out_(0, 0) = pose_1(0, 0);
  test_out_(1, 0) = pose_1(1, 0);
  test_out_(2, 0) = p_1_ub(0, 0);
  test_out_(3, 0) = p_1_ub(1, 0);
  test_out_(4, 0) = p_1_lb(0, 0);
  test_out_(5, 0) = p_1_lb(1, 0);
  test_out_(6, 0) = p_1_in(0, 0);
  test_out_(7, 0) = p_1_in(1, 0);

  Eigen::MatrixXf x_lb_abc_fixed(3, 1), x_ub_abc_fixed(3, 1);

  /**
        For the following:
        1) Compute pose 2 (pose_2)
        2) Compute offsets from pose 2 (c_ub, c_lb, c_mid_2)
        3) Compute constraint points (p_2_ub, p_2_lb)
        4) Compute upper limit represented by lines:
            - line a*x + b*y + c = 0, goes between p_1_ub, p_2_ub
            - select sign of a,b,c such that when evaluated at p_1_in, a*x+b*y+c
    > 0
            - repeat for lower limit
        5) Advance variables so that p_1... becomes the values from p_2
        6) Repeat
    **/

  int num_advances = 0;
  for (int i = 0; i < lookahead; i++) {
    // Increase c_min
    if (i == 1) {
      c_min = -0.3f;
    }

    // Get distance from last point
    pose_2 = local_path.x_des_interp.block(0, i + 1, 3, 1);

    c_ub = x_ub_lims(0, i) - c_mult * x_opt[i].lateral_uncertainty;
    c_lb = x_lb_lims(0, i) + c_mult * x_opt[i].lateral_uncertainty;
    c_mid_2 = (c_ub + c_lb) / 2.0f;

    Eigen::Matrix2f C_0_k;
    compute_C_0_k(C_0_k, pose_2(2, 0));
    Eigen::MatrixXf d_pose_k = C_0_k.transpose() * (pose_2.block(0, 0, 2, 1) -
                                                    pose_1.block(0, 0, 2, 1));

    bool flg_ovrlap_uncert = false;
    if (c_ub - c_lb < c_min) {
      c_ub = c_mid_2 + c_min / 2.0f;
      c_lb = c_mid_2 - c_min / 2.0f;
      flg_ovrlap_uncert = true;
    }

    if (fabs(d_pose_k(0, 0)) > d_max || i == lookahead - 1 ||
        flg_ovrlap_uncert) {
      test_out_(0, i + 1) = pose_2(0, 0);
      test_out_(1, i + 1) = pose_2(1, 0);

      if (fabs(d_pose_k(0, 0)) > 0.01) {
        // Compute point2 (ub and lb)
        p_2_lb = compute_bound_point(pose_2, c_lb);
        p_2_ub = compute_bound_point(pose_2, c_ub);
        test_out_(2, i + 1) = p_2_ub(0, 0);
        test_out_(3, i + 1) = p_2_ub(1, 0);
        test_out_(4, i + 1) = p_2_lb(0, 0);
        test_out_(5, i + 1) = p_2_lb(1, 0);
        test_out_(6, i + 1) = p_1_in(0, 0);
        test_out_(7, i + 1) = p_1_in(1, 0);

        // Compute constraints
        x_lb_abc_fixed =
            compute_linear_abc(p_1_lb, p_2_lb, p_1_in, opt_params.barrier_norm);
        x_ub_abc_fixed =
            compute_linear_abc(p_1_ub, p_2_ub, p_1_in, opt_params.barrier_norm);
      } else {
        x_lb_abc_fixed << 0.0f, 1.0f,
            -(x_lb_lims(0, i) + c_mult * x_opt[i].lateral_uncertainty);
        x_ub_abc_fixed << 0.0f, -1.0f,
            (x_ub_lims(0, i) - c_mult * x_opt[i].lateral_uncertainty);
        test_out_(2, i + 1) = x_lb_abc_fixed(2, 0);
        test_out_(3, i + 1) = x_ub_abc_fixed(2, 0);
        test_out_(4, i + 1) = 9.99;
        test_out_(5, i + 1) = 9.99;
        test_out_(6, i + 1) = 9.99;
        test_out_(7, i + 1) = 9.99;
      }

      // Check if we need to fill all remaining points
      bool flg_fill_rem_points = false;
      int term_pose = i;
      if (c_ub < c_lb) {
        flg_fill_rem_points = true;
        term_pose = lookahead - 1;
      }

      for (int j = k_i; j <= term_pose; j++) {
        // Copy over xy constraints
        x_lb_abc.block(0, j, 3, 1) = x_lb_abc_fixed;
        x_ub_abc.block(0, j, 3, 1) = x_ub_abc_fixed;

        // Copy over theta constraints
        x_lb_abc(3, j) = 1.0f;
        x_lb_abc(4, j) = -local_path.x_des_interp(2, i + 1) - x_lb_lims(1, i);

        x_lb_abc(3, j) = 0.0f;
        x_lb_abc(4, j) = 3.0f;

        x_ub_abc(3, j) = -1.0f;
        x_ub_abc(4, j) = local_path.x_des_interp(2, i + 1) + x_ub_lims(1, i);

        x_ub_abc(3, j) = 0.0f;
        x_ub_abc(4, j) = 3.0f;

        k_i = k_i + 1;
      }

      if (flg_fill_rem_points) {
        break;
      } else {
        // Advance points
        num_advances = num_advances + 1;
        pose_1 = pose_2;
        c_mid_1 = c_mid_2;
        p_1_lb = p_2_lb;
        p_1_ub = p_2_ub;
        p_1_in = compute_bound_point(pose_1, c_mid_1);
      }
    }
  }

  /** Check that the final pose_2 is far enough forward **/
  float d2_xk = fabs(pose_2(0, 0) - x_opt[0].x_k[0]);
  float min_lookahead = 0.3;
  if (false && d2_xk < min_lookahead && num_advances <= 1) {
    CLOG(INFO, "path_tracker")
        << "Overwriting constraints for high uncert / slow speed.";
    // Compute pose 1
    pose_1 << -getSign(v_desired(0, 0)) * (d_max + 0.001), 0,
        0;  // initialize pose_1 behind the robot
    c_mid_1 = (x_ub_lims(0, 0) + x_lb_lims(0, 0)) / 2;
    c_ub = c_mid_1 + 0.05;
    c_lb = c_mid_1 - 0.05;
    p_1_ub = compute_bound_point(pose_1, c_ub);
    p_1_lb = compute_bound_point(pose_1, c_lb);
    p_1_in = compute_bound_point(pose_1, c_mid_1);

    pose_2(0, 0) = x_opt[0].x_k[0] + getSign(v_desired(0, 0)) * min_lookahead;
    c_mid_2 = (x_ub_lims(0, lookahead - 1) + x_lb_lims(0, lookahead - 1)) / 2;
    c_ub = c_mid_2 + 0.01;
    c_lb = c_mid_2 - 0.01;
    p_2_lb = compute_bound_point(pose_2, c_lb);
    p_2_ub = compute_bound_point(pose_2, c_ub);

    // Compute constraints
    x_lb_abc_fixed =
        compute_linear_abc(p_1_lb, p_2_lb, p_1_in, opt_params.barrier_norm);
    x_ub_abc_fixed =
        compute_linear_abc(p_1_ub, p_2_ub, p_1_in, opt_params.barrier_norm);

    for (int j = 0; j < lookahead; j++) {
      x_lb_abc.block(0, j, 3, 1) = x_lb_abc_fixed;
      x_ub_abc.block(0, j, 3, 1) = x_ub_abc_fixed;
    }
  }

  bool flg_nan = false;
  for (int col = 0; col < lookahead; col++) {
    for (int row = 0; row < 5; row++) {
      if (std::isnan(x_lb_abc(row, col)) || std::isnan(x_ub_abc(row, col)) ||
          fabs(x_lb_abc(row, col)) > 5000 || fabs(x_ub_abc(row, col)) > 5000) {
        flg_nan = true;
      }
    }
  }

  if (flg_nan) {
    CLOG(WARNING, "path_tracker")
        << "Computed constraints include nan or large value.";
    // print_mtx_str(x_lb_abc,0,0,5,lookahead,false);
    // print_mtx_str(x_ub_abc,0,0,5,lookahead,false);
  }
}

void MpcSolverXUopt::extract_grad_L() {
  grad_L.setZero(s_bar.rows(), 1);

  // dels_J
  Eigen::MatrixXf delx_J =
      weight_Q * (s_bar.block(x_offset_index, 0, size_x_vec, 1) - x_desired);
  Eigen::MatrixXf delu_J =
      weight_Ru_diag.cwiseProduct(
          s_bar.block(u_offset_index, 0, size_u_vec, 1)) +
      dI_t_Rdu_dI * s_bar.block(u_offset_index, 0, size_u_vec, 1) + weight_u0;
  Eigen::MatrixXf delv_J =
      weight_Rv_diag.cwiseProduct(
          s_bar.block(v_offset_index, 0, size_v_vec, 1) - v_desired) +
      dI_t_Rdv_dI * s_bar.block(v_offset_index, 0, size_v_vec, 1) + weight_v0;

  if (delu_J.rows() != lookahead * size_u || delu_J.cols() != 1) {
    CLOG(WARNING, "path_tracker")
        << "cwiseProduct failed to produce mtx with proper dimensions "
           "(path_tracker_mpc_solver_XUopt_implementation.hpp).";
  }

  // dels_yTh
  int state_index, state_indexM1, state_indexM2, u_index, u_indexM1, v_index,
      v_indexM1, z_lb_index, z_ub_index;
  Eigen::MatrixXf delx_yTh, delx_zTg, delu_yTh, delv_yTh, delv_zTg;
  delx_yTh.setZero(size_x_vec, 1);
  delu_yTh.setZero(size_u_vec, 1);
  delv_yTh.setZero(size_v_vec, 1);
  delx_zTg.setZero(size_x_vec, 1);
  delv_zTg.setZero(size_v_vec, 1);

  Eigen::MatrixXf y_i;
  y_i.setZero(size_x, 1);

  /**
    Notes
    State/Input: a_k = (x_k, x_km1, u_k, u_km1, v_k, v_km1) where x_k in R^n
    (i.e. x_k = (x_1, ... , x_n))

    Process model: x_kp1 = f(a_k)

    State optimization variables: (x_kp1, ... , x_kpKp1)

    Constraints: h_k = x_kp1 - f(a_k) = 0, k = 0..K-1

    Lagrange multipliers: y_k, k = 0..K-1

    Finally: yTh = y.transpose()*h;
    **/

  // index is the index of the mpc look-ahead
  for (int look_ahead_index = 0; look_ahead_index < lookahead;
       look_ahead_index++) {
    state_index = size_x * look_ahead_index;
    state_indexM1 = size_x * (look_ahead_index - 1);
    state_indexM2 = size_x * (look_ahead_index - 2);
    u_index =
        size_u * (look_ahead_index);  // does not include offset index because
                                      // it's going into temp vec first
    u_indexM1 = size_u * (look_ahead_index - 1);
    v_index = size_v * (look_ahead_index);
    v_indexM1 = size_v * (look_ahead_index - 1);
    z_lb_index = z_lb_offset_index + 1 + 2 * look_ahead_index;
    z_ub_index = z_ub_offset_index + 2 * look_ahead_index;

    // Get y_i
    y_i = s_bar.block(y_offset_index + state_index, 0, size_x, 1);

    delx_yTh.block(state_index, 0, size_x, 1) = y_i;
    delu_yTh.block(u_index, 0, size_u, 1) -=
        x_opt[look_ahead_index].grad_u * y_i;
    delv_yTh.block(v_index, 0, size_u, 1) -=
        x_opt[look_ahead_index].grad_v * y_i;

    if (look_ahead_index > 0) {
      delx_yTh.block(state_indexM1, 0, size_x, 1) -=
          x_opt[look_ahead_index].grad_x * y_i;
      delu_yTh.block(u_indexM1, 0, size_u, 1) -=
          x_opt[look_ahead_index].grad_ukm1 * y_i;
      delv_yTh.block(v_indexM1, 0, size_v, 1) -=
          x_opt[look_ahead_index].grad_vkm1 * y_i;
    }

    if (look_ahead_index > 1) {
      delx_yTh.block(state_indexM2, 0, size_x, 1) -=
          x_opt[look_ahead_index].grad_xkm1 * y_i;
    }

    if (opt_params.flg_en_mpcConstraints) {
      // grad_x
      delx_zTg(state_index, 0) =
          s_bar(z_lb_index, 0) * x_lb_abc(0, look_ahead_index) +
          s_bar(z_ub_index, 0) * x_ub_abc(0, look_ahead_index);
      delx_zTg(state_index + 1, 0) =
          s_bar(z_lb_index, 0) * x_lb_abc(1, look_ahead_index) +
          s_bar(z_ub_index, 0) * x_ub_abc(1, look_ahead_index);
      delx_zTg(state_index + 2, 0) =
          s_bar(z_lb_index + 1, 0) * x_lb_abc(3, look_ahead_index) +
          s_bar(z_ub_index + 1, 0) * x_ub_abc(3, look_ahead_index);
    }
  }

  Eigen::MatrixXf x_opt = s_bar.block(x_offset_index, 0, size_x_vec, 1);

  grad_L.block(x_offset_index, 0, size_x_vec, 1) = delx_J - delx_yTh - delx_zTg;
  grad_L.block(u_offset_index, 0, size_u_vec, 1) = delu_J - delu_yTh;
  grad_L.block(v_offset_index, 0, size_v_vec, 1) =
      delv_J - delv_yTh;  // - delv_zTg;
  x_minus_fx_ = x_opt - x_target;
  grad_L.block(y_offset_index, 0, size_y_vec, 1) = -(x_minus_fx_);

  if (opt_params.flg_en_mpcConstraints) {
    int state_K = size_x * (lookahead - 1);
    int z_K = (size_x - 1) * lookahead;

    // grad_x, init and term constraints
    grad_L(x_offset_index, 0) -=
        -s_bar(z_lb_offset_index, 0);  // x > -50, hard-coded
    grad_L(x_offset_index + state_K, 0) -=
        -s_bar(z_ub_offset_index + z_K, 0);  // x_opt(0,0) > -50, hard-coded

    // grad_z, init and term constraints
    grad_L(z_lb_offset_index, 0) =
        -(-s_bar(0, 0) + 50.0 -
          s_bar(w_lb_offset_index, 0));  // x_opt(0,0) > -50, hard-coded
    grad_L(z_ub_offset_index + z_K, 0) =
        -(-s_bar(state_K, 0) + 50.0 -
          s_bar(w_ub_offset_index + z_K, 0));  // x_opt(0,0) > -50, hard-coded

    int el_index_lb, eh_index_lb, el_index_ub, eh_index_ub, x_index;

    for (int index = 0; index < lookahead; index++) {
      x_index = index * size_x;
      el_index_lb = 1 + 2 * index;
      eh_index_lb = 1 + 2 * index + 1;
      el_index_ub = 2 * index;
      eh_index_ub = 2 * index + 1;

      float x_act = s_bar(x_index, 0);
      float y_act = s_bar(x_index + 1, 0);
      float th_act = s_bar(x_index + 2, 0);

      // grad_z, lower bound
      grad_L(z_lb_offset_index + el_index_lb, 0) =
          -(x_lb_abc(0, index) * x_act + x_lb_abc(1, index) * y_act +
            x_lb_abc(2, index) - s_bar(w_lb_offset_index + el_index_lb, 0));
      grad_L(z_lb_offset_index + eh_index_lb, 0) =
          -(x_lb_abc(3, index) * th_act + x_lb_abc(4, index) -
            s_bar(w_lb_offset_index + eh_index_lb, 0));

      // grad_z, upper bound
      grad_L(z_ub_offset_index + el_index_ub, 0) =
          -(x_ub_abc(0, index) * x_act + x_ub_abc(1, index) * y_act +
            x_ub_abc(2, index) - s_bar(w_ub_offset_index + el_index_ub, 0));
      grad_L(z_ub_offset_index + eh_index_ub, 0) =
          -(x_ub_abc(3, index) * th_act + x_ub_abc(4, index) -
            s_bar(w_ub_offset_index + eh_index_ub, 0));
    }

    Eigen::MatrixXf z_vec = s_bar.block(z_lb_offset_index, 0, size_z_vec, 1);
    Eigen::MatrixXf w_vec = s_bar.block(w_lb_offset_index, 0, size_w_vec, 1);

    // grad_w
    if (USE_LOG_BARRIER_FUNC) {
      grad_L.block(w_lb_offset_index, 0, size_w_vec, 1) =
          (z_vec.cwiseProduct(w_vec).array() - mu_value);

    } else if (USE_LIN_BARRIER_FUNC) {
      float a_lin = 400;
      float b_lin = 100;
      Eigen::MatrixXf b_w = b_lin * w_vec;

      for (int i = 0; i < size_w_vec; i++) {
        // Above and below 50, the sigmoid becomes NAN due to computation of
        // exp(+-50)
        b_w(i, 0) = std::min(10.0f, std::max(-10.0f, b_w(i, 0)));
      }

      exp_bw = (b_w).array().exp();
      Eigen::MatrixXf temp = Eigen::MatrixXf::Ones(size_w_vec, 1);
      f_1 = (temp + exp_bw).cwiseInverse();

      temp = f_1.array().pow(2);
      df_1 = -b_lin * exp_bw.cwiseProduct(temp);
      d2f_1 = -b_lin * b_lin * exp_bw.cwiseProduct(temp);
      d2f_1 -= 2 * b_lin * exp_bw.cwiseProduct(f_1.cwiseProduct(df_1));

      // Linear: cost = a_lin*w(i,0)*sigmoid(b_lin*w_vec(i,0))
      // grad_L.block(w_lb_offset_index,0, size_w_vec  ,1) =    z_vec +
      // (-a_lin*f_1-a_lin*w_vec.cwiseProduct(df_1)); d2_cost_w = -2*a_lin*df_1
      // - a_lin*w_vec.cwiseProduct(d2f_1);

      // Square: cost = a_lin*w(i,0)^2*sigmoid(b_lin*w_vec(i,0))
      temp = w_vec.array().square();
      grad_L.block(w_lb_offset_index, 0, size_w_vec, 1) =
          z_vec + 2 * a_lin * w_vec.cwiseProduct(f_1) +
          a_lin * temp.cwiseProduct(df_1);
      d2_cost_w = 2 * a_lin * f_1 + 4 * a_lin * w_vec.cwiseProduct(df_1) +
                  a_lin * temp.cwiseProduct(d2f_1);

      for (int i = 0; i < size_w_vec; i++) {
        // if (b_lin*w_vec(i,0) > 10){
        if (b_lin * w_vec(i, 0) > 0) {
          grad_L(w_lb_offset_index + i, 0) = z_vec(i, 0);
          d2_cost_w(i, 0) = 0.0f;
        } else if (b_lin * w_vec(i, 0) < -10) {
          grad_L(w_lb_offset_index + i, 0) =
              z_vec(i, 0) + 2 * a_lin * w_vec(i, 0);
          d2_cost_w(i, 0) = 2 * a_lin;
        }
      }

      // check for nan/inf
      bool flg_nan = false;
      for (int i = 0; i < size_w_vec; i++) {
        if (std::isnan(d2_cost_w(i, 0)) ||
            std::isnan(grad_L(w_lb_offset_index + i, 0))) {
          flg_nan = true;
        }
      }
      if (flg_nan) {
        CLOG(WARNING, "path_tracker") << "Detected nan in computation of "
                                         "barrier function, grad_L, d2_cost_w:";
        // print_mtx_str(grad_L,w_lb_offset_index,0,size_w_vec,1,true);
        // print_mtx_str(d2_cost_w,0,0,size_w_vec,1,true);
      }
    } else {
      Eigen::MatrixXf exp_c_in_w = (-c_in * w_vec).array().exp();
      grad_L.block(w_lb_offset_index, 0, size_w_vec, 1) =
          z_vec - c_out * c_in * exp_c_in_w;
    }
  }
}

void MpcSolverXUopt::extract_J_grad_L(int iteration) {
  J_grad_L_triplet_list.clear();

  Eigen::MatrixXf y_i;
  y_i.setZero(3, 1);

  int x_index, x_indexM1, x_indexM2, u_index, u_indexM1, v_index, v_indexM1,
      y_index;  //, z_index_lb, z_index_ub, w_index_lb, w_index_ub;

  int size_ds_bar = std::max(0, (int)s_bar.rows() - size_z_vec - size_w_vec);
  Eigen::MatrixXf J_grad_L;
  J_grad_L = Eigen::MatrixXf::Zero(size_ds_bar, size_ds_bar);

  mtx_triplet_list_t identity_mtx_3x3;
  identity_mtx_3x3.push_back(mtx_triplet(0, 0, 1.0f));
  identity_mtx_3x3.push_back(mtx_triplet(1, 1, 1.0f));
  identity_mtx_3x3.push_back(mtx_triplet(2, 2, 1.0f));

  bool transpose_true = true;

  // Create list of entries for the Jacobian
  for (int index = 0; index < lookahead; index++) {
    x_index = x_offset_index + size_x * index;
    x_indexM1 = x_offset_index + size_x * (index - 1);
    x_indexM2 = x_offset_index + size_x * (index - 2);
    u_index = u_offset_index + size_u * index;
    u_indexM1 = u_offset_index + size_u * (index - 1);
    v_index = v_offset_index + size_v * index;
    v_indexM1 = v_offset_index + size_v * (index - 1);
    y_index = y_offset_index + size_x * index;

    y_i = s_bar.block(y_offset_index + index * STATE_SIZE, 0, STATE_SIZE, 1);

    if (index > 0) {
      for (int j = 0; j < size_x; j++) {
        // Jx_(-gx)
        insert_triplet_list(J_grad_L, x_opt[index].Jx_gx[j], y_i(j, 0),
                            x_indexM1, x_indexM1);

        // Jv_(-gx)
        insert_triplet_list(J_grad_L, x_opt[index].Jv_gx[j], y_i(j, 0),
                            x_indexM1, v_index);

        // Jx_(-gv)
        insert_triplet_list(J_grad_L, x_opt[index].Jx_gv[j], y_i(j, 0), v_index,
                            x_indexM1);
      }
    }

    // Jy_(-gx)
    float mult = 1.0f;
    insert_triplet_list(J_grad_L, identity_mtx_3x3, mult, x_index, y_index,
                        -1.0f);
    if (index > 0) {
      insert_mtx(J_grad_L, x_opt[index].grad_x, x_indexM1, y_index);
    }
    if (index > 1) {
      insert_mtx(J_grad_L, x_opt[index].grad_xkm1, x_indexM2, y_index);
    }

    // Jy_(-gu)
    insert_mtx(J_grad_L, x_opt[index].grad_u, u_index, y_index);
    if (index > 0) {
      insert_mtx(J_grad_L, x_opt[index].grad_ukm1, u_indexM1, y_index);
    }

    // Jy_(-gv)
    insert_mtx(J_grad_L, x_opt[index].grad_v, v_index, y_index);
    if (index > 0) {
      insert_mtx(J_grad_L, x_opt[index].grad_vkm1, v_indexM1, y_index);
    }

    // Jx_(-gy)
    float sign = 1.0f;
    insert_triplet_list(J_grad_L, identity_mtx_3x3, mult, y_index, x_index,
                        -1.0f);
    if (index > 0) {
      insert_mtx(J_grad_L, x_opt[index].grad_x, y_index, x_indexM1, sign,
                 transpose_true);
    }
    if (index > 1) {
      insert_mtx(J_grad_L, x_opt[index].grad_xkm1, y_index, x_indexM2, sign,
                 transpose_true);
    }

    // Ju_(-gy)
    insert_mtx(J_grad_L, x_opt[index].grad_u, y_index, u_index, sign,
               transpose_true);
    if (index > 0) {
      insert_mtx(J_grad_L, x_opt[index].grad_ukm1, y_index, u_indexM1, sign,
                 transpose_true);
    }

    // Jv_(-gy)
    insert_mtx(J_grad_L, x_opt[index].grad_v, y_index, v_index, sign,
               transpose_true);
    if (index > 0) {
      insert_mtx(J_grad_L, x_opt[index].grad_vkm1, y_index, v_indexM1, sign,
                 transpose_true);
    }
  }

  float mult = 1.0f;
  insert_triplet_list(J_grad_L, weight_Q_triplet, mult, x_offset_index,
                      x_offset_index);
  insert_triplet_list(J_grad_L, weight_Ru_triplet, mult, u_offset_index,
                      u_offset_index);
  insert_triplet_list(J_grad_L, weight_Rv_triplet, mult, v_offset_index,
                      v_offset_index);

  insert_mtx(J_grad_L, dI_t_Rdu_dI, u_offset_index, u_offset_index);
  insert_mtx(J_grad_L, dI_t_Rdv_dI, v_offset_index, v_offset_index);

  Eigen::MatrixXf A_1, b_1, diag_A2, b_2, b_3;

  /** Compute dz and dw through back-substitution
   *  dw = A_1*dx + b_1
   *  dz = A_2*dw + b_2
   *     = A_2*A_1*dx + (A_2*b_1 + b_2)
   *     = A_3*dx + b_3
   *
   * where:
   *   A_1 = -Jx_gz
   *   b_1 = -gz
   *   A_2 = -(Jz_gw)^-1*(Jw_gw)
   *   b_2 = -(Jz_gw)^-1*(gw)
   */

  if (opt_params.flg_en_mpcConstraints) {
    /** Compute A_1 (for-loop) and b_1 **/
    A_1 = Eigen::MatrixXf::Zero(size_z_vec, size_x_vec);
    b_1 = -grad_L.block(z_lb_offset_index, 0, size_z_vec, 1);

    int index_z_lb, index_z_ub;
    Eigen::MatrixXf Jx_gz_k_lb(2, 3), Jx_gz_k_ub(2, 3);
    for (int index = 0; index < lookahead; index++) {
      x_index = size_x * index;
      index_z_lb = 1 + 2 * index;
      index_z_ub = size_z_vec / 2 + 2 * index;

      A_1(index_z_lb, x_index) = x_lb_abc(0, index);
      A_1(index_z_lb, x_index + 1) = x_lb_abc(1, index);
      A_1(index_z_lb + 1, x_index + 2) = x_lb_abc(3, index);

      A_1(index_z_ub, x_index) = x_ub_abc(0, index);
      A_1(index_z_ub, x_index + 1) = x_ub_abc(1, index);
      A_1(index_z_ub + 1, x_index + 2) = x_ub_abc(3, index);
    }

    // Initial and terminal conditions
    A_1(0, 0) = -1.0f;
    A_1(size_z_vec / 2 + lookahead * (size_x - 1), (lookahead - 1) * size_x) =
        -1.0f;

    /** Compute A_2 and b_2 **/
    if (USE_LOG_BARRIER_FUNC == true) {
      Eigen::MatrixXf diag_Jz_gw_INV =
          s_bar.block(w_lb_offset_index, 0, size_w_vec, 1).cwiseInverse();
      diag_A2 = -diag_Jz_gw_INV.cwiseProduct(
          s_bar.block(z_lb_offset_index, 0, size_w_vec, 1));
      b_2 = -diag_Jz_gw_INV.cwiseProduct(
          grad_L.block(w_lb_offset_index, 0, size_w_vec, 1));

    } else if (USE_LIN_BARRIER_FUNC == true) {
      diag_A2 = -d2_cost_w;
      b_2 = -grad_L.block(w_lb_offset_index, 0, size_w_vec, 1);

    } else {  // Use exponential barrier function
      Eigen::MatrixXf w_vec = s_bar.block(w_lb_offset_index, 0, size_w_vec, 1);
      Eigen::MatrixXf exp_c_in_w = (-c_in * w_vec).array().exp();
      diag_A2 = -c_out * c_in * c_in * exp_c_in_w;
      b_2 = -grad_L.block(w_lb_offset_index, 0, size_w_vec, 1);
    }

    b_3 = diag_A2.cwiseProduct(b_1) + b_2;

    /**
     * Now: dz = A_2*A_1*dx + A_2*b_1 + b_2
     *         = A_2*A_1*dx + b_3
     * And: Jx_gx*dx + Ju_gx*du + Jy_gx*dy + Jz_gx*dz + gx = 0
     *
     **/
    // Update Jz_gx
    float sign_pos = 1.0f;
    Eigen::MatrixXf Jx_gx_update =
        -A_1.transpose() * diag_A2.asDiagonal() * A_1;
    insert_mtx(J_grad_L, Jx_gx_update, x_offset_index, x_offset_index,
               sign_pos);

    // Update gx
    grad_L.block(0, 0, size_x_vec, 1) += -A_1.transpose() * b_3;
  }

  size_ds_bar = std::max(0, (int)s_bar.rows() - size_z_vec - size_w_vec);
  Eigen::MatrixXd ds_bar_dbl(size_ds_bar, 1);

  Eigen::SparseMatrix<double> J_grad_L_mtx_sm;
  J_grad_L_mtx_sm.resize(size_ds_bar, size_ds_bar);

  // Once triplet list is created, reset the index list that keeps track of
  // non-zero entries.
  reset_index_list(J_grad_L_triplet_list);

  // Convert triplet list to sparse matrix
  // populate_mtx(J_grad_L_mtx_sm, J_grad_L_triplet_list);

  //  Solve the newton step
  Eigen::MatrixXd J_grad_dm = J_grad_L.cast<double>();

  // Full piv LU
  Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(J_grad_dm);
  ds_bar_dbl = lu_decomp.solve(
      -1.0f * grad_L.block(0, 0, size_ds_bar, 1).cast<double>());

#ifdef COMPUTE_DW_EXTERNALLY
  // Copy out ds_bar
  Eigen::MatrixXf ds_opt(s_bar.rows(), 1);
  ds_opt.block(0, 0, ds_bar_dbl.rows(), 1) = ds_bar_dbl.cast<float>();

  // Compute dw and dz
  if (opt_params.flg_en_mpcConstraints) {
    ds_opt.block(w_lb_offset_index, 0, size_w_vec, 1) =
        A_1 * ds_opt.block(0, 0, size_x_vec, 1) + b_1;
    ds_opt.block(z_lb_offset_index, 0, size_z_vec, 1) =
        diag_A2.asDiagonal() *
            ds_opt.block(w_lb_offset_index, 0, size_w_vec, 1) +
        b_2;
  }
#else
  // Copy out ds_bar
  Eigen::MatrixXf ds_opt = ds_bar_dbl.cast<float>();
#endif

  step_size_ = 0.2;

  //  Compute step-size
  for (int index = 0; index < lookahead; index++) {
    compute_limiting_step_size(step_size_, s_bar(u_offset_index + index, 0),
                               ds_opt(u_offset_index + index, 0), -1.5f, 1.5f);
    compute_limiting_step_size(step_size_, s_bar(v_offset_index + index, 0),
                               ds_opt(v_offset_index + index, 0), -1.5f, 1.5f);
  }
  float w_min, z_min;
  if (USE_LOG_BARRIER_FUNC == true) {
    z_min = 0.0f;
    w_min = 0.001f;
  } else {
    z_min = 0.0f;
    w_min = -5000.0f;
  }

  if (opt_params.flg_en_mpcConstraints) {
    for (int index = 0; index < size_z_vec; index++) {
      compute_limiting_step_size(
          step_size_, s_bar(w_lb_offset_index + index, 0),
          ds_opt(w_lb_offset_index + index, 0), w_min, 5000.0f);
      compute_limiting_step_size(
          step_size_, s_bar(z_lb_offset_index + index, 0),
          ds_opt(z_lb_offset_index + index, 0), z_min, 5000.0f);
    }
  }

  if (iteration < 3) {
    step_size_ = 0.02;
  } else if (iteration < 6) {
    step_size_ = 0.05;
  } else {
    step_size_ = 0.2;
  }

  // Compute cos_th between gradient and ds_opt
  Eigen::MatrixXf norm2_grad_L = grad_L.transpose() * grad_L;
  Eigen::MatrixXf norm2_ds_opt = ds_opt.transpose() * ds_opt;
  Eigen::MatrixXf grad_L_ds_opt = grad_L.transpose() * ds_opt;

  if (norm2_grad_L.rows() != 1 || norm2_grad_L.cols() != 1) {
    CLOG(WARNING, "path_tracker") << "Not scalar.";
  }

  float f_norm_grad = norm2_grad_L(0, 0);
  float f_norm_ds = norm2_ds_opt(0, 0);
  float f_grad_L_ds_opt = grad_L_ds_opt(0, 0);
  float descent = f_grad_L_ds_opt / (f_norm_grad * f_norm_ds);

  norm_grad_L_ds_opt_vec.push_back(descent);

  Eigen::MatrixXf s_bar_init = s_bar;
  s_bar = s_bar + step_size_ * ds_opt;

  grad_L_im1 = grad_L;
  ds_opt_im1 = ds_opt;

  // Constrain updated values
  int theta_ind;
  for (int i = 0; i < lookahead; i++) {
    theta_ind = (i * 3) + 2;
    if (s_bar(theta_ind, 0) > 3.14159f) {
      s_bar(theta_ind, 0) = s_bar(theta_ind, 0) - 2 * 3.14159f;
    } else if (s_bar(theta_ind, 0) < -3.14159f) {
      s_bar(theta_ind, 0) = s_bar(theta_ind, 0) + 2 * 3.14159f;
    }
  }

  bool sign_flip = false;

  for (int i = 0; i < lookahead * CONTROL_SIZE; i++) {
    float v_max = fabs(v_desired(i));
    float v_min = 0.05;
    float w_max = opt_params.w_max;

    float w_cmd = s_bar(u_offset_index + i, 0);
    float v_cmd = s_bar(v_offset_index + i, 0);

    sign_flip = false;

    if (sign_flip) {
      // All the rest are incorrect?
      v_cmd = 0;
      w_cmd = 0;
      CLOG(WARNING, "path_tracker")
          << "MPC solver setting commands to zero. This should never happen.";
    } else if (getSign(v_cmd * v_desired(i)) < 0) {
      CLOG(DEBUG, "path_tracker")
          << "i: " << i << " v_cmd: " << v_cmd
          << "  v_desired: " << v_desired(i) << "  w_cmd: " << w_cmd;
      // Solution suggests driving in opposite direction
      v_cmd = getSign(v_desired(i)) * v_min;
      sign_flip = true;
      CLOG_EVERY_N(1, WARNING, "path_tracker")
          << "MPC solution suggested direction switch when none was requested. "
             "Setting speed to slow.";
      // w_cmd = -w_cmd;
    } else if (fabs(v_cmd) > v_min) {
      float des_curv = fabs(w_cmd / v_cmd);
      float lim_curv = fabs(w_max / v_max);

      if (des_curv > lim_curv && fabs(w_cmd) > w_max) {
        w_cmd = getSign(w_cmd) * w_max;
        v_cmd = getSign(v_cmd) * w_max / des_curv;
      } else if (des_curv <= lim_curv && fabs(v_cmd) > v_max) {
        v_cmd = getSign(v_cmd) * v_max;
        w_cmd = getSign(w_cmd) * v_max * des_curv;
      }
    }

    if (fabs(v_cmd) < 0.05) {
      v_cmd = getSign(v_desired(i)) * 0.05;
    }
    s_bar(u_offset_index + i, 0) = w_cmd;
    s_bar(v_offset_index + i, 0) = v_cmd;
  }

  if (sign_flip) {
    result_flgs.flg_des_vel_one_point_turn = true;
  }

  if (opt_params.flg_en_mpcConstraints) {
    for (int i = 0; i < size_z_vec; i++) {
      s_bar(w_lb_offset_index + i, 0) =
          std::max(w_min, (float)s_bar(w_lb_offset_index + i, 0));
      s_bar(z_lb_offset_index + i, 0) =
          std::max(z_min, (float)s_bar(z_lb_offset_index + i, 0));
    }
  }

  Eigen::MatrixXf delta = s_bar_init - s_bar;
  delta = delta.cwiseAbs();

  norm_ds_opt.push_back(
      delta.block(0, 0, size_x_vec + size_u_vec + size_v_vec, 1).sum());
  norm_ds.push_back(
      delta.block(0, 0, size_x_vec + size_u_vec + size_v_vec, 1).sum());

  /** Compute delta between x_pred and x_opt
   *  1) We optimize a cost function J(x_opt,u_opt), where x_opt and u_opt are
   *sequences of states/inputs respectively 2) We compute a sequence x_pred
   *using the learned process model, where x_pred_kp1 = f(x_opt_k, u_opt_k), k =
   *0..K-1 3) Through constraints, x_opt should equal x_pred
   **/

  if (size_x_vec > 0) {
    float max_delta = 0;
    float delta = 0;
    float delta_x_avg = 0;
    for (int i = 0; i < size_x_vec; i++) {
      delta = std::abs(s_bar(x_offset_index + i, 0) - x_target(i, 0));
      max_delta = std::max(delta, max_delta);

      delta_x_avg = delta_x_avg + delta;
    }
    delta_x_avg = delta_x_avg / size_x_vec;
    result_flgs.delta_x_pred_opt = delta_x_avg;

    if (delta_x_avg > 0.3f) {
      result_flgs.flg_x_opt_and_pred_dont_match = true;
    } else {
      result_flgs.flg_x_opt_and_pred_dont_match = false;
    }
  }

  // Copy x back into x_opt
  for (int i = 0; i < lookahead; i++) {
    int x_index = size_x * i;
    x_opt[i + 1].x_k = s_bar.block(x_index, 0, size_x, 1);
  }
}

void MpcSolverXUopt::compute_limiting_step_size(float &step_size, float &s_km1,
                                                float &ds, float lb, float ub) {
  float step_size_new;
  if (ds > 0) {
    float delta = s_km1 - lb;
    step_size_new = std::min(0.95f, std::max(0.1f, delta / ds));
  } else {
    float delta = s_km1 - ub;
    step_size_new = std::min(0.95f, std::max(0.1f, delta / ds));
  }
  step_size = std::min(step_size, step_size_new);
}

void MpcSolverXUopt::insert_mtx(mtx_triplet_list_t &triplet_list_out,
                                const Eigen::MatrixXf &gradients_in, int &i,
                                int &j, float sign /* = 1.0f */,
                                bool transpose /* = false */) {
  int num_rows = gradients_in.rows();
  int num_cols = gradients_in.cols();
  for (int col = 0; col < num_cols; col++) {
    for (int row = 0; row < num_rows; row++) {
      if (!transpose) {
        insert_triplet(triplet_list_out, row + i, col + j,
                       gradients_in(row, col), sign);
      } else {
        insert_triplet(triplet_list_out, col + i, row + j,
                       gradients_in(row, col), sign);
      }
    }
  }
}

void MpcSolverXUopt::insert_triplet_list(mtx_triplet_list_t &triplet_list_out,
                                         mtx_triplet_list_t &triplet_list_in,
                                         float &mult, int &i, int &j,
                                         float sign) {
  int i_new, j_new;
  float v_ij_new;

  for (unsigned triplet_ind = 0; triplet_ind < triplet_list_in.size();
       triplet_ind++) {
    i_new = triplet_list_in[triplet_ind].i + i;
    j_new = triplet_list_in[triplet_ind].j + j;
    v_ij_new = triplet_list_in[triplet_ind].v_ij * mult;

    insert_triplet(triplet_list_out, i_new, j_new, v_ij_new, sign);
  }
}

void MpcSolverXUopt::insert_triplet(mtx_triplet_list_t &triplet_list_out,
                                    const int &i, const int &j, float v_ij,
                                    float sign /* = 1.0f */) {
  int index = s_bar.size() * i + j;
  int num_elements = s_bar.size() * s_bar.size();

  if (index > num_elements - 1 || index < 0) {
    CLOG(INFO, "path_tracker")
        << "Solver.insert_mtx:  Trying to add to (" << i << ", " << j
        << ") but mtx is of size (" << (int)s_bar.size() << ", "
        << (int)s_bar.size() << ").";
    CLOG(INFO, "path_tracker")
        << "Solver.insert_mtx:  Trying to add to element " << (int)index
        << " but num_elements = " << num_elements;
  }

  if (J_grad_L_mtx_entries_flg[index]) {
    if (sign > 0) {
      v_ij = triplet_list_out[J_grad_L_mtx_indices[index]].v_ij + v_ij;
    } else {
      v_ij = triplet_list_out[J_grad_L_mtx_indices[index]].v_ij - v_ij;
    }
    triplet_list_out[J_grad_L_mtx_indices[index]].v_ij = v_ij;

  } else {
    J_grad_L_mtx_entries_flg[index] = true;
    J_grad_L_mtx_indices[index] = triplet_list_out.size();
    if (sign > 0) {
      triplet_list_out.push_back(mtx_triplet(i, j, v_ij));
    } else {
      triplet_list_out.push_back(mtx_triplet(i, j, -v_ij));
    }
  }
}

void MpcSolverXUopt::insert_mtx(Eigen::MatrixXf &mtx,
                                const Eigen::MatrixXf &gradients_in, int &i,
                                int &j, float sign /* = 1.0f */,
                                bool transpose /* = false */) {
  if (!transpose) {
    if (sign > 0) {
      mtx.block(i, j, gradients_in.rows(), gradients_in.cols()) += gradients_in;
    } else {
      mtx.block(i, j, gradients_in.rows(), gradients_in.cols()) -= gradients_in;
    }
  } else {
    if (sign > 0) {
      mtx.block(i, j, gradients_in.cols(), gradients_in.rows()) +=
          gradients_in.transpose();
    } else {
      mtx.block(i, j, gradients_in.cols(), gradients_in.rows()) -=
          gradients_in.transpose();
    }
  }
}

void MpcSolverXUopt::insert_triplet_list(Eigen::MatrixXf &mtx,
                                         mtx_triplet_list_t &triplet_list_in,
                                         float &mult, int &i, int &j,
                                         float sign) {
  int i_new, j_new;
  float v_ij_new;

  for (unsigned triplet_ind = 0; triplet_ind < triplet_list_in.size();
       triplet_ind++) {
    i_new = triplet_list_in[triplet_ind].i + i;
    j_new = triplet_list_in[triplet_ind].j + j;
    v_ij_new = triplet_list_in[triplet_ind].v_ij * mult;

    insert_triplet(mtx, i_new, j_new, v_ij_new, sign);
  }
}

void MpcSolverXUopt::insert_triplet(Eigen::MatrixXf &mtx, const int &i,
                                    const int &j, float v_ij,
                                    float sign /* = 1.0f */) {
  int index = s_bar.size() * i + j;
  int num_elements = s_bar.size() * s_bar.size();

  if (index > num_elements - 1 || index < 0) {
    CLOG(INFO, "path_tracker")
        << "Solver.insert_mtx:  Trying to add to (" << i << ", " << j
        << ") but mtx is of size (" << (int)s_bar.size() << ", "
        << (int)s_bar.size() << ").";
    CLOG(INFO, "path_tracker")
        << "Solver.insert_mtx:  Trying to add to element " << (int)index
        << " but num_elements = " << num_elements;
  }

  if (sign > 0) {
    mtx(i, j) += v_ij;
  } else {
    mtx(i, j) -= v_ij;
  }
}

void MpcSolverXUopt::reset_index_list(mtx_triplet_list_t &triplet_list_in) {
  int index;
  for (unsigned i = 0; i < triplet_list_in.size(); i++) {
    index = s_bar.size() * triplet_list_in[i].i + triplet_list_in[i].j;
    J_grad_L_mtx_entries_flg[index] = false;
  }
}

void MpcSolverXUopt::update_mu_value(float &mu_value_in) {
  mu_value_in = std::max(0.0001f, 0.25f * mu_value_in);
}

void MpcSolverXUopt::get_u(Eigen::MatrixXf &u_mat) {
  u_mat = s_bar.block(u_offset_index, 0, size_u_vec, 1);
}

void MpcSolverXUopt::get_v(Eigen::MatrixXf &v_mat) {
  v_mat = s_bar.block(v_offset_index, 0, size_v_vec, 1);
}

void MpcSolverXUopt::get_w(Eigen::MatrixXf &w_mat) {
  w_mat = s_bar.block(w_lb_offset_index, 0, size_w_vec, 1);
}

}  // namespace path_tracker
}  // namespace vtr


// Copyright Louis Dionne 2013-2017
// 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/hana/assert.hpp>
#include <boost/hana/contains.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/map.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/set.hpp>
namespace hana = boost::hana;

int main() {
    {
        auto set = hana::make_set(hana::int_c<1>, hana::integral_c<signed char, 'x'>);

        BOOST_HANA_CONSTANT_CHECK(hana::contains(set, hana::int_c<1>));
        BOOST_HANA_CONSTANT_CHECK(hana::contains(set, hana::integral_c<signed char, 'x'>));
    }

    {
        auto map = hana::make_map(
            hana::make_pair(hana::int_c<1>, 1),
            hana::make_pair(hana::integral_c<signed char, 'x'>, 'x')
        );

        BOOST_HANA_CONSTANT_CHECK(hana::contains(map, hana::int_c<1>));
        BOOST_HANA_CONSTANT_CHECK(hana::contains(map, hana::integral_c<signed char, 'x'>));
    }
}


#include <boost/random/variate_generator.hpp>


/* Boost test/add.cpp
 * test with symbolic operations if the addition algorithm is correct
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: add.cpp,v 1.3 2003/02/05 17:34:35 gmelquio Exp $
 */

#include <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/arith.hpp>
#include <boost/numeric/interval/rounding.hpp>
#include <boost/numeric/interval/rounded_arith.hpp>
#include <boost/numeric/interval/utility.hpp>
#include <boost/numeric/interval/policies.hpp>
#include <boost/test/minimal.hpp>

typedef enum { EXPR_VAR, EXPR_NEG, EXPR_UP, EXPR_DOWN, EXPR_ADD, EXPR_SUB } e_type;

struct expr;
struct pexpr {
  expr *ptr;
  expr* operator->() { return ptr; }
  pexpr(expr *p = NULL): ptr(p) { }
};

struct expr {
  e_type type;
  int var;
  pexpr e;
  pexpr e1, e2;
};

pexpr var(int v) {
  pexpr e = new expr;
  e->type = EXPR_VAR;
  e->var = v;
  return e;
}

pexpr operator+(pexpr, pexpr);
pexpr operator-(pexpr, pexpr);
pexpr operator-(pexpr);

pexpr operator+(pexpr a, pexpr b) {
  if (a->type == EXPR_NEG) return b - a->e;
  if (b->type == EXPR_NEG) return a - b->e;
  if (a->type == EXPR_VAR && b->type == EXPR_VAR && a->var > b->var) return b + a;
  pexpr c = new expr;
  c->type = EXPR_ADD;
  c->e1 = a;
  c->e2 = b;
  return c;
}

pexpr operator-(pexpr a, pexpr b) {
  if (b->type == EXPR_NEG) return a + b->e;
  pexpr c = new expr;
  c->type = EXPR_SUB;
  c->e1 = a;
  c->e2 = b;
  return c;
}

pexpr down(pexpr a) {
  pexpr e = new expr;
  e->type = EXPR_DOWN;
  e->e = a;
  return e;
}

pexpr up(pexpr a) {
  pexpr e = new expr;
  e->type = EXPR_UP;
  e->e = a;
  return e;
}

pexpr operator-(pexpr a) {
  if (a->type == EXPR_NEG) return a->e;
  if (a->type == EXPR_UP) return down(-a->e);
  if (a->type == EXPR_DOWN) return up(-a->e);
  if (a->type == EXPR_SUB) return a->e2 - a->e1;
  if (a->type == EXPR_ADD) return -a->e1 - a->e2;
  pexpr e = new expr;
  e->type = EXPR_NEG;
  e->e = a;
  return e;
}

bool operator==(pexpr a, pexpr b) {
  if (a->type != b->type) return false;
  if (a->type == EXPR_VAR) return a->var == b->var;
  if (a->type == EXPR_DOWN || a->type == EXPR_UP || a->type == EXPR_NEG)
    return a->e == b->e;
  return a->e1 == b->e1 && a->e2 == b->e2;
}

bool operator<=(pexpr, pexpr) { return true; }

namespace boost {
namespace numeric {
namespace interval_lib {

template<>
struct rounding_control<pexpr> {
  typedef enum { RND_U, RND_M, RND_D } rounding_mode;
  static rounding_mode mode;
  rounding_control() { mode = RND_M; }
  void get_rounding_mode(rounding_mode& m) { m = mode; }
  void set_rounding_mode(rounding_mode m)  { mode = m; }
  void upward()   { mode = RND_U; }
  void downward() { mode = RND_D; }
  pexpr force_rounding(pexpr a) {
    switch (mode) {
    case RND_U: return up(a);
    case RND_D: return down(a);
    default: throw "Unset rounding mode";
    }
  }
};

rounding_control<pexpr>::rounding_mode rounding_control<pexpr>::mode = RND_M;

} // namespace interval_lib
} // namespace numeric
} // namespace boost

template<class I>
bool test_neg() {
  I a(var(0), var(1));
  return equal(-a, I(-var(1), -var(0)));
}

template<class I>
bool test_add() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a + b, I(down(var(0) + var(2)), up(var(1) + var(3))));
}

template<class I>
bool test_add1() {
  I a(var(0), var(1));
  return equal(a + var(2), I(down(var(0) + var(2)), up(var(1) + var(2))));
}

template<class I>
bool test_add2() {
  I a(var(0), var(1));
  return equal(var(2) + a, I(down(var(0) + var(2)), up(var(1) + var(2))));
}

template<class I>
bool test_sub() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a - b, I(down(var(0) - var(3)), up(var(1) - var(2))));
}

template<class I>
bool test_sub1() {
  I a(var(0), var(1));
  return equal(a - var(2), I(down(var(0) - var(2)), up(var(1) - var(2))));
}

template<class I>
bool test_sub2() {
  I a(var(0), var(1));
  return equal(var(2) - a, I(down(var(2) - var(1)), up(var(2) - var(0))));
}

template<class I>
bool test_addeq() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a += b, I(down(var(0) + var(2)), up(var(1) + var(3))));
}

template<class I>
bool test_addeq1() {
  I a(var(0), var(1));
  return equal(a += var(2), I(down(var(0) + var(2)), up(var(1) + var(2))));
}

template<class I>
bool test_subeq() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a -= b, I(down(var(0) - var(3)), up(var(1) - var(2))));
}

template<class I>
bool test_subeq1() {
  I a(var(0), var(1));
  return equal(a -= var(2), I(down(var(0) - var(2)), up(var(1) - var(2))));
}

struct my_checking
{
  static pexpr inf() { throw; }
  static pexpr nan() { throw; }
  static bool is_nan(const pexpr&) { return false; }
  static pexpr empty_lower() { throw; }
  static pexpr empty_upper() { throw; }
  static bool is_empty(const pexpr&, const pexpr&) { return false; }
};

template<class Rounding>
struct my_interval {
private:
  typedef boost::numeric::interval_lib::save_state<Rounding> my_rounding;
  typedef boost::numeric::interval_lib::policies<my_rounding, my_checking> my_policies;
public:
  typedef boost::numeric::interval<pexpr, my_policies> type;
};

int test_main(int, char *[]) {
  typedef my_interval<boost::numeric::interval_lib::rounded_arith_std<pexpr> >::type I1;
  typedef my_interval<boost::numeric::interval_lib::rounded_arith_opp<pexpr> >::type I2;
  BOOST_TEST((test_neg<I1>()));
  BOOST_TEST((test_neg<I2>()));
  BOOST_TEST((test_add<I1>()));
  BOOST_TEST((test_add<I2>()));
  BOOST_TEST((test_add1<I1>()));
  BOOST_TEST((test_add1<I2>()));
  BOOST_TEST((test_add2<I1>()));
  BOOST_TEST((test_add2<I2>()));
  BOOST_TEST((test_sub<I1>()));
  BOOST_TEST((test_sub<I2>()));
  BOOST_TEST((test_sub1<I1>()));
  BOOST_TEST((test_sub1<I2>()));
  BOOST_TEST((test_sub2<I1>()));
  BOOST_TEST((test_sub2<I2>()));
  BOOST_TEST((test_addeq<I1>()));
  BOOST_TEST((test_addeq<I2>()));
  BOOST_TEST((test_addeq1<I1>()));
  BOOST_TEST((test_addeq1<I2>()));
  BOOST_TEST((test_subeq<I1>()));
  BOOST_TEST((test_subeq<I2>()));
  BOOST_TEST((test_subeq1<I1>()));
  BOOST_TEST((test_subeq1<I2>()));
  return 0;
}


/*

Copyright (c) 2005-2016, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford 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.

*/

#ifndef AVERAGEDSOURCEPDE_HPP_
#define AVERAGEDSOURCEPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/vector.hpp>

#include "AbstractCellPopulation.hpp"
#include "TetrahedralMesh.hpp"
#include "AbstractLinearEllipticPde.hpp"

/**
 *  An Elliptic PDE which calculates the source term by adding the number of cells
 *  in the element containing that point and scaling by the element area.
 */
template<unsigned DIM>
class AveragedSourcePde : public AbstractLinearEllipticPde<DIM,DIM>
{
    friend class TestCellBasedPdes;

private:

    /** Needed for serialization.*/
    friend class boost::serialization::access;
    /**
     * Serialize the PDE and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
       archive & boost::serialization::base_object<AbstractLinearEllipticPde<DIM, DIM> >(*this);
       archive & mCoefficient;
       archive & mCellDensityOnCoarseElements;
    }

protected:

    /** The cell population member. */
    AbstractCellPopulation<DIM>& mrCellPopulation;

    /** Coefficient of consumption of nutrient by cells. */
    double mCoefficient;

    /** Vector of averaged cell densities on elements of the coarse mesh. */
    std::vector<double> mCellDensityOnCoarseElements;

public:

    /**
     * Constructor.
     *
     * @param rCellPopulation reference to the cell population
     * @param coefficient the coefficient of consumption of nutrient by cells (defaults to 0.0)
     */
    AveragedSourcePde(AbstractCellPopulation<DIM>& rCellPopulation, double coefficient=0.0);

    /**
     * @return const reference to the cell population (used in archiving).
     */
    const AbstractCellPopulation<DIM>& rGetCellPopulation() const;

    /**
     * @return mCoefficient
     */
    double GetCoefficient() const;

    /**
     * Set up the source terms.
     *
     * @param rCoarseMesh reference to the coarse mesh
     * @param pCellPdeElementMap optional pointer to the map from cells to coarse elements
     */
    void virtual SetupSourceTerms(TetrahedralMesh<DIM,DIM>& rCoarseMesh, std::map<CellPtr, unsigned>* pCellPdeElementMap=NULL);

    /**
     * Overridden ComputeConstantInUSourceTerm() method.
     *
     * @param rX The point in space
     * @param pElement the element
     *
     * @return the constant in u part of the source term, i.e g(x) in
     *  Div(D Grad u)  +  f(x)u + g(x) = 0.
     */
    double ComputeConstantInUSourceTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement);

    /**
     * Overridden ComputeLinearInUCoeffInSourceTerm() method.
     *
     * @param rX The point in space
     * @param pElement the element
     *
     * @return the coefficient of u in the linear part of the source term, i.e f(x) in
     *  Div(D Grad u)  +  f(x)u + g(x) = 0.
     */
    double ComputeLinearInUCoeffInSourceTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement);

    /**
     * Overridden ComputeDiffusionTerm() method.
     *
     * @param rX The point in space at which the diffusion term is computed
     *
     * @return a matrix.
     */
    c_matrix<double,DIM,DIM> ComputeDiffusionTerm(const ChastePoint<DIM>& rX);

    /**
     * @return the uptake rate.
     *
     * @param elementIndex the element we wish to return the uptake rate for
     */
    double GetUptakeRateForElement(unsigned elementIndex);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(AveragedSourcePde)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct an AveragedSourcePde.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const AveragedSourcePde<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM>* p_cell_population = &(t->rGetCellPopulation());
    ar & p_cell_population;
}

/**
 * De-serialize constructor parameters and initialise an AveragedSourcePde.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, AveragedSourcePde<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM>* p_cell_population;
    ar >> p_cell_population;

    // Invoke inplace constructor to initialise instance
    ::new(t)AveragedSourcePde<DIM>(*p_cell_population);
}
}
} // namespace ...

#endif /*AVERAGEDSOURCEPDE_HPP_*/


#include <iostream>
#include <memory>
#include <numeric>

#include <boost/simd/pack.hpp>
#include <boost/align/aligned_alloc.hpp>
#include <boost/align/aligned_delete.hpp>

namespace bs = boost::simd;
namespace ba = boost::alignment;

//! [iota-print]
template <typename T>
void iota_and_print()
{
  // The number of elements will be deduced automatically according to the
  // target architecture selected
  using pack_t = bs::pack<T>;

  // Allocates aligned memory using expected alignment
  std::unique_ptr<T[], ba::aligned_delete> data(
    (T*)(ba::aligned_alloc(pack_t::alignment, pack_t::static_size * sizeof(T))));
  std::iota(data.get(), data.get() + pack_t::static_size, T(1));

  // Constructs a pack (which will call `boost::simd::aligned_load<pack_t>` to
  // fill up its data)
  pack_t p{data.get()};

  // Now just print the loaded data
  std::cout << p << std::endl;
}

int main()
{
  iota_and_print<std::int8_t>();
  iota_and_print<std::uint8_t>();
  // --
  iota_and_print<std::int16_t>();
  iota_and_print<std::uint16_t>();
  // --
  iota_and_print<std::int32_t>();
  iota_and_print<std::uint32_t>();
  // --
  iota_and_print<std::int64_t>();
  iota_and_print<std::uint64_t>();
  // --
  iota_and_print<float>();
  // --
  iota_and_print<double>();
  return 0;
}
//! [iota-print]


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2015, Rice University
*  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 Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Mark Moll, Dave Coleman, Ioan Sucan */

#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/tools/thunder/Thunder.h>
#include <ompl/tools/lightning/Lightning.h>
#include <ompl/util/PPM.h>

#include <ompl/config.h>

#include <boost/filesystem.hpp>
#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace ot = ompl::tools;

class Plane2DEnvironment
{
public:

    Plane2DEnvironment(const char *ppm_file, bool useThunder = true)
    {
        bool ok = false;
        try
        {
            ppm_.loadFile(ppm_file);
            ok = true;
        }
        catch(ompl::Exception &ex)
        {
            OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
        }
        if (ok)
        {
            auto space(std::make_shared<ob::RealVectorStateSpace>());
            space->addDimension(0.0, ppm_.getWidth());
            space->addDimension(0.0, ppm_.getHeight());
            maxWidth_ = ppm_.getWidth() - 1;
            maxHeight_ = ppm_.getHeight() - 1;
            if (useThunder)
            {
                expPlanner_ = std::make_shared<ot::Thunder>(space);
                expPlanner_->setFilePath("thunder.db");
            }
            else
            {
                expPlanner_ = std::make_shared<ot::Lightning>(space);
                expPlanner_->setFilePath("lightning.db");
            }
            // set state validity checking for this space
            expPlanner_->setStateValidityChecker([this](const ob::State *state)
                { return isStateValid(state); });
            space->setup();
            expPlanner_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
            vss_ = expPlanner_->getSpaceInformation()->allocValidStateSampler();

            // DTC
            //experience_setup_->setPlanner(std::make_shared<og::RRTConnect>(si_));
            // Set the repair planner
            // experience_setup_->setRepairPlanner(std::make_shared<og::RRTConnect>(si_));
        }
    }

    ~Plane2DEnvironment()
    {
        expPlanner_->save();
    }

    bool plan()
    {
        std::cout << std::endl;
        std::cout << "-------------------------------------------------------" << std::endl;
        std::cout << "-------------------------------------------------------" << std::endl;

        if (!expPlanner_)
        {
            OMPL_ERROR("Simple setup not loaded");
            return false;
        }
        expPlanner_->clear();

        ob::ScopedState<> start(expPlanner_->getStateSpace());
        vss_->sample(start.get());
        ob::ScopedState<> goal(expPlanner_->getStateSpace());
        vss_->sample(goal.get());
        expPlanner_->setStartAndGoalStates(start, goal);

        bool solved = expPlanner_->solve(10.);
        if (solved)
            OMPL_INFORM("Found solution in %g seconds",
                expPlanner_->getLastPlanComputationTime());
        else
            OMPL_INFORM("No solution found");

        expPlanner_->doPostProcessing();

        return false;
    }

private:

    bool isStateValid(const ob::State *state) const
    {
        const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
        const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);

        const ompl::PPM::Color &c = ppm_.getPixel(h, w);
        return c.red > 127 && c.green > 127 && c.blue > 127;
    }

    ot::ExperienceSetupPtr expPlanner_;
    ob::ValidStateSamplerPtr vss_;
    int maxWidth_;
    int maxHeight_;
    ompl::PPM ppm_;
};

int main(int argc, char **)
{
    std::cout << "OMPL version: " << OMPL_VERSION << std::endl;

    boost::filesystem::path path(TEST_RESOURCES_DIR);
    Plane2DEnvironment env((path / "ppm" / "floor.ppm").string().c_str(), argc==1);

    for (unsigned int i = 0; i < 100; ++i)
        env.plan();

    return 0;
}


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

  Basic template class for reading and storing data from a config file in
  the basic UON (UltraOsteon) format (*.conf file).
  Supports Bo's vector and matrix types. Suppors Atk library types
  if the ATK_SUPPORTED directive is defined.

  Copyright (c) 2011
  Alena Bakulina <alena.bakulina@ziti.uni-heidelberg.de>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

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

#ifndef UON_BASIC_CONFIGURATION_HPP_7A47DDA3_838A_4177_AFD0_227E7B76FBAB_
#define UON_BASIC_CONFIGURATION_HPP_7A47DDA3_838A_4177_AFD0_227E7B76FBAB_

#include <vector>
#include <boost/algorithm/string/detail/trim.hpp>
#include <boost/algorithm/string/classification.hpp>

#include "bo/core/vector.hpp"
#include "bo/io/ini_reader.hpp"
#include "bo/io/config_parser/basic_configuration.hpp"
#include "bo/math/blas_extensions.hpp"

// Enable ATK usage only when requested by the library user.
#ifdef ATK_SUPPORTED
#include "utils/vec3.h"
#include "utils/vec4.h"
#include "utils/mat4.h"
#endif

/* Usage example:

    < ... >
    #define ATK_SUPPORTED       // define if you need an Atk data types support
    #include "uon_basic_configuration.hpp"
    < ... >

    < ... some function body ... >

    std::string filename = "..\\_data\\transform0001.conf";

    // Config Parser for Bo data types
    bo::io::config_parser::UonBasicConfig uc;
    uc.read_file(filename);
    uc.parse();

    // Read data from the parser
    // See UonBasicConfig definition for other datafields
    std::vector<bo::Vector<float, 3>> points_3d = uc.contour_points_3d;
    math::matrix<float> transform = uc.image_transformation;
    < ... do job with the data here ... >

    // Config Parser for Atk data types
    #ifdef ATK_SUPPORTED
    bo::io::config_parser::UonBasicConfigAtk uc_atk;
    uc_atk.read_file(filename);
    uc_atk.parse();
    // Read data from the parser
    std::vector<vec3f> points_3d_atk = uc_atk.contour_points_3d;
    mat4f transform_atk = uc_atk.image_transformation;
    < ... do job with the data here ... >
    #endif

    < ... >
}
*/

namespace bo {
namespace io {
namespace config_parser {

/* UON basic configuration format:
 UON basic configuration stores vectors and transformation matrices of float
 values. Vectors can be 2D or 3D, but all are stored in fields of a 3D vector type.
 In an ini file a vector is written in a format:
    <key_name> = [f f f] (for a 3D vector, or [f f] for a 2D case).
 Matrices are represented in a file by their's 3D column vectors, each column
 vector has it's own key in a file. Each matrix is defined by four vectors:
 rotational vectors by X, Y and Z and a translation vector.
*/

// Bo's standard matrix type.
typedef math::matrix<float> BoFloatMatrix;

// Namespace uon_config contains definitions of key names for UON configurations.
namespace uon_config {

// Symbols at the beginning and at the end of a value representing a vector (in an
// ini file).
static const Symbols VECTOR_TRIM_SYMBOLS = "[]";

// Names of the keys in a config file.
static const String CONTOUR_POINT_2D_NAME_PATTERN = "contourPoint\\d{4,}2d";
static const String CONTOUR_POINT_3D_NAME_PATTERN = "contourPoint\\d{4,}3d";
static const String IMAGE_TRANSFORM_ROT_X_NAME = "ImageTransform.Rot.x";
static const String IMAGE_TRANSFORM_ROT_Y_NAME = "ImageTransform.Rot.y";
static const String IMAGE_TRANSFORM_ROT_Z_NAME = "ImageTransform.Rot.z";
static const String IMAGE_TRANSFORM_TRANS_NAME = "ImageTransform.Trans";
static const String SUB_IMAGE_IN_PIX_COS_UL_NAME = "SubImageInPixCosUL";
static const String SUB_IMAGE_IN_PIX_COS_UR_NAME = "SubImageInPixCosUR";
static const String IMAGE_SCALING_NAME = "ImageScaling";
static const String WATER_MEAT_COMPENSATION_NAME = "WaterMeatCompensation";

} // namespace uon_config

/* UonBasicConfiguration_<> class is an abstract class which is used as an
 ancestor of UonBasicConfiguration<> class to provide sharing data fields for
 all specializations of UonBasicConfiguration<> without code repetition.
*/
template <class VectorType, class MatrixType>
class UonBasicConfiguration_ : public BasicConfiguration
{
public:
    UonBasicConfiguration_()
    { }

    virtual ~UonBasicConfiguration_()
    { }

    // Combines three rotational and one translation vector to one 4x4 matrix.
    virtual bool build_transformation_matrix(const VectorType& x_rotation,
                                             const VectorType& y_rotation,
                                             const VectorType& z_rotation,
                                             const VectorType& translation,
                                             MatrixType& transformation_matrix) const = 0;

    // Parses values read by the IniReader and stores in the correspondent data fields.
    virtual bool parse();

    // Data fields
    std::vector<VectorType> contour_points_2d;
    std::vector<VectorType> contour_points_3d;
    MatrixType image_transformation;
    VectorType sub_image_in_pix_cos_ul;
    VectorType sub_image_in_pix_cos_ur;
    VectorType image_scaling;
    VectorType water_meat_compensation;

protected:

    // Stores values in the input vector by parsing a string value from an ini file.
    // A key in taken from a default section and a default subsection.
    // If no such key exists, or value parsing fails, returns false.
    virtual bool parse_vector(const String& key_name, VectorType& vector) const;

    // Stores values in the input vector by parsing a string value from an ini file.
    // A key in taken from a 'section_name' section and a 'subsection_name' subsection.
    // Key's name should match the input key_name_pattern.
    // If no such key exists, or more than one key matches the input name,
    // or value parsing fails, returns false.
    virtual bool parse_vector_by_pattern(const String& section_name,
                                         const String& subsection_name,
                                         const String& key_name_pattern,
                                         VectorType& vector) const;

    // Get the value from IniReader by it's name and trims unnecessary symbols at
    // both sides. Default section and subsection are used. If no such value found,
    // return 'def_value'.
    virtual String prepare_parsed_vector(const String& key_name,
                                         const String& def_value) const;

    // Gets the value from IniReader by it's name and trims unnecessary symbols at
    // both sides. If no such value found, returns 'def_value'.
    virtual String prepare_parsed_vector(const String& section_name,
                                         const String& subsection_name,
                                         const String& key_name,
                                         const String& def_value) const;

    // Parses a string into a vector of floats and stores it in an input vector.
    // If parsing fails, returns false and doesn't change an input vector.
    // Input string is considered to be in a standard UON vector format
    // with VECTOR_TRIM_SYMBOLS at both sides and whitespace delimiters.
    virtual bool string_to_vector(const String& str, VectorType& vector) const = 0;

}; // class UonBasicConfiguration

// Basic template class for processing UON config format.
template <class VectorType, class MatrixType>
class UonBasicConfiguration : public UonBasicConfiguration_<VectorType, MatrixType>
{ };

// UonBasicConfiguration<> class specialization for Bo's types.
template <>
class UonBasicConfiguration<Vector<float, 3>, BoFloatMatrix> :
    public UonBasicConfiguration_<Vector<float, 3>, BoFloatMatrix>
{
public:
    UonBasicConfiguration()
    {   image_transformation.resize(4, 4);
    }

    virtual ~UonBasicConfiguration()
    { }

    virtual bool build_transformation_matrix(const Vector<float, 3>& x_rotation,
                                             const Vector<float, 3>& y_rotation,
                                             const Vector<float, 3>& z_rotation,
                                             const Vector<float, 3>& translation,
                                             BoFloatMatrix& transformation_matrix)
                                                const;
protected:
    virtual bool string_to_vector(const String& str, Vector<float, 3>& vector) const;
}; // class UonBasicConfiguration<Vector<float, 3>, BoFloatMatrix>

#ifdef ATK_SUPPORTED
template <>
class UonBasicConfiguration<vec3<float>, mat4<float>> :
    public UonBasicConfiguration_<vec3<float>, mat4<float>>
{
public:
    UonBasicConfiguration()
    {   image_transformation.setIdentity();
    };

    virtual bool build_transformation_matrix(const vec3<float>& x_rotation,
                                             const vec3<float>& y_rotation,
                                             const vec3<float>& z_rotation,
                                             const vec3<float>& translation,
                                             mat4<float>& transformation_matrix) const;
protected:
    virtual bool string_to_vector(const String& str, vec3<float>& vector) const;
}; // class UonBasicConfiguration<vec3<float>, mat4<float>>
#endif

// Typedefs of configurations.
typedef UonBasicConfiguration<Vector<float, 3>, BoFloatMatrix> UonBasicConfig;
#ifdef ATK_SUPPORTED
typedef UonBasicConfiguration<vec3<float>, mat4<float>> UonBasicConfigAtk;
#endif

//------------------------------------------------------------------------------------------------
// Implementation
//------------------------------------------------------------------------------------------------
template <class VectorType, class MatrixType>
bool UonBasicConfiguration_<VectorType, MatrixType>::parse()
{
    // Read ContourPoints 2D
    contour_points_2d.clear();
    Strings contour_points_2d_names = ini_reader_.get_key_names_by_pattern(
                uon_config::CONTOUR_POINT_2D_NAME_PATTERN);
    VectorType contour_point_2d;
    for (int i = 0; i < static_cast<int>(contour_points_2d_names.size()); ++i)
    {
        if (this->parse_vector(contour_points_2d_names[i], contour_point_2d) == true)
            contour_points_2d.push_back(contour_point_2d);
    }

    // Read ContourPoints 3D
    contour_points_3d.clear();
    Strings contour_points_3d_names = ini_reader_.get_key_names_by_pattern(
                uon_config::CONTOUR_POINT_3D_NAME_PATTERN);
    VectorType contour_point_3d;
    for (int i = 0; i < static_cast<int>(contour_points_3d_names.size()); ++i)
    {
        if (this->parse_vector(contour_points_3d_names[i], contour_point_3d) == true)
            contour_points_3d.push_back(contour_point_3d);
    }

    // Read ImageTransformation
    VectorType x_rotation, y_rotation, z_rotation, translation;
    this->parse_vector(uon_config::IMAGE_TRANSFORM_ROT_X_NAME, x_rotation);
    this->parse_vector(uon_config::IMAGE_TRANSFORM_ROT_Y_NAME, y_rotation);
    this->parse_vector(uon_config::IMAGE_TRANSFORM_ROT_Z_NAME, z_rotation);
    this->parse_vector(uon_config::IMAGE_TRANSFORM_TRANS_NAME, translation);
    this->build_transformation_matrix(x_rotation, y_rotation, z_rotation,
        translation, image_transformation);

    // Read SubImageInPixCosUL
    this->parse_vector(uon_config::SUB_IMAGE_IN_PIX_COS_UL_NAME, sub_image_in_pix_cos_ul);

    // Read SubImageInPixCosUR
    this->parse_vector(uon_config::SUB_IMAGE_IN_PIX_COS_UR_NAME, sub_image_in_pix_cos_ur);

    // Read ImageScaling
    this->parse_vector(uon_config::IMAGE_SCALING_NAME, image_scaling);

    // Read WaterMeatCompensation
    this->parse_vector(uon_config::WATER_MEAT_COMPENSATION_NAME, water_meat_compensation);

    return true;
}

template <class VectorType, class MatrixType>
bool UonBasicConfiguration_<VectorType, MatrixType>::
parse_vector(const String& key_name, VectorType& vector) const
{
    bool result = false;
    String default_value = "";
    String value = prepare_parsed_vector(key_name, default_value);
    if (value != default_value)
        result = this->string_to_vector(value, vector);

    return result;
}

template <class VectorType, class MatrixType>
bool UonBasicConfiguration_<VectorType, MatrixType>::
parse_vector_by_pattern(const String& section_name, const String& subsection_name,
                        const String& key_name_pattern, VectorType& vector) const
{
    bool result = false;
    String default_value = "";
    Strings key_names = ini_reader_.get_key_names_by_pattern(section_name,
        subsection_name, key_name_pattern);

    // Only one name should match the key_name_pattern, otherwise it is an error.
    if (key_names.size() == 1)
    {
        String value = this->prepare_parsed_vector(section_name, subsection_name,
                                                   key_names[0], default_value);
        if (value != default_value)
           result = this->string_to_vector(value, vector);
    }

    return result;
}

template <class VectorType,  class MatrixType>
String UonBasicConfiguration_<VectorType, MatrixType>::
prepare_parsed_vector(const String& key_name, const String& def_value) const
{
    String value = ini_reader_.get_value(key_name, def_value);
    boost::trim_if(value, boost::is_any_of(uon_config::VECTOR_TRIM_SYMBOLS));
    return value;
}

template <class VectorType,  class MatrixType>
String UonBasicConfiguration_<VectorType, MatrixType>::
prepare_parsed_vector(const String& section_name, const String& subsection_name,
                      const String& key_name, const String& def_value) const
{
    String value = ini_reader_.get_value(section_name, subsection_name,
                                         key_name, def_value);
    boost::trim_if(value, boost::is_any_of(uon_config::VECTOR_TRIM_SYMBOLS));
    return value;
}

inline
bool UonBasicConfiguration<Vector<float, 3>, BoFloatMatrix>::
build_transformation_matrix(const Vector<float, 3>& x_rotation,
                            const Vector<float, 3>& y_rotation,
                            const Vector<float, 3>& z_rotation,
                            const Vector<float, 3>& translation,
                            BoFloatMatrix& transformation_matrix) const
{
    bool result = false;

    if (x_rotation.size() == 3 && y_rotation.size() == 3 &&
        z_rotation.size() == 3 && translation.size() == 3)
    {
        transformation_matrix.resize(4, 4);

        transformation_matrix(0, 0) = x_rotation.x();
        transformation_matrix(1, 0) = x_rotation.y();
        transformation_matrix(2, 0) = x_rotation.z();
        transformation_matrix(3, 0) = 0.0;

        transformation_matrix(0, 1) = y_rotation.x();
        transformation_matrix(1, 1) = y_rotation.y();
        transformation_matrix(2, 1) = y_rotation.z();
        transformation_matrix(3, 1) = 0.0;

        transformation_matrix(0, 2) = z_rotation.x();
        transformation_matrix(1, 2) = z_rotation.y();
        transformation_matrix(2, 2) = z_rotation.z();
        transformation_matrix(3, 2) = 0.0;

        transformation_matrix(0, 3) = translation.x();
        transformation_matrix(1, 3) = translation.y();
        transformation_matrix(2, 3) = translation.z();
        transformation_matrix(3, 3) = 1.0;

        result = true;
    }

    return result;
}

inline
bool UonBasicConfiguration<Vector<float, 3>, BoFloatMatrix>::
string_to_vector(const String& str, Vector<float, 3>& vector) const
{
    float x, y, z;
    try
    {
        std::stringstream ss(str);
        ss >> x;
        ss >> y;
        ss >> z;
    }
    catch (...)
    {   return false;
    };

    vector.x() = x;
    vector.y() = y;
    vector.z() = z;
    return true;
}

#ifdef ATK_SUPPORTED
inline
bool UonBasicConfiguration<vec3<float>, mat4<float>>::
string_to_vector(const String& str, vec3<float>& vector) const
{
    float x, y, z;
    try
    {
        std::stringstream ss(str);
        ss >> x;
        ss >> y;
        ss >> z;
    }
    catch (...)
    {   return false;
    };

    vector.x = x;
    vector.y = y;
    vector.z = z;
    return true;
}

inline
bool UonBasicConfiguration<vec3<float>, mat4<float>>::
build_transformation_matrix(const vec3<float>& x_rotation,
                            const vec3<float>& y_rotation,
                            const vec3<float>& z_rotation,
                            const vec3<float>& translation,
                            mat4<float>& transformation_matrix) const
{
    transformation_matrix.x = vec4<float>(x_rotation, 0);
    transformation_matrix.y = vec4<float>(y_rotation, 0);
    transformation_matrix.z = vec4<float>(z_rotation, 0);
    transformation_matrix.w = vec4<float>(translation, 1);
    return true;
}
#endif

} // namespace config_parser
} // namespace io
} // namespace bo

#endif // UON_BASIC_CONFIGURATION_HPP_7A47DDA3_838A_4177_AFD0_227E7B76FBAB_