%%% learning system
% Patton, initiated 5/3/2006 at 9:37:41


%% intiialize variables
clear;
ProgName='learningSystem1.m';
numTrials=100;
fsz=7;
learningRate=.2;
noiseLevel=10;
F=zeros(100,1);
xd=sin((1:100)/20)';
timeIndex=(1:100)';
x=xd;
B=10*ones(1,numTrials);
% B=10*ones(1,numTrials)+10*rand(1,numTrials); % with uncertianty 
B(1:10)=0; % baseline
for i=20:10:99,
  B(i)=0; % catch trial every so many movements
end

%% display start
fprintf('\n\n\n ~ BEGIN %s ~', ProgName)

%% clear all figures
fprintf(' Closing all figs')
lastFig=0;
while(gcf~=lastFig)
  fprintf('.')
  lastFig=gcf;
  figure(gcf); close; 
end
figure(gcf); close; 

%% loop for a varying learning rate 
for learningRate=.1:.4:2.5

  %% initialize plots
  clf
  figure

  % plot of trajectories
  subplot(3,2,1)
  h=plot(timeIndex,xd,'r:');
  hold on;
  h.x=plot(timeIndex,xd,'o','markersize',2,'markerfacecolor',[0 0 1]);
  title('Desired & actual x vs. time','fontSize',fsz)
  ylabel('x','fontSize',fsz)

  % plot of forces
  subplot(3,2,3)
  h.F=plot(timeIndex,F,'o','markersize',2,'markerfacecolor',[0 0 1]);
  hold on;
  title('Force vs time','fontSize',fsz)
  ylabel('Force','fontSize',fsz)

  % plot of error
  subplot(3,2,5)
  h.e=plot(timeIndex,F,'o','markersize',2,'markerfacecolor',[0 0 1]);
  hold on;
  title('Error vs time','fontSize',fsz)
  ylabel('Error','fontSize',fsz)
  xlabel('Time','fontSize',fsz)

  subplot(2,2,2)
  plot(B,'o','markersize',2,'markerfacecolor',[0 0 1]);
  hold on;
  title('Force field strength','fontSize',fsz)

  subplot(2,2,4)
  plot([0 numTrials],[0 0],'k:',...
       [0 numTrials],[10 10],'k:',...
       [0 numTrials],[-10 -10],'k:'); %
  hold on;
  xlabel('Trials','fontSize',fsz)
  title(['Learning Curve for Learning rate = ' num2str(learningRate)])

  pause(.2);

  %% loop for learning trials:
  for i=1:numTrials
    fprintf('.');

    %% simulation loop:
    for t=1:max(timeIndex)
      randomDisturbance=noiseLevel*(rand(1,1)-.5); % random normal dist w/zero mean
      robotDisturbance=-B(i)*sin(t/30);
      x(t)=xd(t)+randomDisturbance+robotDisturbance+F(t); % plant
    end
    e=x-xd;
    %   F=F-learningRate*max(e).*ones(size(x));
      F=F-learningRate*(e);
%     F=F-learningRate*(mean(e)*ones(size(e)));

    set(h.x,'yData',x);
    set(h.F,'yData',F);
    set(h.e,'yData',e);

    subplot(2,2,4)
    plot(i,mean(e),'o','markersize',2,'markerfacecolor',[0 0 1]);
    hold on;

    drawnow; pause(.05);

  end
end

%% display end
fprintf('\n ~ END   %s ~\n', ProgName)