#include "amma/Num/NumMagnitudeEuclidean.hh"
#include "amma/Num/NumMagnitudeSqrEuclidean.hh"
#include "amma/Num/NumMagnitudeMahalanobis.hh"
#include "amma/Num/NumMagnitudeCityBlock.hh"
#include "amma/Num/NumMagnitudeMax.hh"
#include "amma/Num/NumMagnitudeRobust.hh"
#include "amma/Filename.hh"
#include "amma/EntryPnt.hh"
#include <fstream.h>

int testNumMagnitude(int n,char **argv)
{
  // Setup data file
  FilenameC dataFile("/tmp/testNumMagnitude.data");
  dataFile = dataFile.MkTemp();
  cerr << "Starting test. \n";
  
  // Setup covariance matrix with only variances
  MatrixC Cxd (3,3);
  Cxd.SetZero();
  Cxd.P(0,0) = Cxd.P(1,1) = 1;
  Cxd.P(2,2) = 2;
  
  // Setup magnitude measures
  SArray1dC<NumMagnitudeC> magnitudes (6);
  magnitudes[0] = NumMagnitudeEuclideanC ();
  magnitudes[1] = NumMagnitudeSqrEuclideanC ();
  magnitudes[2] = NumMagnitudeMahalanobisC (Cxd);
  magnitudes[3] = NumMagnitudeCityBlockC ();
  magnitudes[4] = NumMagnitudeMaxC ();
  magnitudes[5] = NumMagnitudeRobustC (0.5,magnitudes[0]);
  
  // Setup sample value to measure
  VectorC Y (3); Y[0] = 0.5; Y[1] = 1; Y[2] = 1;
  
  // Save
  {
    ofstream out (dataFile);
    FOR_SARRAY1 (magnitudes,i1) {
      magnitudes[i1].Save (out);
      out << "\n";
    }
    out.close(); // Make sure everthing is written.
  }
  
  // Load and evaluate
  ifstream in (dataFile);
  FOR_SARRAY1 (magnitudes,i2) {
    if(!in.good()) 
      cerr << "Stream bad before loading " << i2 << "\n";
    NumMagnitudeC magnitude (in);
    cout << "Function Name: " << magnitude.GetName() << "\n";
    cout << "Size X and Y: " << magnitude.SizeX() << " " << magnitude.SizeY() << "\n";
    cout << magnitude.GetInfo() << "\n";
    cout << "Output at X=" << Y << " is Y=" << magnitude(Y) << "\n";
    cout << "Jacobian at X is " << magnitude.Jacobian(Y) << "\n";
    cout << "Magnitude of Y=" << Y << " is " << magnitude.Magnitude(Y) << "\n\n";
  }
  
  cout << "TEST PASSED. \n";
  dataFile.Remove();
  return 0;
}

AMMA_ENTRY_POINT(testNumMagnitude);