dataPath = "indian_dataset/"
corrDataPath = "indian_dataset/corr/"
fileNamePrefix = "circuit2_x264.mp4 "

#read data.txt
xs = []
ys = []
accels = []
brakes = []
opticalFlow = []
gears = []
gearFeatures = []

predictedGears = []
considerPreviousGears = 10

with open(dataPath+"data.txt") as f:
    for line in f:
        xs.append(dataPath + fileNamePrefix + str(int(line.split()[0])).zfill(5)+".jpg")
        # No need to convert to radians as here we dont use for training.
        steer_value = float(line.split()[1])
        accel_value = float(line.split()[2])
        brake_value = float(line.split()[3])
        gear_value = float(line.split()[4])
        ys.append(steer_value)
        accels.append(accel_value)
        brakes.append(brake_value)
        gears.append(gear_value)
        gearFeatures.append([steer_value, accel_value, brake_value])

i = 0
with open(corrDataPath+"optFlow.txt") as f:
# with open("driving_dataset/data.txt") as f:
    for line in f:
        # xs.append("driving_dataset/" + line.split()[0])
        opticalFlow.append(float(line.split()[0]))
        gearFeatures[i].append(float(line.split()[0]))
        i += 1

gearModel = RandomForestClassifier()

gearModel.fit(np.array(gearFeatures), np.array(gears))


while(cv2.waitKey(10) != ord('q') and i < num_images-1):

    predictedGear = gearModel.predict(np.array(gearFeatures[i]).reshape(1, -1))
    predictedGears.append(predictedGear)

    # lazy check to see whether all gear predictions in previous 'x' frames same as current prediction
    # if same then take the gear value seriously.
    previousGears = predictedGears[-considerPreviousGears:]
    if (sum(previousGears)/len(previousGears) == predictedGear):
        takeGearSeriously = True
    else:
        takeGearSeriously = False

    # if repeated frames predict a different gear then change the gear.
    if (predictedGear[0] != gear and abs(gear - predictedGear[0]) == 1 and takeGearSeriously): # if gear shift
        gearShift = int(predictedGear[0] - gear)
        gear = int(predictedGear[0])
        print("GEAR CHANGED!!!")