// TrackVector_t.cpp

// Test track vector and track error matrix classes.
// Big test is compilation.

#include "TrackVector.h"
#include <string>
#include <iostream>
#include <cassert>

using std::cout;
using std::cerr;
using std::endl;
using std::string;

using trf::TrfVector;
using trf::TrfMatrix;
using trf::TrfSMatrix;
using trf::TrackVector;
using trf::TrackError;
using trf::TrackDerivative;

int main( ) {

  string ok_prefix = "TrackVector test (I): ";
  string error_prefix = "TrackVector test (E): ";

  cout << ok_prefix
       << "------- Testing component TrackVector. -------" << endl;

  //********************************************************************

  cout << ok_prefix << "Test constructors and indexing." << endl;
  // Create a track vector.
  // Do not change these numbers--they are hardwired below.
  TrackVector tv1;
  assert( tv1.amax() == 0.0 );
  tv1(0) = -1.2;
  tv1(1) = 2.3;
  tv1(2) = -3.4;
  tv1(3) = 4.5;
  tv1(4) = -5.6;
  cout << tv1 << endl;
  // Create an error matrix.
  TrackError te1;
  assert( te1.amax() == 0.0 );
  te1(0,0) = 0.1;
  te1(0,1) = -0.021;
  te1(1,1) = 0.2;
  te1(2,0) = 0.031;
  te1(2,1) = 0.032;
  te1(2,2) = 0.3;
  te1(0,3) = -0.041;
  te1(1,3) = -0.042;
  te1(2,3) = -0.043;
  te1(3,3) = 0.4;
  te1(4,0) = 0.051;
  te1(4,1) = 0.052;
  te1(4,2) = 0.053;
  te1(4,3) = 0.054;
  te1(4,4) = 0.5;
  cout << te1 << endl;
  // Create a derivative matrix.
  TrackDerivative td1;
  assert( td1.amax() == 0.0 );
  for ( int i=0; i<5; ++i ) {
    for ( int j=0; j<5; ++j ) {
      if ( i == j ) td1(i,j) = 1.0;
      else td1(i,j) = 0.1*(i-j);
    }
  }
  cout << td1 << endl;

  //********************************************************************

  // Check min, max, amin and amax.
  cout << ok_prefix << "Test max, min, amax, amin" << endl;
  cout << "Minimum:  " << tv1.min() << "  " << te1.min() 
       << td1.min() << endl;
  cout << "Maximum:  " << tv1.max() << "  " << te1.max() 
       << td1.max() << endl;
  cout << "Abs Min:  " << tv1.amin() << "  " << te1.amin()
       << td1.amin() << endl;
  cout << "Abs Max:  " << tv1.amax() << "  " << te1.amax()
       << td1.amax() << endl;
  assert( tv1.min() == -5.6 );
  assert( tv1.max() ==  4.5 );
  assert( tv1.amin() ==  1.2 );
  assert( tv1.amax() ==  5.6 );
  assert( te1.min() == -0.043 );
  assert( te1.max() ==  0.5 );
  assert( te1.amin() ==  0.021 );
  assert( te1.amax() ==  0.5 );
  assert( td1.min() == -0.1*4 );
  assert( td1.max() ==  1.0 );
  assert( td1.amin() ==  0.1*1 );
  assert( td1.amax() ==  1.0 );

  //********************************************************************

  // Check assignment and copy.
  cout << ok_prefix << "Testing assignment and copy." << endl;
  // vector
  TrackVector tv2(tv1);
  TrackVector dv2 = tv2 - tv1;
  TrackVector tv3 = tv1;
  TrackVector dv3 = tv3 - tv2;
  cout << tv1 << endl;
  cout << tv2 << endl;
  cout << tv3 << endl;
  assert( tv2 == tv1 );
  assert( dv2.amax() == 0.0 );
  assert( tv3 == tv1 );
  assert( dv3.amax() == 0.0 );
  // error
  TrackError te2(te1);
  TrackError de2 = te2 - te1;
  TrackError te3 = te1;
  TrackError de3 = te3 - te2;
  cout << te1 << endl;
  cout << te2 << endl;
  cout << te3 << endl;
  assert( te2 == te1 );
  assert( de2.amax() == 0.0 );
  assert( te3 == te1 );
  assert( de3.amax() == 0.0 );
  // derivative
  TrackDerivative td2(td1);
  TrackDerivative dd2 = td2 - td1;
  TrackDerivative td3 = td1;
  TrackDerivative dd3 = td3 - td2;
  cout << td1 << endl;
  cout << td2 << endl;
  cout << td3 << endl;
  assert( td2 == td1 );
  assert( dd2.amax() == 0.0 );
  assert( td3 == td1 );
  assert( dd3.amax() == 0.0 );

  //********************************************************************

  // Check additon and subtraction.
  cout << ok_prefix << "Testing addition and subtraction." << endl;
  double small = 1.e-10;
  // vector
  TrackVector tv4 = tv1 + tv2;  // = 2*tv1
  TrackVector tv5 = tv4 - tv3 - tv2;  // = 0
  assert( (tv4.amax()-2.0*5.6) < small );
  assert( tv5.amax() < small );
  tv4 -= tv1;  // = tv1
  tv5 += tv2;
  tv5 += tv3;  // = 2*tv1
  assert( (tv5.amax()-2.0*5.6) < small );
  assert( (tv4.min()+5.6) < small );
  // error
  TrackError te4 = te1 + te2;
  TrackError te5 = te4 - te3 - te2;
  assert( (te4.amax()-2.0*0.5) < small );
  assert( te5.amax() < small );
  te4 -= te1;  // = te1
  te5 += te2;
  te5 += te3;  // 2*te1
  assert( (te5.amax()-2.0*0.5) < small );
  assert( (te4.min()+0.043) < small );
  // derivative
  TrackDerivative td4 = td1 + td2;
  TrackDerivative td5 = td4 - td3 - td2;
  assert( (td4.amax()-2.0*1.0) < small );
  assert( td5.amax() < small );
  td4 -= td1;  // = td1
  td5 += td2;
  td5 += td3;  // 2*td1
  assert( (td5.amax()-2.0*1.0) < small );
  assert( (td4.min()+0.4) < small );

  //********************************************************************

  // Check equality and inequality.
  cout << ok_prefix << "Testing equality and inequality" << endl;
  assert( tv1 == tv2 );
  assert( tv1 != tv5 );
  assert( tv1 == tv1 );
  assert( te1 == te2 );
  assert( te1 != te5 );
  assert( te1 == te1 );
  assert( td1 == td2 );
  assert( td1 != td5 );
  assert( td1 == td1 );
  assert( is_equal(tv1,tv2) );
  assert( ! is_equal(tv1,tv5) );
  assert( is_equal(te1,te2) );
  assert( ! is_equal(te1,te5) );
  assert( is_equal(td1,td2) );
  assert( ! is_equal(td1,td5) );

  //********************************************************************

  // invert matrix
  cout << ok_prefix << "Testing inversion." << endl;
  te2 = te1;
  int stat2 = invert( te2 );
  te3 = te2;
  int stat3 = invert( te3 );
  de3 = te3 - te1;
  cout << te1 << endl;
  cout << te2 << endl;
  cout << te3 << endl;
  cout << de3 << endl;
  if ( stat2 || stat3 || de3.amax()>small ) {
    cerr << error_prefix << "Invalid error += or -=" << endl;
    return 13;
  }

  //********************************************************************

  // calculate a chi-square difference
  cout << ok_prefix << "Calculating chi-square difference." << endl;
  double diff = chisq_diff(tv1,te3);
  if ( diff < 0.0 ) {
    cerr << error_prefix << "Unable to evaluate difference." << endl;
    return 14;
  }

  //********************************************************************

  // derivative transpose
  cout << ok_prefix << "Testing transpose." << endl;
  td2 = td1;
  td2.transpose();
  cout << td1 << endl;
  cout << td2 << endl;
  assert( td2 != td1 );
  td2.transpose();
  assert( td2 == td1 );

  //********************************************************************

  // transform
  cout << ok_prefix << "Testing transform." << endl;
  te2 = te1;
  te2.Xform(td1);
  cout << te1 << endl;
  cout << te2 << endl;
  assert( te1 != te2 );

  //********************************************************************

  cout << ok_prefix << "Testing fetching underlying representation."
       << endl;
  TrfVector uvec = tv1.get_vector();
  TrfSMatrix usma = te1.get_matrix();
  TrfMatrix umtx = td1.get_matrix();
  
  //********************************************************************

  cout << ok_prefix << "Testing construction form base class." << endl;
  cout << TrackVector(uvec) << endl;
  cout << TrackError(usma) << endl;
  cout << TrackDerivative(umtx) << endl;

  //********************************************************************

  cout << ok_prefix
       << "------------- All tests passed. -------------" << endl;
  return 0;
}