from time import perf_counter
import cv2
from kuukar.kuukar import kuukar
from LaneDetectionModule import getLaneCurve
from utils import initializeTrackbars
import WebcamModule
from threading import Thread
global speed, sen, car, delay
sen = 1
speed = 0
delay = 0.15
car = kuukar()

##################################################
############### MAIN ROBOT LANE ##################
##################################################


def main():
    global speed, sen, car, delay
    while True:
        stt = perf_counter()
        img = WebcamModule.getImg()
        curveVal = getLaneCurve(img, 2)
        maxVAl = 1.0  # MAX SPEED
        if curveVal > maxVAl:
            curveVal = maxVAl
        if curveVal < -maxVAl:
            curveVal = -maxVAl
        # print(curveVal)
        if curveVal > 0:
            sen = 1.7
            if curveVal < 0.05:
                curveVal = 0
        else:
            if curveVal > -0.08:
                curveVal = 0
        car.motion.skt_drive(speed, -curveVal*sen, delay)
        turn_value = -curveVal*sen
        #print("Curve Value : " + str(curveVal) + " Turn Value : "+ str(round(turn_value,2)))
        #print(f"looptime: {perf_counter()-stt}s")
        cv2.waitKey(1)


if __name__ == '__main__':
    initialTrackBarVals = [107, 134, 89, 221]
    initializeTrackbars(initialTrackBarVals)
    Thread(target=main).start()
    while True:
        speed = float(input("speed: "))
        sen = float(input("sen: "))
        delay = float(input("delay: "))