#ifndef _utl_SVector_h_
#define _utl_SVector_h_

#include <cstddef>
#include <cmath>
#include <iostream>
#include <sstream>

#include <boost/lambda/lambda.hpp>

#include <utl/AugerException.h>
#include <utl/ErrorLogger.h>
#include <utl/ListAssignmentProxy.h>


namespace utl {

  /**
    \class SVector SVector.h "utl/SVector.h"
    \brief Static (small and dense) vector class

    Power of unrolled loops.

    Please refer to the testSMatrixSVector.cc file for usage cases.

    \author Darko Veberic
    \version $Id$
    \date 10 Jun 2008
    \ingroup math
  */

  template<std::size_t n, typename T = double>
  class SVector {
  public:
    typedef T Type;

    enum {
      Size = n
    };

    static std::size_t GetSize() { return n; }

    /// Default constructor does not initialize data!
    SVector() { }

    /** Initialize all vector element to (scalar) value.
      Note that you have to match the type T explicitly.
    */
    explicit SVector(const T& init) { Clear(init); }

    /**
      For all other types than T this ctor will be invoked.
      It assumes v is a vector-like type with operator[] access.
    */
    template<class V>
    explicit SVector(const V& v) { Assign(v); }

    template<typename V>
    void
    Assign(const V& v)
    {
      for (std::size_t i = 0; i < n; ++i)
        fVector[i] = v[i];
    }

    /** Support for comma-separated initialization list.
      Gets invoked when first term in the comma-separated list
      matches the T type of the SVector class:
      \code
        SVector<3, double> v;
        v = 1., 2., 3.;
      \endcode
    */
    ListVectorAssignmentProxy<SVector>
    operator=(const T& e)
    {
      fVector[0] = e;
      return ListVectorAssignmentProxy<SVector>(*this);
    }

    /**
      Assigment of all other types is asummed to be in vector-like form
      with operator[] access.
    */
    template<typename V>
    SVector& operator=(const V& v) { Assign(v); return *this; }

    void
    Clear(const T& value = T())
    {
      for (std::size_t i = 0; i < n; ++i)
        fVector[i] = value;
    }

    T& operator[](const std::size_t i) { return fVector[i]; }

    const T& operator[](const std::size_t i) const { return fVector[i]; }

    /// Element access with bound checking
    T&
    At(const int i)
    {
      if (i < 0 || i >= n) {
        std::ostringstream err;
        err << "Index " << i << " out of bounds (" << n << ')';
        throw utl::OutOfBoundException(err.str());
      }
      return fVector[i];
    }

    const T&
    At(const int i)
      const
    {
      if (i < 0 || i >= n) {
        std::ostringstream err;
        err << "Index " << i << " out of bounds (" << n << ')';
        throw utl::OutOfBoundException(err.str());
      }
      return fVector[i];
    }

    /// static getter
    template<std::size_t i>
    T& Get() { return fVector[i]; }

    /// static getter
    template<std::size_t i>
    const T& Get() const { return fVector[i]; }

    typedef T* Iterator;
    typedef const T* ConstIterator;

    Iterator Begin() { return fVector; }
    ConstIterator Begin() const { return fVector; }

    Iterator End() { return fVector + n; }
    ConstIterator End() const { return fVector + n; }

    double
    GetMag2()
      const
    {
      double mag2 = 0;
      for (std::size_t i = 0; i < n; ++i)
        mag2 += std::pow(std::abs(fVector[i]), 2);
      return mag2;
    }

    double GetMag() const
    { return std::sqrt(GetMag2()); }

    void Normalize() { *this *= 1/GetMag(); }

    SVector
    operator-() const
    {
      SVector<n,T> result;
      transform(this->Begin(), this->End(), result.Begin(), std::negate<T>());
      return result;
    }

    template<typename U>
    SVector&
    operator*=(const U& fact)
    {
      for (std::size_t i = 0; i < n; ++i)
        fVector[i] *= fact;
      return *this;
    }

    template<typename U>
    SVector&
    operator/=(const U& div)
    {
      for (std::size_t i = 0; i < n; ++i)
        fVector[i] /= div;
      return *this;
    }

    template<typename U>
    SVector&
    operator+=(const SVector<n, U>& v)
    {
      for (std::size_t i = 0; i < n; ++i)
        fVector[i] += v[i];
      return *this;
    }

    template<typename U>
    SVector&
    operator-=(const SVector<n, U>& v)
    {
      for (std::size_t i = 0; i < n; ++i)
        fVector[i] -= v[i];
      return *this;
    }

    template<std::size_t nn, typename U>
    bool
    operator==(const SVector<nn, U>& v)
      const
    {
      if (n != nn)
        return false;
      for (std::size_t i = 0; i < n; ++i)
        if (fVector[i] != v.fVector[i])
          return false;
      return true;
    }

    template<std::size_t nn, typename U>
    bool operator!=(const SVector<nn, U>& v) const
    { return !operator==(v); }

  private:
    T fVector[n];

  };


  /// dot product
  template<std::size_t n, typename T, typename U>
  inline
  typename boost::lambda::return_type_2<
    boost::lambda::arithmetic_action<boost::lambda::multiply_action>,
    SVector<n, T>,
    SVector<n, U>
  >::type
  operator*(const SVector<n, T>& a, const SVector<n, U>& b)
  {
    typedef typename boost::lambda::return_type_2<
      boost::lambda::arithmetic_action<boost::lambda::multiply_action>, T, U
    >::type DotProductType;

    DotProductType sum = a[0] * b[0];
    for (std::size_t i = 1; i < n; ++i)
      sum += a[i] * b[i];

    return sum;
  }


  // cross product
  template<typename T, typename U>
  inline
  SVector<
    3,
    typename boost::lambda::return_type_2<
      boost::lambda::arithmetic_action<boost::lambda::multiply_action>, T, U
    >::type
  >
  Cross(const SVector<3, T>& a, const SVector<3, U>& b)
  {
    typedef SVector<
      3,
      typename boost::lambda::return_type_2<
        boost::lambda::arithmetic_action<boost::lambda::multiply_action>, T, U
      >::type
    > ReturnType;

    ReturnType ret;
    ret[0] = a[1]*b[2] - a[2]*b[1];
    ret[1] = a[2]*b[0] - a[0]*b[2];
    ret[2] = a[0]*b[1] - a[1]*b[0];
    return ret;
  }


  template<typename T, typename U>
  inline
  double
  CosAngle(const SVector<3, T>& l, const SVector<3, U>& r)
  {
    const double magnitudeA = l.GetMag();
    const double magnitudeB = r.GetMag();
    return (magnitudeA && magnitudeB) ?
      (l * r) / (magnitudeA * magnitudeB) : 1;
  }


  template<typename T, typename U>
  inline
  double
  Angle(SVector<3, T> left, SVector<3, U> right)
  {
    /*const double cosAngle = CosAngle(left, right);
    return (cosAngle >= 1) ? 0 : acos(cosAngle);*/
    // DV: this is more accurate for small and large angles
    left *= 1/left.GetMag();
    right *= 1/right.GetMag();
    SVector<3, double> diff = left;
    diff -= right;
    const double d2 = diff.GetMag2();
    if (d2 <= 2)
      return 2 * std::asin(0.5 * std::sqrt(d2));
    left += right;
    return 2 * std::acos(0.5 * left.GetMag());
  }


#define UTL_SVECTOR_ARITHMETIC_ACTION(ACTION, OPERATOR)              \
  template<std::size_t n, typename T, typename U>                    \
  inline                                                             \
  typename boost::lambda::return_type_2<                             \
    boost::lambda::arithmetic_action<boost::lambda::ACTION>,         \
    SVector<n, T>,                                                   \
    SVector<n, U>                                                    \
  >::type                                                            \
  operator OPERATOR(const SVector<n, T>& a, const SVector<n, U>& b)  \
  {                                                                  \
    typedef typename boost::lambda::return_type_2<                   \
      boost::lambda::arithmetic_action<boost::lambda::ACTION>, T, U  \
    >::type ActionType;                                              \
                                                                     \
    SVector<n, ActionType> r;                                        \
                                                                     \
    for (std::size_t i = 0; i < n; ++i)                              \
      r[i] = a[i] OPERATOR b[i];                                     \
                                                                     \
    return r;                                                        \
  }

  UTL_SVECTOR_ARITHMETIC_ACTION(plus_action, +)
  UTL_SVECTOR_ARITHMETIC_ACTION(minus_action, -)

#undef UTL_SVECTOR_ARITHMETIC_ACTION


  template<std::size_t n, typename T>
  std::ostream&
  operator<<(std::ostream& os, const SVector<n, T>& v)
  {
    if (n)
      os << v[0];
    for (std::size_t i = 1; i < n; ++i)
      os << ' ' << v[i];
    return os;
  }


  template<std::size_t n, typename T>
  std::istream&
  operator>>(std::istream& is, SVector<n, T>& v)
  {
    for (std::size_t i = 0; i < n; ++i)
      is >> v[i];
    return is;
  }

}


namespace boost {

  namespace lambda {
  
    /// specialization for custom class SVector<>
    template<std::size_t n, typename T, typename U>
    class plain_return_type_2<
      arithmetic_action<multiply_action>,
      utl::SVector<n, T>,
      utl::SVector<n, U>
    > {
    private:
      // delegate to the action result of Action(T, U)
      typedef typename
        return_type_2<
          arithmetic_action<multiply_action>,
          T,
          U
        >::type res_type;

    public:
      typedef res_type type;
    };

#define UTL_SVECTOR_BINARY_RETURN_TYPE_SPECIALIZATION(ACTION) \
    template<std::size_t n, typename T, typename U>           \
    class plain_return_type_2<                                \
      arithmetic_action<ACTION>,                              \
      utl::SVector<n, T>,                                     \
      utl::SVector<n, U>                                      \
    > {                                                       \
    private:                                                  \
      typedef typename                                        \
        return_type_2<                                        \
          arithmetic_action<ACTION>, T, U                     \
        >::type res_type;                                     \
                                                              \
    public:                                                   \
      typedef utl::SVector<n, res_type> type;                 \
    }

    UTL_SVECTOR_BINARY_RETURN_TYPE_SPECIALIZATION(plus_action);
    UTL_SVECTOR_BINARY_RETURN_TYPE_SPECIALIZATION(minus_action);

#undef UTL_SVECTOR_BINARY_RETURN_TYPE_SPECIALIZATION

  }

}


#include <utl/SMatrixSVectorOp.h>


#endif