import RPi.GPIO as GPIO
import threading
import time
import subprocess
import os, signal, sys
from pid_line_follow1 import *
from AlphaBot2 import AlphaBot2
from TRSensors import TRSensor


Ab = AlphaBot2()

IR = 17
PWM = 50

'''def move_and_stop(func, duration=0.5):
	func()
	time.sleep(duration)
	Ab.stop()'''
def setup_gpio():
    """Initialisiert die GPIO-Pins."""
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)
    GPIO.setup(IR, GPIO.IN)

def getkey():
	if GPIO.input(IR) == 0:
		count = 0
		while GPIO.input(IR) == 0 and count < 200:  #9ms
			count += 1
			time.sleep(0.00006)

		if(count < 10):
			#print("None returned")
			return None
		count = 0
		while GPIO.input(IR) == 1 and count < 80:  #4.5ms
			count += 1
			time.sleep(0.00006)
		#print("High-Burst-Length:", count)
		if (count < 20):
			#print("repeat returned")
			return "repeat"

		idx = 0
		cnt = 0
		data = [0,0,0,0]
		for i in range(0,32):
			count = 0
			while GPIO.input(IR) == 0 and count < 15:    #0.56ms
				count += 1
				time.sleep(0.00006)
				
			count = 0
			while GPIO.input(IR) == 1 and count < 40:   #0: 0.56mx
				count += 1                               #1: 1.69ms
				time.sleep(0.00006)
				
			if count > 7:
				data[idx] |= 1<<cnt
			if cnt == 7:
				cnt = 0
				idx += 1
			else:
				cnt += 1
#		print data
		if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF:  #check
			print("OK")
			return data[2]
	return None

def stop_listener():
    """
    Deine Remote‑API muss hier so lange blocken, bis
    ein Key‑Event eintrifft und dir den Code 0x43 liefert.
    """
    while True:
        code = getkey()
        if code == 0x43:
            os.kill(os.getpid(), signal.SIGINT)
            return

setup_gpio()
print('IRremote Test Start ...')
Ab.stop()
last_key = 0
last_key_press_time = 0
current_action = None
try:
	while True:
		key = getkey()
		# 0x43 == line_follow

		if key == "repeat":
			key = last_key
			last_key_press_time = time.time()
		elif key is not None:
			print("getkey:")
			print(hex(key))
			last_key = key
			last_key_press_time = time.time()

		if key is not None:
			last_key_press_time = time.time()

			if key == 0x18 and current_action != "forward":
				Ab.forward()
				print("forward")
				current_action = "forward"
			elif key == 0x08 and current_action != "left":
				Ab.left()
				print("left")
				current_action = "left"
			elif key == 0x1c and current_action != "stop":
				Ab.stop()
				print("stop")
				current_action = "stop"
			elif key == 0x5a and current_action != "right":
				Ab.right()
				print("right")
				current_action = "right"
			elif key == 0x52 and current_action != "backward":
				Ab.backward()
				print("backward")
				current_action = "backward"
			elif key == 0x15:
				if(PWM + 10 < 101):
					PWM += 10
					Ab.setPWMA(PWM)
					Ab.setPWMB(PWM)
					print("PWM:", PWM)
			elif key == 0x07:
				if(PWM - 10 > -1):
					PWM -= 10
					Ab.setPWMA(PWM)
					Ab.setPWMB(PWM)
					print("PWM:", PWM)
			elif key == 0x43:
				tr = TRSensor()
				running = threading.Event()
				process = subprocess.Popen(
					["python", "pid_line_follow1.py"],
				)
				# follow_thread = threading.Thread(target=follow, args=(tr, Ab, running), daemon=True)
				# follow_thread.start()
				key = getkey()
				while(key != 0x43):
					key = getkey()

				# running.set()
				process.terminate()

				Ab.setPWMA(PWM)
				Ab.setPWMB(PWM)

			time.sleep(0.05)

		else:
			if time.time() - last_key_press_time > 0.1 and current_action != "stop":
				Ab.stop()
				print("stopping bc no key is being pressed")
				current_action = "stop"
				
except KeyboardInterrupt:
	GPIO.cleanup()