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
from time import perf_counter
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: "))