% This file is used to set up and run an optimization on an
%   openchain stored in a variable called 'oc' in the Matlab
%   workspace
% (Took lots of comments out...)
% Using rockbot2.m to run the simulation, use rockbot.m to view it

%setting it up for handstand into a roll, currently
% data we sould like to maintain if the optimization is terminated early
global Tspan;
global Xpass;
global Xvel;
global delta_t;
global Xc;
global Xg;
global order;
global start_const;
global end_const;


% delta_t is the time step for recording the states during
% NOTE: this does not affect the step size of the ode integrator,
%   only the times which are recorded...
delta_t = 0.01;

% initial (xt0) and final (xtf) position of the active joints
xt0 = [173.69*pi/180]';
xtf = [0]';


% intial position and velocity of the passive joints:
%init_pos = [0  (.08 + .015 - .005)  0]'; % for handstand
%init_vel = [0 0 0]';  %for handstand
%init_pos = [0 .15 0]'; 
%init_vel = [0 0 0]'; 
init_pos = [.15  0]'; 
init_vel = [0 0]'; 

% final time (intial time = 0)
tf = 0.5;

% NOTE: t1 must be at least length 3 
% NOTE: t1 should be spaced evenly between initial and final time
t1 = [0 1 2 3 4]*tf/4;

% q contains an intial joint path, each column in q should match the
%   corresponding time in t1
%q = [xt0(1) (19.48*pi/180)  -pi/2 -pi/2 xtf(1); ...
%     xt0(2)       0          pi/2  pi/2 xtf(2)];  %hand stand
q = [xt0(1) (-pi/4) 0 0   xtf(1)];

% we can now interpolate a spline through the given joint data:
% order contains the order of spline interpolation, example, order=3 -> cubic
order = 5;

pth = wlpath(t1,q,1,order);

% back out the multipliers
x = getmults(pth);

% pick off parameters in x which can vary and arrange them into a single vector
%   the ordering was done to match bryan martin's earlier code...
[m n] = size(x);
xv = [];
for i=1:n
  xv = [xv x(:,i)'];
end
 
% xv is the variable parameters: note that the number of fixed intial and
%   final multipliers changes based on the spline order (is this necessary?)
%
% start_const: number of initial constraints, 1 -> only initial value
%        of joints are constrained, 2 -> initial value is constrained
%        and initial velocity is zero, 3-> same as 2, but initial acceleration
%        is also zero...etc
% end_const: number of final constraints
start_const = 2;
%end_const = 2;
end_const = 0;
xv = xv(start_const*m+1:length(xv)-m*end_const)

number_of_params = length(xv)

% construct some vectors used to enforce the joint limits - they match
%  the ordering of xv
%
%  (what a pain...we should be able to clean this up..?)
%
aidx = 1;
na = numactive(oc);
nj = numjoints(oc);
Hap = getmap(oc);
lb1 = getlbj(oc);
ub1 = getubj(oc);
for i = 1:nj
  if (Hap(i) == 1)
    tlb(aidx) = lb1(i);
    tub(aidx) = ub1(i);
    aidx = aidx + 1;
  end
end

% we need to arrange the upper and lower joint information to match
%  how the parameters are ordered (i.e. into a vector with the same
%  ordering as xv)
lx = length(xv);
lv = length(tlb);
vlb = zeros(size(xv));
for i=1:(lx/lv)
  vlb((1+lv*(i-1)):(i*lv)) = tlb;
  vub((1+lv*(i-1)):(i*lv)) = tub;
end

% xf will be declared global so we have information even if we hit
%   control-C to stop the optimization
global xf;

% set up some stuff for the optimization:
%myopt = foptions;
%myopt(1) = 1;
%myopt(2) = .01;
%myopt(3) = .01;
%myopt(4) = 0.05;
%myopt(14) = 1400;
%%myopt(9) = 1;
%%myopt(13) = 2;
%%myopt(18) = 0.25;

% use finite difference gradient:
%xf = constr('tumbler_cost',xv,myopt,vlb,vub,[],oc,t1,xt0,xtf,init_pos,init_vel,obs);

% use analytic gradient:
testoptions = optimset('GradObj','on','TolFun',1e-4,'TolX',1e-14,'TolCon',.05);
%testoptions = optimset('Display','final');
%testoptions = optimset('TolX',1e-4);
%testoptions = optimset('TolFun',.01);
%testoptions = optimset('TolCon',1e-6);
%testoptions = optimset('MaxFunEvals',1400);
%xf = fmincon('rockbot_gradtest',xv,[],[],[],[],vlb,vub, ...
%            [],testoptions,oc,t1,xt0,xtf,init_pos,init_vel,obs);
xftest = fmincon('rockbot_gradtest',xv,[],[],[],[],vlb,vub, ...
            [],testoptions,oc,t1,xt0,xtf,init_pos,init_vel,obs);

          
if 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Test the gradient with finite difference gradients...
%% See if the changes works.  JCVA
% I have 12 parameters

delta = 0.0139

% Compute the cost
[ff1,gg] = tumbler_cost(xv,oc,t1,xt0,xtf,init_pos,init_vel,obs)
Xc1 = Xc

%Compute the gradient
[dff,dgg] = tumbler_grad(xv,oc,t1,xt0,xtf,init_pos,init_vel,obs)
Xg1 = Xg

% number of parameter to vary
n1 = 5
xv(n1) = xv(n1) + delta

% Compute the cost
[ff2,gg] = tumbler_cost(xv,oc,t1,xt0,xtf,init_pos,init_vel,obs)
Xc2 = Xc

xv(n1) = xv(n1) - 2*delta

% Compute the cost
[ff3,gg] = tumbler_cost(xv,oc,t1,xt0,xtf,init_pos,init_vel,obs)
Xc3 = Xc

n = length(Xc1);
%n2 = length(Xc2)
%n3 = length(Xc3)

% Make the approximate gradients --
% df/dp1 = Xc2(:,3) - Xc1(:,3)  /  .01;
df_dp1 = (ff2 - ff1) / delta

% df/dp2 = Xc1(:,3) - Xc3(:,3)  /  .01;
df_dp2 = (ff1 - ff3) / delta

%Compare the approximate gradients with the correct
%gradient from dff
df_dp1

df_dp2

dff

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end