// HitXYPlane2Generator_t.cpp

// Test HitXYPlane2Generator.

#include "HitXYPlane2Generator.h"
#include <iostream>
#include <string>
#include <cassert>
#include "SurfXYPlane.h"
#include "trfbase/ETrack.h"
#include "trfbase/Hit.h"
#include "HitXYPlane2.h"
#include "trfutil/trfstream.h"
#include <cmath>

#ifndef DEFECT_NO_STDLIB_NAMESPACES
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::sqrt;
using std::fabs;
#endif

using namespace trf;

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

int main( ) {

  string component = "HitXYPlane2Generator";
  string ok_prefix = component + " (I): ";
  string error_prefix = component + " test (E): ";

  cout << ok_prefix
       << "---------- Testing component " + component
          + ". ----------" << 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 << trf_format;

  cout << ok_prefix << "Test constructor." << endl;
  double dist = 25.0;
  double phi = 1.;
  HitError dhm(2);
  dhm(ClusXYPlane2::IV,ClusXYPlane2::IV) = 0.02;
  dhm(ClusXYPlane2::IV,ClusXYPlane2::IZ) = -0.01;
  dhm(ClusXYPlane2::IZ,ClusXYPlane2::IZ) = 0.03;
  HitXYPlane2Generator gen(dist,phi,dhm);
  cout << gen << endl;
  assert( new HitXYPlane2Generator(dist,phi,dhm) != 0);
  assert( new HitXYPlane2Generator(dist,phi,dhm) != 0);
  assert( new HitXYPlane2Generator(dist,phi,dhm) != 0);
  assert( new HitXYPlane2Generator(dist,phi,dhm) != 0);

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

  cout << ok_prefix << "Generate a list of hits." << endl;
  double v_track = 1.2345;
  double z_track = 6.789;
  SurfXYPlane sxyp(dist,phi);
  TrackVector vec;
  vec(SurfXYPlane::IV) = v_track;
  vec(SurfXYPlane::IZ) = z_track;
  TrackError err;
  VTrack trv(SurfacePtr(sxyp.new_pure_surface()),vec);
  ETrack tre(trv,err);
  ClusterList clusters;
  int nclus = 100;
  for ( int i=0; i<nclus; ++i ) 
    clusters.push_back( gen.new_cluster(trv) );
  // Verify and display the list.
  HitList hits;
  ClusterList::iterator iclu;
  for ( iclu=clusters.begin(); iclu!=clusters.end(); ++iclu ) {
    ClusterPtr pclu = *iclu;
    assert( pclu != 0);
    HitList newhits = pclu->predict(tre);
    assert( newhits.size() == 1 );
    hits.push_back( newhits.front() );
  }
  assert( hits.size() == nclus );
  
  //********************************************************************

  cout << ok_prefix << "Generate list of measured values." << endl;
  double avg1 = 0.0;
  double avg2 = 0.0;
  double sdev11 = 0.0;
  double sdev12 = 0.0;
  double sdev22 = 0.0;
  HitList::iterator ihit;
  for ( ihit=hits.begin(); ihit!=hits.end(); ++ihit ) {
    HitPtr phit = *ihit;
    assert( phit.pointer() != 0);
    double mval1 = phit->measured_vector()(0);
    double mval2 = phit->measured_vector()(1);
    double merr11 = phit->measured_error()(0,0);
    double merr12 = phit->measured_error()(0,1);
    double merr22 = phit->measured_error()(1,1);
    double pval1 = phit->predicted_vector()(0);
    double pval2 = phit->predicted_vector()(1);
    double perr11 = phit->predicted_error()(0,0);
    double perr12 = phit->predicted_error()(0,1);
    double perr22 = phit->predicted_error()(1,1);
    assert( pval1 == v_track);
    assert( pval2 == z_track);
    assert( merr11 ==  dhm(ClusXYPlane2::IV,ClusXYPlane2::IV));
    assert( merr12 ==  dhm(ClusXYPlane2::IV,ClusXYPlane2::IZ));
    assert( merr22 ==  dhm(ClusXYPlane2::IZ,ClusXYPlane2::IZ));
    cout << mval1 << " "<< mval2 <<endl;
    avg1 += mval1;
    avg2 += mval2;
    sdev11 += (mval1-pval1)*(mval1-pval1);
    sdev12 += (mval1-pval1)*(mval2-pval2);
    sdev22 += (mval2-pval2)*(mval2-pval2);
  }
  avg1 /= nclus;
  avg2 /= nclus;
  sdev11 /= nclus;
  sdev12 /= nclus;
  sdev22 /= nclus;
  cout << " Avg1: " << avg1<< " Avg2: " << avg2 << endl;
  cout << "Sdev:[ " << sdev11<<" | "<< sdev12<<" | "<< sdev22<<" ]" << endl;
  double dvv = sqrt(dhm(ClusXYPlane2::IV,ClusXYPlane2::IV));
  double dzz = sqrt(dhm(ClusXYPlane2::IZ,ClusXYPlane2::IZ)); 
  assert( fabs( avg1 - v_track ) < dvv);
  assert( fabs( avg2 - z_track ) < dzz );
  assert( fabs( sqrt(sdev11) - dvv) < dvv );
  assert( fabs( sqrt(sdev22) - dzz) < dzz );
  double dvz = dhm(ClusXYPlane2::IV,ClusXYPlane2::IZ); 
  assert( fabs( sdev12 - dvz) < fabs(dvz) );

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

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

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

}