#include <cassert>
#include <cmath>
#include <iostream>
using namespace std;


#include <fixedpoint.h>
#include <fixedpointtest.h>

// <TODO> find where numberical constant lives.
#define PI 3.14159265

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;
}  



void fixedpointtest::test02()
{

  fixedpoint< f2, 2 > p;

  double x0[2];
  double x[2];

  x0[0] = 20.0;
  x0[1] = -20.0;

  p.inc(x,x0);

  cout << x0[0] << " " << x0[1] << endl;
  cout << x[0] << " " << x[1] << endl;

  x0[0] = x[0]; x0[1]=x[1];
  p.inc(x,x0);
  cout << x[0] << " " << x[1] << endl;

}



/*
 *  Two arms of length a and b.
 *  At angles theta and phi respectively.
 *
 *   
 *  The arms are connected like a human arm where
 *  the origin is the schoulder and (px,py) is a
 *  point above shoulder height.
 *
 *  x = cos(theta) and y = sin(phi) were substited into
 *    px = a*cos(theta) + b*cos(phi) 
 *    py = a*sin(theta) + b*sin(phi)
 *  the algebra simplified to remove the square root
 *  and this non-linear equation solved with the fixed 
 *  point method.
 *  Finally converting back to an angle.
*/

class arm
{
public:

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

  void IDf(double idf[2][2], double const x[2]) const; 

  double a;
  double b;

  double px;
  double py;

  void print( double const x[2] ) const;

};


void arm::print( double const x[2] ) const
{
  double theta = acos(x[0]);
  double phi = asin(x[1]);

  cout << "theta=" << theta*180/PI << endl;
  cout << "phi=" << phi*180/PI << endl;

  cout << endl;
  cout << "a*cos(theta)=" << a*cos(theta) << endl;
  cout << "a*sin(theta)=" << a*sin(theta) << endl;
 
  cout << "b*cos(phi)=" << b*cos(phi) << endl;
  cout << "b*sin(phi)=" << b*sin(phi) << endl;

  cout << "|a*cos(theta)|+|b*cos(phi)|=" << 
    a*abs(cos(theta)) + b*abs(cos(phi)) << endl;
  cout << "|a*sin(theta)|+|b*sin(phi)|=" << 
    a*abs(sin(theta)) + b*abs(sin(phi)) << endl;

  cout << endl << endl;
  cout << "a*cos(theta)+b*cos(phi)=" << 
    a*cos(theta) + b*cos(phi) << endl;
  cout << "|a*sin(theta)|+b*sin(phi)=" << 
    a*sin(theta) + b*sin(phi) << endl;
}

void arm::f(double fx[2], double const x[2]) const
{
  double t;
  t = (px-a*x[0]);
  fx[0] = t*t + b*b*(x[1]*x[1]-1.0);
  t = (py-b*x[1]);
  fx[1] = t*t + a*a*(x[0]*x[0]-1.0);
}

void arm::IDf(double idf[2][2], double const x[2]) const 
{
  double det = 4.0*a*b*(px*py-px*b*x[1]-py*a*x[0]);
  double z = -2.0 / det;

  idf[0][0] = z*b*(py-b*x[1]);
  idf[0][1] = z*b*b*x[1];
  idf[1][0] = z*a*a*x[0];
  idf[1][1] = z*a*(px-a*x[0]);
}

void fixedpointtest::test03()
{
  fixedpoint< arm, 2 > p;

  arm & f(p.f);

  f.a = 3.0;
  f.b = 2.0;
  f.px = 4.0;
  f.py = 1.0;

  double x0[2];
  double x[2];

  x0[0] = 1.0;
  x0[1] = 0.0;


  unsigned int imax = 8;
  for (unsigned int i=0; i<imax; ++i)
  {

    x0[0] = x[0]; x0[1]=x[1];
    p.inc(x,x0);
    cout << x[0] << " " << x[1] << endl;
  }

  f.print(x);
}