clc; clear;
load('hw8_p1.mat');

h = @(i) (x(i+1)-x(i));
f = @(i,j) (y(i)-y(j))/(x(i)-x(j));

% calculate c's
cA =    [   1           0               0               0               0               0;
            h(1)        2*(h(1)+h(2))   h(2)            0               0               0;
            0           h(2)            2*(h(2)+h(3))   h(3)            0               0;
            0           0               h(3)            2*(h(3)+h(4))   h(4)            0;
            0           0               0               h(4)            2*(h(4)+h(5))   h(5);
            0           0               0               0               0           1       ];

cb =    [   0;
            3*(f(3,2)-f(2,1));
            3*(f(4,3)-f(3,2));
            3*(f(5,4)-f(4,3));
            3*(f(6,5)-f(5,4));
            0   ];

c = cA\cb;

% calculate b's
b = zeros(5,1);
for ii = 1:5
    b(ii) = (y(ii+1)-y(ii))/h(ii)-(h(ii)*(2*c(ii)+c(ii+1)))/3;
end

b;

% calculate d's

d = zeros(5,1);
for ii = 1:5
    d(ii) = (c(ii+1)-c(ii))/(3*h(ii));
end

d;

% calculate a's
a = y(1:end-1);

% yt = zeros(5,length(x(
maxes = zeros(5,2);
for ii = 1:5
    xt = x(ii):.1:x(ii+1);
    yt = a(ii) + b(ii)*(xt-x(ii)) + c(ii)*(xt-x(ii)).^2 + d(ii)*(xt-x(ii)).^3;
    [maxes(ii,1),loc] = max(yt);
    maxes(ii,2) = xt(loc);
    plot(xt,yt)
    hold on
end
scatter(x,y)
legend('segment 1', 'segment 2', 'segment 3', 'segment 4', 'segment 5', 'raw data')
xlabel('location [m]')
ylabel('elevation [m]')
hold off
maxes
max = max(maxes(:,1))
location = maxes(find(maxes==max),2)