disp('Team A')
disp('Sarah Finello, Josh Huckery, Mike Lawler, Dan Pushpak, Matt Weiss')

clear all;
close all;
%delete(instrfindall);
delete(timerfindall);
clear;
clc;

%Define constants
LEFT_LED = 15;
RIGHT_LED = 14;
INTO_TOWN_BEAM = 3;
INTO_COUNTRY_BEAM = 2;
BAR_VERTICAL = 70;
BAR_HORIZONTAL = 165;

%Define and initialize/reset arduino/train
a = setupArduino(LEFT_LED, RIGHT_LED);

%Trigger variables
inCountry = 1;
startedLeds = -1;
leftLedOn = -1;

%Extra Credit variables
lastLeavingTownTic = 0;
lastEnteringTownTic = 0;
speedInCountry = 0;
speedInTown = 0;
timeTilStopTown = 0;
timeTilStopCountry = 0;
DIAMETER = 22.5;
CIRCUMFERENCE = pi * DIAMETER;
REQ_DISTANCE = (CIRCUMFERENCE / 2) - 2;
ENTERING_TOWN_RED = 6;
ENTERING_TOWN_GREEN = 7;
ENTERING_COUNTRY_RED = 8;
ENTERING_COUNTRY_GREEN = 9;

a.motorSpeed(1, 255);

while true
    clearBreakers(a, INTO_COUNTRY_BEAM, INTO_TOWN_BEAM);
    beamIntoTownReading = a.analogRead(INTO_TOWN_BEAM);
    beamIntoCountryReading = a.analogRead(INTO_COUNTRY_BEAM);
    
    %If we're ready to check for stopping
    if speedInTown ~= 0 && speedInCountry ~= 0
        if inCountry > 0
            if toc(lastLeavingTownTic) >= timeTilStopCountry %Stop in country
                if a.digitalRead(ENTERING_TOWN_RED) == 1
                    a.motorSpeed(1, 0);
                    while a.digitalRead(ENTERING_TOWN_GREEN) == 0 
                    end
                    a.motorSpeed(1, 255);
                end
            end
        else
            if toc(lastLeavingCountryTic) >= timeTilStopTown %Stop in town
                if a.digitalRead(ENTERING_COUNTRY_RED) == 1
                    a.motorSpeed(1, 0);
                    while a.digitalRead(ENTERING_COUNTRY_GREEN) == 0 
                    end
                    a.motorSpeed(1, 170);
                end
            end
        end
    end
    
    if beamIntoTownReading > 250 && inCountry > 0 %Entering town
        a.motorSpeed(1, 170);
        inCountry = -1;
        startedLeds = -1;
        a.servoWrite(1, BAR_HORIZONTAL);
        tic;
        lastEnteringTownTic = tic;
        
        %If we've tic'd the departure beam and haven't recorded the 
        %speed in the country
        if lastLeavingTownTic ~= 0
            speedInCountry = inchesPerSecond(toc(lastLeavingTownTic));
        end
        
        if speedInTown ~= 0
            timeTilStopTown = REQ_DISTANCE / speedIntown;
        end
        
        display('Entering town');
    elseif beamIntoCountryReading > 250 && inCountry < 0 %Leaving town
        a.digitalWrite(LEFT_LED, 0);
        a.digitalWrite(RIGHT_LED, 0);
        startedLeds = -1;
        a.motorSpeed(1, 255);
        a.servoWrite(1, BAR_VERTICAL);
        inCountry = 1;
        lastLeavingTownTic = tic;
        
        %If we've tic'd the approach beam and haven't recorded the 
        %speed in the town
        if lastEnteringTownTic ~= 0
            speedInTown = inchesPerSecond(toc(lastEnteringTownTic));
        end
        
        if speedInCountry ~= 0
            timeTilStopCountry = REQ_DISTANCE / speedInCountry;
        end
        
        display('Leaving town');
    end
    
    if inCountry < 0 %If in town
        if startedLeds < 0 && toc > 0.5 %Haven't started LED's yet
            startedLeds = 1;
            a.digitalWrite(LEFT_LED, 1);
            a.servoWrite(1, BAR_HORIZONTAL);
            leftLedOn = 1;
            tic;
        elseif startedLeds > 0 %If we're toggling LEDs
            if toc > 0.5
                if leftLedOn > 0
                    a.digitalWrite(LEFT_LED, 0);
                    a.digitalWrite(RIGHT_LED, 1);
                    leftLedOn = -1;
                else
                    a.digitalWrite(LEFT_LED, 1);
                    a.digitalWrite(RIGHT_LED, 0);
                    leftLedOn = 1;
                end
                tic;
            end
        end
    end
end