from picamera import PiCamera
from serial import Serial
import os
import RPi.GPIO as GPIO
import time

GPIO.setmode(GPIO.BOARD)
GPIO.setup(12, GPIO.OUT)


try:
    os.mkdir("/home/pi/Documents/roadPictures")
except:
    pass

f = open("/home/pi/Documents/roadPictures/steer_speed_data.txt", "a") # makes text file

try:
    arduino = Serial("/dev/ttyUSB0", 115200, timeout=1) #find port with ls /dev/tty*
except:
    pass

try:
    arduino = Serial("/dev/ttyUSB1", 115200, timeout=1)
except:
    arduino = Serial("/dev/ttyACM0", 115200, timeout=1)

camera = PiCamera() #opens camera
camera.resolution = (240, 180) #sets resolution, im using(240, 180)
camera.color_effects = (128, 128)
time.sleep(2)

biggest = 0 #finds the starting point to append new images to folder
try:
    listOfDir = os.listdir("/home/pi/Documents/roadPictures/")
    listOfDir.remove("steer_speed_data.txt")
    for i in range(len(listOfDir)):
        listOfDir[i] = listOfDir[i].replace("image", "")
        listOfDir[i] = listOfDir[i].replace(".jpg", "")
        if(int(listOfDir[i]) > biggest):
            biggest = int(listOfDir[i])   
except:
    pass

i = biggest + 1
numOfPictures = 10000

last = time.perf_counter() #time for turning on and off led
state = False
t = last

while(i <= numOfPictures + biggest):  #number of images to take
    arduino.reset_input_buffer()
    
    if(time.perf_counter() - last >= 1):
        GPIO.output(12, state)
        state = not state
        last = time.perf_counter()
        
    try:
        string = arduino.readline().decode("utf-8").rstrip()
        if(string):
            print(string)
            camera.capture("/home/pi/Documents/roadPictures/image%s.jpg" % i) #captures image
            f.write(string + "\n") #captures steering/speed
            i += 1
    except:
        arduino.close()


GPIO.output(12, True)
howLong = time.perf_counter() - t
print("It took " + str(howLong) + " seconds to take " + str(numOfPictures) + " pictures!")
print("That is " + str(numOfPictures/howLong) + " fps!")
f.write("It took " + str(howLong) + " seconds to take " + str(numOfPictures) + " pictures!\n")
f.write("That is " + str(numOfPictures/howLong) + " fps!")
f.close()