#ifndef STAN_MATH_PRIM_MAT_FUN_UNIT_VECTOR_CONSTRAIN_HPP
#define STAN_MATH_PRIM_MAT_FUN_UNIT_VECTOR_CONSTRAIN_HPP

#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/dot_self.hpp>
#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/rev/mat/fun/dot_self.hpp>
#include <cmath>

namespace stan {
  namespace math {

    namespace {
      class unit_vector_elt_vari : public vari {
      private:
        vari** y_;
        const double* unit_vector_y_;
        const int size_;
        const int idx_;
        const double norm_;

      public:
        unit_vector_elt_vari(double val,
                             vari** y,
                             const double* unit_vector_y,
                             int size,
                             int idx,
                             const double norm)
          : vari(val),
            y_(y),
            unit_vector_y_(unit_vector_y),
            size_(size),
            idx_(idx),
            norm_(norm) {
        }
        void chain() {
          const double cubed_norm = norm_ * norm_ * norm_;
          for (int m = 0; m < size_; ++m) {
            y_[m]->adj_
              -= adj_ * unit_vector_y_[m] * unit_vector_y_[idx_] / cubed_norm;
            if (m == idx_)
              y_[m]->adj_ +=  adj_ / norm_;
          }
        }
      };
    }

    /**
     * Return the unit length vector corresponding to the free vector y.
     * See https://en.wikipedia.org/wiki/N-sphere#Generating_random_points
     *
     * @param y vector of K unrestricted variables
     * @return Unit length vector of dimension K
     * @tparam T Scalar type.
     **/
    template <int R, int C>
    Eigen::Matrix<var, R, C>
    unit_vector_constrain(const Eigen::Matrix<var, R, C>& y) {
      check_vector("unit_vector", "y", y);
      check_nonzero_size("unit_vector", "y", y);

      vari** y_vi_array
        = reinterpret_cast<vari**>(ChainableStack::memalloc_
                                   .alloc(sizeof(vari*) * y.size()));
      for (int i = 0; i < y.size(); ++i)
        y_vi_array[i] = y.coeff(i).vi_;

      Eigen::VectorXd y_d(y.size());
      for (int i = 0; i < y.size(); ++i)
        y_d.coeffRef(i) = y.coeff(i).val();

      const double norm = y_d.norm();
      check_positive_finite("unit_vector", "norm", norm);
      Eigen::VectorXd unit_vector_d = y_d / norm;

      double* unit_vector_y_d_array
        = reinterpret_cast<double*>(ChainableStack::memalloc_
                                    .alloc(sizeof(double) * y_d.size()));
      for (int i = 0; i < y_d.size(); ++i)
        unit_vector_y_d_array[i] = unit_vector_d.coeff(i);

      Eigen::Matrix<var, R, C> unit_vector_y(y.size());
      for (int k = 0; k < y.size(); ++k)
        unit_vector_y.coeffRef(k)
          = var(new unit_vector_elt_vari(unit_vector_d[k],
                                         y_vi_array,
                                         unit_vector_y_d_array,
                                         y.size(),
                                         k,
                                         norm));
      return unit_vector_y;
    }

    /**
     * Return the unit length vector corresponding to the free vector y.
     * See https://en.wikipedia.org/wiki/N-sphere#Generating_random_points
     *
     * @param y vector of K unrestricted variables
     * @return Unit length vector of dimension K
     * @param lp Log probability reference to increment.
     * @tparam T Scalar type.
     **/
    template <int R, int C>
    Eigen::Matrix<var, R, C>
    unit_vector_constrain(const Eigen::Matrix<var, R, C>& y, var &lp) {
      Eigen::Matrix<var, R, C> x = unit_vector_constrain(y);
      lp -= 0.5 * dot_self(y);
      return x;
    }

  }
}
#endif
