#include	<stdio.h>
#include	<conio.h>
#include	<stdarg.h>
#include	<dos.h>
#include	<math.h>
#include	<graphics.h>

#define		SYS_N	4
#define		BD_WID	100
#define		BD_HEI	50
#define		WL_RAD	10
#define		LV_LEN	100

#define		MCART	0.5
#define		MPEND	0.2
#define		FRICT	0.1
#define         	INERT	0.006
#define		GRAVT	9.8
#define		LPEND	0.3

void	main(void);
double	df(double t, double y[], double dy[]);
void	cart(int x, int y, int deg, int flg);
int	gprintxy(int xx, int yy, char *fmt, ... );

double	Denom, A1, A2, A3, A4, B1, B2;

void
main(void)
{
	int	gdrv = VGA, gmode = VGAHI;
	int	i, x, posx, posy, deg;
	double	m1[SYS_N], m2[SYS_N], m3[SYS_N], m4[SYS_N];
	double	y[SYS_N], yy[SYS_N];
	double	t, dy, u, h = 0.01;

	initgraph(&gdrv, &gmode, "c:\\borlandc\\bgi");

	Denom = INERT * (MCART + MPEND) + MCART * MPEND * LPEND * LPEND;
	A1 = -(INERT + MPEND * LPEND * LPEND) * FRICT / Denom;
	A2 = -(MPEND * MPEND * GRAVT * LPEND * LPEND) / Denom;
	A3 = (MPEND * LPEND * FRICT) / Denom;
	A4 = MPEND * GRAVT * LPEND * (MCART + MPEND) / Denom;
	B1 = (INERT + MPEND * LPEND * LPEND) / Denom;
	B2 = -MPEND * LPEND / Denom;

	gprintxy(50, 300, "%10.4f %10.4f %10.4f %10.4f", 0., 1., 0., 0.);
	gprintxy(50, 310, "%10.4f %10.4f %10.4f %10.4f", 0., A1, A2, 0.);
	gprintxy(50, 320, "%10.4f %10.4f %10.4f %10.4f", 0., 0., 0., 1.);
	gprintxy(50, 330, "%10.4f %10.4f %10.4f %10.4f", 0., A3, A4, 0.);
	gprintxy(50, 350, "%10.4f %10.4f %10.4f %10.4f", 0., B1, 0., B2);

	posx = 200;
	posy = 200;

	y[0] = 0;	// Initial X position
	y[1] = 0;
	y[2] = 15.;	// Initial Theta
	y[3] = 0;

	for (t = 0; t < 30; t += h)
	{
		x = posx + y[0];
		deg = y[2];
		cart(x , posy, deg, 1);

		for (i = 0; i < SYS_N; i++)
			yy[i] = y[i];
		u = df(t, y, m1);

	for (i = 0; i < SYS_N; i++)
			y[i] = yy[i] + m1[i] * h / 2;
		df(t + h/2, y, m2);

		for (i = 0; i < SYS_N; i++)
			y[i] = yy[i] + m2[i] * h / 2;
		df(t + h/2, y, m3);

		for (i = 0; i < SYS_N; i++)
			y[i] = yy[i] + m3[i] * h;
		df(t + h, y, m4);

		for (i = 0; i < SYS_N; i++) {
			dy = (m1[i] + 2 * (m2[i] + m3[i]) + m4[i]) / 6;
			y[i] = yy[i] + h * dy;
		}

	        delay(30000);
		gprintxy(300, 400, "Time = %5.2lf", t);
		gprintxy(300, 410, "U    = %5.2lf", u);
		cart(x , posy, deg, 0);
		if (x > 500 || x < 10)
			break;
		if (kbhit())
			break;
	}
	cart(x , posy, deg, 1);

	getch();
	closegraph();
}

void
cart(int x, int y, int deg, int flg)
{
	int	x1, x2, y1, y2;
	int	cx, col;
	int	wx, wy;
	double	rad;

	x1 = x;
	y1 = y;
	x2 = x1 + BD_WID;
	y2 = y1 + BD_HEI;
	col = (flg)? LIGHTGRAY: BLACK;
	setcolor(col);
	rectangle(x1, y1, x2, y2);

	col = (flg)? YELLOW: BLACK;
	setcolor(col);
	wx = WL_RAD * sin(M_PI * (x % 8) / 8);
	wy = -WL_RAD * cos(M_PI * (x % 8) / 8);
	y2 += WL_RAD;
	cx = x1	+ WL_RAD * 1.8;
	circle(cx, y2, WL_RAD);
	line(cx-wx,y2-wy, cx+wx,y2+wy);
	cx = x2 - WL_RAD * 1.8;
	circle(cx, y2, WL_RAD);
	line(cx-wx,y2-wy, cx+wx,y2+wy);

	cx = (x1 + x2 ) / 2;
	col = (flg)? LIGHTGRAY: BLACK;
	setcolor(col);
	arc(cx, y1, 0, 180, WL_RAD);

	x1 = x + BD_WID / 2;
	y1 = y;
	rad = deg * M_PI / 180.;
	x2 = x1 + LV_LEN * sin(rad);
	y2 = y1 - LV_LEN * cos(rad);

	col = (flg)? LIGHTRED: BLACK;
	setcolor(col);
	line(x1, y1, x2, y2);
}

int
gprintxy(int xx, int yy, char *fmt, ... )
{
	va_list	argptr;
	char	str[140];
	int	wid, hei;

	va_start(argptr, fmt);

	vsprintf(str, fmt, argptr);
	wid = textwidth(str);
	hei = textheight(str);

	setfillstyle(SOLID_FILL, BLACK

	);
	bar(xx,yy, xx+wid-1,yy+hei-1);

	outtextxy(xx, yy, str);
	va_end(argptr);

	return (wid);
}

double
df(double t, double y[], double dy[])
{
	double	u;

	u = 10. * y[2] + y[0];
	dy[0] = y[1];
	dy[1] = A1 * y[1] + A2 * y[2] + B1 * u;
	dy[2] = y[3];
	dy[3] = A3 * y[1] + A4 * y[2] + B2 * u;

	return (u);
}