exercise_batterypower;

x = [0:1000:20000];
y = [50, 70, 150, 130, 80, 80, 80, 10, 50, 50, 10, 140, 200, 200, 250, 305, 160, 150, 150, 150, 100];

plot(x, y), xlabel('Aerial distance in meters'), ylabel('Altitude in meters'), title('Simulated Terrain'), axis([0 20000 0 2000]), grid

m_rider = [40:1:110];

n = length(y);
s = (y(2:n) - y(1:n-1)) ./ (x(2:n) - x(1:n-1));
alpha = atan(s) * 180/pi;
ds = sqrt(((y(2:n) - y(1:n-1)) .^ 2) + ((x(2:n) - x(1:n-1)) .^ 2));

n = length(ds);
dE = [];
for i=1:n
  Torque = (d_wheel / 2) * F_total(FSA(h_rider, m_rider) + A_segway, v_kmh2msec(v_const), c_D, m_segway + m_rider, abs(alpha(i)));
  P_mech_effective = Torque * v_msd2rad(v_kmh2msec(v_const), d_wheel);
  dE(:, i) = (P_mech_effective * ds(i) / v_kmh2msec(v_const) * (1 / eff_overall))';
end

E_csum = [];
for i=1:length(m_rider)
  E_csum = [E_csum; E_batteries_total];
end
E_csum(1) = E_batteries_total;
for i=1:n
  if (alpha(i) >= 0)
    E_csum(:, i + 1) = E_csum(:, i) - dE(:, i);
  else
    E_csum(:, i + 1) = E_csum(:, i) + (dE(:, i) * eff_overall);
  end
end

%plot(x, E_csum), xlabel('Distance in meters'), ylabel('Energy left in batteries'), title('Energy used on Simulated Terrain'), grid
zero_plane = zeros(length(m_rider), length(x));
mesh(x, m_rider, E_csum), xlabel('Distance in meters'), ylabel('Weight of rider in kg'), zlabel('Energy left in batteries');
hold on;
mesh(x, m_rider, zero_plane, zero_plane - 1000000);

dist_empty = [];
for m=1:length(m_rider)
  i = 2;
  d_travelled = 0;
  while (i <= n)
    if (E_csum(m, i) < 0)
      x_cross = interp1([E_csum(m, i - 1), E_csum(m, i)], [x(i - 1), x(i)], 0);
      d_remain = ds(i) * ((x_cross - x(i - 1)) / (x(i) - x(i - 1)));
      d_travelled = (d_travelled + d_remain) / 1000;
      dist_empty(m) = d_travelled;
%      disp(sprintf('The batteries will be empty after %f km.', d_travelled));
      break
    end
    d_travelled = d_travelled + ds(i - 1);
    i = i + 1;
  end
end

disp(['Weight of rider   Distance after which batteries are empty']);
disp([m_rider; dist_empty]');