#pwmPin2 = 12
#dirPin2 = 16

__SCREEN_WIDTH = 1280 / 4
__SCREEN_HEIGHT = 720 / 4
#__SCREEN_WIDTH = 320
#__SCREEN_HEIGHT = 240

kit = ServoKit(channels=16)

SCREEN_WIDTH = 1280 / 4
SCREEN_HEIGHT = 720 / 4
#SCREEN_WIDTH = 320
#SCREEN_HEIGHT = 240

lane_follower = CobitLaneFollower()
cap = cv2.VideoCapture(0)

cap.set(3, int(SCREEN_WIDTH))
cap.set(4, int(SCREEN_HEIGHT))

IO.setwarnings(False)
IO.setmode(IO.BCM)
IO.setup(pwmPin, IO.OUT)
IO.setup(dirPin, IO.OUT)
#IO.setup(pwmPin2, IO.OUT)
#IO.setup(dirPin2,IO.OUT)

p = IO.PWM(pwmPin, 100)
p.start(0)
import cv2
import logging
from cobit_lane_follower import CobitLaneFollower
from cobit_deep_follower import CobitDeepFollower
import time

video_file = "data/car_video.avi"

cobit_deep_follower = CobitDeepFollower()
cobit_lane_follower = CobitLaneFollower()
cap = cv2.VideoCapture(video_file)

prev_time = 0
curr_time = 0

# skip first second of video.
for i in range(3):
    _, frame = cap.read()

#video_type = cv2.VideoWriter_fourcc(*'XVID')
#video_overlay = cv2.VideoWriter("%s_deep_drive.avi" % video_file, video_type, 20.0, (320, 240))
try:
    i = 0
    while cap.isOpened():
        _, frame = cap.read()
        frame_copy = frame.copy()
        logging.info('Frame %s' % i)
        lane_lines, img_lane = cobit_lane_follower.get_lane(frame)
        angle_lane, combo_image1 = cobit_lane_follower.get_steering_angle(
            img_lane, lane_lines)