JACK is the Bot

#  ============= REFERENCES =============
# 1- (Credit to Dreamhouse for putting together references 2, 3, and 4)https://www.hackster.io/team-elec424-dreamhouse/team-elec424-dreamhouse-c5ad62
# 2- (Computer Vision) https://www.instructables.com/Autonomous-Lane-Keeping-Car-Using-Raspberry-Pi-and/
# 3- (Main Loop Logic) https://www.hackster.io/really-bad-idea/autonomous-path-following-car-6c4992
# 4- (Stop detection/Driver) https://www.hackster.io/colonel-hackers/autonomous-rc-car-elec-424-final-project-732fd1
# 5- MEGG car: For the simplified encoder adjustment logic https://www.hackster.io/m-e-g-g/the-magnificent-m-e-g-g-car-28ec89

# Import Packages
import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
import time
import os
import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM) #Use Broadcom pin numbering system

#Setup pins as output
GPIO.setup(18, GPIO.OUT) 
GPIO.setup(19, GPIO.OUT)

#Creates PWMs for speed and steering at 50Hz
speed = GPIO.PWM(18, 50)
steer = GPIO.PWM(19, 50) 
speed.start(7.5) #Neutral (stop) speed
steer.start(7.5) #Neutral (straight) steer

#Constants for encoding (tuned to our car)
speed_encode = True

#  ============= ENCODER CONSTANTS =============
encoder_path = "/sys/module/gpiod_driver_encoder/parameters/encoder_val" #Accesses time lapsed measured by encoder
encoder_max_rotation = 6400000 #max speed
encoder_min_rotation = 8700000 #min speed

# Camera
frame_width = 160
frame_height = 120
cam_idx = 0

# PD Variables for Lanekeeping
kp = 0.07 #proportional gain
kd = kp * 0.5 #derivative gain

# Speed Values
zero_speed = 7.5 
base_speed = 7.95 # Slowed to keep car stable
speed_change = 0.001 #How much speed is adjusted if encoder detects a discrepancy
zero_turn = 7.5 

global_enc_vals = []

# Max number of loop
max_ticks = 4000

class NotSudo(Exception):
    pass

def reset_car():
    # Updated from the old Adafruit DAC syntax to use GPIO and PWM
    # set speed to zero and steering to neutral
    speed.ChangeDutyCycle(7.5) #puts DC motor on neutral
    steer.ChangeDutyCycle(7.5) #servo straight
    #print("Car: stopped & straightened")    

def start_car():
    #Updated from the old Adafruit DAC syntax to use GPIO and PWM
    # Set speed to base speed and steering to neutral
    print("==== READY =====")
    speed.ChangeDutyCycle(base_speed) #puts DC motor on neutral
    steer.ChangeDutyCycle(7.5) #servo straight

def manage_speed():
    # Adjust the speed based on the speed encoder
    # read the sysfs parameter with the encoder data
    f = open(encoder_path, "r")
    enc = int(f.readline()) #value frome encoder
    f.close()
    ret = enc 

    global_enc_vals.append(enc)

    # If within bounds (wait for car to start / stop)
    ret = 0 #no speed change
    if enc <= encoder_max_rotation: #Encoder time lapses are too short, car is too fast
        ret = -(speed_change) #reduce speed
    elif enc >= encoder_min_rotation: #too slow
        ret = (speed_change) #increase speed
    return ret

def wait(wait_time):
    # Wait for a period of time
    start_time = time.perf_counter() # start
    end_time = start_time + wait_time # end
    # loop until finished
    while (time.perf_counter() < end_time):
        pass
    return
    
def detect_edges(frame):
    # Detetct the blue lines
    # filter for blue lane lines
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    # This color values were adjusted from Paul Walker Team code
    lower_blue = np.array([90, 50, 50], dtype="uint8")
    upper_blue = np.array([130, 255, 255], dtype="uint8")

    # make mask
    mask = cv2.inRange(hsv, lower_blue, upper_blue)

    # Get edges
    edges = cv2.Canny(mask, 50, 100)
    return edges

def region_of_interest(edges):
    # Find region to look at
    height, width = edges.shape
    mask = np.zeros_like(edges)

    # only focus lower half of the screen
    polygon = np.array([[
        (0, height),
        (0, height / 2),
        (width, height / 2),
        (width, height),
    ]], np.int32)

    cv2.fillPoly(mask, polygon, 255)
    
    # Get region to look at 
    cropped_edges = cv2.bitwise_and(edges, mask)

    return cropped_edges

def detect_line_segments(cropped_edges):
    # Find line segments

    # set constants
    rho = 1
    theta = np.pi / 180
    min_threshold = 10
    
    #get lines
    line_segments = cv2.HoughLinesP(cropped_edges, rho, theta, min_threshold,
                                    np.array([]), minLineLength=5, maxLineGap=150)

    return line_segments

def average_slope_intercept(frame, line_segments):
    # finding slope of the line
    lane_lines = []

    if line_segments is None:
        print("No line segments detected")

        return lane_lines

    # set boundaries
    height, width, _ = frame.shape
    left_fit = []
    right_fit = []
    boundary = 1 / 3
    left_region_boundary = width * (1 - boundary)
    right_region_boundary = width * boundary

    # go through line segments and get line of best fit
    for line_segment in line_segments:
        for x1, y1, x2, y2 in line_segment:
            if x1 == x2:
                continue

            fit = np.polyfit((x1, x2), (y1, y2), 1)
            slope = (y2 - y1) / (x2 - x1)
            intercept = y1 - (slope * x1)

            if slope < 0:
                if x1 < left_region_boundary and x2 < left_region_boundary:
                    left_fit.append((slope, intercept))
            else:
                if x1 > right_region_boundary and x2 > right_region_boundary:
                    right_fit.append((slope, intercept))
    
    # take average line of best fit
    left_fit_average = np.average(left_fit, axis=0)
    if len(left_fit) > 0:
        lane_lines.append(make_points(frame, left_fit_average))

    right_fit_average = np.average(right_fit, axis=0)
    if len(right_fit) > 0:
        lane_lines.append(make_points(frame, right_fit_average))

    return lane_lines

def make_points(frame, line):
    # For getting visual of lines
    height, width, _ = frame.shape

    slope, intercept = line

    y1 = height  # bottom of the frame
    y2 = int(y1 / 2)  # make points from middle of the frame down

    if slope == 0:
        slope = 0.1

    x1 = int((y1 - intercept) / slope)
    x2 = int((y2 - intercept) / slope)

    return [[x1, y1, x2, y2]]

def display_lines(frame, lines, line_color=(0, 255, 0), line_width=6):
    # Get visual of lines
    line_image = np.zeros_like(frame)

    if lines is not None:
        for line in lines:
            for x1, y1, x2, y2 in line:
                cv2.line(line_image, (x1, y1), (x2, y2), line_color, line_width)

    line_image = cv2.addWeighted(frame, 0.8, line_image, 1, 1)

    return line_image

def display_heading_line(frame, steering_angle, line_color=(0, 0, 255), line_width=5):
    # Show a hedaing line
    heading_image = np.zeros_like(frame)
    height, width, _ = frame.shape

    steering_angle_radian = steering_angle / 180.0 * math.pi

    x1 = int(width / 2)
    y1 = height
    x2 = int(x1 - height / 2 / math.tan(steering_angle_radian))
    y2 = int(height / 2)

    cv2.line(heading_image, (x1, y1), (x2, y2), line_color, line_width)
    heading_image = cv2.addWeighted(frame, 0.8, heading_image, 1, 1)

    return heading_image

def get_steering_angle(frame, lane_lines):
    # Get the angle to steer towards
    height, width, _ = frame.shape

    if len(lane_lines) == 2:
        _, _, left_x2, _ = lane_lines[0][0]
        _, _, right_x2, _ = lane_lines[1][0]
        mid = int(width / 2)
        x_offset = (left_x2 + right_x2) / 2 - mid
        y_offset = int(height / 2)

    elif len(lane_lines) == 1:
        x1, _, x2, _ = lane_lines[0][0]
        x_offset = x2 - x1
        y_offset = int(height / 2)

    elif len(lane_lines) == 0:
        x_offset = 0
        y_offset = int(height / 2)

    angle_to_mid_radian = math.atan(x_offset / y_offset)
    angle_to_mid_deg = int(angle_to_mid_radian * 180.0 / math.pi)
    steering_angle = angle_to_mid_deg + 90

    return steering_angle

def plot_pd(p_vals, d_vals, error, show_img=False):
    # Plot the proportional, derivative and error
    fig, ax1 = plt.subplots()
    t_ax = np.arange(len(p_vals))
    ax1.plot(t_ax, p_vals, '-', label="P values")
    ax1.plot(t_ax, d_vals, '-', label="D values")
    ax2 = ax1.twinx()
    ax2.plot(t_ax, error, '--r', label="Error")

    ax1.set_xlabel("Frames")
    ax1.set_ylabel("PD Value")
    ax2.set_ylim(-90, 90)
    ax2.set_ylabel("Error Value")

    plt.title("PD Values over time")
    fig.legend()
    fig.tight_layout()
    plt.savefig("pd_plot.png")

    if show_img:
        plt.show()
    plt.clf()

def plot_pwm(speed_pwms, turn_pwms, error, show_img=False):
    # Plot teh speed steering and the error
    fig, ax1 = plt.subplots()
    t_ax = np.arange(len(speed_pwms))
    ax1.plot(t_ax, speed_pwms, '-', label="Speed")
    ax1.plot(t_ax, turn_pwms, '-', label="Steering")
    ax2 = ax1.twinx()
    
    
    ax1.plot(t_ax, error / np.max(error), '--r', label="Error")

    ax1.set_xlabel("Frames")
    ax1.set_ylabel("Speed and Steer Duty Cycle")
    ax2.set_ylabel("Percent Error Value")

    plt.title("Speed and Steer Duty Cycle, and error v.s. time")
    fig.legend()
    plt.savefig("voltage_plot.png")

    if show_img:
        plt.show()
    plt.clf()

# adapted from hackster, changed to adhere to red HSV color segments
def isRedFloorVisible(image):
    """
    Detects whether or not the majority of a color on the screen is a particular color
    :param image:
    :param boundaries: [[color boundaries], [success boundaries]]
    :return: boolean if image satisfies provided boundaries, and an image used for debugging
    """
    # Convert to HSV color space
    hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

    cv2.imwrite("redfloor.jpg", hsv_img)

    # parse out the color boundaries and the success boundaries
    # percentage was adjusted
    #percentage = 25
    percentage = 4.5 #red_threshold

    # lower and upper range for the lower part of red
    lower_red1 = np.array([0, 40, 60], dtype="uint8")
    upper_red1 = np.array([10, 255, 255], dtype="uint8")

    # lower and upper range for the upper part of red
    lower_red2 = np.array([170, 40, 60], dtype="uint8")
    upper_red2 = np.array([180, 255, 255], dtype="uint8")

    # create two masks to capture both ranges of red
    mask1 = cv2.inRange(hsv_img, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv_img, lower_red2, upper_red2)

    # combining the masks
    mask = cv2.bitwise_or(mask1, mask2)

    # applying the mask
    output = cv2.bitwise_and(hsv_img, hsv_img, mask=mask)

    # save the output image
    cv2.imwrite("redfloormask.jpg", output)

    # calculating what percentage of image falls between color boundaries
    percentage_detected = np.count_nonzero(mask) * 100 / np.size(mask)
    # if the percentage percentage_detected is betweeen the success boundaries, we return true,
    # otherwise false for result
    result = percentage < percentage_detected
    if result:
        print(percentage_detected)
    return result, output


def main():
    #setup variables
    lastTime = 0
    lastError = 0
    SecondStopTick = 0


    # arrays for making the final graphs
    p_vals = [] # proportional
    d_vals = [] # Derivative
    err_vals_1 = [] # error
    err_vals_2 = [] # error #DELETE
    speed_vals = [] # speed values
    steer_vals = [] # steering values

    # set up video
    video = cv2.VideoCapture(cam_idx, cv2.CAP_V4L2)
    video.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width)
    video.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height)

    #Starts car
    start_car()
    curr_speed = base_speed
    counter = 0

    passedFirstStopSign = False
    while counter < max_ticks:
           speed.ChangeDutyCycle(zero_speed)
           # manage video
           ret, original_frame = video.read()
           frame = cv2.resize(original_frame, (160, 120))

           # process the frame to determine the desired steering angle
           edges = detect_edges(frame)
           cv2.imshow("edges", edges)
           roi = region_of_interest(edges)
           line_segments = detect_line_segments(roi)
           lane_lines = average_slope_intercept(frame, line_segments)
           lane_lines_image = display_lines(frame, lane_lines)

           steering_angle = get_steering_angle(frame, lane_lines)
           heading_image = display_heading_line(lane_lines_image,steering_angle) #DELETE

           # calculate changes for PD
           now = time.time()
           dt = now - lastTime
           deviation = steering_angle - 90

           # PD Code
           error = deviation
           base_turn = 7.5
           proportional = kp * error
           derivative = kd * (error - lastError) / dt

           # take values for graphs
           p_vals.append(proportional)
           d_vals.append(derivative)
           err_vals_1.append(error)
           speed_vals.append(curr_speed)

           # determine actual turn to do 
           turn_amt = base_turn + proportional + derivative
           steer_vals.append(turn_amt) #makes turn based on turn_amt
           steer.ChangeDutyCycle(turn_amt) #drives the DC motor (forward)
           
           # Stops if sees the red box
           if counter % 15: # looks every 20 ticks
                if not passedFirstStopSign:
                    isStopSign, floorSight = isRedFloorVisible(frame)

                    # Sees red box
                    if isStopSign:
                        wait(3)
                        passedFirstStopSign = True
                        SecondStopTick = counter
                
                # for last box (first sign was seen and enough time has passed)      
                elif passedFirstStopSign and counter > SecondStopTick+70:
                    isStopSign, _ = isRedFloorVisible(frame)
                    if isStopSign:
                        #set_speed(zero_speed)
                        speed.ChangeDutyCycle(zero_speed)
                        print("Reached final stop")
                        break # Stop forever

           # speed encoding
           if speed_encode:
                if counter % 3 == 0: # Check every 3 ticks
                    # adjust speed
                    temp_speed = manage_speed() + curr_speed
                    if temp_speed != curr_speed:
                        speed.ChangeDutyCycle(temp_speed) #probably redundant
                        curr_speed = temp_speed

           speed.ChangeDutyCycle(curr_speed) #Go forward, this is base speed 
           wait(0.023) # Run at speed for a short amount of time

           key = cv2.waitKey(1)
           if key == 27:
                break

           counter += 1

    # Reset car and close everything
    reset_car()
    video.release()
    cv2.destroyAllWindows()
    speed.stop()
    steer.stop()
    GPIO.cleanup()

    # save plots
    plot_pd(p_vals, d_vals, err_vals_1)
    plot_pwm(speed_vals, steer_vals, err_vals_1) 
 
if __name__ == "__main__":
    main()