#include "amma/Num/NumMagnitudeEuclidean.hh"
#include "amma/Num/NumCostFunction.hh"
#include "amma/Num/NumCostCombine.hh"
#include "amma/Num/NumCostInvert.hh"
#include "amma/Num/NumFuncConstant.hh"
#include <fstream.h>

main ()
{
  // Setup Data
  NumMagnitudeEuclideanC magnitude;                // measures distance
  
  VectorC Y (3); Y[0] = 0.5; Y[1] = 1; Y[2] = 1;   // given value
  VectorC Yd (3); Yd[0] = 0; Yd[1] = 1; Yd[2] = 2; // desired value
  
  NumParametersC parameters (3);
  
  // Setup function for optimising.
  
  NumFuncConstantC function (Y);
  // Setup cost functions
  DListC<NumCostC> costs;
  DListC<RealT> weights; weights.InsLast (1.0); weights.InsLast (1.0);
  costs.InsLast (NumCostFunctionC (parameters,Yd,function,magnitude));
  costs.InsLast (NumCostInvertC (costs.First()));
  costs.InsLast (NumCostCombineC (costs,weights));
  // Save
  {
    ofstream out ("testNumCost.data");
    for (ConstDLIterC<NumCostC> elem (costs); elem.IsElm (); elem.Next ())
      elem.Data().Save (out);
  }
  // Load and evaluate
  ifstream in ("testNumCost.data");
  for (IndexT i = 0; i < 3; i++) {
    NumCostC cost (in);
    cout << "Function Name: " << cost.GetName() << "\n";
    cout << "Size X and Y: " << cost.SizeX() << " " << cost.SizeY() << "\n";
    cout << cost.GetInfo() << "\n";
    cout << "Output at X=" << Y << " is Y=" << cost(Y) << "\n";
    cout << "Jacobian at X is " << cost.Jacobian(Y) << "\n";
    cout << "Cost function at X=" << Y << " is " << cost.Cost(Y) << "\n";
  }
}