// file: $isip_ifc/class/algo/Lyapunov/lyap_05.cc
// version: $Id: lyap_05.cc 10636 2007-01-26 22:18:09Z tm334 $
//

// isip include files
//
#include "Lyapunov.h"

// method: apply
//
// arguments:
//  Vector<AlgorithmData>& output: (output) output data
//  const Vector< CircularBuffer<AlgorithmData> >& input: (input) input data
//
// return: a bool8 value indicating status
//
// this method calls computation method with appropriate input and
// output data types
//
bool8 Lyapunov::apply(Vector<AlgorithmData>& output_a,
			  const Vector< CircularBuffer<AlgorithmData> >&
			  input_a) {

  // determine the number of input channels and force the output to be
  // that number
  //
  int32 len = input_a.length();
  output_a.setLength(len);
  bool8 res = true;

  frame_index_d++;
  
  // start the debugging output
  //
  displayStart(this);

  // loop over the channels and call the compute method
  //
  for (int32 c = 0; c < len; c++) {

    // display the channel number
    //
    displayChannel(c);

    // call AlgorithmData::makeMatrixFloat to force the output vector for
    // this channel to be a MatrixFloat, call AlgorithmData::getVectorFloat
    // on the input for this channel to check that the input is
    // already a VectorFloat and return that vector.
    //
    if (input_a(c)(0).getDataType() == AlgorithmData::MATRIX_FLOAT) {
      
      res &= compute(output_a(c).makeVectorFloat(),
		     input_a(c)(0).getMatrixFloat(),
		     AlgorithmData::RPS, c);
    }

    // error unknown coefficient type
    //
    else {
      return Error::handle(name(), L"apply", ERR_UNCTYP,
			   __FILE__, __LINE__);
    }
    
    // set the coefficient type for the output
    //    
    output_a(c).setCoefType(AlgorithmData::LYAPUNOV);
  }

  // possibly display the data
  //
  display(output_a, input_a, name());
    
  // finish the debugging output
  //
  displayFinish(this);

  // exit gracefully
  //
  return res;
}

// method: compute
//
// arguments:
//  VectorFloat& output: (output) Lyapunov matrix
//  const MatrixFloat& input: (input) input RPS matrix
//  AlgorithmData::COEF_TYPE input_coef_type: (input) type of input
//  int32 index: (input) channel number
//
// return: a bool8 value indicating status
//
// this method computes the covariance coefficients from signal
//
bool8 Lyapunov::compute(VectorFloat& output_a,
			    const MatrixFloat& input_a,
			    AlgorithmData::COEF_TYPE input_coef_type_a,
			    int32 index_a) {
  
  
  if (algorithm_d == LINEAR_TANGENT_MAP) {
    int32 num_rows = input_a.getNumRows();
    if (num_rows > (int32)0) {
      computeLyapunovLinearTangentMapMethod(output_a, input_a);
    }
  }
  else {
    return Error::handle(name(), L"compute", ERR_UNKALG, __FILE__, __LINE__);
  }

  // exit gracefully
  //  
  return true;
}

// method: computeLyapunovLinearTangentMapMethod
// 
// arguments:
//  VectorFloat& lyap: (output) Lyapunov matrix
//  const MatrixFloat& rps: (input) RPS matrix
//
//  return: a bool8 value indicating status
//
// this method computes the Lyapunov exponents from an input RPS matrix
//
bool8 Lyapunov::computeLyapunovLinearTangentMapMethod(VectorFloat& lyap_a,
							const MatrixFloat& rps_a) {

  int32 i, j, k;
  int32 global_dim = rps_a.getNumColumns() - (int32)1;
  // num of phase-space points
  //
  int32 N = rps_a.getNumRows();
  VectorFloat continuity_flag;
  rps_a.getColumn(continuity_flag, global_dim);
  VectorLong discontinuity_points(N);
  MatrixFloat rps(rps_a);
  rps.setDimensions(N, global_dim, true);
  int32 N1 = N - evolve_step_d;

  if (reinit_step_d < (int32)0) {
    reinit_step_d = evolve_step_d;
  }
  if (local_dim_d < (int32)0) {
    local_dim_d = global_dim;
  }
  if (num_neighbor_subgroups_d < (int32)0) {
    num_neighbor_subgroups_d = num_neighbors_d;
  }
  //  else if (num_neighbor_subgroups_d <= num_neighbors_d) {
  //  }
  else if (num_neighbor_subgroups_d > num_neighbors_d) {
    // cannot have more number of subgroups than number of neighbors
    //
    return Error::handle(name(), L"computeLyapunovLinearTangentMapMethod", ERR_SUBGRPS,
			 __FILE__, __LINE__);
  }
    
  // need to have at least local_dim_d neighbors to calculate trajectory matrix
  //
  if (num_neighbors_d < local_dim_d) {
    return Error::handle(name(), L"computeLyapunovLinearTangentMapMethod", ERR_NGH,
			 __FILE__, __LINE__);
  }
  
  VectorFloat d(N1);
  Float INFINITY_TMP = 10000.00;
  MatrixFloat B(num_neighbor_subgroups_d, global_dim, Integral::FULL);
  MatrixFloat B_evolve(num_neighbor_subgroups_d, global_dim, Integral::FULL);
  MatrixFloat B_reduced(num_neighbors_d, local_dim_d, Integral::FULL), B_inv;
  MatrixFloat B_evolve_reduced(num_neighbors_d, local_dim_d, Integral::FULL);

  VectorFloat x, y;
  int32 null_diagonal;
  MatrixFloat q(local_dim_d, local_dim_d, Integral::FULL), r;
  int32 num_valid_exponents = 0;
     
  lyap_a.assign(local_dim_d, 0.0);

  // initialize q to identity matrix
  //
  q.assign(0.0);
  for (i = 0; i < local_dim_d; i++) {
    q.setValue(i, i, 1.0);
  }

  int32 num_discontinuities = 0;
  for (i = 0; i < N; i++) {
    Float tmp_flag = continuity_flag(i);
    if (tmp_flag.almostEqual((float32)0.0)) {
      discontinuity_points(num_discontinuities) = i;
      num_discontinuities++;
    }
  }
  for (i = 0; i < num_discontinuities; i++) {
    for (j = 1; j < evolve_step_d; j++ ) {
      continuity_flag(discontinuity_points(i) - j) = (float32)0.0;
    }
  }

  if (N < (reinit_step_d * num_steps_d)) {
     num_steps_d = (int32)(N1 / reinit_step_d);
     //     reinit_step_d = (int32)(N / num_steps_d);
    
    // add warning that num_steps was changed
    //
  }

  for (i = 0; i < num_steps_d * reinit_step_d; i += reinit_step_d) {
    VectorLong I;
    d(i) = INFINITY_TMP;
    Float tmp_flag = continuity_flag(i);
    if (tmp_flag.almostEqual((float32)0.0)) {
      continue;
    }

    rps.getRow(x, i);
    for (j = 0; j < N1; j++) {
      if (j != i) {
	Float tmp_flag = continuity_flag(j);
	if (tmp_flag.almostEqual((float32)0.0)) {
	  d(j) = INFINITY_TMP;
	  continue;
	}
	rps.getRow(y, j);
	
	// L_Inf norm (max magnitude difference) for distance
	//
	y.sub(x);
	d(j) = y.maxMag();

	// Euclidean norm for distance
	//
	//	d(j)=x.distance(y);

	// make sure that the point itself is not taken as neighbor
	// this also serves to define a minimum distance for
	// neighbors, helpful for noise robustness
	//
	if (d(j) <= inner_radius_d) {
	  d(j) = INFINITY_TMP;
	}
      }
    }

    // do not choose same points in neighbor (actually, this chooses 
    // neighbors such that no two neighbors are equidistant)
    //
    d.index(I);
    int32 num_different_neighbors = 1;
    for (j = 1; j < N1; j++) {
      if (Integral::abs(d(I(j)) - d(I(j - 1))) >= inner_radius_d) {
	num_different_neighbors++;
	I(num_different_neighbors - 1) = I(j);
	if (num_different_neighbors == num_neighbors_d) {
	  break;
	}
      }
    }

    // form the (averaged subgroups of) neighborhood matrix and
    // its evolution
    //
    B.assign((float32)0.0);
    B_evolve.assign((float32)0.0);
    for (j = 0; j < num_neighbors_d; j++) {
      int32 subgroup_index = j % num_neighbor_subgroups_d;
      for (k = 0; k < global_dim; k++) {
	Float tmp1;
	rps.getValue(tmp1, I(j), k);
	tmp1 += -x(k) + B.getValue(subgroup_index, k);
	B.setValue(subgroup_index, k, tmp1);
      }
      for (k = 0; k < global_dim; k++) {
	Float tmp1;	
	Float tmp2;
	rps.getValue(tmp1, I(j) + (int32)evolve_step_d, k);
	rps.getValue(tmp2, i + (int32)evolve_step_d, k);
	tmp1 += -tmp2 + B_evolve.getValue(subgroup_index, k);
	B_evolve.setValue(subgroup_index, k, tmp1);
      }
    }
    
    
    /*
    // form the neighborhood matrix and its evolution
    //
    for (j = 0; j < num_neighbors_d; j++) {
      for (k = 0; k < global_dim; k++) {
	Float tmp1;
	rps.getValue(tmp1, I(j), k);
	B.setValue(j, k, tmp1 - x(k));
      }
      for (k = 0; k < global_dim; k++) {
	Float tmp1;	
	Float tmp2;
	rps.getValue(tmp1, I(j) + (int32)evolve_step_d, k);
	rps.getValue(tmp2, (int32)(i + evolve_step_d), k);
	B_evolve.setValue(j, k, tmp1 - tmp2);
      }
    }
    */
    
    // Local SVD reduction of neighborhood matrix and its evolution
    //
    MatrixFloat u, w, v;

    if (local_dim_d < global_dim) {
      B.decompositionSVD(u, w, v);
      v.setDimensions(global_dim, local_dim_d, true);
      B_reduced.mult(B, v);

      B_evolve.decompositionSVD(u, w, v);
      v.setDimensions(global_dim, local_dim_d, true);
      B_evolve_reduced.mult(B_evolve, v);
    }
    else if (local_dim_d == global_dim) {
      B_reduced = B;
      B_evolve_reduced = B_evolve;
    }
    else {
      // local dimension cant be more than global one
      //
      return Error::handle(name(), L"computeLyapunovLinearTangentMapMethod", ERR_DIM,
			   __FILE__, __LINE__);
    }

    // pseudo-inverse of B
    //

    B_reduced.decompositionSVD(u, w, v);
    for (j = 0; j < local_dim_d; j++) {
      w.setValue(j, j, 1.0 / w.getValue(j, j));
    }
    B_inv.mult(v, w);
    u.transpose();
    B_inv.mult(u);

    // calculate trajectory matrix T
    //
    MatrixFloat T;
    T.mult(B_inv, B_evolve_reduced);
    T.transpose();

    // treppen-iteration
    //
    T.mult(q);
    T.decompositionQR(q, r);

    VectorFloat present_lyap_a(local_dim_d);
    null_diagonal = 0;
    for (j = 0; j < local_dim_d; j++) {
      Float r_tmp = r.getValue(j, j);
      r_tmp = r_tmp.abs();
      if (r_tmp > (float32)0.0) {
	present_lyap_a(j) = log(r_tmp);
      }
      else {
	null_diagonal = 1;
	break;
      }
    }
    
    if (!null_diagonal) {
      float32 sampling_time = (float32)1.0 / sample_freq_d;
      float32 tmp = sampling_time * evolve_step_d;
      present_lyap_a.div(tmp);
      lyap_a.add(present_lyap_a);
      num_valid_exponents++;
    }
  }
  if (num_valid_exponents)
    lyap_a.div(num_valid_exponents);
  else
    lyap_a.assign((float32)0);

  // exit gracefully
  //
  return true;//status;
}