%% Adaptive Cruise Control Example from HSCC 2003 paper
%% Dynamics: vel = acc, acc = -3 acc - 4 vel + 3 fvel + gap - 10, gap = fvel - vel, fvel = facc
%% Safe: gap > 0
%% Template: [vel*a + acc*c + fvel*b + gap - d >= 0]
%% Now the front car can accelerate with facc and facc is betwn -5 and 2.
%% Again, we get a very big formula for QEPCAD to solve, but two cases
%% are f-25 <= 0 or f^2-30*f-75<=0, which gives f<=15+10\sqrt(3).
%%  A Another disjunct (Subformula 68) gives
%% 3f-29 >= 0 and f^3-30f^2-75f+400 <= 0, which is f < 31.9 and e >= 0.

on echo;
load_package redlog;
rlset r;

load_package qepcad;

on rlqefbqepcad;      % use QEPCAD as the fallback QE

rlqepcadn(100*10^6);  % The number of cells used by QEPCAD. The
		      % default is (only) 1 * 10^6.

rlqepcadl(200*10^3);  % The size of the prime list used by QEPCAD. The
		      % default is (only) 2 * 10^3.

on rlverbose;
on time;

off rlqefb;

%% Find initial states for which a linear invariant will prove safety

%% Template:
%% a * (fvel-vel)^2 + b*acc^2 + c*(-vel+gap-10)^2 - gap <= 0 assuming af=0
%% and a * (fvel-vel)^2 + b*acc^2 + c*(-vel+gap-10)^2 - gap = 0) 
phi0 := 
	   (acc >= -5 and acc <= 2 and vel >= 0 and fvel >= 0)
    % and (fvel-vel)^2 + acc^2 + (-vel+gap-10)^2 = 900
 impl
 2*a*(fvel-vel)*(-acc) + 2*b*acc*(-3*acc-4*vel+3*fvel+gap-10) + 2*c*(-vel+gap-10)*(-acc+fvel-vel) + 2*(-acc+fvel-vel)*(2*acc+4*vel-3*fvel-gap+10) <= 0 ;

%% We get c = 1 and b = 0 and some large formula....
phi01 := 
	   (acc >= -5 and acc <= 2 and vel >= 0 and fvel >= 0)
    % and (fvel-vel)^2 + acc^2 + (-vel+gap-10)^2 = 900
 impl
 2*a*(fvel-vel)*(-acc) + 2*(-vel+gap-10)*(-acc+fvel-vel) + 2*(-acc+fvel-vel)*(2*acc+4*vel-3*fvel-gap+10) <= 0 ;

%% True with a between 5-\sqrt{24} to 5+\sqrt{24}
%% So, we get a Lyapunov function
%% a*(fvel-vel)^2 + (-vel+gap-10)^2 + (-acc+fvel-vel)^2 

%% Lets see if it works if fveldot=facc
phi02 := 
	   (acc >= -5 and acc <= 2 and vel >= 0 and fvel >= 0
	and facc >= -5 and facc <= 2 )
 impl
 2*a*(fvel-vel)*(facc-acc) + 2*(-vel+gap-10)*(-acc+fvel-vel) + 2*(-acc+fvel-vel)*(2*acc+4*vel-3*fvel-gap+10+facc) <= 0 ;

%% DOES NOT generalize; works only for facc = 0 !

phi04 := 
	   (acc >= -5 and acc <= 2 and vel >= 0 and fvel >= 0)
 impl
 2*a*(fvel-vel)*(-acc) + 2*(-vel-gap+10)*(-acc-fvel+vel) + 2*c*(-acc-fvel+vel)*(4*acc+4*vel-3*fvel-gap+10) <= 0 ;

%% Lets see if a different template works
%% Init: if x is an initial states then x is in the invariant
%phi1 :=   [[ gap = 10 AND acc = 0 AND fvel=e AND vel = f ] impl [vel*a + acc*c + fvel*b + gap - d >= 0]];

%% Safe: if x is in the invariant, then x is in Safe.
%phi2 := [[vel*a + acc*c + fvel*b + gap - d >= 0 and acc >= -5 and acc <= 2 and vel >= 0 and fvel >= 0] impl [ gap >= 0   ]];

%% Induct: if x is on the bdary of the invariant, then vector field points inwards (barrier)
phi3 := [[ vel*a + acc*c + fvel*b + gap - d = 0 and 
	   acc >= -5 and acc <= 2 and vel >= 0 and 
	   fvel >= 0 and facc >= -5 and facc <= 2 ] impl 
  [  -4*vel*c - vel + acc*a - 3*acc*c + 3*fvel*c + fvel + gap*c - 10*c + b*facc >= 0  ]];

% phi := phi1 and phi2 and phi3;

phi03 := (
  ((fvel - vel)^2 + (-vel+gap-10)^2 + (-acc+fvel-vel)^2 <= 2*(1-c)^2*fvel^2 + c^2*fvel^2 and 0 <= vel and 0 <= fvel and vel <= 40 and fvel <= 40 and gap >= 0) impl (gap > 0));
  

phi := (phi03);

psi := rlqe all({gap,acc,vel,fvel,facc,faccdot}, phi)$

rlqepcad( rlex(psi), "acc7.qepcad");

off nat;	% Output same syntax as input (for reduce)
out "acc10.out";
psi;
shut "acc10.out";

end;