// file: $isip_ifc/class/algo/ParticleFilter/pf_02.cc
// version: $Id: pf_02.cc 10409 2006-01-29 05:53:17Z patil $


// isip include files
//
#include "ParticleFilter.h"
#include <Console.h>

// method: diagnose
//
// arguments:
//  Integral::DEBUG level: (input) debug level for diagnostics
//
// return: a boolean value indicating status
//
boolean ParticleFilter::diagnose(Integral::DEBUG level_a) {

  //---------------------------------------------------------------------------
  //
  // 0. preliminaries
  //
  //---------------------------------------------------------------------------

  // output the class name
  //
  if (level_a > Integral::NONE) {
    String output(L"diagnosing class ");
    output.concat(CLASS_NAME);
    output.concat(L": ");
    Console::put(output);
    Console::increaseIndention();
  } 

  //--------------------------------------------------------------------------
  //
  // 1. required public methods
  //     class constructors
  //
  //--------------------------------------------------------------------------

  // set indentation
  //
  if (level_a > Integral::NONE) {
    Console::put(L"testing required public methods...\n");
    Console::increaseIndention();
  }

  // test destructor/constructor(s) and memory management
  //
  ParticleFilter pf0(CONVENTIONAL, AR_SCALAR, 8, 50);
  pf0.setAlgorithm(CONVENTIONAL);
  pf0.setImplementation(AR_SCALAR);
  pf0.setOrder(8);
  pf0.setProcessNoise(0.02);
  pf0.setObsnNoise(0.003);
  pf0.setEstimateError(0.0001);
  pf0.init();

   // this is running into problems
  // 
  ParticleFilter pf1(pf0);
  if (!pf1.eq(pf0)) {
    return Error::handle(name(), L"copy constructor", Error::TEST,
			 __FILE__, __LINE__);
  }
  
  // test large allocation construction and deletion
  //
  if (level_a == Integral::ALL) {
    
    Console::put(L"\ntesting large chunk memory allocation and deletion:\n");
    
    // set the memory to a strange block size so we can hopefully catch any
    // frame overrun errors
    //
    ParticleFilter::setGrowSize((long)500);
    
    ParticleFilter* ppf = new ParticleFilter();

    for (long j = 1; j <= 100; j++) {
      ParticleFilter** ppfs = new ParticleFilter*[j * 100];
      
      // create the objects
      //
      for (long i = 0; i < j * 100; i++) {
	ppfs[i] = new ParticleFilter();
      }
      
      // delete objects
      //
      for (long i = (j * 100) - 1; i >= 0; i--) {
	delete ppfs[i];
      }
      
      delete [] ppfs;
    }
    
    delete ppf;
  }

  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }
  
  //--------------------------------------------------------------------------
  //
  // 2. required public methods
  //     i/o methods 
  //
  //--------------------------------------------------------------------------

  // set indentation
  //
  if (level_a > Integral::NONE) {
    Console::put(L"testing required public methods: i/o methods...\n");
    Console::increaseIndention();
  }
  
  // declare a reference object
  //
  ParticleFilter pf2;
  pf2.setAlgorithm(CONVENTIONAL);
  pf2.setImplementation(AR_SCALAR);
  pf0.setOrder(8);
  pf0.setProcessNoise(0.02);
  pf0.setObsnNoise(0.003);
  pf0.setEstimateError(0.0001);
  pf2.init();
  
  // we need binary and text sof files
  //
  String tmp_filename0;
  Integral::makeTemp(tmp_filename0);
  String tmp_filename1;
  Integral::makeTemp(tmp_filename1);

  // open files in write mode
  //
  Sof tmp_file0;
  tmp_file0.open(tmp_filename0, File::WRITE_ONLY, File::TEXT);
  Sof tmp_file1;
  tmp_file1.open(tmp_filename1, File::WRITE_ONLY, File::BINARY);
  
  pf2.write(tmp_file0, (long)0);
  pf2.write(tmp_file1, (long)0);
  
  // close the files
  //
  tmp_file0.close();
  tmp_file1.close();

  // open the files in read mode
  //
  tmp_file0.open(tmp_filename0);
  tmp_file1.open(tmp_filename1);

  // read the object back
  //
  ParticleFilter pf3;
  pf3.read(tmp_file0, (long)0);
  
  if (!pf2.eq(pf3)) {
    return Error::handle(name(), L"i/o", Error::TEST,
			 __FILE__, __LINE__);
  }
  
  pf3.read(tmp_file1, (long)0);

  if (!pf2.eq(pf3)) {
    return Error::handle(name(), L"i/o", Error::TEST,
			 __FILE__, __LINE__);
  }
  
  // close and delete the temporary files
  //
  tmp_file0.close();
  tmp_file1.close();
  
  File::remove(tmp_filename0);
  File::remove(tmp_filename1);
  
  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }

  //---------------------------------------------------------------------------
  //
  // 3. class-specific public methods:
  //     set and get methods
  //
  //---------------------------------------------------------------------------

  // set indentation
  //
  if (level_a > Integral::NONE) {  
    Console::put(L"testing class-specific public methods: set and get methods...\n");
    Console::increaseIndention();
  }

  // establish an object
  //
  pf0.setAlgorithm(CONVENTIONAL);
  pf0.setImplementation(AR_SCALAR);

  // check that the values were set
  //
  if (pf0.algorithm_d != CONVENTIONAL) {
    return Error::handle(name(), L"setAlgorithm", Error::TEST,
			 __FILE__, __LINE__);
  }
  else if (pf0.implementation_d != AR_SCALAR) {
    return Error::handle(name(), L"setImplementation", Error::TEST,
			 __FILE__, __LINE__);
  }
 
  // test setOrder
  //
  int order = 8;
  pf0.setOrder(order);

  int order_tmp = pf0.getOrder();

  if (order_tmp != order) {
    return Error::handle(name(), L"setOrder", Error::TEST,
			 __FILE__, __LINE__);
  }

  // test setOrder
  //
  int particles = 50;
  pf0.setParticles(order);

  int particle_tmp = pf0.getParticles();

  if (particle_tmp != particles) {
    return Error::handle(name(), L"setParticles", Error::TEST,
			 __FILE__, __LINE__);
  }

  pf0.setOrder(4);
  pf0.setProcessNoise(0.02);
  pf0.setObsnNoise(0.003);
  pf0.setEstimateError(0.0001);
  pf0.init();
  order = 4;
  
  MatrixFloat trans_model_tmp(order, order, L"0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1", Integral::FULL);

  MatrixFloat trans_model(order, order, L"0,0", Integral::FULL);
  pf0.getTransitionModel(trans_model);

  if ( !trans_model.almostEqual(trans_model_tmp)) {
    return Error::handle(name(), L"getTransitionModel", Error::TEST,
			 __FILE__, __LINE__);
  }

  VectorFloat obsn_model_tmp(L"0,0,0,1");
  VectorFloat obsn_model(L"0,0");
  pf0.getObservationModel(obsn_model);

  if ( !obsn_model.almostEqual(obsn_model_tmp)) {
    return Error::handle(name(), L"getObservationModel", Error::TEST,
			 __FILE__, __LINE__);
  }

  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }
  
  //---------------------------------------------------------------------------
  //
  // 4. class-specific public methods:
  //     computation methods
  //
  //---------------------------------------------------------------------------

  // set indentation
  //
  if (level_a > Integral::NONE) {
    Console::put(L"testing class-specific public methods: computational methods...\n");
    Console::increaseIndention();
  }

  // setting up the order and initializing the parameters
  //
  pf0.setOrder(3);
  pf0.setProcessNoise(0.02);
  pf0.setObsnNoise(0.003);
  pf0.setEstimateError(0.0001);
  pf0.init();

  // establish some signals
  //
  VectorFloat data(L"2.5,5.6,7.8,1.2,3.4,9.0,0.002,2.5,6.1,1.2,8.1,1.2,9.2,0.22,0.3,0.4,1.2,9.23");
  VectorFloat lpc(L"0.5365,-1.1431,1.3667");
  MatrixFloat trn;
  trn.setDimensions(order, order);
  trn.setValue(0, 1, 1);
  trn.setValue(1, 2, 1);
  trn.setValue(2, 0, (double)lpc(2));
  trn.setValue(2, 1, (double)lpc(1));
  trn.setValue(2, 2, (double)lpc(0));

  pf0.initializeTransitionModel(data);

  MatrixFloat trn_tmp(order, order, L"0", Integral::FULL);
  pf0.getTransitionModel(trn_tmp);

  if(!trn_tmp.almostEqual(trn)) {
    return Error::handle(name(), L"initTransitionModel",
			 Error::TEST, __FILE__, __LINE__);
  }

  // check for compute method remaining
  //

  //   // to check for the results here,
//   // all the parameters have been
//   VectorFloat output;
//   boolean status = true;
//   VectorFloat filtered_data(L"1, 1.99998, 3.00004, 3.99997, 3.15382, 4.15384, 5.15386, 6.15381, 6.13204, 7.13207, 8.13208, 9.13204, 9.58212, 10.5821, 11.5822");
  
//   pf0.setObservation(data);
//   status = pf0.computeStates();

  
//   if (!status) {
//     Console::put(L"computeStates unsuccessful");
//   }
//   else {
//     Console::put(L"computeStates successful");
//   }
  
//   output = pf0.getResult();
  
//   if (!output.almostEqual(filtered_data)) {
//       return Error::handle(name(), L"computeStates",
// 			  Error::TEST, __FILE__, __LINE__);
//   }
   
  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }
  
  //---------------------------------------------------------------------------
  //
  // 5. print completion message
  //
  //---------------------------------------------------------------------------

  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }
  
  if (level_a > Integral::NONE) {
    String output(L"diagnostics passed for class ");
    output.concat(name());
    output.concat(L"\n");
    Console::put(output);
  }
  
  // exit gracefully
  //
  return true;
}
