#include <math.h>

#include "Syslog.h"

#include "EuclidianMetric.hh"
#include "ScalarMetric.hh"

/*
 * class EuclidianMetric
 */

StateVector::scalar_type EuclidianMetric::operator()(StateVector const &a,
						     StateVector const &b) const {
  StateVector tmp = a;
  tmp -= b;
  return tmp.norm(); 
}

/*
 * class ScalarMetric
 */

StateVector::scalar_type ScalarMetric::operator()(StateVector const &a,
						  StateVector const &b) const {
//   StateVector na(a), nb(b);
  StateVector::scalar_type norma = a.norm(), normb = b.norm();

  // I want the vectors to have a norm value of 1
  
  if( norma==0.0 )
    return normb;
  else if( normb==0.0 )
    return norma;
  else {
    StateVector::scalar_type prod = a*b;
    prod /= norma*normb;
    
    // prod is in [-1 1] with 1 corresponding to perfect alignement
    prod -= 1.0;
    if( prod<0.0 )
      prod = -prod;
    return prod;
  }
}