#ifndef FIXEDPOINT_H
#define FIXEDPOINT_H

#include <cmath>
using namespace std;


/*!  
\brief Fixed Point Iteration.
 
   Solves a system of N equations, as a vector
   function  f(X) = 0.  
   The client supplies template class F with 
     F.f(double[N],double*const) and 
     F.IDf( double[N][N], double const[N][N] ) 
       the inverse of grad f.
 
*/
template< typename F, unsigned int N >
class fixedpoint
{
public:

  F f;

  /** One iteration. */
  void inc(double x[N], double const x0[N]) const; 

  /** sum( abs(x[i]-x0[i]) ) */
  double const norm
  (
    double const x[N], 
    double const x0[N]
  ) const;
};


/*
 *  Example Code 
 *
 *  Since this is a little non-standard I have supplied
 *  example code because I do not know how to communicate
 *  the interface expectations other than as given.
 *
 *  Solve
 *  3x + 2y = 7
 * -5x + 4y = 3
 *
 * x=1,y=2

class f2
{
public:

  // f is the system of equations as a vector function = 0.
  void f(double fx[2], double const x[2]) const; 

  // Inverse Derivative of vector equ's f or inverse grad f. 
  void IDf(double idf[2][2], double const x[2]) const; 

};

void f2::f(double fx[2], double const x[2]) const
{
  fx[0] = 3.0*x[0]+2.0*x[1]-7.0;
  fx[1] = -5.0*x[0]+4*x[1]-3.0;
}  


void f2::IDf(double idf[2][2], double const x[2]) const 
{
  double det = 22.0;

  idf[0][0] = 4.0/det;
  idf[0][1] = -2.0/det;
  idf[1][0] = 5.0/det;
  idf[1][1] = 3.0/det;
} 

*/




//----------------------------------------------------------
// Implementation



template < typename F, unsigned int N >
void fixedpoint<F,N>::inc
(
  double x[N], 
  double const x0[N]
) const
{
  double fx[N];
  f.f(fx,x0);

  for (unsigned int i=0; i<N; ++i)
    x[i] = x0[i];

  double IDF[N][N];
  f.IDf(IDF,x0);

  for (unsigned int i=0; i<N; ++i)
  {
    for (unsigned int k=0; k<N; ++k)
    {
      x[i] -= IDF[i][k]*fx[k];
    }
  }
}

template < typename F, unsigned int N >
double const fixedpoint<F,N>::norm
(
  double const x[N], 
  double const x0[N]
) const
{
  double s = 0.0;
  for ( unsigned int i=0; i<N; ++i )
    s += abs(x[i]-x0[i]);

  return s;
}
  


#endif