// file: $isip/class/numeric/NonlinearOptimization/nonlin_02.cc
// version: $Id: nonlin_02.cc 6926 2001-05-03 18:42:06Z srivasta $
//

// isip include files
//
#include "NonlinearOptimization.h"
#include <Sigmoid.h>
#include <Console.h>

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

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

  // output the class name
  //
  if (level_a > Integral::NONE) {
    SysString output(L"diagnosing class ");
    output.concat(CLASS_NAME);
    output.concat(L": ");
    Console::put(output);
    Console::increaseIndention();
  }
  
  //--------------------------------------------------------------------------
  //
  // 1. required public methods
  //
  //--------------------------------------------------------------------------

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

  // do a quick test of constructors, destructors, and memory management
  // methods
  //
  NonlinearOptimization::setGrowSize(5);
  NonlinearOptimization* nonlin0 = new NonlinearOptimization;
  NonlinearOptimization* nonlin1 = new NonlinearOptimization(*nonlin0);
  NonlinearOptimization* nonlin2 = new NonlinearOptimization[10];

  // clear pointers
  //
  delete [] nonlin2;

  // test the setDebug method
  //
  setDebug(debug_level_d);
  
  // test the eq method
  //
  delete nonlin1;
  nonlin1 = new NonlinearOptimization;
  nonlin2 = new NonlinearOptimization(*nonlin1);

  if (!nonlin2->eq(*nonlin1)) {
    nonlin2->debug(L"nonlin2");
    nonlin1->debug(L"nonlin1");
    return Error::handle(name(), L"eq", Error::TEST, __FILE__, __LINE__);
  }

  // test the clear method
  //
  delete nonlin1;
  nonlin1 = new NonlinearOptimization;
  nonlin2->clear();
  if (!nonlin2->eq(*nonlin1)) {
    nonlin2->debug(L"nonlin2");
    nonlin1->debug(L"nonlin1");
    return Error::handle(name(), L"clear", Error::TEST, __FILE__, __LINE__);
  }

  // test the operator= method
  //
  *nonlin1 = *nonlin0;
  if (!nonlin2->eq(*nonlin0)) {
    nonlin2->debug(L"nonlin2");
    nonlin1->debug(L"nonlin1");
    return Error::handle(name(), L"operator=", Error::TEST, __FILE__,__LINE__);
  }

  // clear the pointers
  //
  delete nonlin0;
  delete nonlin1;
  delete nonlin2;

  // test i/o methods
  //
  NonlinearOptimization nonlin5;
  NonlinearOptimization nonlin6;

  // 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);

  nonlin5.write(tmp_file0, 0);
  nonlin6.write(tmp_file0, 1);

  nonlin5.write(tmp_file1, 0);
  nonlin6.write(tmp_file1, 1);

  // 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 NonlinearOptimizations from the text file
  //
  NonlinearOptimization nonlin7;
  NonlinearOptimization nonlin8;
  nonlin7.read(tmp_file0, 0);
  nonlin8.read(tmp_file0, 1);

  if (!nonlin7.eq(nonlin5) || !nonlin8.eq(nonlin6)) {
    nonlin5.debug(L"nonlin5");
    nonlin6.debug(L"nonlin6");
    nonlin7.debug(L"nonlin7");
    nonlin8.debug(L"nonlin8");
    return Error::handle(name(), L"read/write text", Error::TEST,
			 __FILE__, __LINE__);
  }

  // read the NonlinearOptimizations from the binary file
  //
  nonlin7.clear();
  nonlin8.clear();
  nonlin7.read(tmp_file1, 0);
  nonlin8.read(tmp_file1, 1);

  if (!nonlin7.eq(nonlin5) || !nonlin8.eq(nonlin6)) {
    nonlin5.debug(L"nonlin5");
    nonlin6.debug(L"nonlin6");
    nonlin7.debug(L"nonlin7");
    nonlin8.debug(L"nonlin8");
    return Error::handle(name(), L"read/write binary", Error::TEST,
			 __FILE__, __LINE__);
  }

  // close and  delete the temporary files
  //
  tmp_file0.close();
  tmp_file1.close();
  
  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }
  
  //---------------------------------------------------------------------------
  //
  // 2. class-specific public methods:
  //     Levenberg Marquardt methods
  //
  //---------------------------------------------------------------------------

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

  // declare the measured data
  //
  VectorFloat params_fl;
  VectorDouble params_doub;  

  VectorFloat x_fl;
  VectorDouble x_doub;

  VectorFloat y_fl;
  VectorDouble y_doub;

  VectorFloat stddev_fl;
  VectorDouble stddev_doub;

  VectorFloat final_params_fl;
  VectorDouble final_params_doub;

  // open the test file
  //
  String test_file(L"diagnose.sof");
  Sof test_sof;
  test_sof.open(test_file);

  // read in the test data
  //
  String x_tag(L"x");
  String y_tag(L"y");
  String stddev_tag(L"stddev");
  String init_params_tag(L"initial_params");
  String final_params_tag(L"final_params");

  params_fl.read(test_sof, 0, init_params_tag);
  x_fl.read(test_sof, 0, x_tag);
  y_fl.read(test_sof, 0, y_tag);
  stddev_fl.read(test_sof, 0, stddev_tag);
  final_params_fl.read(test_sof, 0, final_params_tag);
  
  params_doub.read(test_sof, 0, init_params_tag);
  x_doub.read(test_sof, 0, x_tag);
  y_doub.read(test_sof, 0, y_tag);
  stddev_doub.read(test_sof, 0, stddev_tag);
  final_params_doub.read(test_sof, 0, final_params_tag);

  test_sof.close();
  
  // test levenbergMarquardt
  //
  float chi_sq_fl = 0;
  double chi_sq_doub = 0;
  NonlinearOptimization::levenbergMarquardt(params_fl, chi_sq_fl, x_fl,
					    y_fl, stddev_fl,
				    NonlinearOptimization::diagnoseSigmoidFl);
  NonlinearOptimization::levenbergMarquardt(params_doub, chi_sq_doub, x_doub,
					    y_doub, stddev_doub,
				   NonlinearOptimization::diagnoseSigmoidDoub);

  // verify that results match
  //
  if (!final_params_fl.almostEqual(params_fl)) {
    params_fl.debug(L"actual params");
    final_params_fl.debug(L"expected params");    
    return Error::handle(name(), L"levenbergMarquardt (float)", Error::TEST,
			 __FILE__, __LINE__);
  }
  
  if (!final_params_doub.almostEqual(params_doub)) {
    params_doub.debug(L"actual params");
    final_params_doub.debug(L"expected params");    
    return Error::handle(name(), L"levenbergMarquardt (double)", Error::TEST,
			 __FILE__, __LINE__);
  }
  
  // reset indentation
  //
  if (level_a > Integral::NONE) {
    Console::decreaseIndention();
  }
  
  //---------------------------------------------------------------------------
  //
  // 4. print completion message
  //
  //---------------------------------------------------------------------------

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

  // exit gracefully
  //
  return true;
}


// method: diagnoseSigmoidFl
//
// arguments:
//  float& y: (output) f(x)
//  VectorFloat& derivatives: (output) df/dparam(i) (x)
//  const float x: (input) test point
//  const VectorFloat& params: (input) parameters to test
//
// return: boolean value indicating status
//
// method to compute a sigmoid and its derivatives
//
boolean NonlinearOptimization::diagnoseSigmoidFl(float& y_a,
						 VectorFloat& derivatives_a,
						 const float x_a,
						 const VectorFloat& params_a) {

  // setup the sigmoid
  //
  Sigmoid sig;
  sig.set(params_a(0), params_a(1), 0, params_a(2));

  // find f(x) and the derivatives 
  //
  float tmp;
  sig.compute(y_a, x_a);
  sig.derivativeGain(tmp, x_a);
  derivatives_a(0) = tmp;
  sig.derivativeSlope(tmp, x_a);
  derivatives_a(1) = tmp;
  sig.derivativeYOffset(tmp, x_a);
  derivatives_a(2) = tmp;

  // exit gracefully
  //
  return true;
}


// method: diagnoseSigmoidDoub
//
// arguments:
//  double& y: (output) f(x)
//  VectorDouble& derivatives: (output) df/dparam(i) (x)
//  const double x: (input) test point
//  const VectorDouble& params: (input) parameters to test
//
// return: boolean value indicating status
//
// method to compute a sigmoid and its derivatives
//
boolean NonlinearOptimization::diagnoseSigmoidDoub(double& y_a,
						VectorDouble& derivatives_a,
  					        const double x_a,
						const VectorDouble& params_a) {

  // setup the sigmoid
  //
  Sigmoid sig;
  sig.set(params_a(0), params_a(1), 0, params_a(2));

  // find f(x) and the derivatives 
  //
  double tmp;
  sig.compute(y_a, x_a);
  sig.derivativeGain(tmp, x_a);
  derivatives_a(0) = tmp;
  sig.derivativeSlope(tmp, x_a);
  derivatives_a(1) = tmp;
  sig.derivativeYOffset(tmp, x_a);
  derivatives_a(2) = tmp;

  // exit gracefully
  //
  return true;
}


