%
% Chapter 7, Example 7.2, pages 327-329
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function example_72

[Xlow,Xup] = bisection('example1',0.1,10)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function  [XL,XR] = bisection(fun,XL,XR)

% This program determines the lower bound of the feasible region

  % display line
    fprintf('\n\n%s\t%s\t%s\n','Iter.','X_L','X_U');

  % evaluate feasibility at lower, upper and mid-point of variable
    Flag_L = feval(fun,XL);
    Flag_U = feval(fun,XR);
    Flag_M = feval(fun,(XL+XR)/2);

% refine until convergence criteria is satisfied:
  epsilon_t = 1e-3;
  counter = 0;
  max_fun_eval = 1e4;
  while abs(XR-XL) > epsilon_t
    counter = counter + 1;
    if counter > max_fun_eval
      disp('Maximum Number of Function Evaluations exceeded!')
      break
    end
    fprintf(['%0.0f\t%0.2f\t%0.2f\n'],counter,XL,XR);
    if Flag_M < 0
        XL = (XL+XR)/2;
        Flag_L = Flag_M;
        Flag_M = feval(fun,(XL+XR)/2);
    elseif Flag_M > 0
        XR = (XL+XR)/2;
        Flag_R = Flag_M;
        Flag_M = feval(fun,(XL+XR)/2);
    else
        disp('Linprog Reached Maximum Number of Iterations!')
    end;
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [exitflag] = example1(Tf);
%
% Program to Solve for the Input Shaped Control Profile for a 
% Harmonic Oscillator using Linear Programming
% An iterative approach which successively reduces the 
% maneuver time till no feasible solution is possible.
%
%
X0 = [0;0];					% Inital States
Xf = [1;0];					% Final States


	%Tf = Tf/1.01;				% Final time reduction operation

	No_samp = 101;				% Sampling Time
	T = Tf/(No_samp-1);

	G = [cos(T) sin(T); -sin(T) cos(T)];	% Transition Matrix
	H = [-cos(T)+1;sin(T)];			% Control Influence Matrix

	f = zeros(No_samp,1);			% Cost function coefficient

	% Equality Constraint calculation
	Aeq1 = [];
	for ind = 1:(No_samp)
   		Aeq1 = [G^(ind-1)*H Aeq1];
	end;
	beq1 = Xf - G^No_samp*X0;

	% Equality constraint which requires final input values be 1
	Aeq2 = zeros(1,No_samp);
	Aeq2(1,end) = 1;
	beq2 = 1;

	Aeq = [Aeq1;Aeq2];
	beq = [beq1;beq2];

	vlb = zeros(No_samp,1);
	vub = 1*ones(No_samp,1);
	u0 = 0.5*ones(size(f));
	% Minimum Time Input Shaper
% 	[u,fval,exitflag] = linprog(f,[],[],Aeq,beq,vlb,vub,u0);

	% Inequality constraint which requires control input to 
	% progressively increase to the final value
	A = zeros(No_samp-1,No_samp);
	for ind = 1:(No_samp-1)
		A(ind,ind) = 1;
		A(ind,ind+1) = -1;
	end;
	b = zeros(No_samp-1,1);

	[u,fval,exitflag] = linprog(f,A,b,Aeq,beq,vlb,vub,u0);

	if exitflag > 0
	t = linspace(0,Tf,No_samp);
	C = eye(2); D = [0;0];
	sys = ss(G,H,C,D,T);
	figure(1)
	[ys,ts,xs] = lsim(sys,u);
	subplot(211), stairs(ts,ys(:,1)); xlabel('Time'); ylabel('Displacement');
	subplot(212), stairs(ts,ys(:,2)); xlabel('Time'); ylabel('Velocity');
	figure(2)
	stairs(t,u); xlabel('Time'); ylabel('Velocity');

	end;
