clear
clc

load('torques.mat');            % Ideal torque vector for selected segment

%User Defined Properties
a = arduino('Com3');             % define the Arduino Communication port
xLabel = 'Elapsed Time (s)';    % x-axis label
yLabel = 'Torque(N*m)';         % y-axis label
yMax  = 180;                     % y Maximum Value
yMin  = 0;                       % y minimum Value
min = -180;                     % set y-min
max = 180;                      % set y-max
delay = .4;                     % Refresh rate, determined by target speed

%Define Function Variables
time = 0;
data = 0;
data2 = 0;
diff = 0;
nlong = 0;
nlat = 0;
count = 0;

% Set up user-input plot
subplot(3, 2, 1)
plotGraph = plot(time,data,'-r' )
hold on
title('User Input','FontSize',15);
xlabel(xLabel,'FontSize',15);
ylabel(yLabel,'FontSize',15);
axis([yMin yMax min max]);
grid('on');
tic

% Set up ideal-inpit plot
subplot(3, 2, 3)
plotGraph2 = plot(time,data2,'-b' )
hold on
title('Simulated Input','FontSize',15);
xlabel(xLabel,'FontSize',15);
ylabel(yLabel,'FontSize',15);
axis([yMin yMax min max]);
grid('on');
tic

% Set up difference plot
subplot(3, 2, 5);
plotGraph3 = plot(time,diff,'-g') ;
hold on;
title('Difference','FontSize',15);
xlabel(xLabel,'FontSize',15);
ylabel(yLabel,'FontSize',15);
axis([yMin yMax min max]);
grid('on');
tic;

% Set up mini-map plot
routename = 'FSGP 2018 MPH.txt';
[xx,yy,elev,dist,lat,long] = loadTrack(routename,2.4);
lapLength = xx(end).*(39.6./12./5280);

subplot(3, 2, [2, 4]);
plot(long, lat); title('Full Track');
hold on;
plotGraph4 = plot(nlong, nlat, 'LineWidth', 8);
axis([-98.354 -98.346 40.576 40.582]);
title('Ideal Track Location');
tic;


% Update plots
while ishandle(plotGraph) %Loop when Plot is Active will run until plot is closed
         deccel = ((a.readVoltage('A0')-4.5601)/-0.5621)*180;
         accel = ((a.readVoltage('A1')-4.6041)/-0.5425)*180;
         dat = accel-deccel; %Data from the arduino
         count = count + 1;
         time(count) = toc;
         data(count) = dat(1);
         data2(count) = ttt(count);
         diff(count) = data2(count) - data(count);
         nlong(count) = long(count);
         nlat(count) = lat(count);

         % Updates exisiting plot instead of replotting
         % Decreases operation time by up to a factor of 20
         set(plotGraph,'XData',time,'YData',data);
         set(plotGraph2,'XData',time,'YData',data2);
         set(plotGraph3,'XData',time,'YData',diff);
         set(plotGraph4,'XData',nlong,'YData',nlat);

         pause(delay);
end

% Save data for processing in SRSim, display close message
save('Simulation.mat', 'data');
disp('Program closed, output saved to "Simulation.mat"');



function [xx,yy,elev,dist,lat,long,xLong,yLat,dydx,th] = loadTrack(filename,dx)
%% GPS Conversion Constant
latLongDist = 69; %miles between 1 latitude/longitude degree

%% Google Earth Text File I/O
i = 1;
fh = fopen(filename);
headers = fgetl(fh); %Extract data headers
line = fgetl(fh);
while ischar(line)
    vals = str2num(line);
    dist(i) = vals(1);
    elev(i) = vals(2);
    lat(i) = vals(3);
    long(i) = vals(4);

    line = fgetl(fh);
    i = i + 1;
end
fclose(fh);
y = (elev - min(elev));
x = dist - dist(1);

%% Smooth GPS Noise (avoid exploding derivatives)
%ys = y; %don't smooth to save time
ys = smooth(y,0.01,'rlowess');
xx = (x(1):dx:x(end));
yy = (spline(x,ys,xx));
lat = (spline(x,lat,xx));
long = (spline(x,long,xx));
yy = yy - yy(1);
end