#ifndef _SCALAR_FW_ACTIVE_SET_IMPL_H_
#define _SCALAR_FW_ACTIVE_SET_IMPL_H_

#include <iostream>
#include <iomanip>

#include "active_set.h"

template <typename ix_t, typename val_t>
ActiveSet<ix_t, val_t>::ActiveSet(const ix_t dim) : m_dim(dim), m_x(dim)
{
   (void) m_x.setConstant(0);
}

template <typename ix_t, typename val_t>
ActiveSet<ix_t, val_t>::ActiveSet(ActiveSet<ix_t, val_t> && other)
{
   m_dim = other.m_dim;
   m_x   = std::move(other.m_x);
   m_as.swap(other.m_as);
}

template <typename ix_t, typename val_t> ActiveSet<ix_t, val_t>::~ActiveSet()
{
}

template <typename ix_t, typename val_t>
ix_t
ActiveSet<ix_t, val_t>::size() const
{
   return m_as.size();
}

template <typename ix_t, typename val_t>
bool
ActiveSet<ix_t, val_t>::validate() const
{
   val_t sum_weights = 0;
   for (const entry_t & e : m_as)
      sum_weights += e.first;

   return (sum_weights == 1.0);
}

template <typename ix_t, typename val_t>
val_t
ActiveSet<ix_t, val_t>::weight_from_atom(const vec_t<val_t>& vector) const
{
   ix_t i_ix = find_atom(vector);
   if ( i_ix != INVALID_T )
      return m_as[i_ix].first;
   return 0;
}

template <typename ix_t, typename val_t>
const vec_t<val_t>&
ActiveSet<ix_t, val_t>::x() const
{
   return m_x;
}

template <typename ix_t, typename val_t>
std::pair<typename ActiveSet<ix_t, val_t>::entry_t, typename ActiveSet<ix_t, val_t>::entry_t>
ActiveSet<ix_t, val_t>::argminmax(const vec_t<val_t> & direction) const
{
   ix_t  min_idx = 0;
   val_t min_val = m_as[0].second.dot(direction);

   ix_t  max_idx = 0;
   val_t max_val = min_val;

   for(std::size_t ix = 1; ix < m_as.size(); ++ix)
   {
      const val_t dot = m_as[ix].second.dot(direction);

      if ( dot < min_val )
      {
         min_idx = ix;
         min_val = dot;
      }

      if ( dot > max_val )
      {
         max_idx = ix;
         max_val = dot;
      }
   }

   return std::make_pair(
      std::make_pair(m_as[min_idx].first, m_as[min_idx].second),
      std::make_pair(m_as[max_idx].first, m_as[max_idx].second) );
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::initialize(const vec_t<val_t> & v)
{
   m_as.clear();
   (void) m_as.emplace_back(std::make_pair(1.0, v));
   m_x = v;
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::push(const entry_t & entry)
{
   (void) m_as.emplace_back(entry);
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::remove(const ix_t ix)
{
   (void) m_as.erase(ix);
}

template <typename ix_t, typename val_t>
typename ActiveSet<ix_t, val_t>::entry_t &
ActiveSet<ix_t, val_t>::operator[](const ix_t ix)
{
   return m_as[ix];
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::update(const entry_t & entry, bool renorm)
{
   for (std::size_t i = 0; i < m_as.size(); ++i)
      m_as[i].first *= (1.0 - entry.first);

   ix_t i_ix = find_atom(entry.second);

   if ( i_ix == INVALID_T )
      push(entry);
   else
      m_as[i_ix].first += entry.first;

   if ( renorm )
   {
      cleanup();
      renormalize(); /*lint !e523*/
   }

   m_x = (1.0 - entry.first) * m_x + entry.first * entry.second;
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::cleanup(const val_t weight_purge_threshold)
{
   typename std::vector<entry_t>::iterator it = m_as.begin();
   while ( it != m_as.end() )
   {
      if ( it->first < weight_purge_threshold )
         it = m_as.erase(it);
      else
         ++it;
   }

   /*
   auto it = std::remove_if(m_as.begin(), m_as.end(),
      [weight_purge_threshold](const entry_t & e)
      { return e.first < weight_purge_threshold; });
   (void) m_as.erase(it, m_as.end());
   */
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::renormalize()
{
   val_t lambda = 0;
   for (const entry_t & e : m_as)
      lambda += e.first;

   for(entry_t & e : m_as)
      e.first /= lambda;
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::update_x()
{
   (void) m_x.setConstant(0);
   for(const entry_t & e : m_as)
      m_x += e.first * e.second;
}

template <typename ix_t, typename val_t>
ActiveSet<ix_t, val_t>
ActiveSet<ix_t, val_t>::copy()
{
   ActiveSet<ix_t, val_t> other(m_dim);
   other.m_x = m_x;
   std::copy(m_as.begin(), m_as.end(), std::back_inserter(other.m_as));

   return other;
}

template <typename ix_t, typename val_t>
void
ActiveSet<ix_t, val_t>::print()
{
   std::cout << "Active set, size = " << m_as.size() << ": " << std::endl;
   for(ix_t ix = 0; ix < m_as.size(); ++ix)
   {
      const auto& entry = m_as[ix];
      std::cout << "\t[" << std::setw(2) << ix << "]" << std::setw(11) << " / " << entry.first << std::endl;
   }
}

template <typename ix_t, typename val_t>
ix_t
ActiveSet<ix_t, val_t>::find_atom(const vec_t<val_t>& vector)
{
   for (ix_t ix = 0; (unsigned) ix < m_as.size(); ++ix)
   {
      if ( m_as[ix].second == vector )
         return ix;
   }
   return INVALID_T;
}

#endif // _SCALAR_FW_ACTIVE_SET_IMPL_H_
