/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2014 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.opensource.org/licenses/osl-3.0.php          //
//                                                         //
/////////////////////////////////////////////////////////////

template<class T,int n>
simplex_method<T,n>::simplex_method(nfunction<T,n>* f)
{
  m_func=f;
}


template<class T,int n>
nvector<T,n> simplex_method<T,n>::reflect(int idx)
{
  nvector<T,n> sum=nvector<T,n>(T(0));
  for(int i=0;i<n+1;i++){
    if(i!=idx){
      sum+=m_vert[i];
    }
  }
  sum+=sum;
  sum=sum/T(n);
  return sum-m_vert[idx];
}

template<class T,int n>
void simplex_method<T,n>::insert(const nvector<T,n>& vert,T val,int idx)
{
  bool up=true;
  bool down=true;

  m_vert[idx]=vert;
  m_val[idx]=val;
  
  int i=idx;
  while(up && (i<n)){
    if(m_val[i]>m_val[i+1]){
      up=false;
    } else {
      // swap
      nvector<T,n> h_vert=m_vert[i];
      T h_val=m_val[i];
      m_vert[i]=m_vert[i+1];
      m_val[i]=m_val[i+1];
      m_vert[i+1]=h_vert;
      m_val[i+1]=h_val;
      i++;
    }
  }
  while(down && (i>0)){
    if(m_val[i]<m_val[i-1]){
      down=false;
    } else {
      // swap
      nvector<T,n> h_vert=m_vert[i];
      T h_val=m_val[i];
      m_vert[i]=m_vert[i-1];
      m_val[i]=m_val[i-1];
      m_vert[i-1]=h_vert;
      m_val[i-1]=h_val;
      i--;
    }
  }
}

template<class T,int n>
void simplex_method<T,n>::shrink()
{
  nvector<T,n> center=m_vert[0];
  for(int i=1;i<n+1;i++){
    center+=m_vert[i];
  }
  center=center/T(n+1);
  for(int i=0;i<n+1;i++){
    m_vert[i]=center+(m_vert[i]-center)/T(2);
    m_val[i]=(*m_func)(m_vert[i]);
  }
  sort();
}

template<class T,int n>
void simplex_method<T,n>::sort()
{
  for(int i=0;i<n-1;i++){
    for(int j=i;j<n;j++){
      if(m_val[j]<m_val[j+1]){
	nvector<T,n> h_vert=m_vert[j];
	T h_val=m_val[j];
	m_vert[j]=m_vert[j+1];
	m_val[j]=m_val[j+1];
	m_vert[j+1]=h_vert;
	m_val[j+1]=h_val;
      }
    }
  }
}

template<class T,int n>
nvector<T,n> simplex_method<T,n>::solve(T lim, const nvector<T,n> & start,int max)
{
  int idx;
  for(int i=0;i<n+1;i++){
    m_val[i]=T(0);
  }
  // set initial simplex vertices
  insert(start,(*m_func)(start),0);
  for(int i=0;i<n;i++){
    nvector<T,n> temp_vert=start+nvector<T,n>::unit(i);
    T temp_val=(*m_func)(temp_vert);
    insert(temp_vert,temp_val,i+1);
  }
  T eps=m_val[0];
  int count=0;
  while((eps>lim)&&((count<max)||(max==-1))){
    idx=0;
    bool found=false;
    while((idx<n+1) && !found){
      nvector<T,n> temp_vert=reflect(idx);
      T temp_val=(*m_func)(temp_vert);
      if(temp_val<m_val[idx]){
	insert(temp_vert,temp_val,idx);
	found=true;
      } else {
	idx++;
      }
    }
    if(idx==n+1) shrink();
    //for(int i=0;i<n+1;i++){
    //  cout << m_vert[i] << " " << m_val[i] << endl;
    //}
    //cout << "------------" << endl;
    eps=m_val[n];
    count++;
    //std::cout << "step : " << count << "  eps : " << eps << std::endl;
  }
  return m_vert[n]; 
}
