/*
 * Optimise.cpp
 *
 *  Created on: 17. 4. 2015
 *      Author: michal
 */
#include "Optimise.h"
using namespace arma;

ControlPoints::ControlPoints()
{
	CPMat.set_size(0);
	nPoints=0;
}

ControlPoints::ControlPoints(vec &r, vec &phi, vec &z, vec &val)
{
	if ((r.n_rows==phi.n_rows) && (phi.n_rows==z.n_rows))
	{
		nPoints=r.n_rows;
		CPMat.set_size(nPoints,4);
		CPMat.col(0)=r;
		CPMat.col(1)=phi;
		CPMat.col(2)=z;
		CPMat.col(3)=val;
	}
	else
	{
		cout << "Error in ControlPoints definition"<<endl;
	}
}

ControlPoints::ControlPoints(char *fname)
{
	CPMat.load(fname,raw_ascii);
	nPoints=CPMat.n_rows;
}

ControlPoints::~ControlPoints()
{
	nPoints=0;
	CPMat.reset();
}

ControlPoints& ControlPoints::operator =(ControlPoints &B)
{
	this->CPMat.copy_size(B.CPMat);
	this->CPMat=B.CPMat;
	this->nPoints=B.nPoints;
	return *this;
}


Optimise::Optimise(double pv_z, cube &T_0, double poverlap,char *pfilename, double pdr0,double pdz0, double pdphi, double pT_inf, double pdtau,ControlPoints &pCP)
{
	ConstM_fname=pfilename; //const matrix
	Settings_fname="Input/OptSettings.txt";
	Settings.load(Settings_fname);
	dr0=pdr0;
	dz0=pdz0;
	dphi=pdphi;
	T_inf=pT_inf;
	dtau=pdtau;

	overlap=poverlap;
	p_s.set_size(2);p_s(0)=0.5;p_s(1)=0.5;
	epsilon=0.1;
	delta=1;
	rho=0.00;
	nElem=Settings(4);
	pmax=Settings(3); // no of PHA iterations
	Heatpmax=Settings(5); //no of HeatComp iterations
	opt=true;
	parallel=true;

	n_offset=0;
	nXs=0;
	Param.set_size(nElem+1);
	Param.fill(500);
	Param(nElem)=pv_z;
	T=T_0;
	ndr=T.n_rows;
	ndphi=T.n_cols;
	ndz=T.n_slices;

	CP=pCP;
	ElemOfCP.set_size(CP.CPMat.n_rows); //for all  cont. points index of element they are in
	OptimalHtcs.zeros(nElem);
	htcmax=Settings(1);
	htcmin=Settings(0);
	htc_step=Settings(2);

	RealTertiaryLength=5;

	Elem.set_size(nElem,1);
	ElemL.set_size(nElem,1);
	ElemR.set_size(nElem,1);
	X_s.set_size(nElem,1);
	X_L.set_size(nElem,1);


	SetElements();

	nX.set_size(nElem);
	nX(0)=Elem(0,0).n_rows*Elem(0,0).n_cols*Elem(0,0).n_slices+1; //+1 for htc
	nXs+=nX(0);

	for (int i=1;i<nElem;i++)
	{
		nX(i)=Elem(i,0).n_rows*Elem(i,0).n_cols*(Elem(i,0).n_slices-ElemL(i,0).n_slices)+1;
		nXs+=nX(i);
	//	cout <<nX(i)<<endl;
	}
	nXs+=1; //v_z

	X_s_av.zeros(nXs);
	Vecv_z.zeros(nElem);
	Vecv_z.fill(Param(nElem));
	X_s_opt.set_size(nXs);

	ProfileMat.set_size(T.n_rows,T.n_slices);

}

Optimise::~Optimise()
{
		ConstM_fname=NULL; //const matrix
		dr0=0;
		dphi=0;
		T_inf=0;
		dtau=0;

		epsilon=0;
		delta=0;
		rho=0;
		nElem=0;
		n_offset=0;
		nXs=0;
		pmax=0;
		Heatpmax=0;
		opt=0;
		Settings_fname="0";

		nX.reset();

		Elem.reset();
		ElemL.reset();
		ElemR.reset();

		X_s.reset();
		X_L.reset();
		X_s_opt.reset();
		p_s.reset();
		X_s_av.reset();
		Vecv_z.reset();
		Param.reset();
		Settings.reset();

		T.reset();
		ProfileMat.reset();
		setT.reset();

		ElemOfCP.reset(); //for all  cont. points index of element they are in
		OptimalHtcs.reset();
		htcmax=0;
		htc_step=0;
}

vec Optimise::ToVect(arma::cube &S)
{
	vec v;
	int c=0;

	v.set_size(S.n_elem);

	for (int i=0;i<S.n_slices;i++)
	{
		for (int j=0;j<S.n_cols;j++)
		{
			for (int k=0;k<S.n_rows;k++)
			{
				v(c)=S(k,j,i);
				c++;
			}
		}
	}
	return v;
}

cube Optimise::DeVect(vec &v,int m,int n,int o)
{
	cube S;
	S.set_size(m,n,o);
	int c=0;

	for (int i=0;i<o;i++)
	{
		for (int j=0;j<n;j++)
		{
			for (int k=0;k<m;k++)
			{
				S(k,j,i)=v(c);
				c++;
			}
		}
	}
	return S;
}

mat Optimise::DeVectM(vec &v,int m,int n,int o)
{
	cube pX(m,n,o);
	pX=DeVect(v,m,n,o);
	mat S(m,o);

	for (int i=0;i<pX.n_slices;i++)
		S(span::all,i)=pX.slice(i)(span::all,1);

	return S;

}

void Optimise::SetElements()
{
	double sumElem=0;
	int nInElem;

	for (int i=0;i<nElem-1;i++)
		sumElem+=(1.0-overlap);

	nInElem=ceil(ndz/(sumElem+1));
	nInElem=nInElem-(nInElem %(int)(1.0/0.25)); //to achieve sensible partition

	//----------This whole section needs to be checked--------------

	Elem(0,0)=T.slices(0,nInElem-1);
	ElemL(0,0)=T.slices(0,overlap*nInElem-1);
	ElemR(0,0)=T.slices(nInElem*(1-overlap),nInElem-1);

	for (int i=1;i<nElem-1;i++)
	{
		Elem(i,0)=T.slices(i*nInElem*(1-overlap),i*nInElem*(1-overlap)+nInElem-1);
		ElemL(i,0)=ElemR(i-1,0);
		ElemR(i,0)=T.slices((i+1)*nInElem*(1-overlap),(1-overlap)*nInElem*i+nInElem-1);
	}
	Elem(nElem-1,0)=T.slices((nElem-1)*nInElem*(1-overlap),ndz-1);
	ElemL(nElem-1,0)=ElemR(nElem-2);

	n_offset=ElemR(1,0).n_slices;

	cout << "Setting Elements done.... \n\n";
	cout <<"Number of z slices in elements:"<<endl;
	for (int i=0;i<nElem;i++)
	{
		cout << Elem(i,0).n_slices<<" with overlap: " <<ElemL(i,0).n_slices<<endl;
	}

	//Managing Control Points

	//transform real coordinates to coordinates in discretization, WARNING CPs must NOT be in overlaying areas and now ONLY for constant disc.
	int k=0;
	for (int i=0;i<CP.CPMat.n_rows;i++)
	{
		CP.CPMat(i,0)=ceil(CP.CPMat(i,0)/dr0)-1;
		CP.CPMat(i,1)=ceil(CP.CPMat(i,1)/dphi)-1;
		CP.CPMat(i,2)=ceil(CP.CPMat(i,2)/dz0)-1;

		int diff=0;
		int c=0;
		int diff_old=0;

		if (CP.CPMat(i,2)-Elem(0,0).n_slices<0) //NEEDS TO BE RECHECKED
		{
			ElemOfCP(i)=0;
			//CP.CPMat(i,2); //is the same as original index, no chagne here needed
		}
		else
		{
			c=CP.CPMat(i,2);
			diff=c;
			while(diff>=0)
			{
				diff=diff-Elem(k,0).n_slices+n_offset;
				if (diff<0)
				{
					ElemOfCP(i)=k;
					CP.CPMat(i,2)=diff_old-1; //to the z coord we write the "local" index of element they belong to; -1:we index form zero
				}
				k++;
				diff_old=diff;
			}
			k=0;
			diff=0;
		}
	}

	cout<<endl<<"Setting Control Points done:"<<endl;
	for (int i=0;i<nElem;i++)
		cout << "Point "<< i+1 << " in element " <<ElemOfCP(i)+1<<" at position "<< CP.CPMat(i,2)<<"."<<endl;
}

vec Optimise::CompScen(int ind,arma::cube &T_old,arma::vec Param ,arma::vec &W_old,arma::vec &X_av_old, cube &Tf, double htcl)
{
	//int pmax_heat=2500;
	double v_z=X_av_old(X_av_old.n_rows-1);

//	if (ind>0)
//		T_old.slice(0)=Tf; //speed-up the convergence
	vec X(T_old.n_elem);
	X=ToVect(T_old);
	vec X_s;
	//X.save("X_old.vec",arma_ascii);

//	if (ind==1)
//	{
//	mat A(ndr,T_old.n_slices);
//	for (int k=0;k<T_old.n_slices;k++)
//		A(span::all,k)=T_old.slice(k)(span::all,1);
//		cout <<A<<endl<<endl;
//		A.save("A.mat",raw_ascii);
//	}
	X_s.set_size(T_old.n_elem+1+0); //+0 for v_z, we wont optimise it ATM
	X_s(span(0,T_old.n_elem-1))=X(span::all);
	X_s(X_s.n_rows-1)=Param(ind);

	mat C;
//	X_s(X_s.n_rows-1)=v_z;
//	T_old.save("T_old.cub",arma_ascii);
	X.reset();


	if (opt)
	{
		//----------------------Optimisation----------------------------
		vec Xhelp;
		Xhelp=X_s(span(0,X_s.n_rows-2)); //now we want only temperatures
	//	X_s_opt.set_size(X_s.n_rows+0); //no v_z

		X_s(span(0,X_s.n_rows-1))=OptimiseScen(ind,Xhelp, v_z,500, W_old,X_av_old,Tf,htcl);
		Xhelp.reset();
	}
	else
	{
		//------------------Only HeatComp---------------
		cube T1;

		//Setting length of cooling zone in Element
		double TertLengthforElem=RealTertiaryLength;

		if (ind<nElem-1)
		{
			for (int i=nElem-2;i>=ind;i--)
			{
				TertLengthforElem=TertLengthforElem-(nElem-1-i)*dz0*(Elem(i+1,0).n_slices-n_offset);
			}
		}

		HeatComp S(dr0,dz0,dphi,T_old,ConstM_fname,v_z, T_inf,dtau,500,TertLengthforElem,htcl,n_offset);
		//cout << ind << ": Tertiary Length: "<<TertLengthforElem<<endl;
//		cube T_L;
//		T_L=T_old.slices(0,1);

		T1=S.ComputeOpt(ind,Heatpmax,Tf);

		X.set_size(T1.n_elem);
		X=ToVect(T1);
		//	X.save("X2.vec",arma_ascii);
		//	T_old.save("T1.cub",arma_ascii);
		X_s(span(0,T_old.n_elem-1))=X(span::all);
	}
	//------------------------------------------------------------------




	//------------------------------------------------------------------

	vec X_s_opt;
	X_s_opt=X_s;
	vec NoL=X_s_opt(span(0,ndr*ndphi*n_offset-1-0));

	X_L(ind,0)=NoL; // 0 for v_z, wont mark it after this line

	switch (ind) //switch trimming of X_L from X_s ->will trim all but the first elem
	{
	case 0: break;

	default:
		NoL.set_size(X_s_opt.n_rows-X_L.n_rows);
		NoL=X_s_opt(span(ndr*ndphi*n_offset,X_s_opt.n_rows-1));
		X_s_opt.reset();
		X_s_opt=NoL;
		break;
	}
	return X_s_opt;
}

vec Optimise::OptimiseScen(int ind, arma::vec &pElemi, double v_z, double htc0, arma::vec &Wi,arma::vec &X_av_oldi, cube &Tf,double htcl)
{
	//This is main optimization algorithm. Its very crude, using nearly full enumeration for parameter htc. This is still faster than using cleverer, but not specialized opt algorithms.

	vec ObjFValue(ceil((htcmax-htcmin)/htc_step)+1); //values of computed Objective functions for each setting of htc
	vec DiffToPrint;
	DiffToPrint.copy_size(ObjFValue);
	field<vec> XT(ceil((htcmax-htcmin)/htc_step)+1,1); //field in which values of temperatures will be stored for iteration (i,0)
	uword min_index=0; //for writing index of minimal iteration
	vec htcs;//store htcs for selecting optimal htc
	htcs.copy_size(ObjFValue);
	//vec diff(pElemi.n_rows-1); //no htc

	//Get which ControlPoint we need to use

	int CPIndex=0;


	for (int s=0;s<CP.CPMat.n_rows;s++) //which point do we choose for this element
	{
		if (ind==ElemOfCP(s))
			CPIndex=s;
	}

	//Tertiary cooling setup
	double TertLengthforElem=RealTertiaryLength;

		if (ind<nElem-1)
		{
			for (int i=nElem-2;i>=ind;i--)
			{
				TertLengthforElem=TertLengthforElem-(nElem-1-i)*dz0*(Elem(i+1,0).n_slices-n_offset);
			}
		}

	int ItIndex=0;
	//Optimisation
	//htc0;
	for (double i=htcmin;i<=htcmax;i+=htc_step)
	{
		htcs(ItIndex)=i;
		//Compute temperature for this htc
		cube T_old(Elem(ind,0).n_rows,Elem(ind,0).n_cols,Elem(ind,0).n_slices);
		T_old=DeVect(pElemi,Elem(ind,0).n_rows,Elem(ind,0).n_cols,Elem(ind,0).n_slices);
		HeatComp U(dr0,dz0,dphi,T_old,ConstM_fname,v_z, T_inf,dtau,i,TertLengthforElem,htcl,n_offset);
		cube T1;
		T1.copy_size(T_old);
		T_old.save("T_old.cub",arma_ascii);
		Tf.save("Tf.vec",arma_ascii);
		T1=U.ComputeOpt(ind,Heatpmax,Tf); //ERROR not T_old
		//cout << ItIndex << endl;// << T1<<endl;
		U.erase();
		XT(ItIndex,0)=ToVect(T1);
		XT(ItIndex,0).resize(XT(ItIndex,0).n_rows+2);
		XT(ItIndex,0)(XT(ItIndex,0).n_rows-2)=htcs(ItIndex);
		XT(ItIndex,0)(XT(ItIndex,0).n_rows-1)=v_z;
		//Compute Objective Function value
		//ObjFValue(ItIndex)=(abs(CP.CPMat(CPIndex,3)-)); //this line means ControlPointWantedValue-RealValueAtControlPoint
//		cout<< CP.CPMat(CPIndex,0) <<endl;
//		cout<< CP.CPMat(CPIndex,1) <<endl;
//		cout<< CP.CPMat(CPIndex,2) <<endl;
//		cout << Wi.n_rows<<endl;
//		cout << X_av_oldi.n_rows<<endl;
//		cout << XT(ItIndex,0).n_rows<<endl;
		vec nrm;
//		nrm=X_av_oldi-XT(ItIndex,0);
//		double NRN;
//		NRN=norm(nrm,2);
//		double prod=0;
//		for (int z=0;z<Wi.n_rows;z++)
//		{
//			prod+=Wi(z)*X_av_oldi(z);
//		}

		DiffToPrint(ItIndex)=T1(CP.CPMat(CPIndex,0),CP.CPMat(CPIndex,1),CP.CPMat(CPIndex,2))-CP.CPMat(CPIndex,3);
		ObjFValue(ItIndex)=abs(DiffToPrint(ItIndex));//+prod+(rho/2.0)*pow(NRN,2.0);
		//cout << ItIndex << ": ObjFValue: "<< ObjFValue(ItIndex)<< " from "<< T1(CP.CPMat(CPIndex,0),CP.CPMat(CPIndex,1),CP.CPMat(CPIndex,2))<< " - "<< CP.CPMat(CPIndex,3)<< endl;
		ItIndex++;
		//prod=0;
	}
	ObjFValue.min(min_index);
	OptimalHtcs(ind)=htcs(min_index);
	printf("\n\n Scenario %d \n",(ind+1));
	printf(": Minimal ObjFValue: %d \n",(int)DiffToPrint(min_index));
	XT(min_index).resize(XT(min_index).n_rows-1);
	return XT(min_index,0);
}

void Optimise::ComputePHA()
{

	int p=0;
	vec X_s_av_old=X_s_av;
	field<vec> X_av_old(nElem,1);
	field<vec> X_av(nElem,1);
	field<vec> W(nElem,1);
	field<vec> sum2(nElem,1);
	field<vec> W_s(nElem,1);
	field<vec> W_s_old(nElem,1);
	int debug=0;
	vec W_i;
	vec OptimalHtcs_old(nElem);
	char fnameOctave[]="Profile.mat";

	//----------------PHA initialisation-------------------------

	for (int i=0;i<nElem;i++) //initialize all fields and vectors filling them with zeros
	{
		W_i.zeros(nX(i)+1); //+1 for v_z; W_i is only to simplify the code, no use elsewhere, we want also the offset
		if (i>0)
			W_i.zeros(nX(i)+1+n_offset*ndr*ndphi);
		W(i,0)=W_i;
		sum2(i,0).zeros(1);
		X_av(i,0)=W(i,0);
		X_av_old(i,0)=W(i,0);
		W_i.set_size(nXs);
		W_s(i,0)=W_i.zeros();
		W_s_old(i,0)=W_i;
		X_s_opt(i,0)=W_i;
		W_i.reset();

		X_av_old(i,0)(X_av_old(i,0).n_rows-1)=Vecv_z(i);
	}
	field<cube> Tf(nElem,1);
	field<vec> ToTf(nElem,1);
	OptimalHtcs_old.fill(500);
	vec htcl(nElem);


	//----------------PHA main iteration cycle-------------------------

 while(p<pmax)
 {
	 p++;
	 printf("---------------------------------------------------------------\n %d. PHA iteration out of %d total. \n---------------------------------------------------------------\n",p,pmax);

	 //Compute all scenarios
	 #pragma omp parallel for
	 for (int i=0;i<nElem;i++)
	 {
		 if (i==0)
		 {
			 htcl(i)=OptimalHtcs_old(0);
			 X_s(0,0)=CompScen(0,Elem(0,0),Param,W(0,0),X_av_old(0,0),Tf(i,0),htcl(i));
//			 X_s(0,0).save("HeatCompOut/X_s.cub",arma_ascii);
//			 Elem(0,0).save("elem.el",arma_ascii);
		//	 Tf=X_s(0,0)(span(X_s(0,0).n_rows-2-n_offset*ndr*ndphi,X_s(0,0).n_rows-2-(n_offset-1)*ndr*ndphi));
		 }
		 else
		 {

			 if (parallel==false)
			 {
				ToTf(i,0)=X_s(i-1,0)(span(X_s(i-1,0).n_rows-1-n_offset*ndr*ndphi,X_s(i-1,0).n_rows-2-(n_offset-1)*ndr*ndphi)); //if parallel will get values from iteration before...
				htcl(i)=OptimalHtcs(i-1);
			 }
			 else
			 {
				 //this step is no longer necessary
				 if (p>1)
				 {
					 //cout <<X_av_old(i-1,0)(span(X_av_old(i-1,0).n_rows-2-n_offset*ndr*ndphi,X_av_old(i-1,0).n_rows-3-(n_offset-1)*ndr*ndphi))<<endl;
					 ToTf(i,0)=X_av_old(i-1,0)(span(X_av_old(i-1,0).n_rows-2-n_offset*ndr*ndphi,X_av_old(i-1,0).n_rows-3-(n_offset-1)*ndr*ndphi));
				 }
				 else
				 {
					cube Help(ndr,ndphi,1);
				 	Help.slice(0)=Elem(i-1,0).slice(Elem(i-1,0).n_slices-n_offset);
				 	ToTf(i,0)=ToVect(Help);
					//ToTf=X_s(i-1,0)(span(X_s(i-1,0).n_rows-1-n_offset*ndr*ndphi,X_s(i-1,0).n_rows-2-(n_offset-1)*ndr*ndphi)); //if parallel will get values from iteration before...
				 }

				 htcl(i)=OptimalHtcs_old(i-1);
			 }
			 Tf(i,0)=DeVect(ToTf(i,0),ndr,ndphi,1);
			 ToTf(i,0).reset();
			 X_s(i,0)=CompScen(i,Elem(i,0),Param,W(i,0),X_av_old(i,0),Tf(i,0),htcl(i));
		 }
	 }
	 OptimalHtcs_old=OptimalHtcs;

	 // Compute averages //in X_av1 tehre seems to be a mistake, something is zero probably, first 4 elements are wrong

	 //X_av1 is vector helping us to average needed elements
	 vec X_av1;

	 if (p<pmax+1)
	 {

		 X_av1.set_size(X_s(0,0).n_rows+1);
		 X_av1(span(0,X_av1.n_rows-2))=X_s(0,0); //copy whole body+htc
		 X_av1(X_av1.n_rows-1)=Vecv_z(1); //copy speed from next element
		 X_av1(span(X_s(0.0).n_rows-1-X_L(1,0).n_rows+1-1,X_s(0,0).n_rows-1-1+1-1))=X_L(1,0); //rewriting the right side of helping matrix by another element's left side



		 X_av1(X_av1.n_rows-1-1)=X_s(1,0)(X_s(1,0).n_rows-1); // we need to also sum the parameters, remember, that X_s has one parameter less (no speed atm)

		 X_s(0,0).resize(X_s(0,0).n_rows+1); //+1 = now add also the speed
		 X_s(0,0)(X_s(0,0).n_rows-1)=Vecv_z(0);

		 X_av(0,0)=p_s(0)*X_s(0,0)+p_s(1)*X_av1;

	 	 for (int i=1;i<nElem-1;i++)
	 	 {
		 	 X_av1.set_size(X_s(i,0).n_rows+1);
		 	 X_av1(span(0,X_av1.n_rows-2))=X_s(i,0); //copy whole body
		 	 X_av1(X_av1.n_rows-1)=Vecv_z(i+1); //copy speed from next element/
		 	 X_av1(span(X_s(i,0).n_rows-1-X_L(i+1,0).n_rows+1-1,X_s(i,0).n_rows-1-1-1+1))=X_L(i+1,0); //rewriting the right side of helping matrix by another element's left side
			 X_av1(X_av1.n_rows-1-1)=X_s(i+1,0)(X_s(i+1,0).n_rows-1); // we need to also sum the parameters, remember, that X_s has one parameter less (no speed atm)

		 	 X_s(i,0).resize(X_s(i,0).n_rows+1); //+1 = now add also the speed
		 	 X_s(i,0)(X_s(i,0).n_rows-1)=Vecv_z(i);

		 	 X_av(i,0)=p_s(0)*X_s(i,0)+p_s(1)*X_av1;
		 	 // X_av(i,0)(span(0,X_L(i-1,0).n_rows-1))=X_av(i-1,0)(span(X_av(i,0).n_rows-1-1-X_L(i+1,0).n_rows+1-1,X_s(i,0).n_rows-1-1-1+1-1)); //we need also to add averages to the left overlaying side of i-th part, we take them for before averaged right side of i-1 element
	 	 }


	 	 X_av1.set_size(X_s(nElem-1,0).n_rows+1); //+vz
	 	 X_av1(span(0,X_av1.n_rows-2))=X_s(nElem-1,0); //add whole body
	 	 X_av1(X_av1.n_rows-1-1)=X_s(nElem-2,0)(X_s(nElem-2,0).n_rows-1-1); //to the second last element give htc
	 	 X_av1(X_av1.n_rows-1)=Vecv_z(nElem-1);
	 	 //cout <<X_av1<<endl;

	 	 X_s(nElem-1,0).resize(X_s(nElem-1,0).n_rows+1); //+1 = now add also the speed
	 	 X_s(nElem-1,0)(X_s(nElem-1,0).n_rows-1)=Vecv_z(nElem-1); //to the last give speed


	 	 X_av(nElem-1,0)=p_s(1)*X_s(nElem-1,0)+p_s(0)*X_av1;
 	 }
	 else
	 	 {
	 		for (int i=0;i<nElem;i++)
	 		{
	 			X_av(i,0).set_size(X_s(i,0).n_rows+1);
	 			X_av(i,0)(span(0,X_av(i,0).n_rows-2))=X_s(i,0); //copy whole body
	 			X_av(i,0)(X_av(i,0).n_rows-1)=Vecv_z(i);

			 	X_s(i,0).resize(X_s(i,0).n_rows+1); //+1 = now add also the speed
			 	X_s(i,0)(X_s(i,0).n_rows-1)=Vecv_z(i);
	 		}


	 	 }

	 X_av1.reset();

	 //Set the average solution vector X_s_av and weights W_s

	 int c=0; //index pointer where to write the values
	 for (int i=0;i<nElem;i++)
	 {
		 X_s_av(span(c,nX(i)+c-1))=X_av(i,0)(span(0,X_av(i,0).n_rows-2)); //save each average solution (T+htc) to its position in overall vector, we substract speed and solve it separately
		 X_s_av(X_s_av.n_rows-1)+=(1.0/nElem)*X_av(i,0)(X_av(i,0).n_rows-1); //average speed, sum normalized parts

		 c+=nX(i);
	 }


	 //Compute and update delta (termination parameter)

	 vec norm1=(X_s_av-X_s_av_old);
	 double sum2s=0;
	 c=0;
	 double sum1=0;

	 if (p<pmax+1)
	 {
		 for (int s=0;s<nElem;s++)
		 {
			 X_s_opt(s,0)=X_s_av;
			 X_s_opt(s,0)(span(c,nX(s)+c-1))=X_s(s,0)(span(0,X_s(s,0).n_rows-2)); //-1 because of v_z is all the same for every element
			 vec norm2=(X_s_opt(s,0)-X_s_av);
			 W_s(s,0)=W_s_old(s,0)+rho*norm2;

			 sum1=0;
			 for (int i=0;i<nXs;i++)
			 {
				 sum1+=(norm1(i)*norm1(i));
				 sum2(s,0)(0)+=norm2(i)*norm2(i);
			 }

			 sum2s+=0.5*(p_s(0)+p_s(1))*sum2(s,0)(0);
			 c+=nX(s);
		 }
	 }
	 delta=sqrt(sum1+sum2s);

	 sum2s=sum1=0;

	 //Get ready for another iteration - again Set elements etc.
	 field<vec> ToDeVect(nElem,1);

	 X_s_av_old=X_s_av;
	 X_av_old=X_av;

	 field<mat>printTD(nElem,1);
	 mat output(ndr,ndz);
	 c=0;
	 for (int i=0;i<nElem;i++)
	 {
		 W_s_old(i,0)=W_s(i,0);

		 ToDeVect(i,0).zeros(X_av(i,0).n_rows-3); //we dont want to devect htc+v_z
		 ToDeVect(i,0)=X_av(i,0)(span(0,X_av(i,0).n_rows-3)); //set it as vector of temp, so we omit htc and v_z
		 Param(i)=X_av(i,0)(X_av(i,0).n_rows-2); //update htc

		 if ((i>0) && (i<nElem-1))
		 {
			 ToDeVect(i,0).set_size(X_av(i,0).n_rows-2+n_offset*ndr*ndphi); //resize to add LEFT overlaying part to its element
			 ToDeVect(i,0)(span(0,n_offset*ndr*ndphi-1))=ToDeVect(i-1,0)(span(ToDeVect(i-1,0).n_rows-1-n_offset*ndr*ndphi+1,ToDeVect(i-1,0).n_rows-1)); //get there the average, which was written to the right side of previous element
			 ToDeVect(i,0)(span(n_offset*ndr*ndphi,ToDeVect(i,0).n_rows-1))=X_av(i,0)(span(0,X_av(i,0).n_rows-3)); //copy the whole body except PHA part
		 }
		 if (i==nElem-1)
		 {
			 ToDeVect(nElem-1,0).set_size(X_av(nElem-1,0).n_rows-2+n_offset*ndr*ndphi); //resize to add LEFT overlaying part to its element
			 ToDeVect(nElem-1,0)(span(0,n_offset*ndr*ndphi-1))=ToDeVect(nElem-1-1,0)(span(ToDeVect(nElem-1-1,0).n_rows-1-n_offset*ndr*ndphi+1,ToDeVect(nElem-1-1,0).n_rows-1)); //get there the average, which was written to the right side of previous element
			 ToDeVect(nElem-1,0)(span(n_offset*ndr*ndphi,ToDeVect(nElem-1,0).n_rows-1))=X_av(nElem-1,0)(span(0,X_av(nElem-1,0).n_rows-3));

		 }

		 printTD(i,0)=DeVectM(ToDeVect(i,0),ndr,ndphi,Elem(i,0).n_slices);

		 Elem(i,0)=DeVect(ToDeVect(i,0),ndr,ndphi,Elem(i,0).n_slices);

		 //update weights for scenarios
		 if (i==0)
		 {
		 W(i,0)(span(0,W(i,0).n_rows-2))=W_s(i,0)(span(c,nX(i)+c-1)); //from W_s we copy Ts+htc for i-th element
		 W(i,0)(W(i,0).n_rows-1)=W_s(i,0)(W_s(i,0).n_rows-1); //v_z

		 c+=nX(i);
		 }
		 else
		 {
			 W(i,0)(span(0,W(i,0).n_rows-2))=W_s(i,0)(span(c,nX(i)+c-1+ndr*ndphi*n_offset)); //from W_s we copy Ts+htc for i-th element
			 W(i,0)(W(i,0).n_rows-1)=W_s(i,0)(W_s(i,0).n_rows-1); //v_z

			 c+=nX(i)-ndr*ndphi*n_offset;
		 }

		 //zero everything not needed for another iteration to zeros or reset it to avoid numerical errors

	 }
//	 printTD(0,0).save("printTD0.mat",raw_ascii);
//	 printTD(1,0).save("printTD1.mat",raw_ascii);
//	 printTD(2,0).save("printTD2.mat",raw_ascii);
//	 printTD(3,0).save("printTD3.mat",raw_ascii);
//	 output(span::all,span(0,59))=printTD(0,0)(span::all,span(0,59));
//	 output(span::all,span(60,104))=printTD(1,0)(span::all,span(15,59));
//	 output(span::all,span(105,149))=printTD(2,0)(span::all,span(15,59));
//	 output(span::all,span(150,199))=printTD(3,0)(span::all,span(15,64));
//	 output(span::all,59)=(1.0/2)*(output(span::all,58)+output(span::all,60));
//	 output(span::all,104)=(1.0/2)*(output(span::all,103)+output(span::all,105));
//	 output(span::all,149)=(1.0/2)*(output(span::all,148)+output(span::all,150));
//
//	 output.save("HeatCompOut/output.mat",raw_ascii);


 } //end of while

//return temperature matrix
 	//T.zeros();
	int c=0;
	for (int i=0;i<nElem-1;i++)
	{
		if (i==0)
		{
			T.slices(c,Elem(i,0).n_slices-1)=Elem(i,0);
			c+=Elem(i,0).n_slices;
		}
		else
		{
			T.slices(c,c+Elem(i,0).n_slices-n_offset-1)=Elem(i,0).slices(n_offset-1+1,Elem(i).n_slices-1);
			c+=Elem(i,0).n_slices-ElemL(i,0).n_slices;
		}
//		cout <<c<<endl;
	}
//	cout << ElemL(nElem-1,0).n_slices<< endl;
//	cout << Elem(nElem-1,0).n_slices<< endl;
	T.slices(c,T.n_slices-1)=Elem(nElem-1,0).slices(ElemL(nElem-1,0).n_slices-1+1,Elem(nElem-1,0).n_slices-1); //ERROR here
//	double v_z=X_s_av(X_s_av.n_rows-1);

	T.save("HeatCompOut/T_opt.cub",arma_ascii);
	//create matrix representing temperature profile - useful for visualisation, which is better using matlab/octave

	for ( int k=0; k<T.n_slices; k++)
			ProfileMat(span::all,k)=T.slice(k)(span::all,1);


	//Interpolate errors
	c=Elem(0,0).n_slices-1;
	for (int i=0;i<nElem-1;i++)
		ProfileMat(span::all,c+i*(Elem(i,0).n_slices-n_offset))=(1.0/2)*(T.slice(c+i*(Elem(i,0).n_slices-n_offset)-1)(span::all,1)+T.slice(c+i*(Elem(i,0).n_slices-n_offset)+1)(span::all,1));
	char str0[50];
	sprintf(str0,"HeatCompOut/%s",fnameOctave);

	ProfileMat.save(str0,raw_ascii);

	char str[100];
	sprintf(str,"cd \HeatCompOut; octave -q --eval \"visualiseF('%s')\";exit",fnameOctave);
	system(str);

	cout << "Optimal htcs are: "<< endl;
	cout << OptimalHtcs<<endl;
}
