//																23.APR.2013											tripendolo.cc

#include <curses.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <corso.h>

extern XVideoData xvd;

void func(double *t,double *y,double *dydt);
void FrogAcc(double *t,double *x, double *dxdt,double *d2xdt2,double step);
void VerlAcc(double *t,double *x, double *dxdt,double *d2xdt2);

main()
{
	int DeltaSave,i,ii,ix,ix0,iy,iy0,iyf,iyf0,mx,my,nComm,nPar;
	double alpha,dt,step,tMax,tMin,tWait,yScale,yScale2;
	double *tl,*tr,*tv,*y;
	double *d2xdt2,*dxdt,*dxldt,*x,*xl;
	char answ[122],buff[40];
	char **comandi,**ParName,**ParVal;
	clock_t musec1,musec2;
	timespec ts1,ts2;
	FILE *hnd;
	// ............................................................. modo grafico
	StartXWindow(800,600,0,0,"Tripendolo-molla",2);
	// ...................................................... allocazione memoria
	nPar=8;
	ParName=new char * [nPar];
	ParVal=new char * [nPar];
	for (i=0;i<nPar;i++) ParVal[i]=new char [24];
	ParName[0]="Lunghezza";
	ParName[1]="Massa";
	ParName[2]="g";
	ParName[3]="Costante di Hooke";
	ParName[4]="Smorzamento Runge";
	ParName[5]="Smorzamento Verlet";
	ParName[6]="Smorzamento Leapfrog";
	ParName[7]="Intervallo/ms";
	
	// .................................................................. comandi
	nComm=4;
	comandi=new char * [nComm];
	comandi[0]="Caduta";
	comandi[1]="Calcione";
	comandi[2]="Ferma";
	comandi[3]="Fine";
	// .......................................................... allocate arrays
	tl=new double [6];
	tr=new double [6];
	tv=new double [6];
	y=new double [4];
	x=new double [2];
	xl=new double [2];
	dxdt=new double [2];
	d2xdt2=new double [2];
	dxldt=new double [2];
	// ............................................................... intervallo
	tWait=5.0;
	// ................................................................. costanti
	tv[1]=tr[1]=tl[1]=300.0; //           lunghezza
	tv[2]=tr[2]=tl[2]=1.0; //             massa
	tv[3]=tr[3]=tl[3]=9.81; //            g
	tv[4]=tr[4]=tl[4]=20.0; //           costante di Hooke
	tv[5]=tr[5]=tl[5]=0.005; //    smorzamento
	sprintf(ParVal[0],"%.4lf",tr[1]);
	sprintf(ParVal[1],"%.4lf",tr[2]);
	sprintf(ParVal[2],"%.4lf",tr[3]);
	sprintf(ParVal[3],"%.4lf",tr[4]);
	sprintf(ParVal[4],"%.4lf",tr[5]);
	sprintf(ParVal[5],"%.4lf",tv[5]);
	sprintf(ParVal[6],"%.4lf",tl[5]);
	sprintf(ParVal[7],"%.4lf",tWait);
	// .......................................... condizioni iniziali Runge-Kutta
	y[0]=tr[1]; //               x0=lunghezza
	y[1]=0.0; //         velocita' x iniziale
	y[2]=0.0; //                         y0=0
	y[3]=0.0; //         velocita' y iniziale
	// ............................................... condizioni iniziali Verlet
	x[0]=tr[1]; //               x0=lunghezza
	dxdt[0]=0.0; //     velocita' x iniziale
	x[1]=0.0; //                        y0=0
	dxdt[1]=0.0; //     velocita' y iniziale
	// ............................................. condizioni iniziali leapfrog
	xl[0]=tl[1]; //              x0=lunghezza
	dxldt[0]=0.0; //     velocita' x iniziale
	xl[1]=0.0; //                        y0=0
	dxldt[1]=0.0; //     velocita' y iniziale
	// ...................................................... intervallo di tempo
	step=0.02;
	// ............................................................ campionamento
	DeltaSave=1;
	// ........................................................... punti iniziali
	ix0=400;
	iy0=20;
	// .......................................................... disegna il menu
	XWinPermMenu(0,"Tripendolo con molla",ParName,ParVal,nPar,16,comandi,nComm,0);
	// ............................................... primo passo di Runge-Kutta
	tv[0]=tr[0]=tl[0]=0;
	RungeStep(tr,y,4,step,0,func);
	verlet(tv,x,dxdt,d2xdt2,2,step,VerlAcc,0);
	LeapFrog(tl,xl,dxldt,2,step,0,FrogAcc);
	XSync(xvd.display,TRUE);
	// ................................................ 0 secondi nell'intervallo
	ts1.tv_sec=0;
	// ..................................................... ciclo di Runge Kutta
	musec1=clock();
	for (;;)
	{
		RungeStep(tr,y,4,step,1,func);
		verlet(tv,x,dxdt,d2xdt2,2,step,VerlAcc,1);
		LeapFrog(tl,xl,dxldt,2,step,1,FrogAcc);
		// ................................ seleziona e pulisci il contesto grafico
		XSetForeground(xvd.display,xvd.gc,xvd.white);
		XFillRectangle(xvd.display,xvd.backpix[0],xvd.gc,0,0,xvd.width[0],
									 xvd.height[0]);
		// ............................................... disegna la circonferenza
		ii=int(tr[1]+0.5);
		XSetForeground(xvd.display,xvd.gc,xvd.yellow[0]);
		XDrawArc(xvd.display,xvd.backpix[0],xvd.gc,ix0-ii,iy0-ii,2*ii,2*ii,0,23040);
		// ...................................... disegna il pendolo di Runge-Kutta
		XSetForeground(xvd.display,xvd.gc,xvd.black);
		XFillArc(xvd.display,xvd.backpix[0],xvd.gc,ix0-2,iy0-2,4,4,0,23040);
		ix=ix0+int(y[0]+0.5);
		iy=iy0+int(y[2]+0.5);
		XDrawLine(xvd.display,xvd.backpix[0],xvd.gc,ix0,iy0,ix,iy);
		XSetForeground(xvd.display,xvd.gc,xvd.red[1]);
		ix-=10;
		iy-=10;
		XFillArc(xvd.display,xvd.backpix[0],xvd.gc,ix,iy,20,20,0,23040);
		// ........................................... disegna il pendolo di Verlet
		XSetForeground(xvd.display,xvd.gc,xvd.black);
		XFillArc(xvd.display,xvd.backpix[0],xvd.gc,ix0-2,iy0-2,4,4,0,23040);
		ix=ix0+int(x[0]+0.5);
		iy=iy0+int(x[1]+0.5);
		XDrawLine(xvd.display,xvd.backpix[0],xvd.gc,ix0,iy0,ix,iy);
		XSetForeground(xvd.display,xvd.gc,xvd.blue[1]);
		ix-=10;
		iy-=10;
		XFillArc(xvd.display,xvd.backpix[0],xvd.gc,ix,iy,20,20,0,23040);
		// ........................................... disegna il pendolo ranocchio
		XSetForeground(xvd.display,xvd.gc,xvd.black);
		XFillArc(xvd.display,xvd.backpix[0],xvd.gc,ix0-2,iy0-2,4,4,0,23040);
		ix=ix0+int(xl[0]+0.5);
		iy=iy0+int(xl[1]+0.5);
		XDrawLine(xvd.display,xvd.backpix[0],xvd.gc,ix0,iy0,ix,iy);
		XSetForeground(xvd.display,xvd.gc,xvd.green[1]);
		ix-=10;
		iy-=10;
		XFillArc(xvd.display,xvd.backpix[0],xvd.gc,ix,iy,20,20,0,23040);
		// .......................................................... scrivi i nomi
		XSetForeground(xvd.display,xvd.gc,xvd.red[1]);
		XPrintf(0,10,530,1,TRUE,"Runge-Kutta");
		XSetForeground(xvd.display,xvd.gc,xvd.blue[1]);
		XPrintf(0,10,550,1,TRUE,"Verlet");
		XSetForeground(xvd.display,xvd.gc,xvd.green[1]);
		XPrintf(0,10,570,1,TRUE,"Leapfrog");
		// ........................................................................
		XRedraw();
		musec2=clock();
		ts1.tv_nsec=int(tWait*1.0e6)-(musec2-musec1)*1000;
		if (ts1.tv_nsec>0) nanosleep(&ts1,&ts2);
		musec1=clock();
		XSync(xvd.display,FALSE);
		// .......................................................... leggi il menu
		ii=XWinPermMenu(0,"X Pendolo con molla",ParName,ParVal,nPar,16,
										comandi,nComm,1);
		if (ii==0)
		{ // ............................................................... caduta
			tv[0]=tr[0]=tl[0]=0;
			y[0]=tr[1]; //                 x0=lunghezza
			y[1]=0.0; //           velocita' x iniziale
			y[2]=0.0; //                           y0=0
			y[3]=0.0; //           velocita' y iniziale
			// ............................................................... Verlet
			x[0]=tr[1]; //                 x0=lunghezza
			dxdt[0]=0.0; //       velocita' x iniziale
			x[1]=0.0; //                          y0=0
			dxdt[1]=0.0; //       velocita' y iniziale
			// ............................................................. Leapfrog
			xl[0]=tl[1]; //                 x0=lunghezza
			dxldt[0]=0.0; //       velocita' x iniziale
			xl[1]=0.0; //                          y0=0
			dxldt[1]=0.0; //       velocita' y iniziale
		}
		else if (ii==1)
		{ // ............................................................. calcione
			tv[0]=tr[0]=tl[0]=0;
			y[0]=0; //                                   x0=0
			y[1]=sqrt(tr[3]*tr[1]); //   velocita' x iniziale
			y[2]=tr[1]; //                       y0=lunghezza
			y[3]=0.0; //                 velocita' y iniziale
			// ............................................................... Verlet
			x[0]=0; //                                   x0=0
			dxdt[0]=sqrt(tr[3]*tr[1]); // velocita' x iniziale
			x[1]=tr[1]; //                       y0=lunghezza
			dxdt[1]=0.0; //              velocita' y iniziale
			// ............................................................. Leapfrog
			xl[0]=0; //                                   x0=0
			dxldt[0]=sqrt(tl[3]*tl[1]); // velocita' x iniziale
			xl[1]=tl[1]; //                       y0=lunghezza
			dxldt[1]=0.0; //              velocita' y iniziale
		}
		else if (ii==2)
		{ // ..................................................... ferma il pendolo
			tv[0]=tr[0]=0;
			y[0]=0; //                               x0=0
			y[1]=0;; //              velocita' x iniziale
			y[2]=tr[1]; //                   y0=lunghezza
			y[3]=0.0; //             velocita' y iniziale
			// ............................................................... Verlet
			x[0]=0; //                               x0=0
			dxdt[0]=0;; //              velocita' x iniziale
			x[1]=tr[1]; //                   y0=lunghezza
			dxdt[1]=0.0; //             velocita' y iniziale
			// ............................................................. Leapfrog
			xl[0]=0; //                               x0=0
			dxldt[0]=0;; //              velocita' x iniziale
			xl[1]=tl[1]; //                   y0=lunghezza
			dxldt[1]=0.0; //             velocita' y iniziale
		}
		else if (ii==3) break;
		else if (ii==nComm)
		{ // ........................................ riparti con i nuovi parametri
			tv[1]=tr[1]=tl[1]=atof(ParVal[0]);//         lunghezza
			tv[2]=tr[2]=tl[2]=atof(ParVal[1]);//         massa
			tv[3]=tr[3]=tl[3]=atof(ParVal[2]);//         g
			tv[4]=tr[4]=tl[4]=atof(ParVal[3]);//         Hooke
			tr[5]=atof(ParVal[4]);//         smorzamento Runge
			tv[5]=atof(ParVal[5]);//         smorzamento Verlet
			tl[5]=atof(ParVal[6]);//         smorzamento leapfrog
			tWait=atof(ParVal[7]);//         intervallo
			sprintf(ParVal[0],"%.4lf",tr[1]);
			sprintf(ParVal[1],"%.4lf",tr[2]);
			sprintf(ParVal[2],"%.4lf",tr[3]);
			sprintf(ParVal[3],"%.4lf",tr[4]);
			sprintf(ParVal[4],"%.4lf",tr[5]);
			sprintf(ParVal[5],"%.4lf",tv[5]);
			sprintf(ParVal[6],"%.4lf",tl[5]);
			sprintf(ParVal[7],"%.4lf",tWait);
		}
		XSync(xvd.display,TRUE);
	}
	// .................................................... last Runge-Kutta step
	RungeStep(tr,y,4,step,2,func);
	verlet(tv,x,dxdt,d2xdt2,2,step,VerlAcc,2);
	LeapFrog(tl,xl,dxldt,2,step,2,FrogAcc);
	// ............................................................... erase menu
	XWinPermMenu(0,"Tripendolo con molla",ParName,ParVal,nPar,16,comandi,nComm,2);
	// ..........................................................................
	delete [] x;
	delete [] xl;
	delete [] dxdt;
	delete [] dxldt;
	delete [] y;
	delete [] tr;
	delete [] tv;
	delete [] tl;
	CloseXWindow();
}

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

void func(double *t,double *y,double *dydt)
{
	double delta,hooke,g,k,length,m,restlength;
	
	restlength=t[1];
	m=t[2];
	g=t[3];
	hooke=t[4];
	k=t[5];
	// .........................................................................
	length=sqrt(y[0]*y[0]+y[2]*y[2]);
	delta=length-restlength;
	// ................................................................ velocita'
	dydt[0]=y[1];
	dydt[2]=y[3];
	// .......................................................... accelerazione x
	dydt[1]=-(hooke*delta*y[0]/length+k*y[1])/m;
	// .......................................................... accelerazione y
	dydt[3]=g-(hooke*delta*y[2]/length+k*y[3])/m;
}
// ----------------------------------------------------------------------------

void FrogAcc(double *t,double *x, double *dxdt,double *d2xdt2,double step)
{
	int i;
	double delta,hooke,g,k,length,m,restlength,v[2];
	
	restlength=t[1];
	m=t[2];
	g=t[3];
	hooke=t[4];
	k=t[5];
	// .........................................................................
	length=sqrt(x[0]*x[0]+x[1]*x[1]);
	delta=length-restlength;
	d2xdt2[0]=-hooke*delta*x[0]/length;
	d2xdt2[1]=m*g-hooke*delta*x[1]/length;
	v[0]=(dxdt[0]+0.5*step*d2xdt2[0])/(1+0.5*k);
	v[1]=(dxdt[1]+0.5*step*d2xdt2[1])/(1+0.5*k);
	d2xdt2[0]-=k*v[0];
	d2xdt2[1]-=k*v[1];
	d2xdt2[0]/=m;
	d2xdt2[1]/=m;
}

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

void VerlAcc(double *t,double *x, double *dxdt,double *d2xdt2)
{
	int i;
	double delta,hooke,g,k,length,m,restlength;
	
	restlength=t[1];
	m=t[2];
	g=t[3];
	hooke=t[4];
	k=t[5];
	// .........................................................................
	length=sqrt(x[0]*x[0]+x[1]*x[1]);
	delta=length-restlength;
	d2xdt2[0]=-(hooke*delta*x[0]/length+k*dxdt[0])/m;
	d2xdt2[1]=g-(hooke*delta*x[1]/length+k*dxdt[1])/m;
}