//this is already done:
//to include fortran libraries read especially "Intel Fortran/C Mixed-Language Programs Overview" in Intel Visual Fortran User Guide Vol I
//but it also works without including libifcore.lib
//you just have to set in the ctest Project property tab at linker/input/additional dependencies to ..\Lib2\Debug\Lib2.lib or  ..\Lib2\Release\Lib2.lib
//and the linker/common/additional library directory to D:\Programme\Intel\Fortran\compiler80\IA32\LIB
//rightclick on Projectmappe to set dependencies and define the file to be debugged and started

//if linker does not find ifconsol.lib then change setting in properties of whole project/linker/allgemein/zusaetzliche bibliotheksverzeichnisse to the proper directory of the intel fortran compiler

// testlapack.cpp : Definiert den Einstiegspunkt für die Konsolenanwendung.
//in the fortran project:
//Remove "/libdir:noauto". In the project property pages, this is Fortran..Libraries..Disable default library search rules - change to "No".
//This setting, which is "Yes" by default in static library projects, tells the compiler to not include in the object file directives to pull in the run-time libraries. This is correct when the library is used by another Fortran project, but not when the main program is not Fortran.
//In order to avoid library collisions also Librarian..General..Ignore all default libraries to yes
//Take care that the project type in Fortran..Libraries is set to multithreaded debug/ multithreaded release respectively (if the current c project has the same settings)

//todo change that phi_ini and phi_final have only xanzzoomed values and not xanz: also QMOperator has to be changed
#include <vector>
#include <float.h>

#include <gsl/gsl_sf_bessel.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include "octtool.h"


#include "stdafx.h"
#include "lapack.h"
#include <stdio.h>
#include <iostream>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#ifndef WIN32
#include <string.h>
#endif
using namespace std;

char *itoa2(int val,char* str, int base){
	
	static char buf[32] = {0};
	
	int i = 30;
	
	for(; val && i ; --i, val /= base)
	
		buf[i] = "0123456789abcdef"[val % base];
	
	strcpy(str, &buf[i+1]);
	return str;
}

//double findminimum (functionPtr fptr,double start,double stop,int max_iter )
//{
//  int status;
//  int iter = 0;
//  const gsl_min_fminimizer_type *T;
//  gsl_min_fminimizer *s;
//  double m=(start+stop)/2.;
//  gsl_function F;
//
//  F.function = fptr;
//  F.params = 0;
//
//  T = gsl_min_fminimizer_brent;
//  s = gsl_min_fminimizer_alloc (T);
//  gsl_min_fminimizer_set (s, &F, m, start, stop);
//
// 
//
//  do
//    {
//      iter++;
//      status = gsl_min_fminimizer_iterate (s);
//
//      m = gsl_min_fminimizer_minimum (s);
//      start = gsl_min_fminimizer_x_lower (s);
//      stop = gsl_min_fminimizer_x_upper (s);
//
//      status 
//        = gsl_min_test_interval (start, stop, 1e-9, 0.0);
//
//    }
//  while (status == GSL_CONTINUE && iter < max_iter);
//
//  return m;
//}




void tridag(cdouble a[],cdouble b[],cdouble c[],cdouble r[],cdouble u[],cdouble gam[], unsigned long n){
	long j;
	cdouble bet;
	cdouble czero;
	//std::vector<cdouble> gam(n);  //one vector of workspace
	//if(b[0]==czero) printf("Error 1 in tridag"); //If this happens then you should rewrite your eqaution as a set of order N-1, with u2 trivially eleminated
	u[0]=r[0]/(bet=b[1]);
	for(j=1;j<n;++j){	//Decomposition and forward substitution
		gam[j]=c[j-1]/bet;
		bet=b[j]-a[j]*gam[j];
		//if(bet==czero) printf("Error 2 in tridag"); //algorithm fails see nr p 51
		u[j]=(r[j]-a[j]*u[j-1])/bet;
	}
	for (j=(n-2);j>=0;--j){
		u[j]-=gam[j+1]*u[j+1]; //Backsubstitution
	}
}



void  numerov( double *g,long glen,double h,double *Y,bool secure){
	long i;
	long innerturningpoint=0,outerturningpoint=1e9;
	int status=1;
	double hxhdiv12=h*h/12;
	for(i=glen-1;i>=0;i--){
		Y[i]=0;
		if ((g[i]<0)&&(status==1)){
			outerturningpoint=i;
			status=2;
		}
		if ((g[i]<0)&&(status==2)){
			innerturningpoint=i;
		}
		if ((g[i]>0)&&(status==2)){
			status=3;
		}

	}
	if(h<0){//integrate inward
		Y[glen-2]=1e-20;Y[glen-1]=0;
		double sum=Y[glen-2]*Y[glen-2]+Y[glen-1]*Y[glen-1];
		for(i=glen-1-1;i>=1;i--){//do numerov
			Y[i-1]= ( (2+10*g[i]*hxhdiv12)*Y[i]  -Y[i+1]*(1-g[i+1]*hxhdiv12) )/(1-g[i-1]*hxhdiv12);
			sum+=Y[i-1]*Y[i-1];
			
			if(secure){
				if ( (i<innerturningpoint) && (fabs(Y[i-1])>fabs(Y[i]) ) ) Y[i-1]=1e-99 ;
				if ( (Y[i-1]*Y[i]<0) && (Y[i]*Y[i+1]<0) ) break;
			}
		}
		if(secure){
			sum=sqrt(sum);//!! sum is now sqrt(sum) !!!
			for(i=0;i<glen;++i){//normalize
				Y[i] /= sum;
			}
		}

	}
	else{//integrate outward
		Y[0]=0;Y[1]=1e-20;
		double sum=Y[1]*Y[1]+Y[0]*Y[0];
		for(i=1;i<=glen-2;++i){//do numerov
			Y[i+1]= ( (2+10*g[i]*hxhdiv12)*Y[i]  -Y[i-1]*(1-g[i-1]*hxhdiv12) )/(1-g[i+1]*hxhdiv12);
			sum+=Y[i+1]*Y[i+1];
		
			if(secure){
				if ( (i>outerturningpoint) && (fabs(Y[i+1])>fabs(Y[i]) ) ) Y[i+1]=1e-99 ;	
				if ( (Y[i+1]*Y[i]<0) && (Y[i]*Y[i-1]<0) ) break;

			}
		}
		if(secure){
			sum=sqrt(sum);//!! sum is now sqrt(sum) !!!
			for(i=0;i<glen;++i){//normalize
				Y[i] /= sum;
			}
		}
	}
	
	return;

}

TimePropagator::TimePropagator(
							   int xanz_,int xanzzoomed_,double Dx_,double Dt_,double hbar_,double mass_,int dim_):xanz(xanz_),xanzzoomed(xanzzoomed_),Dx(Dx_),Dt(Dt_),hbar(hbar_),mass(mass_),dim(dim_){
	zero=new double[dim];
	int i;
	for(i=0;i<dim;i++) zero[i]=0;
}

TimePropagator::~TimePropagator(){
	delete[] zero;
}

SplitOpChebyshev::SplitOpChebyshev(int xanz_,int xanzzoomed_,double Dx_,double Dt_,double hbar_,double mass_,int dim_,double Vmin_,double Vmax_):
 TimePropagator(xanz_,xanzzoomed_,Dx_,Dt_,hbar_,mass_,dim_),Vmin(Vmin_),Vmax(Vmax_){
	int n;
	for(n=0;n<3;n++)
	  psi[n]=(fftw_complex *) fftw_malloc(sizeof(fftw_complex) * xanzzoomed*dim);
	tmpdata=(fftw_complex *) fftw_malloc(sizeof(fftw_complex) * xanzzoomed*dim);
	kinop=new double[xanzzoomed];
	fftw_forward_plan = fftw_plan_dft_1d(xanzzoomed, tmpdata, psi[0], FFTW_FORWARD,FFTW_MEASURE|FFTW_PRESERVE_INPUT);
	fftw_backward_plan = fftw_plan_dft_1d(xanzzoomed,tmpdata, psi[0], FFTW_BACKWARD,FFTW_MEASURE|FFTW_PRESERVE_INPUT);

    Pi=3.1415926535897932384626433832795028841971693;
	DV=Vmax-Vmin;
	Tmin=0;
	kstep=2.*Pi/(double(xanzzoomed)*Dx);
	Tmax=(hbar*Pi/Dx)*(hbar*Pi/Dx)/2./mass; //Tmax=(kstep*xanzzoomed/2)^2/2
	DT=Tmax-Tmin;
	
	int i;
	
	DV=Vmax-Vmin;
	Emin=Tmin+Vmin;
	Emax=Tmax+Vmax;
	DE=Emax-Emin;

	cdouble I(0,1);
	cdouble MinusIToThePowerOfN[]={1,-I,-1,I};
	MaxCoeff=int((DE*Dt/2./hbar+1.)*4.);
	cout <<"Chebyshev coefficients:"<<MaxCoeff<<endl;
	CoeffTbl=new cdouble[MaxCoeff];
	
	//CoeffTbl[0]=gsl_sf_bessel_Jn(0,DE*Dt/2./hbar)*( cos((DE/2.+Emin)*Dt/hbar)+I*sin((DE/2.+Emin)*Dt/hbar) );//orig
	CoeffTbl[0]=gsl_sf_bessel_Jn(0,DE*Dt/2./hbar)*( cos((DE/2.+Emin)*Dt/hbar)+I*sin(-(DE/2.+Emin)*Dt/hbar) );//corrected pivanov
	for(n=1;n<MaxCoeff;n++)
		
		//CoeffTbl[n]=2.*MinusIToThePowerOfN[n%4]*gsl_sf_bessel_Jn(n,DE*Dt/2./hbar)*( cos((DE/2.+Emin)*Dt/hbar)+I*sin((DE/2.+Emin)*Dt/hbar) );//orig
		CoeffTbl[n]=2.*MinusIToThePowerOfN[n%4]*gsl_sf_bessel_Jn(n,DE*Dt/2./hbar)*( cos((DE/2.+Emin)*Dt/hbar)+I*sin(-(DE/2.+Emin)*Dt/hbar) );//corrected pivanov
		
		
	for(i=0;i<=xanzzoomed/2;++i){
		kinop[i] = (hbar*kstep*i)*(hbar*kstep*i)/2./mass;
	}
	for(i=xanzzoomed/2+1;i<xanzzoomed;++i){
		kinop[i] =(hbar*kstep*(i-xanzzoomed))*(hbar*kstep*(i-xanzzoomed))/2./mass;
	}
 }

SplitOpChebyshev::~SplitOpChebyshev(){
	fftw_destroy_plan(fftw_forward_plan);
	fftw_destroy_plan(fftw_backward_plan);

	delete[] CoeffTbl;
	delete[] kinop;
	
	fftw_free(tmpdata);
	int n;
	for(n=0;n<3;n++)
		fftw_free(psi[n]);

}

//summedV is not really needed!
void SplitOpChebyshev::propagate(cdouble *phi,cdouble *summedVs,double dir,double *momentum,cdouble *summedV){//summedVs is at time t, summedV at time t+dt, we will use summedVs

	int i;
	int n,c,r;
	
	if(momentum==0) momentum=zero;
	
	//Calculate Kinetic energy operator T on phi by using the FFT(phi)
	for(r=0;r<dim;r++){
		for(i=0;i<xanzzoomed;++i) ((cdouble*)psi[0])[i+r*xanzzoomed]=phi[i+r*xanz];
			
		fftw_execute_dft(fftw_forward_plan,&(((fftw_complex *)psi[0])[r*xanzzoomed]),tmpdata); /* repeat as needed */
		//now tempdata stores the FFT with positive k values in increasing order and negative k values starting from the middle in increasing order, 
		//so that the smallest absolute value is at the final position
		
		for(i=0;i<=xanzzoomed/2;++i){
			((cdouble *)tmpdata)[i] *=  (hbar*kstep*i+momentum[r])*(hbar*kstep*i+momentum[r])/2./mass;
		}
		for(i=xanzzoomed/2+1;i<xanzzoomed;++i){
			((cdouble *)tmpdata)[i] *=  (hbar*kstep*(i-xanzzoomed)+momentum[r])*(hbar*kstep*(i-xanzzoomed)+momentum[r])/2./mass;
		}
		fftw_execute_dft(fftw_backward_plan,tmpdata,&(((fftw_complex *)psi[1])[r*xanzzoomed]));
	}
	for(r=0;r<dim;r++){
		for(i=0;i<xanzzoomed;++i){
			((cdouble *)psi[1])[i+r*xanzzoomed]*=2./DE/double(xanzzoomed);
			for(c=0;c<dim;c++)
				((cdouble *)psi[1])[i+r*xanzzoomed]+=((cdouble *)psi[0])[i+c*xanzzoomed]*(2./DE*summedVs[i+(r*dim+c)*xanz]);
			((cdouble *)psi[1])[i+r*xanzzoomed]+=((cdouble *)psi[0])[i+r*xanzzoomed]*(-2.*Emin/DE-1.);
			//((cdouble *)psi[1])[i+r*xanzzoomed]+=((cdouble *)psi[0])[i+r*xanzzoomed]*(2./DE*summedVs[i+(r*dim+c)*xanz]-2.*Emin/DE-1.);

			phi[i+r*xanz]=CoeffTbl[0]*((cdouble *)psi[0])[i+r*xanzzoomed]+dir*CoeffTbl[1]*((cdouble *)psi[1])[i+r*xanzzoomed];
		}
	}
	int m;
	for(m=2;m<MaxCoeff;++m){
		for(r=0;r<dim;r++){
			fftw_execute_dft(fftw_forward_plan,&(((fftw_complex *)psi[(m-1)%3])[r*xanzzoomed]),tmpdata); /* repeat as needed */
			//now tempdata stores the FFT with positive k values in increasing order and negative k values starting from the middle in increasing order, 
			//so that the smallest absolute value is at the final position
			
			for(i=0;i<=xanzzoomed/2;++i){
				((cdouble *)tmpdata)[i] *= (hbar*kstep*i+momentum[r])*(hbar*kstep*i+momentum[r])/2./mass;
			}
			for(i=xanzzoomed/2+1;i<xanzzoomed;++i){
				((cdouble *)tmpdata)[i] *=(hbar*kstep*(i-xanzzoomed)+momentum[r])*(hbar*kstep*(i-xanzzoomed)+momentum[r])/2./mass;
			}
			fftw_execute_dft(fftw_backward_plan,tmpdata,&(((fftw_complex *)psi[m%3])[r*xanzzoomed]));
		}
		for(r=0;r<dim;r++){		
			for(i=0;i<xanzzoomed;++i){
				((cdouble *)psi[m%3])[i+r*xanzzoomed]*=4./DE/double(xanzzoomed);
				for(c=0;c<dim;c++)
					((cdouble *)psi[m%3])[i+r*xanzzoomed]+=((cdouble *)psi[(m-1)%3])[i+c*xanzzoomed]*(4./DE*summedVs[i+(r*dim+c)*xanz]);
				
				((cdouble *)psi[m%3])[i+r*xanzzoomed]+=((cdouble *)psi[(m-1)%3])[i+r*xanzzoomed]*(-4.*Emin/DE-2.);
				((cdouble *)psi[m%3])[i+r*xanzzoomed]+=	-((cdouble *)psi[(m-2)%3])[i+r*xanzzoomed];
				if(m&1)//m is odd
					phi[i+r*xanz]+=dir*CoeffTbl[m]*((cdouble *)psi[m%3])[i+r*xanzzoomed];
				else
					phi[i+r*xanz]+=CoeffTbl[m]*((cdouble *)psi[m%3])[i+r*xanzzoomed];
			}		
		}
	}
	cdouble I(0,1);
	//add global phase
	//for(r=0;r<dim;r++){		
	//	for(i=0;i<xanzzoomed;++i){
			//phi[i+r*xanz]*=exp(-I*(Emin*2+DE)*Dt/hbar);//original publication ks
			//phi[i+r*xanz]*=exp(I*(Emin*2+DE)*Dt/(2.*hbar));//pivanov correction
	//	}
	//}
}

void SplitOpChebyshev::forward(cdouble *phi,cdouble *summedVs,double *momentum,cdouble *summedV){
	propagate(phi,summedVs,1.,momentum,summedV);
};

void SplitOpChebyshev::backward(cdouble *phi,cdouble *summedVs,double *momentum,cdouble *summedV){
	propagate(phi,summedVs,-1.,momentum,summedV);
};



SplitOp::SplitOp(int xanz_,int xanzzoomed_,double Dx_,double Dt_,double hbar_,double mass_,int dim_):
		TimePropagator(xanz_,xanzzoomed_,Dx_,Dt_,hbar_,mass_,dim_){
	if(dim>1) cerr <<"Split operator does not support dim>1 !"<<endl;
	int n;
	tmpdata=(fftw_complex *) fftw_malloc(sizeof(fftw_complex) * xanzzoomed);
	tmpdata2=(fftw_complex *) fftw_malloc(sizeof(fftw_complex) * xanzzoomed);
	kinop=new cdouble[xanzzoomed];
	kinopback=new cdouble[xanzzoomed];
	
	fftw_forward_plan = fftw_plan_dft_1d(xanzzoomed, tmpdata, tmpdata2, FFTW_FORWARD,FFTW_MEASURE|FFTW_PRESERVE_INPUT);
	fftw_backward_plan = fftw_plan_dft_1d(xanzzoomed,tmpdata, tmpdata2, FFTW_BACKWARD,FFTW_MEASURE|FFTW_PRESERVE_INPUT);

    Pi=3.1415926535897932384626433832795028841971693;
	kstep=2.*Pi/(double(xanzzoomed)*Dx);
	int i;
	cdouble I(0,1);
	double val;
	
	for(i=0;i<=xanzzoomed/2;++i){
		val=-(hbar*kstep*i)*(hbar*kstep*i)/2./mass/hbar*Dt;
		kinop[i] = cos(val)+I*sin(val) ;
		kinopback[i] = cos(val)-I*sin(val) ;
	}
	for(i=xanzzoomed/2+1;i<xanzzoomed;++i){
		val=-(hbar*kstep*(i-xanzzoomed))*(hbar*kstep*(i-xanzzoomed))/2./mass/hbar*Dt;
		kinop[i] = cos(val)+I*sin(val) ;
		kinopback[i] = cos(val)-I*sin(val);
	}
}
SplitOp::~SplitOp(){
	fftw_destroy_plan(fftw_forward_plan);
	fftw_destroy_plan(fftw_backward_plan);
	delete[] kinop;
	delete[] kinopback;

	fftw_free(tmpdata);fftw_free(tmpdata2);
	
}
void SplitOp::propagate(cdouble *phi,cdouble *summedVs,double dir,double *momentum,cdouble *summedV){//summedVs is at time t, summedV at time t+dt, we will use summedVs
	if(momentum==0) momentum=zero;
	int x;int n;cdouble I(0,1);	cdouble val;

	for(x=0;x<xanzzoomed;++x){
		val=-summedVs[x]*Dt*dir/2./hbar;
		((cdouble *)tmpdata2)[x]=cos(val)+I*sin(val);
	}
	for(x=0;x<xanzzoomed;++x){
		phi[x]*=((cdouble *)tmpdata2)[x];
	}
	//Calculate Kinetic energy operator T on phi by using the FFT(phi)
	fftw_execute_dft(fftw_forward_plan,(fftw_complex *)phi,(fftw_complex *)tmpdata); /* repeat as needed */
	//now tempdata stores the FFT with positive k values in increasing order and negative k values starting from the middle in increasing order, 
	//so that the smallest absolute value is at the final position
	if(dir>0)
		for(x=0;x<xanzzoomed;++x){
			((cdouble *)tmpdata)[x] *= kinop[x];
		}
	else
		for(x=0;x<xanzzoomed;++x){
			((cdouble *)tmpdata)[x] *= kinopback[x];
		}
	fftw_execute_dft(fftw_backward_plan,(fftw_complex *)tmpdata,(fftw_complex *)phi);
	for(x=0;x<xanzzoomed;++x){
		phi[x]*=((cdouble *)tmpdata2)[x]/double(xanzzoomed);
	}
	
}
void SplitOp::forward(cdouble *phi,cdouble *summedVs,double *momentum,cdouble *summedV){
	propagate(phi,summedVs,1.,momentum,summedV);
}
void SplitOp::backward(cdouble *phi,cdouble *summedVs,double *momentum,cdouble *summedV){
	propagate(phi,summedVs,-1.,momentum,summedV);
}

//for the timedependent schroedinger equation:
// I hbar d phi/dt = (-hbar^2/(2 m) d^2/dx^2+Sum_i( Vi(x)*SCALEi(t) ) phi
//written in crank nicholson scheme: with wavefunction u_position^time
// (I hbar/Dt -hbar^2/(Dx^2 2 m)-1/2 Sum_i(Vi_j SCALEi^{n+1}) u_j^{n+1})+hbar^2/(Dx^2 4 m) u_{j+1}^{n+1}+hbar^2/(Dx^2 4 m) u_{j-1}^{n+1}
//=(I hbar/Dt +hbar^2/(Dx^2 2 m)+1/2 Sum_i(Vi_j SCALEi^{n}) u_j^{n}  )-hbar^2/(Dx^2 4 m) u_{j+1}^{n}  -hbar^2/(Dx^2 4 m) u_{j-1}^{n}
//
//this can be written in a tridiagonal form (see function tridiag) with the following coefficients:
//
// a_j=hbar^2/(Dx^2 4 m)       b_j=(I hbar/ Dt - hbar^2/(Dx^2 2 m) - 1/2 Sum_i(Vi_j SCALEi^{n})    c_j=a_j
// as_j=-hbar^2/(Dx^2 4 m)     bs_j=(I hbar/ Dt + hbar^2/(Dx^2 2 m) + 1/2 Sum_i(Vi_j SCALEi^{n+1})   cs_j=-as_j
//
// TridiagonalMatrixOfa_b_c_ * (u_1^n+1,u_2^n+1,...)=TridiagonalMatrixOf_as_bs_cs * (u_1^n,u_2^n,...)
//
//where u_j^n are known and u_j^{n+1} are unknown for time forward propagation
CrankNicholson::CrankNicholson(int xanz_,int xanzzoomed_,double Dx_,double Dt_,double hbar_,double mass_,int dim_):
  TimePropagator(xanz_,xanzzoomed_,Dx_,Dt_,hbar_,mass_,dim_){
	cdouble I(0,1);
	a=hbar*hbar/(Dx*Dx*4*mass);
	b1=I*hbar/Dt-hbar*hbar/(Dx*Dx*2*mass);
	cc=a;
	as=-a;
	bs1=I*hbar/Dt+hbar*hbar/(Dx*Dx*2*mass);
	cs=-a;
	
	rr=new cdouble[xanzzoomed*dim];
	array_a=new cdouble[xanzzoomed*dim];
	array_b=new cdouble[xanzzoomed*dim];
	array_c=new cdouble[xanzzoomed*dim];
	if(dim>1) {
		cerr <<"dim>1 not supported"<<endl;
	}
	gam=new cdouble[xanzzoomed];
}
CrankNicholson::~CrankNicholson(){
	delete[] rr;
	delete[] array_a;
	delete[] array_b;
	delete[] array_c;
	delete[] gam;

}
void CrankNicholson::forward(cdouble *phi,cdouble *summedVs,double *momentum,cdouble *summedV){
	if(momentum==0) momentum=zero;
	int x;

	for(x=0;x<xanzzoomed;++x){
		array_a[x]=a; array_b[x]=(b1-0.5*summedV[x]); array_c[x]=cc; //a[0] und c[xanz-1] wird von tridiag nicht benutzt
		if(x==0)			rr[0]=(bs1-0.5*summedVs[0])*phi[0]+cs*phi[1];
		else if(x==xanzzoomed-1)	rr[xanzzoomed-1]=as*phi[xanzzoomed-2]+(bs1+0.5*summedVs[xanzzoomed-1])*phi[xanzzoomed-1];
		else				rr[x]=as*phi[x-1]+(bs1+0.5*summedVs[x])*phi[x]+cs*phi[x+1];
	}
	tridag(array_a,array_b,array_c,rr,phi,gam,xanzzoomed);

}
void CrankNicholson::backward(cdouble *phi,cdouble *summedVs,double *momentum,cdouble *summedV){
	if(momentum==0) momentum=zero;
	int x;
	for(x=0;x<xanzzoomed;++x){
		array_a[x]=as; array_b[x]=(bs1+0.5*summedVs[x]); array_c[x]=cs; //a[0] und c[xanz-1] wird von tridiag nicht benutzt
		if(x==0)			rr[0]=(b1-0.5*summedV[0])*phi[0]+cc*phi[1];
		else if(x==xanzzoomed-1)	rr[xanzzoomed-1]=a*phi[xanzzoomed-2]+(b1-0.5*summedV[xanzzoomed-1])*phi[xanzzoomed-1];
		else				rr[x]=a*phi[x-1]+(b1-0.5*summedV[x])*phi[x]+cc*phi[x+1];
	}
	tridag(array_a,array_b,array_c,rr,phi,gam, xanzzoomed);
};

void shiftwfn(cdouble *phi,int shift,int xanzzoomed){
	int x;
	if(shift>0){
		for(x=0;x<xanzzoomed-shift;++x){//set phi to phi(x,0)
			phi[x]=phi[x+shift];
		}
		for(x=xanzzoomed-shift;x<xanzzoomed;++x) phi[x]=0;
	}
	else if(shift<0){
		for(x=xanzzoomed-1;x>=-shift;--x){//set phi to phi(x,0)
			phi[x]=phi[x+shift];
		}
		for(x=-shift;x>=0;--x) phi[x]=0;
	}		
}
	
void QMOperators::FFTPowerSpectrum(cdouble *phi,double *spec){
	fftw_execute_dft(fftw_forward_plan,(fftw_complex *)phi,tmpdata); /* repeat as needed */
	int i;
	for(i=0;i<=xanzzoomed/2;++i){
		if(i+xanzzoomed/2==xanzzoomed/2) break;
		spec[i+xanzzoomed/2] = real( ((cdouble *)tmpdata)[i] *conj( ((cdouble *)tmpdata)[i]));
	}
	for(i=xanzzoomed/2+1;i<xanzzoomed;++i){
		spec[i-xanzzoomed/2-1] = real( ((cdouble *)tmpdata)[i]*conj(((cdouble *)tmpdata)[i]));
	}
}
QMOperators::QMOperators(TimePropagator *tp):xanzzoomed(tp->xanzzoomed),Dx(tp->Dx),Dt(tp->Dt),hbar(tp->hbar),mass(tp->mass),dim(tp->dim),xanz(tp->xanz){
	tmpdata=(fftw_complex *) fftw_malloc(sizeof(fftw_complex) * xanzzoomed);
	psi=(fftw_complex *) fftw_malloc(sizeof(fftw_complex) * xanzzoomed);
	fftw_forward_plan = fftw_plan_dft_1d(xanzzoomed, tmpdata, psi, FFTW_FORWARD,FFTW_MEASURE|FFTW_PRESERVE_INPUT);
	fftw_backward_plan = fftw_plan_dft_1d(xanzzoomed,tmpdata, psi, FFTW_BACKWARD,FFTW_MEASURE|FFTW_PRESERVE_INPUT);	
	double Pi=3.1415926535897932384626433832795028841971693;
	kstep=2.*Pi/(double(xanzzoomed)*Dx);
}
QMOperators::~QMOperators(){
	fftw_destroy_plan(fftw_forward_plan);
	fftw_destroy_plan(fftw_backward_plan);
	fftw_free(tmpdata);
	fftw_free(psi);
}
double QMOperators::MomentumExpectationValue(cdouble *phi,int dimnum){
#ifdef nomomentumtrans
	return 0;
#endif
	int i;
	for(i=0;i<xanzzoomed;++i) ((cdouble*)psi)[i]=phi[i+xanz*dimnum];//TODO change here xanz*dimnum to xanzzoomed*dimnum
	fftw_execute_dft(fftw_forward_plan,(fftw_complex *)psi,tmpdata); /* repeat as needed */
	//now tempdata stores the FFT with positive k values in increasing order and negative k values starting from the middle in increasing order, 
	//so that the smallest absolute value is at the final position
	

	for(i=0;i<=xanzzoomed/2;++i){
		((cdouble *)tmpdata)[i] *= (hbar*kstep*i);
	}
	for(i=xanzzoomed/2+1;i<xanzzoomed;++i){
		((cdouble *)tmpdata)[i] *=(hbar*kstep*(i-xanzzoomed));
	}

	
	fftw_execute_dft(fftw_backward_plan,tmpdata,(fftw_complex *)psi);
	
	cdouble momentumexpval(0,0);
	double norm=0;
	for(i=0;i<xanzzoomed;++i){ 
		momentumexpval+=((cdouble*)psi)[i]*conj(phi[i+xanz*dimnum])*Dx;//TODO change here xanz*dimnum to xanzzoomed*dimnum
		norm+=(((cdouble*)phi)[i+xanz*dimnum]*conj(phi[i+xanz*dimnum])).real()*Dx;//TODO change here xanz*dimnum to xanzzoomed*dimnum
	}
	return momentumexpval.real()/double(xanzzoomed)/norm;

}
double QMOperators::PositionExpectationValue(cdouble *phi,int dimnum){
	cdouble posexpval(0,0);
	int i;
	double norm=0;
	for(i=0;i<xanzzoomed;++i){
		posexpval+=double(i)*Dx*((cdouble*)phi)[i+xanz*dimnum]*conj(phi[i+xanz*dimnum])*Dx;//TODO change here xanz*dimnum to xanzzoomed*dimnum
		norm+=(((cdouble*)phi)[i+xanz*dimnum]*conj(phi[i+xanz*dimnum])).real()*Dx;//TODO change here xanz*dimnum to xanzzoomed*dimnum
	}
	return posexpval.real()/norm;
}
void QMOperators::MomentumDisplace(cdouble *phi,double displacement,int offset,int dimnum){
#ifdef nomomentumtrans
	return;
#endif
	cdouble I(0,1);
	int i;
	for(i=0;i<xanzzoomed;++i) ((cdouble *)phi)[i+xanz*dimnum]*=exp(-I*double(i+offset)*Dx*displacement/hbar);//TODO change here xanz*dimnum to xanzzoomed*dimnum
}



void makevideo(
//input from matlab
double *phi_ini,double *phi_final,double **V,double Dx,double **SCALE,double Dt,int nanz,double hbar,double mass,double **oneOverLambda,
//info about data
int xanz,int tanz,int amountOfElectrodes,
//output to matlab
double *scale_optimized_vid,double *phi_r_vid,double *phi_i_vid,double *overlap,
int *phi_reached_offset,int xanzzoomed,timepropagationmethod method,int optTransOrSplit){
	int i,j,t,x,phi_ini_offset=0,phi_final_offset=0,current_phi_reached_offset,next_phi_reached_offset;
	if(xanzzoomed==-1) {
		xanzzoomed=xanz;//dimensionality of phi_ini,phi_final and phi is either xanz or xanzzoomed
	}
	else{//find offset and check limits
		double summedVini,summedVminIni=1e99;
		int	summedVminposIni=-1;
		for(x=0;x<xanz;++x){//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
			summedVini=0.;
			for(j=0;j<amountOfElectrodes;j++){
				summedVini+=V[j][x]*SCALE[j][0];
			}
			if(summedVini<summedVminIni){
				summedVminIni=summedVini;
				summedVminposIni=x;
			}
		}
		phi_ini_offset=summedVminposIni-xanzzoomed/2;
		if(phi_ini_offset<0) 
			phi_ini_offset=0;
		if(phi_ini_offset>xanz-xanzzoomed) 
			phi_ini_offset=xanz-xanzzoomed;
	}
	int tanzvideo=0;
	if (nanz<0) { //negative nanz means only calculate -nanz timestep for video
		tanzvideo=tanz/(-nanz);
		nanz=-tanz/tanzvideo;
	}		   
	//use cranck nicholson scheme or chebyshev to get phi(x,T) from phi(x,0)=phi_ini
	TimePropagator *timepropagate;
	
	switch(method){
	case cranknicholson:
		timepropagate=new CrankNicholson(xanz,xanzzoomed,Dx,Dt,hbar,mass);
		break;
	case splitopchebyshev:
		timepropagate=new SplitOpChebyshev(xanz,xanzzoomed,Dx,Dt,hbar,mass);	
		break;
	case splitop:
		timepropagate=new SplitOp(xanz,xanzzoomed,Dx,Dt,hbar,mass);	
		break;
	};
	QMOperators op(timepropagate);
	//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
	cdouble *summedVs=new cdouble[xanzzoomed],*summedV=new cdouble[xanzzoomed];
	cdouble *chi= (cdouble *)fftw_malloc(sizeof(cdouble) * xanzzoomed), //fftw_malloc guaranties proper alignment of data
			*phi=(cdouble *)fftw_malloc(sizeof(cdouble) * xanzzoomed);
	current_phi_reached_offset=phi_ini_offset;
	next_phi_reached_offset=current_phi_reached_offset;
	for(x=0;x<xanzzoomed;++x){//set phi to phi(x,0)
		phi[x]=phi_ini[x+current_phi_reached_offset];
	}
	//calculate phi(x,T) by starting with phi_ini and integrating with cranck nicholson scheme
	double *ptr;
	int tvideo=0;
	double momentum[]={op.MomentumExpectationValue(phi)};
	op.MomentumDisplace(phi,momentum[0],current_phi_reached_offset);
	double displacement;
	for(t=0;t<tanz-1;++t){
		
	
		if (t%tanzvideo==0){//store summedV and phi for every t 
			phi_reached_offset[tvideo]=current_phi_reached_offset;

			ptr=phi_r_vid+xanzzoomed*tvideo;
			for(i=0;i<xanzzoomed;++i) ptr[i]=real(phi[i]);
			ptr=phi_i_vid+xanzzoomed*tvideo;
			for(i=0;i<xanzzoomed;++i) ptr[i]=imag(phi[i]);
			ptr=scale_optimized_vid+xanzzoomed*tvideo;
			if(optTransOrSplit==1){
				for(i=0;i<xanzzoomed;++i) ptr[i]=summedV[i].real();
			}
			else
			{
				for(i=0;i<xanzzoomed;++i) ptr[i]=summedV[i].real()-V[0][i];
			}
			tvideo++;
		}
		current_phi_reached_offset=next_phi_reached_offset;

		if(t>=tanz-1) break;
	

		double summedVmin=1e99;
		int	summedVminpos=-1;
			for(x=0;x<xanzzoomed;++x){//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
			summedVs[x]=0.;summedV[x]=0.;
			for(j=0;j<amountOfElectrodes;j++){
				summedVs[x]+=V[j][x+current_phi_reached_offset]*SCALE[j][t];
				summedV[x]+=V[j][x+current_phi_reached_offset]*SCALE[j][t+1];
			}
			if(summedV[x].real()<summedVmin){
				summedVmin=summedV[x].real();
				summedVminpos=x;
			}
		}
		timepropagate->forward(phi,summedVs,momentum,summedV);
		next_phi_reached_offset=current_phi_reached_offset+int(op.PositionExpectationValue(phi)/Dx)-xanzzoomed/2;
		if(next_phi_reached_offset<0) 
			next_phi_reached_offset=0;
		if(next_phi_reached_offset>xanz-xanzzoomed) 
			next_phi_reached_offset=xanz-xanzzoomed;
		shiftwfn(phi,next_phi_reached_offset-current_phi_reached_offset,xanzzoomed);
		displacement=op.MomentumExpectationValue(phi);
		op.MomentumDisplace(phi,displacement,next_phi_reached_offset);
		momentum[0]+=displacement;
	
	}



	

	delete[] summedVs;delete[] summedV;
	delete timepropagate;
	fftw_free(chi);fftw_free(phi);
}

//*phi_ini: initial wavefunction dimension is xanz
//*phi_final: final wavefunction dimension is xanz
//**V: is a 2 dimenstional array, first dimension is amountOfElectodes, second is xanz: it is the potential generated in space by each electrode
//Dx step in x coordinate
//**SCALE: is a 2 dimensional array, first dimenstion is amountOfElectrodes, second is tanz: it is the timedependent voltage applied to each electrode
//Dt step in time
//nanz: amount of optimization steps
//xanzzoomed: is the window size in x. If -1 we do not use windows

void optimalcontrol(
//input from matlab
double *phi_ini,double *phi_final,double **V,double Dx,double **SCALE,double Dt,int nanz,double hbar,double mass,double **oneOverLambda,
//info about data
int xanz,int tanz,int amountOfElectrodes,
//output to matlab
double **SCALEnew,cdouble *phi_reached,double *overlap,
int &phi_reached_offset,int xanzzoomed,timepropagationmethod method){
	int phi_ini_offset=0,phi_final_offset=0,
		i,j,t,x,current_phi_reached_offset,current_chi_reached_offset,next_phi_reached_offset,next_chi_reached_offset;
	double **SCALEtmp;
	
	if(xanzzoomed==-1) {
		xanzzoomed=xanz;//dimensionality of phi_ini,phi_final and phi is either xanz or xanzzoomed
	}
	else{//find offset and check limits
		double summedVini,summedVfinal,summedVminIni=1e99,summedVminFinal=1e99;
		int	summedVminposIni=-1,summedVminposFinal=-1;
		for(x=0;x<xanz;++x){//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
			summedVini=0.;summedVfinal=0.;
			for(j=0;j<amountOfElectrodes;j++){
				summedVini+=V[j][x]*SCALE[j][0];
				summedVfinal+=V[j][x]*SCALE[j][tanz-1];
			}
			if(summedVini<summedVminIni){
				summedVminIni=summedVini;
				summedVminposIni=x;
			}
			if(summedVfinal<summedVminFinal){
				summedVminFinal=summedVfinal;
				summedVminposFinal=x;
			}
		}
		phi_ini_offset=summedVminposIni-xanzzoomed/2;
		if(phi_ini_offset<0) 
			phi_ini_offset=0;
		if(phi_ini_offset>xanz-xanzzoomed) 
			phi_ini_offset=xanz-xanzzoomed;
		phi_final_offset=summedVminposFinal-xanzzoomed/2;
		if(phi_final_offset<0) 
			phi_final_offset=0;
		if(phi_final_offset>xanz-xanzzoomed) 
			phi_final_offset=xanz-xanzzoomed;
	}
	TimePropagator *timepropagate;
	switch(method){
	case cranknicholson:
		timepropagate=new CrankNicholson(xanz,xanzzoomed,Dx,Dt,hbar,mass);
		break;
	case splitopchebyshev:
		timepropagate=new SplitOpChebyshev(xanz,xanzzoomed,Dx,Dt,hbar,mass);	
		break;
	case splitop:
		timepropagate=new SplitOp(xanz,xanzzoomed,Dx,Dt,hbar,mass);	
		break;
	};
	QMOperators op(timepropagate);
	//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
	cdouble *summedVs=new cdouble[xanzzoomed],*summedV=new cdouble[xanzzoomed];
	cdouble *chi= (cdouble *)fftw_malloc(sizeof(cdouble) * xanzzoomed), //fftw_malloc guaranties proper alignment of data
			*phi=(cdouble *)fftw_malloc(sizeof(cdouble) * xanzzoomed);
	cdouble *phi_iniI=new cdouble[xanzzoomed];
	for(x=0;x<xanzzoomed;++x){//set phi_finalI to phi_final
		phi_iniI[x]=phi_ini[x+phi_ini_offset];
	}
	double phi_ini_momentum=op.MomentumExpectationValue(phi_iniI);
	op.MomentumDisplace(phi_iniI,phi_ini_momentum,phi_ini_offset);

	cdouble *phi_finalI=new cdouble[xanzzoomed];
	for(x=0;x<xanzzoomed;++x){//set phi_finalI to phi_final
		phi_finalI[x]=phi_final[x+phi_final_offset];
	}
	double phi_final_momentum=op.MomentumExpectationValue(phi_finalI);
	op.MomentumDisplace(phi_finalI,phi_final_momentum,phi_final_offset);
	
	
	
	current_phi_reached_offset=phi_ini_offset;
	for(x=0;x<xanzzoomed;++x){//set phi to phi(x,0)
		phi[x]=phi_iniI[x];
	}
	//calculate phi(x,T) by starting with phi_ini and integrating with cranck nicholson scheme
	next_phi_reached_offset=current_phi_reached_offset;
	double phimomentum[]={phi_ini_momentum};
	double chimomentum[]={0};
	double displacement;
	for(t=0;t<tanz-1;++t){
		current_phi_reached_offset=next_phi_reached_offset;
		double summedVmin=1e99;
		int	summedVminpos=-1;
		for(x=0;x<xanzzoomed;++x){
			//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
			summedVs[x]=0.;summedV[x]=0.;
			for(j=0;j<amountOfElectrodes;j++){
				summedVs[x]+=V[j][x+current_phi_reached_offset]*SCALE[j][t];
				summedV[x]+=V[j][x+current_phi_reached_offset]*SCALE[j][t+1];
			}
			if(summedV[x].real()<summedVmin){
				summedVmin=summedV[x].real();
				summedVminpos=x;
			}
		}
		timepropagate->forward(phi,summedVs,phimomentum,summedV);
		next_phi_reached_offset=current_phi_reached_offset+int(op.PositionExpectationValue(phi)/Dx)-xanzzoomed/2;
		if(next_phi_reached_offset<0) 
			next_phi_reached_offset=0;
		if(next_phi_reached_offset>xanz-xanzzoomed) 
			next_phi_reached_offset=xanz-xanzzoomed;
		shiftwfn(phi,next_phi_reached_offset-current_phi_reached_offset,xanzzoomed);
		displacement=op.MomentumExpectationValue(phi);
		op.MomentumDisplace(phi,displacement,next_phi_reached_offset);
		phimomentum[0]+=displacement;
	}





	
	//now phi is equal to phi(x,T)
	for(int n=0;n<=nanz;++n){
	//starting iteration
	//calculate chi(x,T) by using eqn.(25)
		cout <<n<<" ";
		cdouble integral_of_phi_final_and_phixT(0,0);

		cdouble I(0,1);
		int i;
#ifdef momentumtransbackandforth
		op.MomentumDisplace(phi_finalI,-phi_final_momentum,phi_final_offset);////
		op.MomentumDisplace(phi,-phimomentum,current_phi_reached_offset);////
#endif
		for(x=1;x<xanzzoomed;++x){
			cdouble addition;
			if( (x+current_phi_reached_offset-phi_final_offset>=0) && (x+current_phi_reached_offset-phi_final_offset<xanzzoomed)) 
				addition=phi_finalI[x+current_phi_reached_offset-phi_final_offset]*conj(phi[x]);
			else 
				addition=0;
			cdouble momentumphase=exp(I*Dx*double(current_phi_reached_offset)*(phi_final_momentum-phimomentum[0]))* \
				(-I*hbar)/(Dx*(phi_final_momentum-phimomentum[0]))* \
				(exp(I*Dx/hbar*double(x+1)*(phi_final_momentum-phimomentum[0]))-exp(I*Dx/hbar*double(x)*(phi_final_momentum-phimomentum[0])));
#ifdef nomomentumtrans
			momentumphase=1;
#endif
#ifdef momentumtransbackandforth
			momentumphase=1;
#endif
			integral_of_phi_final_and_phixT+=addition*momentumphase;
		}
#ifdef momentumtransbackandforth
		op.MomentumDisplace(phi_finalI,phi_final_momentum,phi_final_offset);////
		op.MomentumDisplace(phi,phimomentum,current_phi_reached_offset);////
#endif
		integral_of_phi_final_and_phixT*=Dx;
		overlap[n]=abs(integral_of_phi_final_and_phixT);
		if(n==nanz) break;
		current_chi_reached_offset=phi_final_offset;
		chimomentum[0]=phi_final_momentum;
		for(x=0;x<xanzzoomed;++x){
			chi[x]=phi_finalI[x]*integral_of_phi_final_and_phixT;
		}
		//now get chi(x,0) by using cranck nicholson backwards
		next_chi_reached_offset=phi_final_offset;
		
		for(t=tanz-2;t>=0;--t){
			current_chi_reached_offset=next_chi_reached_offset;
			double summedVmin=1e99;
			int	summedVminpos=-1;
			for(x=0;x<xanzzoomed;++x){
				//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
				summedVs[x]=0.;summedV[x]=0.;
				for(j=0;j<amountOfElectrodes;j++){
					summedVs[x]+=V[j][x+current_chi_reached_offset]*SCALE[j][t];
					summedV[x]+=V[j][x+current_chi_reached_offset]*SCALE[j][t+1];
				}
				if(summedV[x].real()<summedVmin){
					summedVmin=summedV[x].real();
					summedVminpos=x;
				}
			}
			timepropagate->backward(chi,summedVs,chimomentum,summedV);
			next_chi_reached_offset=current_chi_reached_offset+int(op.PositionExpectationValue(chi)/Dx)-xanzzoomed/2;
			if(next_chi_reached_offset<0) 
				next_chi_reached_offset=0;
			if(next_chi_reached_offset>xanz-xanzzoomed) 
				next_chi_reached_offset=xanz-xanzzoomed;
			shiftwfn(chi,next_chi_reached_offset-current_chi_reached_offset,xanzzoomed);
			displacement=op.MomentumExpectationValue(chi);
			op.MomentumDisplace(chi,displacement,next_chi_reached_offset);
			chimomentum[0]+=displacement;
		}
		//now chi is equal to chi(x,0)
		
		//now for each timestep update SCALES by using eq 27 and 28
		//then evolve chi forward by Dt using old scales and phi forward using new scales 
		//(this does not work as I do not have SCALEnew[t+1] so use old scales but then evolve phi again forward using new scales by going to the next iteration);
		//set phi to phi(x,0)
		current_phi_reached_offset=current_chi_reached_offset;
		for(x=0;x<xanzzoomed;++x){
			if( (x+current_phi_reached_offset-phi_ini_offset>=0) && (x+current_phi_reached_offset-phi_ini_offset<xanzzoomed)) 
				phi[x]=phi_iniI[x+current_phi_reached_offset-phi_ini_offset];
			else 
				phi[x]=0;
		}
		phimomentum[0]=phi_ini_momentum;
		next_phi_reached_offset=current_phi_reached_offset;
		next_chi_reached_offset=current_chi_reached_offset;
		for(t=0;t<tanz;++t){
			current_phi_reached_offset=next_phi_reached_offset;
			current_chi_reached_offset=next_chi_reached_offset;
			double summedVmin=1e99;
			int	summedVminpos=-1;
#ifdef momentumtransbackandforth
			op.MomentumDisplace(phi,-phimomentum[0],current_phi_reached_offset);////
			op.MomentumDisplace(chi,-chimomentum[0],current_chi_reached_offset);////
#endif			
			for(j=0;j<amountOfElectrodes;j++){
				cdouble K(0,0);
				for(x=0;x<xanzzoomed;x++){
					cdouble addition=phi[x]*V[j][x+current_chi_reached_offset]*conj(chi[x]);
					cdouble momentumphase=exp(I*Dx*double(current_chi_reached_offset)*(phimomentum[0]-chimomentum[0]))* \
						(-I*hbar)/(Dx*(phimomentum[0]-chimomentum[0]))* \
						(exp(I*Dx/hbar*double(x+1)*(phimomentum[0]-chimomentum[0]))-exp(I*Dx/hbar*double(x)*(phimomentum[0]-chimomentum[0])));
#ifdef nomomentumtrans
			momentumphase=1;
#endif
#ifdef momentumtransbackandfort
			momentumphase=1;
#endif
					K+=addition  *momentumphase;
				}
				K*=Dx/hbar;
				SCALEnew[j][t]=SCALE[j][t]+2*oneOverLambda[j][t]*imag(K);
			}
#ifdef momentumtransbackandforth
			op.MomentumDisplace(phi,phimomentum[0],current_phi_reached_offset);////
			op.MomentumDisplace(chi,chimomentum[0],current_chi_reached_offset);////
#endif		
			//evolve phi(x,t) to phi(x,t+Dt) with new scales but do not use cranck nickolson for V
			for(x=0;x<xanzzoomed;++x){
				//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
				summedVs[x]=0.;summedV[x]=0.;
				for(j=0;j<amountOfElectrodes;++j){
					summedVs[x]+=V[j][x+current_phi_reached_offset]*SCALEnew[j][t];
					//summedV[x]+=V[j][x]*SCALE[j][t+1];
					summedV[x]+=V[j][x+current_phi_reached_offset]*SCALEnew[j][t];
				}
				if(summedV[x].real()<summedVmin){
					summedVmin=summedV[x].real();
					summedVminpos=x;
				}
			}
			timepropagate->forward(phi,summedVs,phimomentum,summedV);
			next_phi_reached_offset=current_phi_reached_offset+int(op.PositionExpectationValue(phi)/Dx)-xanzzoomed/2;
			if(next_phi_reached_offset<0) 
				next_phi_reached_offset=0;
			if(next_phi_reached_offset>xanz-xanzzoomed) 
				next_phi_reached_offset=xanz-xanzzoomed;			
			shiftwfn(phi,next_phi_reached_offset-current_phi_reached_offset,xanzzoomed);
			displacement=op.MomentumExpectationValue(phi);
			op.MomentumDisplace(phi,displacement,next_phi_reached_offset);
			phimomentum[0]+=displacement;
		

			//evolve chi(x,t) to chii(x,t+Dt)
		
			for(x=0;x<xanzzoomed;++x){
				//calculate summedVs=Sum_i(Vi_j SCALEi^{n+1}) u_j^{n} 
				summedVs[x]=0.;summedV[x]=0.;
				for(j=0;j<amountOfElectrodes;++j){
					summedVs[x]+=V[j][x+current_chi_reached_offset]*SCALE[j][t];
					summedV[x]+=V[j][x+current_chi_reached_offset]*SCALE[j][t+1];
				}
				if(summedV[x].real()<summedVmin){
					summedVmin=summedV[x].real();
					summedVminpos=x;
				}
			}
			timepropagate->forward(chi,summedVs,chimomentum,summedV);
			//next_chi_reached_offset=current_chi_reached_offset+int(op.PositionExpectationValue(chi)/Dx)-xanzzoomed/2;
			next_chi_reached_offset=next_phi_reached_offset;
			if(next_chi_reached_offset<0) 
				next_chi_reached_offset=0;
			if(next_chi_reached_offset>xanz-xanzzoomed) 
				next_chi_reached_offset=xanz-xanzzoomed;			
			shiftwfn(chi,next_chi_reached_offset-current_chi_reached_offset,xanzzoomed);
			displacement=op.MomentumExpectationValue(chi);
			op.MomentumDisplace(chi,displacement,next_chi_reached_offset);
			chimomentum[0]+=displacement;
		}
		//now updata SCALE from SCALEnew
//#define MEANFILTER
#ifndef MEANFILTER
		SCALEtmp=SCALE;
		SCALE=SCALEnew;
		SCALEnew=SCALEtmp;
#else
		//run mean filter over scale to ovoid oscillations
		int meananz=3;//+-meananz elements 
		for(j=0;j<amountOfElectrodes;j++){
			for(t=0;t<meananz;t++){
				SCALE[j][t]=SCALEnew[j][t];
				SCALE[j][tanz-1-t]=SCALEnew[j][tanz-1-t];
			}
			for(t=meananz;t<tanz-meananz;t++)
			{
				SCALE[j][t]=SCALEnew[j][t];
				for(int t2=1;t2<=meananz;t2++)
					SCALE[j][t]+=(SCALEnew[j][t-t2]+SCALEnew[j][t+t2]);
				SCALE[j][t]/=(meananz*2+1);
			}
		}
#endif
		
	}
	op.MomentumDisplace(phi,-phimomentum[0],0);
	
	for(x=0;x<xanz;++x)
		phi_reached[x]=0.0;
	for(x=current_phi_reached_offset;x<current_phi_reached_offset+xanzzoomed;++x){
		if(x>=xanz) break;
		phi_reached[x]=phi[x-current_phi_reached_offset];
	}
	
	delete[] summedVs;
	delete[] summedV;
	delete timepropagate;
	fftw_free(chi);fftw_free(phi);
}


