/* -*- MaTX -*-
 *  
 *  NAME
 *      ctr_demo() - Demonstration of control system design toolbox
 *
 *  SYNOPSIS
 *     ctr_demo()
 *
 *  DESCRIPTION
 *    	ctr_demo() brings up a menu of the available demonstrations.
 *
 *  SEE ALSO
 *      demo
 */

Func void ctr_demo()
{
	Integer i;
	List mm;

	void ctr_demo_transfer_function(), ctr_demo_poleassign();
	void ctr_demo_servo(), ctr_demo_observer(), ctr_demo_lqr();
	
	mm = {"Demonstration of Control System Toolsbox",
		  "Transfer function and frequency response",
		  "Pole assignment problem",
		  "Type-1 servo control system",
		  "Full-order state observer",
		  "LQ optimal regulator",
		  "Quit"};

	i = 1;
	for (;;) {
		i = menu(mm, i);
		switch (i) {
		  case 1:
			ctr_demo_transfer_function();
			break;
		  case 2:
			ctr_demo_poleassign();
			break;
		  case 3:
			ctr_demo_servo();
			break;
		  case 4:
			ctr_demo_observer();
			break;
		  case 5:
			ctr_demo_lqr();
			break;
		}
		if (i == length(mm)-1) { break; }
		i++;
	}
}

/*
 * Tranferfunction, time response, and frequency response
 */
Func void ctr_demo_transfer_function()
{
	Integer win;
	Matrix A, B, C, D, P;
	Matrix num, den, kk;
	Rational g;
	Polynomial s;
	Array w, ga, ph, re, im, k, t, y;
	CoMatrix po, ze, ro;

	win = mgplot_cur_win();

	clear;
	print "\n";
	print "Plant in transfer function form\n";
	print "                   2                    \n";
	print "              0.2 s  +  0.3 s  +  1     \n";
	print "G(s)  =  -------------------------------\n";
	print "           2                            \n";
	print "         (s  +  0.4 s  +  1) (s  +  0.5)\n";
	print "\n";
	print "can be entered as\n";
	print "\n";
	print "\ts = Polynomial(\"s\");\n";
	print "\tg = (0.2*s^2 + 0.3*s + 1)/((s^2 + 0.4*s + 1)*(s + 0.5));\n";
	print "\n";
	pause;

	s = Polynomial("s");
	g = (0.2*s^2 + 0.3*s + 1)/((s^2 + 0.4*s + 1)*(s + 0.5));

	print "Coefficients of the numerator and the demonimator:\n";
	print "\n";
	print "\t{num,den} = tfn2tf(g);\n";
	print "\tprint num, den;\n";
	print "\n";
	pause;

	{num,den} = tfn2tf(g);
	print num, "\n", den, "\n";

	print "State space representation\n";
	print "\t .           \n";
	print "\tx = Ax + Bu \n";
	print "\ty = Cx + Du \n";
	print "\n";
	print "\t{A,B,C,D} = tf2ss(num,den);\n";
	print "\tprint A,B,C,D;\n";
	print "\n";
	pause;

	{A,B,C,D} = tf2ss(num,den);
	print A,B,C,D;

	print "\n";
	print "Poles, zeros, and gain:\n";
	print "\n";
	print "\t{ze,po,kk} = ss2zp(A,B,C,D);\n";
	print "\tprint po,ze;\n";
	print "\n";
	pause;

	{ze,po,kk} = ss2zp(A,B,C,D);
	print po,"\n",ze;

	print "\n";
	print "The given transfer function\n";
	print "\n";
	print "\tg = ss2tfn(A, B, C, D)\n";
	print "\n";
	pause;
	print g = ss2tfn(A, B, C, D);

	// Root locus

	print "\n";
	print "Root-locus with desired gains:\n";
	print "\n";
	print "\tk = [0:0.5:10];\n";
	print "\tro = rlocus(num,den,k);\n";
	print "\n";
	print "\tmgplot(win,Re(ro),Im(ro));\n";
	print "\tmgplot_title(win,\"Root-locus\");\n";
	print "\tmgplot_xlabel(win,\"Real part\");\n";
	print "\tmgplot_ylabel(win,\"Img part\");\n";
	print "\n";
	pause;

	k = [0:0.5:10];
	ro = rlocus(num,den,k);

	mgplot_reset(win);
	mgplot_title(win,"Root-locus");
	mgplot_xlabel(win,"Real part");
	mgplot_ylabel(win,"Img part");
	mgplot(win,Re(ro),Im(ro),{"","",""},{"1","1","1"});

	// Time response

	print "Step response (time response):\n";
	print "\n";
	print "\tt = [0:0.3:15];\n";
	print "\t{y} = step(A,B,C,D,1,t);\n";
	print "\n";
	print "\tmgplot(win,t,y,{\"y\"});\n";
	print "\tmgplot_title(win,\"Step response\");\n";
	print "\tmgplot_xlabel(win,\"time(sec)\");\n";
	print "\n";
	pause;

	t = [0:0.3:15];
	{y} = step(A,B,C,D,1,t);

	mgplot_reset(win);
	mgplot(win,t,y,{"y"});
	mgplot_title(win,"Step response");
	mgplot_xlabel(win,"time(sec)");

	// Frequency response

	print "Bode plot (frequency response):\n";
	print "\n";
	print "\tw = logspace(0.001, 1000.0);\n";
	print "\t{ga, ph} = bode_tfn(g, w);\n";
	print "\tga = 20*log10(ga);\n";
	print "\n";
	print "\tmgplot_subplot(win, 2, 1, 1);\n";
	print "\tmgplot_semilogx(win, w, ga, {\"gain\"});\n";
	print "\tmgplot_subplot(win, 2, 1, 2);\n";
	print "\tmgplot_semilogx(win, w, ph, {\"phase\"});\n";
	print "\n";
	pause;

	w = logspace(0.001, 1000.0);
	{ga,ph} = bode_tfn(g, w);
	ga = 20*log10(ga);

	win = mgplot_cur_win();
	mgplot_reset(win);
	mgplot_hold(win, 1);
	mgplot_subplot(win, 2, 1, 1);
	mgplot_semilogx(win, w, ga, {"gain"});
	mgplot_subplot(win, 2, 1, 2);
	mgplot_semilogx(win, w, ph, {"phase"});
	mgplot_hold(win, 0);

	print "Nyquist plot:\n";
	print "\n";
	print "\t{re, im} = nyquist_tfn(g, w);\n";
	print "\n";
	print "\tmgplot_subplot(win, 1, 1);\n";
	print "\tmgplot_cmd(win, \"set xrange [-3:1]\");\n";
	print "\tmgplot_cmd(win, \"set yrange [-3:1]\");\n";
	print "\tmgplot_grid(win, 1);\n";
	print "\tmgplot(win, re, im, {\"nyquist\"});\n";
	print "\n";
	pause;

	{re,im} = nyquist_tfn(g,w);

	mgplot_reset(win);
	mgplot_hold(win, 1);
	mgplot_subplot(win,1,1);
	mgplot_cmd(win, "set xrange [-3:1]");
	mgplot_cmd(win, "set yrange [-3:1]");
	mgplot_grid(win, 1);
	mgplot(win, re, im, {"nyquist"});
	mgplot_hold(win, 0);

	pause;
}

/*
 * Pole assignment problem
 */
Func void ctr_demo_poleassign()
{
	Complex i;
	Matrix A, B, C, D;
	Matrix V, N, T, J, F;
	Polynomial p, pc;

	print "\nInput A, B, C, and D matrices\n\n";

	print "  A = [[ 0    1   0]\n";
	print "       [ 0    0   1]\n";
	print "       [-6, -11, -6]]\n";
	print "  B = [[0]\n";
	print "       [0]\n";
	print "       [10]]\n";
	print "  C = [1 0 0]\n";
	print "  D = [0]\n";
	print "\n";
	pause;

	A = [[ 0    1   0]
		 [ 0    0   1]
		 [-6, -11, -6]];
	B = [[0]
		 [0]
		 [10]];
	C = [1 0 0];
	D = [0];

	print "Controllability matrix:\n";
	print "\n";
	print "  V = ctrm(A, B)\n";
	print "\n";
	pause;

	print V = ctrm(A, B);

	print "\n";
	print "Check the controllability\n";
	print "\n";
	print "  rank(V)\n";
	print "\n";
	pause;

	print rank(V);

	print "\n";
	print "Characteristic polynomial of A\n";
	print "\n";
	print "  p = makepoly(A)\n";
	print "\n";
	pause;

	print p = makepoly(A);

	print "\n";
	print "Transformation matrix T to controllable canonical form.\n";
	print "\n";
	print "  N = [[p(1) p(2) 1]\n";
	print "       [p(2)  1   0]\n";
	print "       [ 1    0   0]];\n";
	print "  T = V*N\n";
	print "\n";
	pause;

	N = [[p(1) p(2) 1]
		 [p(2)  1   0]
		 [  1   0   0]];

	print T = V*N;

	print "\n";
	print "Characteristic polynomial with desired closed-loop poles.\n";
	print "\n";
	print "  i = (0,1);\n";
	print "  pc = Re(makepoly([-2+3*i, -2-3*i, -10.0]))\n";
	print "\n";
	pause;

	i = (0,1);
	print pc = Re(makepoly([-2+3*i, -2-3*i, -10.0]));

	print "\n";
	print "State feedback gain matrix F\n";
	print "\n";
	print "  F = [pc(0)-p(0), pc(1)-p(1), pc(2)-p(2)]*inv(T)\n";
	print "\n";
	pause;

	print F = [pc(0)-p(0), pc(1)-p(1), pc(2)-p(2)]*inv(T);

	print "\n";
	print "Check the pole of the closed-loop system\n";
	print "\n";
	print "  eigval(A - B*F)\n";
	print "\n";
	pause;

	print eigval(A - B*F);

	print "\n";
	pause;
}

/*
 * Design of servo system
 */
Func void ctr_demo_servo()
{
	Integer win;
	Real Fi;
	Complex i;
	Matrix A, B, C, D, F;
	Matrix Aa, Ba, Fa, V, P;
	Matrix Ac, Bc, Cc, Dc;
	Array T, X, Y;

	print "\n";
	print "Input A, B, C, and D matrices\n";
	print "\n";
	print "  A = [[   0 1 0 0]\n";
	print "       [  20 0 0 0]\n";
	print "       [   0 0 0 1]\n";
	print "       [-0.5 0 0 0]];\n";
	print "  B = [[ 0 ]\n";
	print "       [ -1]\n";
	print "       [ 0 ]\n";
	print "       [0.5]];\n";
	print "  C = [0 0 1 0];\n";
	print "  D = [0];\n";
	print "\n";
	pause;

	A = [[   0 1 0 0]
		 [  20 0 0 0]
		 [   0 0 0 1]
		 [-0.5 0 0 0]];
	B = [[0][-1][0][0.5]];
	C = [0 0 1 0];
	D = [0];

	print "Augmented system:\n";
	print "\n";
	print "  Aa = [[ A, Z(4,1)]\n";
	print "        [-C,   Z(1)]]\n";
	print "  Ba = [[B]\n";
	print "        [0]]\n";
	print "\n";
	pause;

	print Aa = [[ A Z(4,1)]
				[-C, Z(1)]];
	print Ba = [[B][0]];

	print "\n";
	print "Controllability matrix:\n";
	print "\n";
	print "  V = ctrm(Aa, Ba)\n";
	print "\n";
	pause;

	print V = ctrm(Aa, Ba);

	print "\n";
	print "Check the controllability:\n";
	print "\n";
	print "  rank(V)\n";
	print "\n";
	pause;

	print rank(V);

	print "\n";
	print "Desired closed-loop poles\n";
	print "\n";
	print "  i = (0,1);\n";
	print "  P = [[-1+sqrt(3)*i]\n";
	print "       [-1-sqrt(3)*i]\n";
	print "       [      -5    ]\n";
	print "       [      -5    ]\n";
	print "       [      -5    ]]\n";
	print "\n";
	pause;

	i = (0,1);
	print P = [[-1+sqrt(3)*i]
			   [-1-sqrt(3)*i]
			   [    -5      ]
			   [    -5      ]
			   [    -5      ]];

	print "\n";
	print "Solve the pole assignment problem\n";
	print "\n";
	print "  Fa = pplace(Aa, Ba, P)\n";
	print "  Fi = - Fa(5);\n";
	print "\n";
	pause;

	print Fa = pplace(Aa, Ba, P);
	print "\n";
	print Fi = - Fa(5);
	print "\n";
	pause;

	print "State feedback gain matrix F\n";
	print "\n";
	print "  F = Fa(1:4)\n";
	print "\n";
	pause;

	print F = Fa(1:4);

	print "\n";
	print "Closed-loop system\n";
	print "\n";
	print "  Ac = [[ A - B*F, B*Fi]\n";
	print "        [   -C   , Z(1)]];\n";
	print "  Bc = [[0]\n";
	print "        [0]\n";
	print "        [0]\n";
	print "        [1]];\n";
	print "  Cc = [C Z(1)];\n";
	print "  Dc = [0];\n";
	print "\n";
	pause;

	print Ac = [[A - B*F, B*Fi]
				[  -C   , Z(1)]];
	print Bc = [[0][0][0][0][1]];
	print Cc = [C Z(1)];
	print Dc = [0];

	print "\n";
	print "Step response of the closed-loop system\n";
	print "\n";
	print "  T = linspace(0.0, 5.0);\n";
	print "  {Y,X} = step_ss(Ac, Bc, Cc, Dc, 1, T);\n";
	print "\n";
	pause;

	T = linspace(0.0, 5.0);
	{Y, X} = step_ss(Ac, Bc, Cc, Dc, 1, T);

	print "\n";
	print "Plot the step response (y vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 2, 1, 1);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot_title(win, \"Output y versus Time t\");\n";
	print "  mgplot_xlabel(win, \"Sec\");\n";
	print "  mgplot_ylabel(win, \"Output y = x3\");\n";
	print "  mgplot(win, T, Y, {\"y\"});\n";
	print "\n";
	pause;

	win = mgplot_cur_win();
	mgplot_reset(win);
	mgplot_subplot(win, 2, 1, 1);
	mgplot_grid(win, 1);
	mgplot_title(win, "Output y versus Time t");
	mgplot_xlabel(win, "Sec");
	mgplot_ylabel(win, "Output y = x3");
	mgplot(win, T, Y, {"y"});

	print "\n";
	print "Plot the step response (x vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 2, 1, 2);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot_title(win, \"Response Curves for x1, x2, x3, x4, x5\");\n";
	print "  mgplot_xlabel(win, \"Sec\");\n";
	print "  mgplot_ylabel(win, \"x1, x2, x3, x4, x5\");\n";
	print "  mgplot_text(win, \"x1\", 1.2,  0.05);\n";
	print "  mgplot_text(win, \"x2\", 1.2, -0.32);\n";
	print "  mgplot_text(win, \"x3\", 1.2,  0.33);\n";
	print "  mgplot_text(win, \"x4\", 2.2,  0.25);\n";
	print "  mgplot_text(win, \"x5\", 1.5,  1.28);\n";
	print "  mgplot(win, T, X, {\"x1\", \"x2\", \"x3\", \"x4\", \"x5\"});\n";
	print "\n";
	pause;

	mgplot_subplot(win, 2, 1, 2);
	mgplot_grid(win, 1);
	mgplot_title(win, "Response Curves for x1, x2, x3, x4, x5");
	mgplot_xlabel(win, "Sec");
	mgplot_ylabel(win, "x1, x2, x3, x4, x5");
	mgplot_text(win, "x1", 1.2,  0.05);
	mgplot_text(win, "x2", 1.2, -0.32);
	mgplot_text(win, "x3", 1.2,  0.33);
	mgplot_text(win, "x4", 2.2,  0.25);
	mgplot_text(win, "x5", 1.5,  1.28);
	mgplot(win, T, X, {"x1", "x2", "x3", "x4", "x5"});

	pause;
}

/*
 * Design of full-order state observer
 */
Func void ctr_demo_observer()
{
	Complex i;
	Matrix A, C, N, W, T, Fo;
	Polynomial p, po;

	print "\n";
	print "Input A and C\n";
	print "\n";
	print "  A = [[ 0    1   0]\n";
	print "       [ 0    0   1]\n";
	print "       [-5, -10, -5]]\n";
	print "  C = [1 0 0]\n";
	print "\n";
	pause;

	A = [[ 0   1  0]
		 [ 0   0  1]
		 [-5,-10,-5]];
	C = [1 0 0];

	print "Observability matrix\n";
	print "\n";
	print "  N = obsm(A,C)\n";
	print "\n";
	pause;

	print N = obsm(A,C);

	print "\n";
	print "Check the observability\n";
	print "\n";
	print "  rank(N)\n";
	print "\n";
	pause;

	print rank(N);

	print "\n";
	print "Characteristic polynomial of A\n";
	print "\n";
	print "  p = makepoly(A)\n";
	print "\n";
	pause;

	print p = makepoly(A);

	print "\n";
	print "Trasformation matrix to observable canonical form\n";
	print "\n";
	print "  W = [[p(1) p(2) 1]\n";
	print "       [p(2)  1   0]\n";
	print "       [ 1    0   0]];\n";
	print "  T = W*N'\n";
	print "\n";
	pause;

	W = [[p(1) p(2) 1]
		 [p(2)  1   0]
		 [ 1    0   0]];

	print T = W*N';

	print "\n";
	print "Characteristic polynomial with observer poles\n";
	print "\n";
	print "  i = (0,1);\n";
	print "  po = Re(makepoly([-2+i, -2-i, -5]))\n";
	print "\n";
	pause;

	i = (0,1);
	print po = Re(makepoly([-2+i, -2-i, -5]));

	print "\n";
	print "Gain matrix of observer\n";
	print "\n";
	print "  Fo = T \\ [po(0)-p(0), po(1)-p(1), po(2)-p(2)]'\n";
	print "\n";
	pause;

	print Fo = T \ [po(0)-p(0), po(1)-p(1), po(2)-p(2)]';
	print "\n";

	pause;
}

/*
 * Design of quadratic optimal regulator system
 */
Func void ctr_demo_lqr()
{
	Integer win;
	Matrix A, B, C, D, Ac, Cc;
	Matrix Q, R, P, F;
	CoMatrix pc;
	Array to, xo, yo, tc, xc, yc;

	print "\n";
	print "Input A, B, C, and D\n";
	print "\n";
	print "  A = [[  0    1   0]\n";
	print "       [  0    0   1]\n";
	print "       [-35, -27, -9]]\n";
	print "  B = [[0]\n";
	print "       [0]\n";
	print "       [1]]\n";
	print "  C = I(3);\n";
	print "  D = Z(3,1);\n";
	print "\n";
	pause;

	A = [[  0    1   0]
		 [  0    0   1]
		 [-35, -27, -9]];
	B = [[0][0][1]];
	C = I(3);
	D = Z(3,1);

	print "Input weighting matrices Q and R\n";
	print "\n";
	print "  Q = 100 * I(3)\n";
	print "  R = [1]\n";
	print "\n";
	pause;

	Q = 100 * I(3);
	R = [1];

	print "Optimal feedback gain\n";
	print "\n";
	print "  {F} = lqr(A,B,Q,R);\n";
	print "\n";
	pause;
	
	{F} = lqr(A, B, Q, R);
	print F;

	print "\n";
	print "Closed-loop poles\n";
	print "\n";
	print "  pc = eigval(A - B*F)\n";
	print "\n";
	pause;

	print pc = eigval(A - B*F);

	print "\n";
	print "Step response of the open-loop system\n";
	print "\n";
	print "  to = linspace(0.0, 5.0);\n";
	print "  {yo, xo} = step_ss(A, B, C, D, 1, to);\n";
	print "\n";
	pause;

	to = linspace(0.0, 5.0);
	{yo, xo} = step_ss(A, B, C, D, 1, to);

	print "Plot the step response (yo vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 3, 1, 1);\n";
	print "  mgplot_title(win, \"\");\n";
	print "  mgplot(win, t, yo(1,:), {\"yo1\"});\n";
	print "\n";
	pause;

	win = mgplot_cur_win();
	mgplot_reset(win);

	mgplot_subplot(win, 3, 1, 1);
	mgplot_grid(win, 1);
	mgplot(win, to, yo(1,:), {"yo1"});

	print "Plot the step response (yo2 vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 3, 1, 2);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot(win, t, yo(2,:), {\"yo2\"});\n";
	print "\n";
	pause;

	mgplot_subplot(win, 3, 1, 2);
	mgplot_grid(win, 1);
	mgplot(win, to, yo(2,:), {"yo2"});

	print "Plot the step response (yo3 vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 3, 1, 3);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot(win, t, yo(3,:), {\"yo3\"});\n";
	print "\n";
	pause;

	mgplot_subplot(win, 3, 1, 3);
	mgplot_grid(win, 1);
	mgplot(win, to, yo(3,:), {"yo3"});

	print "Step response of the closed-loop system\n";
	print "\n";
	print "  Ac = A - B*F;\n";
	print "  Cc = C - D*F;\n";
	print "  tc = linspace(0.0, 5.0);\n";
	print "  {yc, xc} = step_ss(Ac, B, Cc, D, 1, tc);\n";
	print "\n";
	pause;

	Ac = A - B*F;
	Cc = C - D*F;
	tc = linspace(0.0, 5.0);
	{yc, xc} = step_ss(Ac, B, Cc, D, 1, tc);

	print "Plot the step response (yo1, yc1 vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 3, 1, 1);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot(win, t, [[yo(1,:)][yc(1,:)]], {\"yo1\",\"yc1\"});\n";
	print "\n";
	pause;

	mgplot_subplot(win, 3, 1, 1);
	mgplot_grid(win, 1);
	mgplot(win, tc, [[yo(1,:)][yc(1,:)]], {"yo1","yc1"});

	print "Plot the step response (yo2, yc2 vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 3, 1, 2);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot(win, t, [[yo(2,:)][yc(2,:)]], {\"yo2\",\"yc2\"});\n";
	print "\n";
	pause;

	mgplot_subplot(win, 3, 1, 2);
	mgplot_grid(win, 1);
	mgplot(win, tc, [[yo(2,:)][yc(2,:)]], {"yo2","yc2"});

	print "Plot the step response (yo3, yc3 vs t)\n";
	print "\n";
	print "  mgplot_subplot(win, 3, 1, 3);\n";
	print "  mgplot_grid(win, 1);\n";
	print "  mgplot(win, t, [[yo(3,:)][yc(3,:)]], {\"yo3\",\"yc3\"});\n";
	print "\n";
	pause;

	mgplot_subplot(win, 3, 1, 3);
	mgplot_grid(win, 1);
	mgplot(win, tc, [[yo(3,:)][yc(3,:)]], {"yo3","yc3"});
	pause;
}