//   Match 2.0.1 -- aligns data series using dynamic programming
//   Copyright (C) 2001-2003  Lorraine E. Lisiecki and Philip A. Lisiecki 
//
//This program is free software; you can redistribute it and/or
//modify it under the terms of the GNU General Public License
//as published by the Free Software Foundation; version 2
//of the License.
//
//This program 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 General Public License for more details.
//
//You should have received a copy of the GNU General Public License
//along with this program; if not, write to the Free Software
//Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
//02111-1307, USA.
//
//The original authors reserve the right to license this program or
//modified versions of this program under other licenses at our
//discretion.
//
//If you publish results generated by this software please cite
//Lisiecki, L. E. and P. A. Lisiecki, The application of dynamic
//  programming to the correlation of paleoclimate records,
//  Paleoceanography, 17(D4), 1049, doi:10.1029/2001PA000733, 
//  2002. 
//
//Any questions regarding this license or the operation of this
//software may be directed to Lorraine Lisiecki
//<zogalum@alum.mit.edu>.


#include <iomanip>
#include <getopt.h>
#include <math.h>
#include "series.hh"
#include "config.hh"
#include "normalize.hh"
#include "assert.h"

namespace std {}
using namespace std;

#define TIE_NO_MATCH_PEN 1.0e6

int verbose=0;

config conf;

float *speedpenalties=0;

void setspeedpenalties() {
  speedpenalties=new float[conf.nspeeds];
  for(int i=0;i<conf.nspeeds;i++) {
    float r=((float)conf.targetspeed[0])*conf.speeds[i][1]/conf.targetspeed[1]
      /conf.speeds[i][0];
    if(r<1) r=1/r;
    r--;
    speedpenalties[i]=conf.sppen*r*r;
  }
}

inline float no_match_penalty (int d, int i) {
  // d=number of intervals, i=which series
  return d*conf.nomatch;
}

inline float speed_ch_penalty (int i1, int i2) {
  int i=i1-i2;
  return conf.spch*(i*i);
}

inline float speed_penalty (int i2) {
  return speedpenalties[i2];
}

inline float point_penalty (float f1, float f2) {
  float f=f1-f2;
  return f*f;
}

inline float tie_penalty (bool transpose, float x1, float x2) {
  float f=x1-x2;
  if(transpose)
    return conf.tiepenT*f*f;
  else
    return conf.tiepen*f*f;
}

// Warning: Data memory is shared amongst copies of array3's
template < class T >
struct array3 {
  int x,y,z;
  T * values;
  int * refcnt;

  array3(int x=1, int y=1, int z=1):
    x(x),y(y),z(z), values(new T[x*y*z]), refcnt(new int(1)) {}

  array3(const array3 &a):
    x(a.x), y(a.y), z(a.z), values(a.values), refcnt(a.refcnt) {
    (*refcnt)++;
  }
  ~array3() {
    (*refcnt)--;
    if(*refcnt==0) {
      delete [] values;
      delete refcnt;
    }
  }
  array3 &operator=(const array3 &a) {
    if (a.values == values) return *this;
    (*refcnt)--;
    if(*refcnt==0) {
      delete [] values;
      delete refcnt;
    }
    x=a.x;
    y=a.y;
    z=a.z;
    values=a.values;
    refcnt=a.refcnt;
    (*refcnt)++;
    return *this;
  }

  T &at(int xx, int yy, int zz){
    return(values[xx*y*z+yy*z+zz]);
  }

  const T &at(int xx, int yy, int zz) const{
    return(values[xx*y*z+yy*z+zz]);
  }
};

template < class T >
struct edges {
  vector<vector<array3<T> > > values;
  edges(int c1, int c2): values(c1,vector<array3<T> >(c2)){}
};
    

inline float intervals_pt_penalty(const core &s1, 
				  int pd1i, int d1i,
				  float i1b,float i1e,
				  const core &s2,
				  float i2b, float i2e,
				  int &hint) {
  float ptscore=0;
  int* i1 = s1.div.indices;
  for(int s1i=i1[pd1i]; s1i<i1[d1i]; s1i++) {
    bool inrange;
    float y2=s2.interp(intervalmap(i1b,i1e,i2b,i2e,s1[s1i].x),
		       hint, inrange);
    if(inrange)
      ptscore+=point_penalty(s1[s1i].y,y2); 
  }
  return ptscore;
}

inline float intervals_tie_penalty(bool transpose,
				   const tiepts::corepairties &ties,
				   int *itie1,
				   int pd1i, int d1i,
				   float i1b,float i1e,
				   float i2b, float i2e) {
  float ptscore=0;
  for(int tie1i=itie1[pd1i]; tie1i<itie1[d1i]; tie1i++) {
    ptscore+= tie_penalty(transpose,ties.ties[tie1i].depth2,
			  intervalmap(i1b,i1e,i2b,i2e,
				      ties.ties[tie1i].depth1));
  }
  return ptscore;
}

struct penaltycomponents {
  float no_match;
  float tie_no_match;
  float point;
  float tie;
  float speed;
  float speed_change;
  float gap;
  float total;

  penaltycomponents() {
    no_match=tie_no_match=point=tie=speed=speed_change=gap=total=0.0;
  }

  void accumulate(float *where, float value) {
    *where += value;
    total += value;
  }
};

ostream &operator<<(ostream &s, const penaltycomponents &pcomp) {
  s.setf(ios_base::fixed,ios_base::floatfield);
  s.precision(3);
  s<<"---Penalties---"<<endl;
  s<<"      point: "<<setw(12)<<pcomp.point<<endl;
  s<<"    nomatch: "<<setw(12)<<pcomp.no_match<<endl;
  s<<"      speed: "<<setw(12)<<pcomp.speed<<endl;
  s<<"speedchange: "<<setw(12)<<pcomp.speed_change<<endl;
  s<<"        tie: "<<setw(12)<<pcomp.tie<<endl;
  s<<"        gap: "<<setw(12)<<pcomp.gap<<endl;
  s<<"tie nomatch: "<<setw(12)<<pcomp.tie_no_match<<endl;
  s<<"-------------------------"<<endl;
  s<<"      Total: "<<setw(12)<<pcomp.total<<endl;
  s.precision(0);
  s.setf(ios_base::fmtflags(0),ios_base::floatfield);
  return s;
}

struct corematchresult {
  array3<signed char> path;
  array3<float> scoreout1, scoreout2;
};

corematchresult corematch(const vector<core*> &cv1, 
			  const vector<core*> &cv2, 
			  const array3<float> &scorein1, 
			  const array3<float> &scorein2, 
			  const tiepts::corepairties &ties,
			  const tiepts::corepairties &tiesT,
			  ostream &log){ 
  const core &c1_0=*(cv1[0]);
  const core &c2_0=*(cv2[0]);

  ofstream *matrixout=0;
  if(conf.matrixfile!="") {
    matrixout=new ofstream(conf.matrixfile.c_str());
  }

  // Changing the following line will disable space-saving feature
  //  int modulus=c1_0.div.count+2;
  int modulus=2;
  for(int i=0; i<conf.nspeeds; i++) {
    if(conf.speeds[i][0]>modulus) {
      modulus=conf.speeds[i][0];
    }
  }
  modulus++;

  array3<float> matrix(modulus, c2_0.div.count+1,conf.nspeeds);
  corematchresult cmresult;
  cmresult.scoreout1=array3<float>(c1_0.div.count+1,1,conf.nspeeds);
  cmresult.scoreout2=array3<float>(c2_0.div.count+1,1,conf.nspeeds);
  cmresult.path=array3<signed char>(c1_0.div.count+1, c2_0.div.count+1,conf.nspeeds);
  vector<int> hint1(cv1.size(),0), hint2(cv2.size(),0);
  int *itie1=ties.find_indices(c1_0);
  int *itie2=tiesT.find_indices(c2_0);

  for(int d1i=0; d1i<=c1_0.div.count; d1i++){
    for(int d2i=0; d2i<=c2_0.div.count; d2i++){
      for(int spi=0; spi<conf.nspeeds; spi++){
	signed char bpath=-1;
	float best=1e17;

	if(d1i==0 && (d2i!=0 || 
		      scorein2.at(d2i,0,spi)<scorein1.at(d1i,0,spi))) {
	  best=scorein2.at(d2i,0,spi);   // +no_match_penalty(d2i);
	  best += TIE_NO_MATCH_PEN*(itie2[d2i]-itie2[0]);
	  bpath = -2;
	}

	else if(d2i==0) {
	  best=scorein1.at(d1i,0,spi);   // +no_match_penalty(d1i);
	  best += TIE_NO_MATCH_PEN*(itie1[d1i]-itie1[0]);
	  bpath = -3;
	}

	else {
	  int pd1i=d1i-conf.speeds[spi][0];
	  int pd2i=d2i-conf.speeds[spi][1];

	  if(pd1i>=0 && pd2i>=0) {
	    float ptscore=0.0;
	    float i1b=c1_0.div.beginval(pd1i), i1e=c1_0.div.beginval(d1i);
	    float i2b=c2_0.div.beginval(pd2i), i2e=c2_0.div.beginval(d2i);

	    for(unsigned int sig=0; sig<cv1.size(); sig++) {
	      ptscore += intervals_pt_penalty(*cv1[sig],pd1i,d1i,i1b,i1e,
					      *cv2[sig],i2b,i2e,
					      hint1[sig]);

	      ptscore += intervals_pt_penalty(*cv2[sig],pd2i,d2i,i2b,i2e,
					      *cv1[sig],i1b,i1e,
					      hint2[sig]);
	    }

	    ptscore += intervals_tie_penalty(false,ties,itie1,pd1i,d1i,
					     i1b,i1e,i2b,i2e);

      	    ptscore += intervals_tie_penalty(true,tiesT,itie2,pd2i,d2i,
					     i2b,i2e,i1b,i1e);

	    ptscore += speed_penalty(spi);

	    for(int pspi=0; pspi<conf.nspeeds; pspi++) {
	      float score=(speed_ch_penalty(pspi,spi)+ptscore
			   +matrix.at(pd1i%modulus,pd2i,pspi));
	      if(score<best) {
		best=score;
		bpath=pspi;
	      }
	    }
	  }
	}
	if(d1i==c1_0.div.count) {
	  best += TIE_NO_MATCH_PEN*(itie2[c2_0.div.count]-itie2[d2i]);
	  cmresult.scoreout2.at(d2i,0,spi)=best;
	}
	if(d2i==c2_0.div.count) {
	  best += TIE_NO_MATCH_PEN*(itie1[c1_0.div.count]-itie1[d1i]);
	  cmresult.scoreout1.at(d1i,0,spi)=best;
	}
	  
	matrix.at(d1i%modulus,d2i,spi)=best;
	cmresult.path.at(d1i,d2i,spi)=bpath;
	if(matrixout) {
	  (*matrixout)<<best<<" ";
	}
      }	
      if(matrixout) {
	(*matrixout)<<endl;
      }
    }
  }
  if(matrixout) {
    delete matrixout;
  }
  delete [] itie1;
  delete [] itie2;

  return cmresult;
}

void corematch_penalty(const vector<core*> &cv1, 
		       const vector<core*> &cv2, 
		       const tiepts::corepairties &ties,
		       const tiepts::corepairties &tiesT,
		       penaltycomponents *pcomp,
		       int d1i, int d2i, int spi, int pspi) {
  const core &c1_0=*(cv1[0]);
  const core &c2_0=*(cv2[0]);

  vector<int> hint1(cv1.size(),0), hint2(cv2.size(),0);
  int *itie1=ties.find_indices(c1_0);
  int *itie2=tiesT.find_indices(c2_0);

  if(d1i==0) {
    pcomp->accumulate(&pcomp->tie_no_match,
		      TIE_NO_MATCH_PEN*(itie2[d2i]-itie2[0]));
  }

  else if(d2i==0) {
    pcomp->accumulate(&pcomp->tie_no_match,
		      TIE_NO_MATCH_PEN*(itie1[d1i]-itie1[0]));
  }

  else {
    int pd1i=d1i-conf.speeds[spi][0];
    int pd2i=d2i-conf.speeds[spi][1];

    if(pd1i>=0 && pd2i>=0) {
      float ptscore=0.0;
      float i1b=c1_0.div.beginval(pd1i), i1e=c1_0.div.beginval(d1i);
      float i2b=c2_0.div.beginval(pd2i), i2e=c2_0.div.beginval(d2i);

      for(unsigned int sig=0; sig<cv1.size(); sig++) {
	ptscore += intervals_pt_penalty(*cv1[sig],pd1i,d1i,i1b,i1e,
					*cv2[sig],i2b,i2e,
					hint1[sig]);
	
	ptscore += intervals_pt_penalty(*cv2[sig],pd2i,d2i,i2b,i2e,
					*cv1[sig],i1b,i1e,
					hint2[sig]);
      }
      pcomp->accumulate(&pcomp->point, ptscore);

      ptscore=0;
      ptscore += intervals_tie_penalty(false,ties,itie1,pd1i,d1i,
				       i1b,i1e,i2b,i2e);
      
      ptscore += intervals_tie_penalty(true,tiesT,itie2,pd2i,d2i,
				       i2b,i2e,i1b,i1e);
      pcomp->accumulate(&pcomp->tie, ptscore);

      pcomp->accumulate(&pcomp->speed, speed_penalty(spi));

      pcomp->accumulate(&pcomp->speed_change, speed_ch_penalty(pspi,spi));

      if(d1i==c1_0.div.count) {
	pcomp->accumulate(&pcomp->tie_no_match,
			  TIE_NO_MATCH_PEN*(itie2[c2_0.div.count]-itie2[d2i]));
      }
      if(d2i==c2_0.div.count) {
	pcomp->accumulate(&pcomp->tie_no_match,
			  TIE_NO_MATCH_PEN*(itie1[c1_0.div.count]-itie1[d1i]));
      }
    }
  }
  delete [] itie1;
  delete [] itie2;
}

array3<float> fill_nomatch_plane(const series &s, int ci,
				 const tiepts::tiemap_t &tiemap) {
  const core &c=*(s.cores[ci][0]);
  array3<float> scorein(c.div.count+1,1,conf.nspeeds);

  int nties_skipped=0;
  for(int i=0; i<ci; i++)
    nties_skipped += tiemap.lookup(i,0).s2ties_left;
  float tie_nomatch_score = TIE_NO_MATCH_PEN*nties_skipped;

  for(int i=0; i<c.div.count+1; i++) {
    float score=(c.div.beginval(i)-s.start)*conf.nomatch;
    score += tie_nomatch_score;
    for(int sp=0; sp<conf.nspeeds; sp++) {
      scorein.at(i,0,sp)=score;
    }
  }
  return scorein;
}

void nomatch_plane_penalty(const series &s, int ci,
			   const tiepts::tiemap_t &tiemap,
			   int di, penaltycomponents *pcomp) {
  const core &c=*(s.cores[ci][0]);

  int nties_skipped=0;
  for(int i=0; i<ci; i++)
    nties_skipped += tiemap.lookup(i,0).s2ties_left;
  float tie_nomatch_score = TIE_NO_MATCH_PEN*nties_skipped;
  pcomp->accumulate(&pcomp->tie_no_match, tie_nomatch_score);

  float score=(c.div.beginval(di)-s.start)*conf.nomatch;
  pcomp->accumulate(&pcomp->no_match, score);
}


array3<float> fill_coregap_plane(const series &s, int ci, 
				 const series &s2, int c2i,
				 float smultiplier, float s2multiplier,
				 const tiepts::tiemap_t &tiemap,
				 const edges<float> &scoreout,
				 array3<correspondence> *corrin) {
  const core &c=*(s.cores[ci][0]);
  int numint = s2.cores[c2i-1][0]->div.count;
  array3<float> scorein(c.div.count+1,1,conf.nspeeds);
  *corrin=array3<correspondence>(c.div.count+1,1,conf.nspeeds);
  float expectedgap=((s2.cores[c2i][0]->div.beginval(0)-
		      s2.cores[c2i-1][0]->div.beginval(numint))*
		     s2multiplier);

  // i = interval index within core ci
  // c = core number ci
  // ci = core number within series
  // p* = same variable for previous core

  float tie_nomatch_scores[ci+1];
  int nties_skipped=0;
  for(int pci=ci; pci>=0; pci--) {
    if(pci!=ci) 
      nties_skipped += tiemap.lookup(pci,c2i).s2ties_left;
    tie_nomatch_scores[pci]=(TIE_NO_MATCH_PEN *
			     (nties_skipped + 
			      tiemap.lookup(pci+1,c2i-1).s1ties_left));
  }

  for(int i=0; i<c.div.count+1; i++) {
    float gapend=c.div.beginval(i);
    float best=1e16;
    correspondence corr;

    for(int pci=0; pci<=ci; pci++) {
      const core &pc = *(s.cores[pci][0]);
      const float tie_nomatch_score = tie_nomatch_scores[pci];

      for(int pi=0; pi<pc.div.count+1; pi++) {
	float gapstart = pc.div.beginval(pi);
	float gappenalty = smultiplier*(gapend-gapstart)-expectedgap;
	gappenalty = gappenalty*gappenalty*conf.gap;

	for(int pcsp=0; pcsp<conf.nspeeds; pcsp++) {
	  float score=scoreout.values[pci][c2i-1].at(pi,0,pcsp);
	  score += gappenalty + tie_nomatch_score;
	  if(score<best) {
	    best=score;
	    corr=correspondence(pci,pi,c2i-1,numint,pcsp);
	  }
	}
      }
    }	  
    for(int sp=0; sp<conf.nspeeds; sp++) {
      scorein.at(i,0,sp)=best;
      corrin->at(i,0,sp)=corr;
    }
  }
  return scorein;
}

void coregap_plane_penalty(const series &s, int ci, 
			   const series &s2, int c2i,
			   float smultiplier, float s2multiplier,
			   const tiepts::tiemap_t &tiemap,
			   penaltycomponents *pcomp,
			   int pci, int i, int pi) {
  const core &c=*(s.cores[ci][0]);
  int numint = s2.cores[c2i-1][0]->div.count;
  float expectedgap=((s2.cores[c2i][0]->div.beginval(0)-
		      s2.cores[c2i-1][0]->div.beginval(numint))*
		     s2multiplier);

  // i = interval index within core ci
  // c = core number ci
  // ci = core number within series
  // p* = same variable for previous core

  float tie_nomatch_score=0.0;
  int nties_skipped=0;
  for(int tpci=ci; tpci>=pci; tpci--) {
    if(tpci!=ci) 
      nties_skipped += tiemap.lookup(tpci,c2i).s2ties_left;
  }
  tie_nomatch_score=(TIE_NO_MATCH_PEN *
		     (nties_skipped + 
		      tiemap.lookup(pci+1,c2i-1).s1ties_left));
  pcomp->accumulate(&pcomp->tie_no_match, tie_nomatch_score);

  float gapend=c.div.beginval(i);
  const core &pc = *(s.cores[pci][0]);
  float gapstart = pc.div.beginval(pi);
  float gappenalty = smultiplier*(gapend-gapstart)-expectedgap;
  gappenalty = gappenalty*gappenalty*conf.gap;
  pcomp->accumulate(&pcomp->gap, gappenalty);
}


correspondence find_path_end(const edges<float> &scoreout, const series &s,
			     const series &s2, const tiepts::tiemap_t &tiemap,
			     float *best, penaltycomponents *pcomp) {
  *best = 1e16;
  penaltycomponents tpcomp=*pcomp;
  tpcomp.accumulate(&tpcomp.no_match,1e16);
  correspondence corr;
  int c2i = s2.cores.size()-1;
  int i2 = s2.cores[s2.cores.size()-1][0]->div.count;
  float end=s.start+s.interval*s.count;
  int nties_skipped=0;

  for(int ci=s.cores.size()-1; ci>=0; ci--) {
    const array3<float> &scoreout_ci=scoreout.values[ci][c2i];
    const core &c=*(s.cores[ci][0]);

    if(ci != int(s.cores.size())-1)
      nties_skipped += tiemap.lookup(ci+1,0).s2ties_left;
    float tie_nomatch_score = TIE_NO_MATCH_PEN*nties_skipped;

    for(int i=0; i<c.div.count+1; i++) {
      float nm_score=(end-c.div.beginval(i))*conf.nomatch;
      if(nm_score<0) {
	nm_score = 0;
      }
      float nm_tie_score = nm_score + tie_nomatch_score;

      for(int sp=0; sp<conf.nspeeds; sp++) {
	float score=scoreout_ci.at(i,0,sp) + nm_tie_score;
	if(score<*best) {
	  *best=score;
	  tpcomp=*pcomp;
	  tpcomp.accumulate(&tpcomp.no_match,nm_score);
	  tpcomp.accumulate(&tpcomp.tie_no_match,tie_nomatch_score);
	  corr=correspondence(ci,i,c2i,i2,sp);
	}
	if(nm_score<0) {
	  cout << ci <<","<< i <<" score: "<< score <<"  nm: "<<nm_score << endl;
	}
      }
    }
  }
  *pcomp=tpcomp;
  return corr;
}

path get_path(edges<correspondence> &corrin1, 
	      edges<correspondence> &corrin2, 
	      edges<signed char> &corepaths,
	      correspondence end) {

  path result;
  correspondence location=end; 

  while(true) {
    result.push_back(location);

    // speed at which the previous match point was reached
    signed char bpath=(corepaths.values[location.core1num][location.core2num]
		       .at(location.int1,location.int2,location.speed));
    if(verbose>1) cout << location.core1num << ":" << location.int1 << ", "
		       << location.core2num << ":" << location.int2 << ", "
		       << location.speed << ", " << (int)bpath << endl;

    if(bpath==-1) break;
    else if (bpath==-2) {
      if(location.core1num == 0) break;
      location=(corrin2.values[location.core2num][location.core1num]
		.at(location.int2,0,location.speed).transpose());
    } 
    else if (bpath==-3) {
      if(location.core2num == 0) break;
      location=(corrin1.values[location.core1num][location.core2num]
		.at(location.int1,0,location.speed));
    } 
    else {
      location.int1-=conf.speeds[location.speed][0];
      location.int2-=conf.speeds[location.speed][1];
      location.speed = bpath;
    }
  }
  return result;
}

path seriesmatch(const series &s1, const series &s2, const tiepts &tie, 
		 ostream &log) {  

  edges<correspondence> corrin1(s1.cores.size(),s2.cores.size()), 
    corrin2(s2.cores.size(),s1.cores.size());
  edges<float> scoreout1(s1.cores.size(),s2.cores.size()), 
    scoreout2(s2.cores.size(),s1.cores.size());
  edges<signed char> pathmatrices(s1.cores.size(),s2.cores.size());

  for(unsigned c1i=0; c1i<s1.cores.size(); c1i++) {
    for(unsigned c2i=0; c2i<s2.cores.size(); c2i++) {

      const core &c1=*(s1.cores[c1i][0]);
      const core &c2=*(s2.cores[c2i][0]);
      array3<float> scorein1(c1.div.count+1,1,conf.nspeeds),
	scorein2(c2.div.count+1,1,conf.nspeeds);
      
      if(c1i==0) {
      	scorein2 = fill_nomatch_plane(s2, c2i, tie.c2_c1_tieT);
      } else {
	scorein2 = fill_coregap_plane(s2, c2i, s1, c1i,
				      1.0, conf.gapsize_multiplier,
				      tie.c2_c1_tieT, scoreout2, 
				      &corrin2.values[c2i][c1i]);
      }

      if(c2i==0) {
      	scorein1 = fill_nomatch_plane(s1, c1i, tie.c1_c2_tie);
      } else {
	scorein1 = fill_coregap_plane(s1, c1i, s2, c2i,
				      conf.gapsize_multiplier, 1.0,
				      tie.c1_c2_tie, scoreout1, 
				      &corrin1.values[c1i][c2i]);
      }

      corematchresult cmresult=corematch(s1.cores[c1i],
					 s2.cores[c2i], 
					 scorein1, scorein2, 
					 tie.c1_c2_tie.lookup(c1i,c2i),
					 tie.c2_c1_tieT.lookup(c2i,c1i),
					 log);
      scoreout1.values[c1i][c2i]=cmresult.scoreout1;
      scoreout2.values[c2i][c1i]=cmresult.scoreout2;
      pathmatrices.values[c1i][c2i]=cmresult.path;
    }
  }

  penaltycomponents pcomp;
  float best=1e16;
  correspondence corr;
  {
    float score;
    penaltycomponents tpcomp;
    correspondence corr1=find_path_end(scoreout1, s1, s2, tie.c1_c2_tie, 
				       &score, &tpcomp); 
    //    cout << "scoreout1 best " << score << endl;
    if(score<best) {
      corr=corr1;
      best=score;
      pcomp=tpcomp;
    }
  }
  {
    float score;
    penaltycomponents tpcomp;
    correspondence corr2=find_path_end(scoreout2, s2, s1, tie.c2_c1_tieT, 
				       &score, &tpcomp);
    //    cout << "scoreout2 best " << score << endl;
    if(score<best) {
      corr=corr2.transpose();
      best=score;
      pcomp=tpcomp;
    }
  }

  log<<endl<<"---Score---"<<endl;
  log<<"Total: "<<best<<endl<<endl; 

  if(verbose>0) {
    cout<<"---Score---"<<endl;
    cout<<"Total: "<<best<<endl<<endl; 
  }

  path result = get_path(corrin1,corrin2,pathmatrices,corr);

  for (int ri=int(result.size())-1; ri>=0; ri--) {
    int pspi=result[ri].speed;
    int c1i=result[ri].core1num;
    int c2i=result[ri].core2num;
    if(ri==int(result.size())-1) {
      if(result[ri].core1num==0 && result[ri].int1==0) {
      	nomatch_plane_penalty(s2, result[ri].core2num, 
			      tie.c2_c1_tieT, result[ri].int2, &pcomp);
      } else {
      	nomatch_plane_penalty(s1, result[ri].core1num, 
			      tie.c1_c2_tie, result[ri].int1, &pcomp);
      }	
    } else {
      if(result[ri].core1num==result[ri+1].core1num && 
	 result[ri].core2num==result[ri+1].core2num) {
	pspi=result[ri+1].speed;
      } else {
	correspondence location=result[ri];
	signed char bpath=(pathmatrices
			   .values[location.core1num][location.core2num]
			   .at(location.int1,location.int2,location.speed));
	if (bpath==-2) {
	  coregap_plane_penalty(s2, c2i, s1, c1i,
				1.0, conf.gapsize_multiplier,
				tie.c2_c1_tieT, &pcomp,
				result[ri+1].core2num, result[ri].int2,
				result[ri+1].int2);
	} else if(bpath==-3) {
	  coregap_plane_penalty(s1, c1i, s2, c2i,
				conf.gapsize_multiplier, 1.0,
				tie.c1_c2_tie, &pcomp, 
				result[ri+1].core1num, result[ri].int1,
				result[ri+1].int1);
	} else {
	  cout << "Internal error recontructing path at "<<ri<<endl;
	  log << "Internal error recontructing path at "<<ri<<endl;
	  exit(1);
	}
      }
    }
    corematch_penalty(s1.cores[c1i],s2.cores[c2i], 
		      tie.c1_c2_tie.lookup(c1i,c2i),
		      tie.c2_c1_tieT.lookup(c2i,c1i),
		      &pcomp, result[ri].int1, result[ri].int2,
		      result[ri].speed, pspi);
  }
  log<<pcomp;
  if(verbose>0) {
    cout<<pcomp; 
  }

  return result;
}

void write_path(ostream &f, path &p, series &s1, series &s2) {

  int i;
  for(i=p.size()-1; i>=0; i--) {
    f << s1.cores[p[i].core1num][0]->label << " " 
      << s1.cores[p[i].core1num][0]->div.beginval(p[i].int1) << " " 
      << s2.cores[p[i].core2num][0]->label << " " 
      << s2.cores[p[i].core2num][0]->div.beginval(p[i].int2) << endl;
  }
}

void write_new_series(ostream &f, path &p, series &s1, series &s2, 
		      int sig) {
  
  int i;
  for(i=p.size()-2; i>=0; i--) {
    if(p[i].core1num != p[i+1].core1num) {
      f << "NaN NaN NaN" << endl;
      continue;
    }

    const core &c1=*(s1.cores[p[i].core1num][sig]);
    int *i1=c1.div.indices;
    string c2_label=as_string(s2.cores[p[i].core2num][sig]->label);

    float i1b=c1.div.beginval(p[i+1].int1);
    float i1e=c1.div.beginval(p[i].int1);
    float i2b=s2.cores[p[i+1].core2num][sig]->div.beginval(p[i+1].int2);
    float i2e=s2.cores[p[i].core2num][sig]->div.beginval(p[i].int2);

    if (p[i].core2num != p[i+1].core2num) {
      if(i2b>=i2e) {
	f << "NaN NaN NaN" << endl;
	continue;
      } 
      c2_label="NaN";
    }

    for(int s1i=i1[p[i+1].int1]; s1i<i1[p[i].int1]; s1i++) {
      f << c2_label << " "
	<< intervalmap(i1b,i1e,i2b,i2e,c1[s1i].x) << " "
	<< c1[s1i].y << endl;
    }
  }
}

void cleanseries(ostream &log, series &s, string name,float &st,float &end) {

  for(unsigned i=0; i<s.cores.size(); i++) {
    core &c=*(s.cores[i][0]);

    if(c.size<2) {
      log<<name<<": Too few data points: "<<c.size<<"\n";
      cerr<<name<<": Too few data points: "<<c.size<<"\n";
      exit(1);
    }
    log<<name<<": "<<c.size<<" data points.\n";

    if(!c.sorted()) {
      if(verbose>0) cout<<name<<": Not sorted. Sorting...\n";
      log<<name<<": Not sorted. Sorting...\n";
      c.sort();
    }
    int numduplicates=c.averageduplicates();
    if(numduplicates) {
      if(verbose>0) {
	cout<<name<<": "<<numduplicates
	    <<" duplicate points were found and averaged out.\n";
      }
      log<<name<<": "<<numduplicates
	 <<" duplicate points were found and averaged out.\n";
    }
    if(i==0 && st < c[0].x) {
      cerr<<name<<" ERROR: Configuration begin value "<< st <<" precedes\n" 
	  << "            the first point in the series: "<<c[0].x<<endl; 
      log<<name<<" ERROR: Configuration begin1 value "<< st <<" precedes\n"  
	 << "            the first point in series1: "<< c[0].x <<endl; 
      exit(1);
    }

    if(i==s.cores.size()-1 && end > c[c.size-1].x) {
      cerr<<name<<" ERROR: Configuration end value "<< end <<" follows\n" 
	  << "            the last point in the series: "<<c[c.size-1].x<<endl; 
      log<<name<<" ERROR: Configuration end value "<< end <<" follows\n" 
	 << "            the last point in the series: "<<c[c.size-1].x<<endl; 
      exit(1);
    }
  }
  log<<"\n";
}

void divideseries(ostream &log, series &s, string name,
		  float &st, float &end, int count) {

  float interval=(end-st)/count;
  s.start=st;
  s.interval=interval;
  s.count=count;
  
  for(unsigned i=0; i<s.cores.size(); i++) {
    float cst=0.0, cend=0.0; 
    bool first=true;
    for(unsigned int j=0; j<s.cores[i].size(); j++) {
      core &c=*(s.cores[i][j]);
      if(c.size>0) {
	if(first || c[0].x<cst)
	  cst=c[0].x;
	if(first || c[c.size-1].x>cend)
	  cend=c[c.size-1].x;
	first=false;
      }
    }
    assert(!first);
    if (cend <= st || cst >= end) {
      s.cores.erase(s.cores.begin()+i);
      i--;
    } else {
      if(cst<st) cst=st;
      if(cend>end) cend=end;    
      //      cout << "divide " << cst << ", " << cend << ", "
      //	   << interval << ", " << ceilf((cend-cst)/interval-.01) << endl;
      for(unsigned int j=0; j<s.cores[i].size(); j++) {
	core &c=*(s.cores[i][j]);
	c.setdivision(cst,interval,(int)ceilf((cend-cst)/interval-.01));
      }
    }
  }
  try {
    s.build_labelmap();
  } catch (format_error &e) {
    cerr<<name<<" ERROR: " << (e.what()) << endl;
    log<<name<<" ERROR: " << (e.what()) << endl;
    exit(1);
  }
    
}

series getseries(ostream &log, vector<string> &conf_series, 
		 string which_series,
		 float start, float end, int count,
		 floatnan normst, floatnan normend, 
		 vector<floatnan> mean, vector<floatnan> stddev,
	         string gapfile) {
  if(normst.my_isnan()) normst=start;
  if(normend.my_isnan()) normend=end;

  vector<series> sv;
  for(unsigned int sig=0; sig<conf_series.size(); sig++) {
    sv.push_back(readfile(conf_series[sig]));
    floatnan smean, sstddev;
    if(mean.size()) smean=mean[sig];
    if(stddev.size()) sstddev=stddev[sig];
    if(!smean.my_isnan() || !sstddev.my_isnan()) {
      if(!normalize(sv.back(), 0, normst, normend, smean, sstddev)) {
	cerr<<"ERROR: Too few points or stddev=0 in series "
	    <<conf_series[sig]<<endl;
	log<<"ERROR: Too few points or stddev=0 in series "
	   <<conf_series[sig]<<endl;
	exit(1);
      }
    }
    cleanseries(log,sv.back(), conf_series[sig], start, end);
  }
 
  series s=merge_signal_series(sv);

  if(gapfile!=""){
    try {
      gaps g=read_gapfile(gapfile);
      g.apply_gaps(s);
    } catch (format_error &e) {
      log<<(e.what())<<" in "<<gapfile<<endl;
      cerr<<(e.what())<<" in "<<gapfile<<endl;
      exit(1);
    }
  }

  divideseries(log,s,which_series,start,end,count);

  return s;
}

void usage(const char *program) {
  cerr<<"Usage: "<<program<<" [-v] [-v] config_file\n";
}

int main(int argc, char* argv[]) {

  cout<<"Match 2.0\nCopyright (C) 2001-2003  Lisiecki and Lisiecki\n\n";

  while(1){
    int c=getopt(argc, argv, "v");
    if(c==-1) break;
    switch(c){
    case 'v':
      verbose++;
      break;
    default:
      usage(argv[0]);
      exit(1);
    }
  }
  if(optind!=argc-1){
    usage(argv[0]);
    exit(1);
  }    

  if(!conf.read(argv[optind])) {
    cerr<<"Configuration file "<<argv[optind]<<" is not valid.\n";
    exit(1);
  }
  ofstream log(conf.logfile.c_str());
  if(!log) {
    cerr<<"The log "<<conf.logfile<<" could not be opened.\n";
    exit(1);
  }

  if(conf.gapsize_multiplier<0) {
    conf.gapsize_multiplier=(((conf.end2-conf.st2)/int(conf.cnt2))/
			     ((conf.end1-conf.st1)/int(conf.cnt1)));
  }

  log<<"Match 2.0 (Lisiecki and Lisiecki, 2002) \n\n";
  log<<"---Configuration---\n";
  conf.write(log);
  log<<"\n";
  log.flush();

  if(verbose>0) {
    cout<<"---Configuration---\n";
    conf.write(cout);
    cout<<"\n";
  }
  setspeedpenalties();
  series s1, s2;
  gaps g1, g2;
  tiepts tie;

  log<<"---Series Information---\n";
  s1=getseries(log,conf.series1,"series1",
	       conf.st1,conf.end1,int(conf.cnt1),
	       conf.normst1, conf.normend1, conf.mean1, conf.stddev1,
	       conf.s1gapfile);
  s2=getseries(log,conf.series2,"series2",
	       conf.st2,conf.end2,int(conf.cnt2),
	       conf.normst2, conf.normend2, conf.mean2, conf.stddev2,
	       conf.s2gapfile);


  if(conf.tiefile!="") {
    try {
      tie=read_tiefile(conf.tiefile, log);
      tie.buildmaps(s1,s2);
    } catch (format_error &e) {
      log<<(e.what())<<" in "<<conf.tiefile<<endl;
      cerr<<(e.what())<<" in "<<conf.tiefile<<endl;
      exit(1);
    }
  }

  if(verbose>0) cout << endl;

  path xmatch;
  xmatch=seriesmatch(s1, s2, tie, log);

  series news1, news2;
  if(xmatch.size()<2) {
    cerr<<"No overlap was found. Try increasing nomatch.\n";
    log<<"No overlap was found. Try increasing nomatch.\n";
    exit(2);
  }
#if 0
  news1=newx(xmatch,s1);
  news2=subseries(s2,xmatch[0].y,xmatch[xmatch.size-1].y);
  writefile(conf.series1+".new",news1);
  writefile(conf.series2+".new",news2);
  writefile(conf.matchfile,xmatch);
#endif

  {
    ofstream f(conf.matchfile.c_str());
    write_path(f,xmatch,s1,s2);
    f.flush();
    if(!f) {
      cerr << "Failed to write " << conf.matchfile << endl;
      log << "Failed to write " << conf.matchfile << endl;
      exit(1);
    }
  }
  {
    path xmatchT=xmatch.transpose();
    for(unsigned int sig=0; sig<conf.series1.size(); sig++) {
      ofstream f((conf.series1[sig]+".new").c_str());
      write_new_series(f,xmatch,s1,s2,sig);
      f.flush();
      if(!f) {
	cerr << "Failed to write " << conf.series1[sig] 
	     << ".new" << endl;
	log << "Failed to write " << conf.series1[sig] 
	    << ".new" << endl;
	exit(1);
      }
      ofstream g((conf.series2[sig]+".new").c_str());
      write_new_series(g,xmatchT,s2,s1,sig);
      g.flush();
      if(!g) {
	cerr << "Failed to write " << conf.series2[sig] 
	     << ".new" << endl;
	log << "Failed to write " << conf.series2[sig] 
	    << ".new" << endl;
	exit(1);
      }
    }
  }
  log<<"Execution successful.\n";
  return(0);
}
