import cv2
from kuukar.kuukar import kuukar
from LaneDetectionModule import getLaneCurve
from utils import initializeTrackbars
import WebcamModule

car = kuukar()
 
 
##################################################
############### MAIN ROBOT LANE ##################
##################################################
 
def main():
    
    
    
    img = WebcamModule.getImg()
    curveVal= getLaneCurve(img,2)
 
    sen = 0.8  # SENSITIVITY
    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(0.4,-curveVal*sen,0.05)
    turn_value = -curveVal*sen
    print("Curve Value : " + str(curveVal) + " Turn Value : "+ str(round(turn_value,2))
    cv2.waitKey(1)
     
 
if __name__ == '__main__':
    initialTrackBarVals = [70, 124, 4, 195]
    initializeTrackbars(initialTrackBarVals)
    
    while True:
        main()