// HitVector_t.cpp

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

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

using trf::TrfVector;
using trf::TrfMatrix;
using trf::TrfSMatrix;
using trf::TrackError;
using trf::HitVector;
using trf::HitError;
using trf::HitDerivative;

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

int main( ) {

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

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

  // Make sure assert is enabled.
  bool assert_flag = false;
  assert ( ( assert_flag = true, assert_flag ) );
  if ( ! assert_flag ) {
    cerr << "Assert is disabled" << endl;
    return 1;
  }

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

  cout << ok_prefix << "Test constructor." << endl;
  // vector
  HitVector hv1(3);
  cout << hv1 << endl;
  assert( hv1.size() == 3 );
  assert( hv1.amax() == 0.0 );
  // error
  HitError he1(3);
  cout << he1 << endl;
  assert( he1.size() == 3 );
  assert( he1.amax() == 0.0 );
  // derivative
  HitDerivative hd1(3);
  cout << hd1 << endl;
  assert( hd1.size() == 3 );
  assert( hd1.amax() == 0.0 );

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

  cout << ok_prefix << "Test accessors and copy constructor." << endl;
  // vector
  hv1(0) = 1.0;
  hv1(1) = -2.0;
  hv1(2) = 3.0;
  cout << hv1 << endl;
  const HitVector hv2 = hv1;
  assert( hv2(0) == 1.0 );
  assert( hv2(1) == -2.0 );
  assert( hv2(2) == 3.0 );
  // error
  he1(0,0) = 11.0;
  he1(0,1) = -12.0;
  he1(1,1) = 22.0;
  he1(0,2) = 31.0;
  he1(2,1) = -32.0;
  he1(2,2) = 33.0;
  cout << hv1 << endl;
  const HitError he2 = he1;
  assert( he2(0,0) == 11.0 );
  assert( he2(0,1) == -12.0 );
  assert( he2(1,1) == 22.0 );
  assert( he2(2,0) == 31.0 );
  assert( he2(1,2) == -32.0 );
  assert( he2(2,2) == 33.0 );
  // derivative
  hd1(0,0) = 1.0;
  hd1(0,1) = 0.1;
  hd1(0,2) = 0.0;
  hd1(0,3) = 0.0;
  hd1(0,4) = 0.0;
  hd1(1,0) = -0.1;
  hd1(1,1) = 1.0;
  hd1(1,2) = 0.0;
  hd1(1,3) = 0.0;
  hd1(1,4) = 0.001;
  hd1(2,0) = 0.0;
  hd1(2,1) = 0.0;
  hd1(2,2) = 1.0;
  hd1(2,3) = -0.2;
  hd1(2,4) = 0.2;
  cout << hv1 << endl;
  const HitDerivative hd2 = hd1;
  assert( hd2(0,0) == 1.0 );
  assert( hd2(0,1) == 0.1 );
  assert( hd2(0,2) == 0.0 );
  assert( hd2(0,3) == 0.0 );
  assert( hd2(0,4) == 0.0 );
  assert( hd2(1,0) == -0.1 );
  assert( hd2(1,1) == 1.0 );
  assert( hd2(1,2) == 0.0 );
  assert( hd2(1,3) == 0.0 );
  assert( hd2(1,4) == 0.001 );
  assert( hd2(2,0) == 0.0 );
  assert( hd2(2,1) == 0.0 );
  assert( hd2(2,2) == 1.0 );
  assert( hd2(2,3) == -0.2 );
  assert( hd2(2,4) == 0.2 );

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

  cout << ok_prefix << "Test min and max." << endl;
  // vector
  cout << hv1.min() << ' ' << hv1.max() << ' '
       << hv1.amin() << ' ' << hv1.amax() << endl;
  assert( hv1.min() == -2.0 );
  assert( hv1.max() ==  3.0 );
  assert( hv1.amin() == 1.0 );
  assert( hv1.amax() == 3.0 );
  // errorr
  cout << he1.min() << ' ' << he1.max() << ' '
       << he1.amin() << ' ' << he1.amax() << endl;
  assert( he1.min() == -32.0 );
  assert( he1.max() ==  33.0 );
  assert( he1.amin() == 11.0 );
  assert( he1.amax() == 33.0 );
  // vector
  cout << hd1.min() << ' ' << hd1.max() << ' '
       << hd1.amin() << ' ' << hd1.amax() << endl;
  assert( hd1.min() == -0.2 );
  assert( hd1.max() ==  1.0 );
  assert( hd1.amin() == 0.0 );
  assert( hd1.amax() == 1.0 );

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

  cout << ok_prefix << "Test equality." << endl;
  // vector
  HitVector hv3(3);
  hv3(0) = 2.0;
  hv3(1) = 4.0;
  hv3(2) = 6.0;
  cout << hv1 << endl;
  cout << hv2 << endl;
  assert( hv1 == hv2 );
  assert( ! (hv1 != hv2) );
  cout << hv3 << endl;
  cout << hv2 << endl;
  assert( hv3 != hv2 );
  assert( ! (hv3 == hv2) );
  HitVector hv4(2);
  hv4(0) = hv1(0);
  hv4(1) = hv1(1);
  cout << hv4 << endl;
  cout << hv1 << endl;
  assert( hv4 != hv1 );
  // error
  HitError he3(3);
  cout << he1 << endl;
  cout << he2 << endl;
  assert( he1 == he2 );
  cout << he3 << endl;
  cout << he2 << endl;
  assert( he3 != he2 );
  assert( ! (he3 == he2) );
  HitError he4(2);
  he4(0,0) = he1(0,0);
  he4(0,1) = he1(0,1);
  he4(1,1) = he1(1,1);
  cout << he4 << endl;
  cout << he1 << endl;
  assert( he4 != he1 );
  assert( ! (he4 == he1) );
  // derivative
  HitDerivative hd3(3);
  cout << hd1 << endl;
  cout << hd2 << endl;
  assert( hd1 == hd2 );
  cout << hd3 << endl;
  cout << hd2 << endl;
  assert( hd3 != hd2 );
  assert( ! (hd3 == hd2) );
  HitDerivative hd4(2);
  hd4(0,0) = hd1(0,0);
  hd4(0,1) = hd1(0,1);
  hd4(1,1) = hd1(1,1);
  cout << hd4 << endl;
  cout << hd1 << endl;
  assert( hd4 != hd1 );
  assert( ! (hd4 == hd1) );

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

  cout << ok_prefix << "Test assignment." << endl;
  // vector
  hv3 = hv2;
  cout << hv2 << endl;
  cout << hv3 << endl;
  assert( hv2 == hv3 );
  // error
  he3 = he2;
  cout << he2 << endl;
  cout << he3 << endl;
  assert( he2 == he3 );
  // derivative
  hd3 = hd2;
  cout << hd2 << endl;
  cout << hd3 << endl;
  assert( hd2 == hd3 );

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

  cout << ok_prefix << "Test addition and subtraction." << endl;
  double diff;
  double maxdiff = 1.e-12;
  // vector
  hv3 = hv2;                   // hv1
  hv3 += hv2;                  // 2*hv1
  HitVector hv5 = hv3 + hv2;   // 3*hv1
  cout << hv5 << endl;
  cout << hv1 << endl;
  assert( hv5 != hv1 );
  hv5 = hv5 - hv2;
  hv5 -= hv2;
  cout << hv5 << endl;
  diff = (hv5-hv1).amax();
  cout << diff << endl;
  assert( diff < maxdiff );
  // error
  he3 = he2;                   // he1
  he3 += he2;                  // 2*he1
  HitError he5 = he3 + he2;    // 3*he1
  cout << he5 << endl;
  cout << he1 << endl;
  assert( he5 != he1 );
  he5 = he5 - he2;
  he5 -= he2;
  cout << he5 << endl;
  diff = (he5-he1).amax();
  cout << diff << endl;
  assert( diff < maxdiff );
  // derivative
  hd3 = hd2;                       // hd1
  hd3 += hd2;                      // 2*hd1
  HitDerivative hd5 = hd3 + hd2;   // 3*hd1
  cout << hd5 << endl;
  cout << hd1 << endl;
  assert( hd5 != hd1 );
  hd5 = hd5 - hd2;
  hd5 -= hd2;
  cout << hd5 << endl;
  diff = (hd5-hd1).amax();
  cout << diff << endl;
  assert( diff < maxdiff );

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

  cout << ok_prefix << "Test construction from an array." << endl;
  double tmp[100];
  int i, j, ij;
  int sz;
  // vector
  sz = he1.size();
  for ( i=0; i<sz; ++i ) tmp[i] = hv1(i);
  HitVector hv6( hv1.size(), tmp );
  cout << hv1 << endl;
  cout << hv6 << endl;
  assert( hv1 == hv6 );
  // error
  sz = he1.size();
  ij = 0;
  for ( i=0; i<sz; ++i )
   for ( j=0; j<=i; ++j ) tmp[ij++] = he1(i,j);
  HitError he6( he1.size(), tmp );
  cout << he1 << endl;
  cout << he6 << endl;
  assert( he1 == he6 );
  // derivative
  sz = hd1.size();
  ij = 0;
  for ( i=0; i<sz; ++i )
   for ( j=0; j<5; ++j ) tmp[ij++] = hd1(i,j);
  HitDerivative hd6( hd1.size(), tmp );
  cout << hd1 << endl;
  cout << hd6 << endl;
  assert( hd1 == hd6 );

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

  cout << ok_prefix << "Test 1D construction values." << endl;
  // vector
  HitVector hv01(1.0);
  cout << hv01 << endl;
  assert( hv01.size() == 1 );
  assert( hv01(0) == 1.0 );
  // error
  HitError he01(0.11);
  cout << he01 << endl;
  assert( he01.size() == 1 );
  assert( he01(0,0) == 0.11 );

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

  cout << ok_prefix << "Test 2D construction values." << endl;
  // vector
  HitVector hv02(1.0,2.0);
  cout << hv02 << endl;
  assert( hv02.size() == 2 );
  assert( hv02(0) == 1.0 );
  assert( hv02(1) == 2.0 );
  // error
  HitError he02(0.11,0.21,0.22);
  cout << he02 << endl;
  assert( he02.size() == 2 );
  assert( he02(0,0) == 0.11 );
  assert( he02(1,0) == 0.21 );
  assert( he02(1,1) == 0.22 );

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

  cout << ok_prefix << "Test 3D construction values." << endl;
  // vector
  HitVector hv03(1.0,2.0,3.0);
  cout << hv03 << endl;
  assert( hv03.size() == 3 );
  assert( hv03(0) == 1.0 );
  assert( hv03(1) == 2.0 );
  assert( hv03(2) == 3.0 );
  // error
  HitError he03(0.11,0.21,0.22,0.31,0.32,0.33);
  cout << he03 << endl;
  assert( he03.size() == 3 );
  assert( he03(0,0) == 0.11 );
  assert( he03(1,0) == 0.21 );
  assert( he03(1,1) == 0.22 );
  assert( he03(2,0) == 0.31 );
  assert( he03(2,1) == 0.32 );
  assert( he03(2,2) == 0.33 );

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

  cout << ok_prefix << "Test constructing hit error from track error."
       << endl;
  TrackError terr;
  HitDerivative hder(2);
  double der = 1.0;
  for ( i=0; i<5; ++i ) {
    terr(i,i) = double(i+1);
    hder(0,i) = sqrt(der);
    hder(1,i) = 2.0*hder(0,i);
    der /= 10.0;
  }
  double e11 = 1.2345;
  HitError expected_err(e11,2.0*e11,4.0*e11);
  HitError calculated_err(hder,terr);
  HitError diff_err = calculated_err - expected_err;
  cout << hder << endl;
  cout << terr << endl;
  cout << expected_err << endl;
  cout << calculated_err << endl;
  cout << diff_err << endl;
  assert( diff_err.amax() < 1.e-12 );

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

  cout << ok_prefix << "Test inversion." << endl;
  HitError test_err(2.0,1.0,3.0);
  HitError inverted_err = test_err;
  cout << inverted_err << endl;
  int stat = inverted_err.invert();
  cout << stat << endl;
  assert( stat == 0 );
  HitError original_err = inverted_err;
  stat = original_err.invert();
  cout << stat << endl;
  assert( stat == 0 );
  cout << inverted_err << endl;
  cout << original_err << endl;
  assert( (original_err - test_err).amax() < 1.e-12 );

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

  cout << ok_prefix << "Test fetching underlying representation."
       << endl;
  TrfVector uvec = hv1.get_vector();
  TrfSMatrix usma = he1.get_matrix();
  TrfMatrix umtx = hd1.get_matrix();

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

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

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

}