#include <string.h>
#include <iostream>
#include <stdlib.h>
#include "Vector.hh"
#include "Matrix.hh"
#include "lorenz.hh"
#include "lorenz1.hh"
#include "lorenz3.hh"
#include "lorenz4.hh"
#include "Clock.hh"
#include "Solve.hh"

static const int LARGENUM=1000000;

void distFromString(Vector& v, char *s, double& dist, double& err){
  double T;
  Matrix *po = returnPO(s, T, err);
  int indx; double shft;
  findMinDist(*po, v, indx, shft, dist);
  delete po;
}

void distFromString(Vector& v, char *s, double& dist){
  double err;
  distFromString(v, s, dist, err);
  if(err > 1e-10)
    dist = LARGENUM;

}

void rotateString2Front(Vector& v, char *s){
  double T;
  double err;
  Matrix *po = returnPO(s, T, err);
  if(err > 1e-10){
    cout<<"error in po too large"<<endl;
    abort();
  }
  double mindist;
  double shft;
  int indx;
  findMinDist(*po, v, indx, shft, mindist);
  Matrix potemp(po->getm(), po->getn());
  int i,j;
  int n = po->getn();
  for(i=0; i < po->getm(); i++)
    for(j=0; j < n; j++)
      potemp(i,j) = (*po)(i, (j+indx)%n);
  findSymString(potemp, s);
}

//uses orbits loaded into library in lorenz1.C
void matchPoint2String(Vector& v, int len, char *s, double& dist){
  findClosestString(v, len, s);
  rotateString2Front(v, s);
  distFromString(v, s, dist);
}

void findLookAheadBits(int lindx, int lwidth, char *ss){
  assert(lwidth > 0);
  assert((0<=lindx)&&(lindx < pow(2,lwidth)));
  int i;
  for(i=0; i < lwidth; i++){
    ss[i] = (lindx%2==0)? 'A':'B';
    lindx = lindx/2;
  }
  ss[lwidth] = '\0';
}

void findLookAheadString(char *s, int sindx, int lindx, int lwidth,
			 char*ss){
  strcpy(ss, s);
  assert((0<=sindx)&&(sindx<=strlen(s)));
  ss[sindx] = '\0';
  char sbits[100];
  findLookAheadBits(lindx, lwidth, sbits);
  strcat(ss, sbits);
  strcat(ss, s+sindx);
}
  
void findBestLookAheadString(Vector& v, char *s, int lwidth,
			     char *ss, double& dist){
  static int trialCount = 0;
  //static ofstream ofile("DATA6/trials");
  double mindist=LARGENUM;
  char stemp[200];
  int i, j;
  for(i=strlen(s); ((i >= strlen(s)-6)&&(i>0)); i--)
    if(i==strlen(s)-6)
      i=1;
    for(j=0; j < pow(2.0, lwidth); j++){
      findLookAheadString(s, i, j, lwidth, stemp);
      trialCount++;
      double tempdist;
      //ofile<<"trying ("<<trialCount<<") "<<stemp<<endl;
      Clock clk;
      clk.wtic();
      distFromString(v, stemp, tempdist);
      //ofile<<"distance: "<<tempdist<<endl;
      //ofile<<"time: "<<clk.wtoc()<<"secs"<<endl;
      if(tempdist < mindist){
	mindist = tempdist;
	strcpy(ss, stemp);
      }
    }
  dist = mindist;
}

void updateFit(Vector& v, char *s1,
	       char *ss, double& dist){
  findBestLookAheadString(v, s1, 1, ss, dist);
}

void updateFit(Vector& v, char *s1, char *s2,
	       char *ss, double& dist){
  char ss1[200];
  char ss2[200];
  double dist1, dist2;
  findBestLookAheadString(v, s1, 1, ss1, dist1);
  findBestLookAheadString(v, s2, 2, ss2, dist2);
  if(dist1 < dist2){
    dist = dist1;
    strcpy(ss, ss1);
  }
  else{
    dist = dist2;
    strcpy(ss, ss2);
  }
}

void updateFit(Vector& v, char *s1, char *s2, char *s3,
	       char *ss, double& dist){
  char ss1[200];
  char ss2[200];
  char ss3[200];
  double dist1, dist2, dist3;
  findBestLookAheadString(v, s1, 1, ss1, dist1);
  findBestLookAheadString(v, s2, 2, ss2, dist2);
  findBestLookAheadString(v, s3, 3, ss3, dist3);
  if((dist1 < dist2)&&(dist1 < dist3)){
    dist = dist1;
    strcpy(ss, ss1);
  }
  else if(dist2 < dist3){
    dist = dist2;
    strcpy(ss, ss2);
  }
  else{
    dist = dist3;
    strcpy(ss, ss3);
  }
}

void updateFit(Vector& v, char *s1, char *s2, char *s3, char *s4,
	       char *ss, double& dist){
  char ss1[200];
  char ss2[200];
  char ss3[200];
  char ss4[200];
  double dist1, dist2, dist3, dist4;
  findBestLookAheadString(v, s1, 1, ss1, dist1);
  findBestLookAheadString(v, s2, 2, ss2, dist2);
  findBestLookAheadString(v, s3, 3, ss3, dist3);
  findBestLookAheadString(v, s4, 4, ss4, dist4);
  if((dist1 < dist2)&&(dist1 < dist3)&&(dist1< dist4)){
    dist = dist1;
    strcpy(ss, ss1);
  }
  else if((dist2 < dist3)&&(dist2 < dist4)){
    dist = dist2;
    strcpy(ss, ss2);
  }
  else if (dist3 < dist4){
    dist = dist3;
    strcpy(ss, ss3);
  }
  else{
    dist = dist4;
    strcpy(ss, ss4);
  }
}


/****************************************************************************
***********************More functions to approx a point *********************
****************************************************************************/
const double InvalidDist = 1000;
const int InvalidCount = -100;
char InvalidString[50] = "invalid";
const char InvalidChar = 'N';
int RepeatMax = 10;

void setRepeatMax(int n){
  RepeatMax = n;
}


void advancePoint(Vector& pt1, double h, Vector& pt2){
  int dim = 3;
  int maxsteps = 2;
  Solve ode(lorenz, lorenzJ, dim, maxsteps, RK7);
  ode.setStart(pt1, h);
  ode.advance(2);
  ode.getSoln(1, pt2);
}

void advancePoint(Vector& pt1, double h){
  Vector pt2(3);
  advancePoint(pt1, h, pt2);
  pt1 = pt2;
}

//pt must be just below the Poincare section
void advancePoint2Section(Vector& pt1, double h){
  if(fabs(pt1(2) - 27.0) < 1e-12)
    return;
  Vector pt2(3);
  advancePoint(pt1, h, pt2);
  if(fabs(pt2(2) - 27.0) < 1e-12){
    pt1 = pt2;
    return;
  }
  if(pt2(2) > 27.0)
    advancePoint2Section(pt1, h/2);
  else{
    pt1 = pt2;
    advancePoint2Section(pt1, h);
  }
}

//point can be anywhere
void advancePoint2ANYSection(Vector& pt1, double h){
  if(fabs(pt1(2) - 27.0) < 1e-6)
    return;
  Vector pt2(3);
  advancePoint(pt1, h, pt2);
  if(fabs(pt2(2) - 27.0) < 1e-6){
    pt1 = pt2;
    return;
  }
  while(!(((pt2(2) > 27.0) && (pt1(2) < 27.0))||
	  ((pt2(2) < 27.0) && (pt1(2) > 27.0)))){
    pt1 = pt2;
    advancePoint(pt1, h, pt2);
  }
  h = h/2;
  advancePoint2ANYSection(pt1, h);
}

void shift2FirstPoint(Matrix& po, char *s){
  char* ss = new char[2*strlen(s)];
  findSymString(po, ss);
  int indxss;
  int lenoffit;
  stringbestfit(s, 0, ss, indxss, lenoffit);
  if(!isequal(ss, s)){
    cout<<"shift2FirstPoint: ss doesnt equal s"<<endl;
    cout<<"s: "<<s<<endl;
    cout<<"ss: "<<ss<<endl;
    delete[] ss;
    return;
  }
  int tindx;
  double h;
  findTransIndx(po, indxss, tindx);
  int flag = 1;
  findShift(po, tindx, h, flag);
  Matrix temp(po.getm(), po.getn());
  shift(po, h, temp);
  po = temp;
  delete[] ss;
}

void findFirstPoint(Matrix& po, char *s, Vector& fpt){
  char* ss = new char[2*strlen(s)];
  findSymString(po, ss);
  int indxss;
  int lenoffit;
  stringbestfit(s, 0, ss, indxss, lenoffit);
  if(!isequal(ss, s)){
    cout<<"findFirstPoint: ss doesnt equal s"<<endl;
    cout<<"s: "<<s<<endl;
    cout<<"ss: "<<ss<<endl;
    fpt(0) = -100;
    fpt(1) = -100;
    fpt(2) = -100;
    delete[] ss;
    return;
  }
  int tindx;
  double h;
  findTransIndx(po, indxss, tindx);
  int flag = 1;
  findShift(po, tindx, h, flag);
  Matrix temp(po.getm(), po.getn());
  shift(po, h, temp);
  fpt = temp.getColumn(tindx);
  delete[] ss;
}

void findFirstPoint(char *s, Vector& fpt){
  Matrix *po;
  double T, err;
  po = returnPO(s, T, err);
  if(err < 1e-10)
    findFirstPoint(*po, s, fpt);
  else{
    fpt(0) = err;
    fpt(1) = err;
    fpt(2) = err;
  }
  delete po;
}

void makesymstr(char *s, int len){
  int i;
  for(i=0; i < len; i++){
    s[i] = 'A';
    s[len+i] = 'B';
  }
  s[2*len] = '\0';
}
  
void distFromAnBn(Vector& pt, Vector& dist){
  assert(pt.getSize()==3);
  assert(dist.getSize()>25);
  int i;
  cout<<pt(0)<<" "<<pt(1)<<" "<<pt(2)<<endl;
  char dir[50] = "DATA3/";
  dist(0) = -1;
  for(i=1; i <= 25; i++){
    Matrix *po;
    double T, err;
    po = loadANDreturnOrbit(dir, i, T, err);
    Vector fpt(3);
    char s[100];
    makesymstr(s, i);
    findFirstPoint(*po, s, fpt);
    fpt.sub(pt);
    dist(i) = fpt.norm();
    delete po;
  }
}
  
double distFromFirstPoint(Vector& pt, char *s){
  double T;
  double err;
  Matrix *po = returnPO(s, T, err);
  Vector fpt(3);
  findFirstPoint(*po, s, fpt);
  fpt.sub(pt);
  double dist = fpt.norm();
  if(err > 1e-10)//err is 100 if symstr(*po) != s
    dist = dist+err;
  delete po;
  return dist;
}
  
void extendSwithBn(char *s, int n, char *tail, char *ss){
  int len = strlen(s);
  strcpy(ss, s);
  int i;
  for(i=len; i < len+n; i++)
    ss[i] = 'B';
  ss[len+n] = 'A';
  ss[len+n+1] = '\0';
  strcat(ss, tail);
}


void distFromSBn(Vector& pt, char *s, char *tail, 
		 Vector& count, Vector& dist){
  char *ss = new char[strlen(s)+50];
  count(0) = InvalidCount;
  count(1) = InvalidCount;
  count(2) = 1;
  dist(0) = InvalidDist;
  dist(1) = InvalidDist;
  extendSwithBn(s, 1, tail, ss);
  dist(2) = distFromFirstPoint(pt, ss); 
  int i;
  for(i=2; i<=RepeatMax; i++){
    extendSwithBn(s, i, tail, ss);
    //cout<<ss<<endl;
    double tempdist;
    tempdist = distFromFirstPoint(pt, ss);
    //cout<<tempdist<<endl;
    count(0) = count(1); dist(0) = dist(1);
    count(1) = count(2); dist(1) = dist(2);
    count(2) = i; dist(2) = tempdist;
    if(dist(2) > dist(1))
      break;
  }
  if((count(2) == RepeatMax) && (dist(2) < dist(1))){
    dist(0) = InvalidDist;
    dist(1) = InvalidDist;
    dist(2) = InvalidDist;
  }
}


void extendSwithAn(char *s, int n, char *tail, char *ss){
  int len = strlen(s);
  strcpy(ss, s);
  int i;
  for(i=len; i < len+n; i++)
    ss[i] = 'A';
  ss[(len+n)] = 'B';
  ss[(len+n)+1] = '\0';
  strcat(ss, tail);
}

void distFromSAn(Vector& pt, char *s, char *tail, 
		 Vector& count, Vector& dist){
  char *ss = new char[(strlen(s)+50)];
  count(0) = InvalidCount;
  count(1) = InvalidCount;
  count(2) = 1;
  dist(0) = InvalidDist;
  dist(1) = InvalidDist;
  extendSwithAn(s, 1, tail, ss);
  dist(2) = distFromFirstPoint(pt, ss); 
  int i;
  for(i=2; i<=RepeatMax; i++){
    extendSwithAn(s, i, tail, ss);
    //cout<<ss<<endl;
    double tempdist;
    tempdist = distFromFirstPoint(pt, ss);
    //cout<<tempdist<<endl;
    count(0) = count(1); dist(0) = dist(1);
    count(1) = count(2); dist(1) = dist(2);
    count(2) = i; dist(2) = tempdist;
    if(dist(2) > dist(1))
      break;
  }
  if((count(2)==RepeatMax)&&(dist(2) < dist(1))){
    dist(0) = InvalidDist;
    dist(1) = InvalidDist;
    dist(2) = InvalidDist;
  }
}


FwdFit::FwdFit(Vector& ptt, char *taill){
  char emptystring[10] = "";
  FwdFit(ptt, emptystring, taill);
};

FwdFit::FwdFit(Vector& ptt, char *head, char *taill)
  :pt(3), dist(NF), tridist(3*NF)
{
  pt = ptt;
  strcpy(tail, taill);
  if((fabs(pt(2)-27.0) > 1e-10) || (pt(0) >= 0))
    cout<<"FwdFit object not formed properly"<<endl;
  int i;
  for(i=0; i < NF; i++){
    ffit[i] = new char[100];
    strcpy(ffit[i], InvalidString);
    dist(i) = InvalidDist;
  }
  exchar = InvalidChar;
  for(i=0; i < 3*NF; i++){
    triffit[i] = new char[100];
    strcpy(triffit[i], InvalidString);
    tridist(i) = InvalidDist;
  }
  triexchar = InvalidChar;
  initialize(head);
}

FwdFit::~FwdFit(){
  int i;
  for(i=0; i < NF; i++)
    delete[] ffit[i];
  for(i=0; i < 3*NF; i++)
    delete[] triffit[i];
}

void repeatchar(char c, int n, char *ss){
  assert(n>=0);
  int i = 0;
  for(i=0; i < n; i++)
    ss[i] = c;
  ss[n] = '\0';
}

void FwdFit::initialize(char *head){
  char s[100];
  strcpy(s, head);
  int len = strlen(s);
  Vector ddist(3);
  Vector count(3);
  char ichar;
  if((len!=0)&&(s[len-1] == 'A')){
    ichar = 'B';
    distFromSBn(pt, s, tail, count, ddist);
    exchar = 'A';
  }
  else{
    ichar = 'A';
    distFromSAn(pt, s, tail, count, ddist);
    exchar = 'B';
  } 
  dist(0) = ddist(0);
  dist(1) = ddist(1);
  dist(2) = ddist(2);
  char ss[40];
  if(dist(0) != InvalidDist){
    strcpy(ffit[0], s);
    repeatchar(ichar, (int)count(0), ss);
    strcat(ffit[0], ss);
  }
  if(dist(1) != InvalidDist){
    strcpy(ffit[1], s);
    repeatchar(ichar, (int)count(1), ss);
    strcat(ffit[1], ss);
  }
  if(dist(2) != InvalidDist){
    strcpy(ffit[2], s);
    repeatchar(ichar, (int)count(2), ss);
    strcat(ffit[2], ss);
  }
  trifurcate();
  sorttrifits();
}
    
void FwdFit::trifurcateAt(int ind){
  if(dist(ind) == InvalidDist){
    tridist(3*ind) = InvalidDist;
    tridist(3*ind+1) = InvalidDist;
    tridist(3*ind+2) = InvalidDist;
    strcpy(triffit[3*ind], InvalidString);
    strcpy(triffit[3*ind+1], InvalidString);
    strcpy(triffit[3*ind+2], InvalidString);
    return;
  }
  char s[50];
  strcpy(s, ffit[ind]);
  cout<<"trifurcating s="<<s<<endl;
  Vector count(3);
  Vector ddist(3);
  if(exchar == 'A')
    distFromSAn(pt, s, tail, count, ddist);
  else 
    distFromSBn(pt, s, tail, count, ddist);
  tridist(3*ind) = ddist(0);
  tridist(3*ind+1) = ddist(1);
  tridist(3*ind+2) = ddist(2);
  char ss[25];
  strcpy(triffit[3*ind], s);
  strcpy(triffit[3*ind+1], s);
  strcpy(triffit[3*ind+2], s);
  if(ddist(0) != InvalidDist){
    repeatchar(exchar, (int)count(0), ss);
    strcat(triffit[3*ind], ss);
  }
  if(ddist(1) != InvalidDist){
    repeatchar(exchar, (int)count(1), ss);
    strcat(triffit[3*ind+1], ss);
  }
  if(ddist(2) != InvalidDist){
    repeatchar(exchar, (int)count(2), ss);
    strcat(triffit[3*ind+2], ss);
  }
}    


void FwdFit::trifurcate(){
  int i;
  if(exchar == 'A')
    triexchar = 'B';
  else if(exchar == 'B')
    triexchar = 'A';
  else
    triexchar = InvalidChar;
  for(i=0; i < NF; i++)
    trifurcateAt(i);
}

static void swapstrings(char *s, char* ss){
  int len = strlen(s)+strlen(ss);
  char *temp = new char[len];
  strcpy(temp, s);
  strcpy(s, ss);
  strcpy(ss, temp);
  delete[] temp;
}

static void swapvector(Vector& v, int i, int j){
  double temp;
  temp = v(i);
  v(i) = v(j);
  v(j) = temp;
}

void FwdFit::sorttrifits(){
  int i, j;
  for(i=0; i < 3*NF-1; i++)
    for(j=i+1; j < 3*NF; j++){
      if (tridist(i) > tridist(j)){
	swapstrings(triffit[i], triffit[j]);
	swapvector(tridist, i, j);
      }
    }
}

void FwdFit::copytrifits(){
  int i;
  for(i=0; i < NF; i++){
    dist(i) = tridist(i);
    strcpy(ffit[i], triffit[i]);
    exchar = triexchar;
  }
}

void FwdFit::updatefit(){
  copytrifits();
  trifurcate();
  sorttrifits();
}

void FwdFit::printfit(ostream& ofile){
  int i;
  ofile<<"printing top five ..."<<endl;
  for(i=0; i < NF; i++)
    if(dist(i) != InvalidDist)
      ofile<<ffit[i]<<"  dist:"<< dist(i)<<endl;
  ofile<<endl;
}

void FwdFit::printallleaves(ostream& ofile){
  int i;
  ofile<<"printing leaves ..."<<endl;
  for(i=0; i < 3*NF; i++)
    if(tridist(i) != InvalidDist)
      ofile<<triffit[i]<<"  dist:"<< tridist(i)<<endl;
  ofile<<endl<<endl;
}
    
void FwdFit::getBestFit(char *s, double& distance){
  strcpy(s, triffit[0]);
  int len = strlen(s);
  s[len] = triexchar;
  s[len+1] = '\0';
  strcat(s, tail);
  distance = tridist(0);
}

ZeroFit::ZeroFit(char *s, double error){
  strcpy(sf, s);
  err = error;
  po = NULL;
}

ZeroFit::~ZeroFit(){
  if(po!=NULL)
    delete po;
}

int numoftrailingBs(char *s){
  int len = strlen(s);
  int num = 0;
  for(int i=len-1; i>= 0; i--)
    if(s[i] != 'B')
      break;
    else
      num++;
  return num;
}

void ZeroFit::nextstring(){
  int nb = numoftrailingBs(sf);
  int len = strlen(sf);
  double tol = 1e-8;
  if((err > tol) && (nb < RepeatMax)){//begin putting in As
    sf[len] = 'A';
    int i;
    for(i=1; i <= RepeatMax; i++)
      sf[len+i] = 'B';
    sf[len+RepeatMax+1] = '\0';
  }
  else if((err > tol) && (nb==RepeatMax)){//begin taking away Bs
    sf[len-nb-1] = 'B';
    sf[len-1] = '\0';
    sf[len-2] = '\0';
  }
  else if(nb < RepeatMax){//take away a B
    sf[len-1] = '\0';
  }
  else if(nb == RepeatMax){//put in an A
    sf[len-nb] = 'A';
    sf[len] = 'B';
    sf[len+1] = '\0';
  }
  cout<<"sf: "<<sf<<endl;
}

void ZeroFit::findpo(){
  if (po != NULL)
    delete po;
  double T;
  po = returnPO(sf, T, err);
}

void ZeroFit::updateFit(){
  nextstring();
  findpo();
}

void ZeroFit::getBestFit(char *s, double& error){
  error = err;
  strcpy(s, sf);
}

double ZeroFit::returnDistance(){
  if((po == NULL)||(err > 1e-8))
    return -1;
  Vector zero(3);
  zero(0) = 0; zero(1) = 0; zero(2) = 0;
  double dist = returnMinDist(*po, zero);
  return dist;
}