// file: $isip_ifc/class/algo/KalmanFilter/kf_02.cc
// version: $Id: kf_02.cc 10602 2006-08-17 17:43:12Z ss754 $
//

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

// method: diagnose
//
// arguments:
//  Integral::DEBUG level: (input) debug level for diagnostics
//
// return: a boolean value indicating status
//
boolean KalmanFilter::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
  //
  KalmanFilter kf0(CONVENTIONAL, AR_SCALAR);
  kf0.setAlgorithm(CONVENTIONAL);
  kf0.setImplementation(AR_SCALAR);
  kf0.setOrder(8);
  kf0.setProcessNoise(0.02);
  kf0.setObsnNoise(0.03);
  kf0.setEstimateError(0.0002);
  
  kf0.init();

   // this is running into problems
  // 
  KalmanFilter kf1(kf0);
  if (!kf1.eq(kf0)) {
    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
    //
    KalmanFilter::setGrowSize((long)500);
    
    KalmanFilter* pkf = new KalmanFilter();

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

  // 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
  //
  KalmanFilter kf2;
  kf2.setAlgorithm(CONVENTIONAL);
  kf2.setImplementation(AR_SCALAR);
  kf2.setOrder(8);
  kf2.setProcessNoise(0.02);
  kf2.setObsnNoise(0.03);
  kf2.setEstimateError(0.0002);
  kf2.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);
  
  kf2.write(tmp_file0, (long)0);
  kf2.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
  //
  KalmanFilter kf3;
  kf3.read(tmp_file0, (long)0);
  
  if (!kf2.eq(kf3)) {
    kf2.debug(L"kf2");
    kf3.debug(L"kf3");
    
    return Error::handle(name(), L"i/o", Error::TEST,
			 __FILE__, __LINE__);
  }
  
  kf3.read(tmp_file1, (long)0);

  if (!kf2.eq(kf3)) {
    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
  //
  kf0.setAlgorithm(CONVENTIONAL);
  kf0.setImplementation(AR_SCALAR);

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

  int order_tmp = kf0.getOrder();

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

  kf0.setOrder(4);
  kf0.init();
  kf0.setProcessNoise(0.02);
  kf0.setObsnNoise(0.03);
  kf0.setEstimateError(0.0002);
   order = 4;
  
  MatrixDouble model_trn_tmp(4,4, L"0,1,0,0,0,0,1,0,0,0,0,1,-1,-1,-1,-1");

  MatrixDouble model_trn(order, order, Integral::FULL);;
  kf0.getTransitionModel(model_trn);

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

  VectorDouble model_obs_tmp(L"0, 0, 0, 1");
  VectorDouble model_obs(L"0");
  kf0.getObservationModel(model_obs);

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

  MatrixDouble covar_nse_pro_tmp(order, order, Integral::FULL);
  covar_nse_pro_tmp.setDiagonal(0.02);
  MatrixDouble covar_nse_pro(order, order, Integral::FULL);
  kf0.getProcessNoise(covar_nse_pro);

  if (!covar_nse_pro.almostEqual(covar_nse_pro_tmp)) {
    return Error::handle(name(), L"getProcessNoise", Error::TEST,
			 __FILE__, __LINE__);
  }

  Double covar_nse_obs_tmp = 0.03;
  Double covar_nse_obs = 0.03;
  kf0.getObservationNoise(covar_nse_obs);

  if (!covar_nse_obs.almostEqual(covar_nse_obs_tmp)) {
    return Error::handle(name(), L"getObservationNoise", Error::TEST,
			 __FILE__, __LINE__);
   }

  MatrixDouble covar_err_est_tmp(order, order, Integral::FULL);
  covar_err_est_tmp.setDiagonal(0.0002);
  MatrixDouble covar_err_est(order, order, Integral::FULL);
  kf0.getEstimateError(covar_err_est);

  if (!covar_err_est.almostEqual(covar_err_est_tmp)) {
    return Error::handle(name(), L"getEstimateError", 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
  //
  order = 3;
  kf0.setOrder(order);
  kf0.init();

  // establish some signals
  //
  VectorDouble 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");
  VectorDouble lpc(L"-0.1177746653,-0.33404213,-0.25359899");
  MatrixDouble 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));

  kf0.initTransitionModel(data);

  MatrixDouble trn_tmp(order, order, Integral::FULL);
  kf0.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
//   VectorDouble output;
//   boolean status = true;
//   VectorDouble 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");
  
//   kf0.setObservation(data);
//   status = kf0.computeStates();

  
//   if (!status) {
//     Console::put(L"computeStates unsuccessful");
//   }
//   else {
//     Console::put(L"computeStates successful");
//   }
  
//   output = kf0.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;
}
