/*****************************************************************************
 *                                                                           *
 * FOX Controller Library, Version 1.3.3.                                    *
 *                                                                           *
 * Copyright (C) 1998,1999,2000 Russell Smith (rl.smith@auckland.ac.nz)      *
 *                                                                           *
 * The FOX Controller Library is free software; you can redistribute it      *
 * and/or modify it under the terms of the GNU Library General Public        *
 * License as published by the Free Software Foundation; either version      *
 * 2 of the License, or (at your option) any later version.                  *
 *                                                                           *
 * The FOX Controller Library is distributed in the hope that it will be     *
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of    *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
 * Library General Public License for more details.                          *
 *                                                                           *
 * You should have received a copy of the GNU Library General Public         *
 * License along with the Fox Controller Library; see the file COPYING.LIB.  *
 * If not, write to the Free Software Foundation, Inc., 59 Temple Place -    *
 * Suite 330, Boston, MA 02111-1307, USA.                                    *
 *                                                                           *
 *****************************************************************************/

/*

TODO:

* Instead of storing C*Lambda, store Lambda and do just one multiplication by
  C when calculating delta values.
* Store ny2=ny*ny


*/

#include "trace-n.h"
#include "foxstuff.h"


TraceN::TraceN (int _ny, cftype *_A, cftype *_C, int _n, GetAtoi *_atoi)
{
  valid = 0;
  lambda = NULL;
  ny = _ny;
  checkthat (ny <= MAX_NY,"ny > %d in TraceN ctor",MAX_NY);
  A = _A;
  C = _C;
  n = _n;
  atoi = _atoi;
  checkmemptr (lambda = new cftype [n * ny]);
  for (int i=0; i < (n*ny); i++) lambda[i] = 0.0;
  Reset();
  valid = 1;
}


TraceN::~TraceN()
{
  delete[] lambda;
}


void TraceN::Reset()
{
  T = -1;			// so next time is 0
  p = -1;			// so next buffer position is 0
  T0 = 0;
  for (int i=0; i < ny; i++) Lambda[i] = 0.0;
  for (int i=0; i < ny; i++) CAtop[i] = C[i];	// Get C*A^p = C
}


void TraceN::Next (cftype e)
{
  T++;
  p++;
  if (p >= n) p = 0;
  if (p==0) {
    T0 = T;
    for (int i=0; i<ny; i++) Lambda[i] = 0.0;
    for (int i=0; i<ny; i++) CAtop[i] = C[i];		// Get C*A^p = C
  }

  // Get Lambda += e * C * A^p
  for (int i=0; i<ny; i++) Lambda[i] += e * CAtop[i];

  // save Lambda in lambda[p]
  cftype *lambda_ptr = lambda + p*ny;
  for (int i=0; i<ny; i++) *(lambda_ptr++) = Lambda[i];

  // Get C*A^p = C*A^p*A = C*A^(p+1) ... for next time around
  cftype tmp[MAX_NY];
  MatrixMultiply1 (ny,tmp,CAtop,A);	// tmp = CAtop * A
  for (int i=0; i<ny; i++) CAtop[i] = tmp[i];
}


cftype * TraceN::GetDelta (int t)
{
  static cftype result [MAX_NY];
  int i;
  myassert (!(t > T || t < 0),"Bad t in TraceN::GetDelta (1)");
  i = t-T0;		// i = buffer position for this time
  if (i < 0) {
    i += n;
    myassert (i > p,"Bad t in TraceN::GetDelta (2)");
  }

  // now i is in the range 0..n-1
  myassert (i != p+1,"Bad t in TraceN::GetDelta (3)");
  if (i <= p) {
    if (i > 0) {
      // get result = (lambda[p] - lambda[i-1]) * A^(-i)
      cftype tmp[MAX_NY];
      for (int j=0; j<ny; j++)
	tmp[j] = lambda[p*ny + j] - lambda[(i-1)*ny + j];
      atoi->PostMulByAtoMinus (result,tmp,i);
      return result;
    }
    else {
      // get result = lambda[p] * A^(-i)
      atoi->PostMulByAtoMinus (result,lambda + p*ny,i);
      return result;
    }
  }
  else {
    // get result = (lambda[n-1]-lambda[i-1])*A^(-i) + lambda[p]*A^(T0-t)
    cftype tmp[MAX_NY];
    for (int j=0; j<ny; j++)
      tmp[j] = lambda[(n-1)*ny + j] - lambda[(i-1)*ny + j];
    atoi->PostMulByAtoMinus (result,tmp,i);
    atoi->PostMulByAtoPlus (tmp,lambda + p*ny,T0-t);
    for (int j=0; j<ny; j++) result[j] += tmp[j];
    return result;
  }
}

//***************************************************************************
// Testing

/*

#include <stdlib.h>
#include <time.h>


cftype frandom()
{
  return cftype(random())/cftype(RAND_MAX);
}


// Data for a 4x4 matrix with these eigenvalues: -0.2,-0.4,-0.6,-0.8
static cftype Adata[16] = {
  -0.32146477377367,  0.01342048538994, -0.32683564574661, -0.25927359552655,
  -0.47937936441504, -0.59080345828772,  0.75112028936896,  0.01403422329742,
   0.13624192938211,  0.09771365160561, -0.68725222645531, -0.30401969355492,
   0.04009943559339, -0.01845933875761, -0.15025144173708, -0.40047954148329};

static cftype Cdata[4] = {
   0.72864159177390,  0.31798791565074,  0.84764863450093,  0.14789135101475};



int main()
{
  int ny = 4;
  int n = 200;
  cftype e,estore[n];


  // calculate and store C*A^i for i=1..19
  cftype CAi[20][4];
  cftype tmp[4];
  for (int i=0; i<4; i++) tmp[i] = CAi[0][i] = Cdata[i];
  for (int i=1; i<20; i++) {
    // get CAi[i] = tmp * Adata
    for (int j=0; j<4; j++) {
      cftype sum = 0.0;
      for (int k=0; k<4; k++) sum += tmp[k] * Adata[k*4 + j];
      CAi[i][j] = sum;
    }
    for (int j=0; j<4; j++) tmp[j] = CAi[i][j];
  }

  // print out those matrices
  //  for (int i=0; i<20; i++) {
  //    for (int j=0; j<4; j++) printf ("%8.4f ",CAi[i][j]);
  //    printf ("\n");
  //  }

  GetAtoi atoi (4, Adata, 20);
  TraceN trace (4, Adata, Cdata, 20, &atoi);

  srandom(time(NULL));

  e = 0;
  for (int T=0; T<n; T++) {
    e += 0.1*(frandom()-0.5);
    estore[T] = e;

    printf ("Next value: T=%d, error=%.8f\n",T,e);
    trace.Next (e);

    int tstart = 0;
    if (T >= 18) tstart = T-18;

    for (int t=tstart; t<=T; t++) {

      // calc delta_t the hard way
      cftype delta1[4];
      for (int i=0; i<4; i++) delta1[i] = 0.0;
      for (int i=t; i<=T; i++) {
	for (int j=0; j<4; j++) delta1[j] += estore[i] * CAi[i-t][j];
      }
      // printf ("T=%3d  t=%3d  delta1 = %.8f %.8f %.8f %.8f\n",T,t,
      //   delta1[0], delta1[1], delta1[2], delta1[3]);

      // calc delta_t the easy way
      cftype *delta2 = trace.GetDelta (t);

      printf ("T=%3d  t=%3d  delta1-delta2=%12.8f %12.8f %12.8f %12.8f\n",T,t,
	      delta1[0]-delta2[0],
	      delta1[1]-delta2[1],
	      delta1[2]-delta2[2],
	      delta1[3]-delta2[3]);
    }
  }

  return 0;
}
*/