#include<iostream>
#include<fstream>

#include "../def.h"

using namespace std;

void setInitialValues(GlobExp *ex, int nP, double *parameters, double *yValues)
{
  long i,j,k;
  double xi, d, w1, w2;
  long nobs=ex->nobs, nvar=ex->nvar, nMeasure=ex->nMeasure;
  long nPoints=ex->nPoints;
  
  /* set initial values */
  if (yValues) 
    {
      for (j=1; j<=nvar; ++j) 
	ex->yTry[1][j]=yValues[j];
      i=2;
    } 
  else
    i=1;
  k=1;
  
  while (i<=nPoints) 
    {
      xi=ex->mesh[i];
      // make educated guess for yTry(xi)
      
      // could be improved (polynomial interpolation/extrapolation etc.)
      
      if (ex->xMeasure[k]>=xi) 
	{
	  // before first measurement: take first measured value
	  for (j=1; j<=nvar; ++j)
	    if (j<=nobs)
	      ex->yTry[i][j]=ex->yMeasure[k][j];
	    else
	      ex->yTry[i][j]=1;
	} else if (k==nMeasure) 
	  {
	    // after last measurement: take last measured value
	    for (j=1; j<=nvar; ++j)
	      if (j<=nobs)
		ex->yTry[i][j]=ex->yMeasure[k][j];
	      else
		ex->yTry[i][j]=1;
	  } 
      else 
	{
	  // interpolate linearly
	  while (k<nMeasure && ex->xMeasure[k]<xi) 
	    ++k;
	  d=ex->xMeasure[k]-ex->xMeasure[k-1];
	  w1=(ex->xMeasure[k]-xi)/d;
	  w2=(xi-ex->xMeasure[k-1])/d;
	  for (j=1; j<=nvar; ++j) 
	    if (j<=nobs)
	      ex->yTry[i][j]=w1*ex->yMeasure[k-1][j] + w2*ex->yMeasure[k][j];
	    else
	      ex->yTry[i][j]=1;
	}
      ++i;
    }  
}
