% y = m*x + c
np = 30;  m0 = 0.3;  c0 = 1;
nstddev = 0.2;
x0 = 0:10;  y0 = m0*x0 + c0;  % actual line

% Observed data
x = 10*rand(1,np);
y = m0*x + c0 + nstddev*randn(size(x));

% Plot errors for actual line
fh = figure;
yp = m0*x + c0;  % nearest point
plot(x0,y0,'--');
hold on;  sh = scatter(x,y,'r+');  hold off;
for i=1:np
  hold on;  lh = line([x(i) x(i)],[yp(i) y(i)]);  hold off
  set(lh,'color','r');
end
title('Points and actual straight line');
axis([0 10 0 5]);
exportfig(gcf,'lsqline_fig01.eps','width',10,'height',6.5,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

% Plot errors for bad fit line
fh2 = figure;
m1 = 0.5;  c1 = 0;  
y1 = m1*x0 + c1;
plot(x0,y1,'r-');
yc = m1*x + c1;
axis([0 10 0 5]);
hold on;  sh = scatter(x,y,'r+');  hold off;
for i=1:np
  hold on;  lh = line([x(i) x(i)],[y(i) yc(i)]);  hold off;
  set(lh,'color','r');
end
title('Points and bad fit straight line');
exportfig(gcf,'lsqline_fig02.eps','width',10,'height',6.5,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

% Plot best fit line
fh3 = figure;
A = [x' ones(length(x),1)];
th = inv(A'*A)*A'*y';
m_lsq = th(1);  c_lsq = th(2);
y_lsq = m_lsq*x0 + c_lsq;
plot(x0,y0,'--',x0,y_lsq,'-');
legend('Actual','Best fit');
yc = m0*x + c0;
hold on;  sh = scatter(x,y,'r+');  hold off;
for i=1:np
  hold on;  lh = line([x(i) x(i)],[y(i) yc(i)]);  hold off;
  set(lh,'color','r');
end
title('Points and best fit straight line');
axis([0 10 0 5]);
exportfig(gcf,'lsqline_fig03.eps','width',10,'height',6.5,'fontmode','fixed', 'fontsize',8,'Color','cmyk');

% Explicitly calculate cost over possible parameter values
mv = -0.5:0.1:1.5;
cv = -5:0.2:5;
E2m = zeros(length(mv),length(cv));
for mi=1:length(mv)
  for ci=1:length(cv)
    m = mv(mi);  c = cv(ci);
    yp2 = m*x + c;
    ev = y - yp2;
    E2m(mi,ci) = sum(ev.^2);
  end
end
fh3 = figure;
subplot(1,2,1);  imagesc(cv,mv,sqrt(E2m));  colormap(gray);  axis tight;  %axis equal
xlabel('c');  ylabel('m');
subplot(1,2,2);  sh = scatter(x,y,'r+');
axis([0 10 0 5]);
hv = [];
c_curr = 0.60000;  m_curr = 0.38;
firstpass = 1;
while 1
  if ~isempty(hv), delete(hv);  hv = [];  end
  
  % Find derivative at current point
  A = [x' ones(size(x'))];  theta = [m_curr; c_curr];
  gE2 = -2*A'*y' + 2*A'*A*theta;
  
  % Draw point and gradient
  subplot(1,2,1);
  hold on;  sh = scatter(c_curr,m_curr,'r+');  hold off;  hv = [hv; sh];
  lambda = 0.01;
  lh = line([c_curr; c_curr+lambda*gE2(2)], [m_curr; m_curr+lambda*gE2(1)]);
  hv = [hv; lh];
  
  yp = m_curr*x + c_curr;
  y1 = m_curr*x0 + c_curr;
  subplot(1,2,2);
  hold on;  ph = plot(x0,y0,'--',x0,y1,'-');  hold off;  hv = [hv; ph];
  for i=1:np
    hold on;  lh = line([x(i) x(i)],[y(i) yp(i)]);  hold off;
    set(lh,'color','r');  hv = [hv; lh];
  end
  if firstpass
    exportfig(gcf,'lsqline_fig04.eps','width',10,'height',5.5,'fontmode','fixed', 'fontsize',8,'Color','cmyk');
    firstpass = 0;
  end
  subplot(1,2,1);  [c_curr,m_curr,b] = ginput(1);
  if b==3, break; end
end
  
