#include "Vector.hh"
#include "Matrix.hh"
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include "lorenz.hh"
#include "lorenz1.hh"
#include "lorenz3.hh"
#include "PeriodicOrbit.hh"

static int LARGENUM=1000000; //used by findmin(), findMinDist()
static double returnNormalizingFactor(int n, double T){
  double lambda = 0.905;
  lambda = pow(2.71828182845905, lambda);
  return pow(lambda, T/n);
}

//po(input) = 3xn matrix
//T(input) = its period
//return value is its top Lyapunov exponent
double returnLyap(Matrix& po, double T){
  assert(po.getm()==3);
  int n = po.getn();
  Matrix A(9, n);
  ComputeLinearization(po, A, lorenzJ);
  A.scale(T/(2*PI)); //forgot to do this initially!
  Matrix Y(9, n);
  FindFundSolnB(A, Y);
  Matrix m(3,3);
  Matrix mm;
  int i, j;
  for(i=0; i < 3; i++)
    for(j=0; j < 3; j++)
      m(i,j) = (i==j)?1:0;
  double factor = returnNormalizingFactor(n, T);
  for(i=0; i < n; i++){
    mm.shadow(Y.getColumn(i), 3, 3);
    mult(mm, m);
    m.scale(1/factor);
  }
  Vector er(3), ei(3);
  eig(m, er, ei);
  assert(fabs(ei(0))+fabs(ei(1))+fabs(ei(2))<1e-10);
  double ee;
  ee = fabs(er(0));
  if(fabs(er(1)) > ee) 
    ee = fabs(er(1));
  if(fabs(er(2)) > ee)
    ee = fabs(er(2));
  double lyap;
  lyap = log(ee) + n*log(factor);
  lyap /= T;
  return lyap;
}

//dir(input) = directory
//N(input) = index of po in directory
//return values is its Lyapunov exponents
double returnLyap(char *dir, int N){
  char s1[100] = "orbit";
  char s2[100] = "T";
  char s3[100] = "n";
  char tempstr[100];
  strcpy(tempstr, dir); strcat(tempstr, s1); strcpy(s1, tempstr);
  strcpy(tempstr, dir); strcat(tempstr, s2); strcpy(s2, tempstr);
  strcpy(tempstr, dir); strcat(tempstr, s3); strcpy(s3, tempstr);
  char s[50];
  sprintf(s, "%d.dat", N);
  strcat(s1, s);
  strcat(s2, s);
  strcat(s3, s);
  Matrix tt(1,1);
  tt.input(s2);
  double T = tt(0,0);
  tt.input(s3);
  int n = tt(0,0);
  Matrix po(3,n);
  po.input(s1);
  return returnLyap(po, T);
}

//dir = directory
//L exponents of all pos with indices from m(input) to 
//n(input) are computed and saved  
void saveLyap(char *dir, int m, int n){
  char fname[100];
  int i;
  for(i=m; i <= n; i++){
    double lyap = returnLyap(dir, i);
    sprintf(fname, "%slyap%d.dat",dir, i);
    Matrix tt(1,1);
    tt(0,0) = lyap;
    cout<<"saving "<<fname<<" ..."<<endl;
    tt.output(fname);
  }
}



//v(input) = a vector thought of as a periodic signal
//indx(input) = index just preceding a local min
//shft(output) = shft needed to reach local min
//mmin(output) =  the local min
void  findLocalMin(Vector& v, int indx, double& shft, double& lmin){
  double min;
  int n = v.getSize();
  Vector vd(n);
  diff(v, vd);
  int i=0;
  int j=0;
  i = indx;
  assert(vd(i)*vd((i+1)%n) <= 0);
  double t1 = 0;
  double t2 = 2*PI/n;
  double y1 = vd(i);
  double y2 = vd((i+1)%n);
  Vector vdshft(n);
  int numItns = 20;
  j = 0;
  while(j < numItns){
    j++;
    if(fabs(y1) < 1e-11)
      break;
    if(fabs(y2) < 1e-11){
      t1 = t2;
      break;
    }
    double dy1 = (y2 - y1)/(t2 - t1);
    double tt = t1 - y1/dy1;
    t2 = t1;
    y2 = y1;
    t1 = tt;
    shift(vd, t1, vdshft);
    y1 = vdshft(i);
  }
  shft = t1;
  shift(v, shft, vdshft);
  min = vdshft(i);
  lmin = min;
}

//v(input) = vector thought of as periodic signal
//iindx(output) = index preceding min
//sshft(output) = shift to reach min
//gmin(output) = global min of v
void findMin(Vector& v, int& iindx, double& sshft, double& gmin){
  const int SCAN = 20;
  double min[SCAN];
  int indx[SCAN];
  double shft[SCAN];
  int n = v.getSize();
  Vector vd(n);
  diff(v, vd);
  int i=0;
  for(i=0; i < SCAN; i++)
    min[i] = LARGENUM;
  for(i=0; i < n; i++){
    int flag = ((vd(i)<=0) && (vd((i+1)%n)>=0));
    if(!flag)
      continue;
    flag = (v(i)< min[SCAN-1]);
    if(!flag)
      continue;
    indx[SCAN-1] = i;
    min[SCAN-1] = v(i);
    int j;
    int k=0;
    for(j=0; j < SCAN; j++)
      if(min[j] > min[k])
	k = j;
    int tempindx;
    double tempmin;
    tempmin = min[k];
    min[k] = min[SCAN-1];
    min[SCAN-1] = tempmin;//largest min is at SCAN-1
    tempindx = indx[k];
    indx[k] = indx[SCAN-1];
    indx[SCAN-1] = tempindx;
  }
  for(i=0; i < SCAN; i++)
    if(min[i] < LARGENUM/2.0)
     findLocalMin(v, indx[i], shft[i], min[i]);
  double mmin;
  int k=0;
  for(i=0; i < SCAN; i++)
    if(min[i] < min[k])
      k = i;
  mmin = min[k];
  iindx = indx[k];
  sshft = shft[k];
  //cout<<"index: "<<iindx<<endl;
  //cout<<"shift factor: "<<sshft*n/(2*PI)<<endl;
  gmin =mmin;
}
  


//po1(input) = 3xn matrix
//factor(input) = replication factor
//po2(output) = 3x(factor*n) matrix
void replicateMatrix(Matrix& po1, int factor, Matrix& po2){
  assert(po1.getm()==po2.getm());
  assert(po1.getn()*factor==po2.getn());
  Matrix poshft(po1.getm(), po1.getn());
  int i, j, k;
  double h = 2*PI/po1.getn();
  h = h/factor;
  for(j=0; j < factor; j++){
    shift(po1, h*j, poshft);
    for(i=0; i < po1.getn(); i++)
      for(k=0; k < po1.getm(); k++)
	po2(k,i*factor+j) = poshft(k,i);
  }
}

//po(input) = 3xn matrix
//v(input) = vector of size 3
//iindx(output) = index preceding min
//sshft(output) = sshft to reach min
//mindist(output) = min distance from v to po
void findMinDist(Matrix& po, Vector& v, 
		     int& iindx, double& sshft, double& mindist){
  int n = po.getn();
  assert(po.getm()==v.getSize());
  int m = po.getm();
  Matrix popo(m, 2*n);
  replicateMatrix(po, 2, popo);
  Vector temp(m);
  Vector dist(2*n);
  int i;
  for(i=0; i < 2*n; i++){
    temp = popo.getColumn(i);
    temp.sub(v);
    dist(i) = temp.norm();
    dist(i) = dist(i)*dist(i);
  }
  double min; int indx; double shft;
  findMin(dist, indx, shft, min);
  Matrix poshift(m,2*n);
  shift(popo, shft, poshift);
  temp = poshift.getColumn(indx);
  temp.sub(v);
  min = temp.norm();
  //return min;
  mindist = min;
  sshft = shft;
  sshft += (indx%2==0)?(0):(PI/n);
  iindx = indx/2;
  assert(iindx<n);
  assert(sshft<=2*PI/n);
}


//po(input) = 3xn matrix
//v(input) = vector of size 3
double returnMinDist(Matrix& po, Vector& v){
  double dist;
  int indx;
  double shft;
  findMinDist(po, v, indx, shft, dist);
  return(dist);
}
//po1 =3xn1 matrix
//po2 =3xn2 matrix
//return value is min distance between the two pos
double returnMinDist(Matrix& po1, Matrix& po2){
  int n2 = po2.getn();
  assert(po1.getm()==po2.getm());
  Vector dist(n2);
  int i;
  double min=LARGENUM; 
  for(i=0; i < n2; i++){
    Vector vtemp;
    vtemp.shadow(po2.getColumn(i));
    dist(i) = returnMinDist(po1, vtemp);
    if(dist(i) < min)
      min = dist(i);
  }
  po1.output("DBG/po1.dat");
  po2.output("DBG/po2.dat");
  dist.output("DBG/dist.dat");
  return min;
}

//dir1(input) = directory
//indx1(input) = index of po in dir1
//dir2(input) = directory
//indx2(input) = index of po in dir2
//return value is distance betwen the two pos
double returnDist(char *dir1, int indx1, char *dir2, int indx2){
  Matrix *po1;
  Matrix *po2;
  double T1, T2;
  double err1, err2;
  po1 = loadANDreturnOrbit(dir1, indx1, T1, err1);
  cout<<"loaded orbit #"<<indx1<<" from "<<dir1<<endl;
  po2 = loadANDreturnOrbit(dir2, indx2, T2, err2);
  cout<<"loaded orbit #"<<indx2<<" from "<<dir2<<endl;
  double rval;
  rval = returnMinDist(*po1, *po2);
  delete po1;
  delete po2;
  return rval;
}