clear all;
clc;

pause(15)

%% Delete old serial port objects
instrs = instrfind;                         % Locate open ports
if (~isempty(instrs))                       % Close any open ports
    disp('Cleaning old serial ports...');
    delete(instrs);
end

%% Set up the com port

disp('Allocating serial port...');
s_port = serial('COM4');                    % Select the COM port
set(s_port, 'BaudRate', 9600);            % Baud rate matches Arduino
set(s_port, 'Timeout', 10);                  % Set port timeout
set(s_port, 'Terminator','CR');

%% Open the com port

disp('Trying to open serial port...');
fopen(s_port);
disp('Opened!');
pause(1);

%% Initialize Figures

figure(1)
set(gcf,'OuterPosition',[ 95         658        1830         540])
    h1=plot(0,'bo-','LineWidth',2);
    xlabel('Time (min)')
    ylabel('Speed (mph)')
    grid on
    set(gca,'YMinorGrid','on')
    ylim([0 20])
    set(findall(gcf,'-property','FontSize'),'FontSize',30)
    f1 = gcf;
    a1 = f1.CurrentAxes;
    set(a1,'GridAlpha',1)
    set(a1,'MinorGridAlpha',1)

figure(2)
set(gcf,'OuterPosition',[ 0         125        1927         544])
    h2=plot(0,'ro-','LineWidth',2);
    xlabel('Time (min)')
    ylabel('Energy (at 20% Eff.)')
    grid on
    set(gca,'YMinorGrid','on')
    set(findall(gcf,'-property','FontSize'),'FontSize',30)
    f2 = gcf;
    a2 = f2.CurrentAxes;
    set(a2,'GridAlpha',1)
    set(a2,'MinorGridAlpha',1)

figure(3)
set(gcf,'OuterPosition',[ 2   669   214   532])
    h3 = bar(1);
    ylim([0 20])
    set(a1,'GridAlpha',.7)

pause(1)

%% Read in data and plot


flag.start = 0;
flag.runningColor = 0;
bikeSpeed = 0;

j=2;
while 1
    
    % Tell Arduino to start measuring speed 
    fprintf(s_port,1);
    pause(.01);
    
    % Read serial port twice (once for time, once for trainer speed)
    t_in = fscanf(s_port,'%f');         % s
    trainerSpeed = fscanf(s_port,'%f'); % rps

    t(j) = t_in;
    
    % Tire Geometry
    tireToCylRatio = mean([12.25,12.7]);
    rpsWheel = trainerSpeed / tireToCylRatio;
    wheelDia = 27*1.57828E-05; % mi
    wheelCirc = wheelDia*pi;
    
    % Bike Speed in MPH
    bikeSpeed(j) = wheelCirc * rpsWheel*60*60; % MPH
    
    % Integrate speed to find distance in Miles
    distance = trapz(t/60/60,bikeSpeed);
    
    % Power curve of Kurt Kinetic Trainer
    Power(j) = (5.244820) * bikeSpeed(j) + (0.019168) * bikeSpeed(j).^3; % W;
    
    % Integrate power to find total energy
    Energy(j) = trapz(t,Power) * 0.000239006; % kCal
    
    tWindow = 3;
    EWindow = 25;
    
    set(h1,'XData',t/60)
    set(h1,'YData',bikeSpeed)
    
    set(h2,'YData',Energy/.20)
    set(h2,'XData',t/60)
    set(a2,'YLim',[0  max(ceil(max(Energy)/.20),.1)])
    
    set(h3,'YData', bikeSpeed(j))

    if t(j) > tWindow*60;
        set(a1,'XLim',[ceil(max(t)/60)-tWindow  ceil(max(t)/60)])
        set(a2,'XLim',[ceil(max(t)/60)-tWindow  ceil(max(t)/60)])
    else
        set(a1,'XLim',[0  tWindow])
        set(a2,'XLim',[0  tWindow])
    end
    
    if Energy(j) > EWindow*.2;
        set(a2,'YLim',[ceil(max(Energy/.2))-EWindow  ceil(max(Energy/.2))])
    else
        set(a2,'YLim',[0  EWindow])
    end
    refreshdata
    drawnow
    
    if flag.runningColor == 0
        set(h3,'FaceColor',[0 0 1])
        flag.runningColor = 1;
    else
        set(h3,'FaceColor',[0 .5 1])
        flag.runningColor = 0;
    end
    
    % Quit if energy requirment is met
    humanEff = 0.20;
    energy_limit = (400) * humanEff;
    if Energy(j-1) > energy_limit
        % Change color of plot to indicate completion
        set(h1,'Color',[1 0 1])
    end
    
    j=j+1;
    
end

%% Close the port

fclose(s_port);
delete(s_port);