import RPi.GPIO as GPIO
import time
import threading
import lirc # https://github.com/tompreston/python-lirc
from l293d import L293D


class Robot():
    def __init__(self, motor_left_pin1=17, motor_left_pin2=27, motor_right_pin1=23, motor_right_pin2=24,
                 line_follow_pin_left=19, line_follow_pin_right=6 ):
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        
        # init modules
        self.motor = L293D(motor_left_pin1, motor_left_pin2, motor_right_pin1, motor_right_pin2)
        self.line_follow_pin_left  = line_follow_pin_left
        self.line_follow_pin_right = line_follow_pin_right
        
        GPIO.setup(self.line_follow_pin_left,  GPIO.IN)
        GPIO.setup(self.line_follow_pin_right, GPIO.IN)
    
    def lineFollowModeOn(self):
     
        status_left  = False
        status_right = False
        
        while True:
            status_left  = bool(GPIO.input(self.line_follow_pin_left))  # False: not on line / sensor too distant from bottom
            status_right = bool(GPIO.input(self.line_follow_pin_right)) # True: on line
            
            if status_left and status_right:
                # one the line, follow straight on
                self.motor.forward()
            elif status_left:
                # line is on the left, move left (motor right)
                self.motor.forwardRight()
            elif status_right:
                # line is on the right, move right (motor left)
                self.motor.forwardLeft()
            else:
                # have gone astray, search line. first go back some cm
                self.motor.backward()
                time.sleep(7.5/self.motor.DIST_PER_SEC)
                self.motor.stop()
                # rotate x degrees to search the line
                degrees_to_search = 45.0
                self.motor.forwardRight()
                s = GPIO.wait_for_edge(self.line_follow_pin_left, GPIO.RISING, timeout=int(1000 * self.motor.SEC_PER_TURN / 360.0 * degrees_to_search))
                self.motor.stop()
                if s is not None:
                    # line found, continue
                    continue
                else:
                    # nothing found, go back to original position
                    self.motor.backwardRight()
                    time.sleep(self.motor.SEC_PER_TURN / 360.0 * degrees_to_search)
                    # search in other side
                    self.motor.forwardLeft()
                    s = GPIO.wait_for_edge(self.line_follow_pin_right, GPIO.RISING, timeout=int(1000 * self.motor.SEC_PER_TURN / 360.0 * degrees_to_search))
                    self.motor.stop()
                    if s is not None:
                        # line found, continue
                        continue
                    else:
                        # line could not be found, go back to original position, stop
                        self.motor.backwardLeft()
                        time.sleep(self.motor.SEC_PER_TURN / 360.0 * degrees_to_search)
                        self.motor.stop()
                        break
            time.sleep(0.001)
    
    def IRControl(self):
        sockid = lirc.init("robot_ir", "lircrc")
        dist_per_leypress = 30.0 #cm
        while True:
            #self.motor.stop()
            code = lirc.nextcode()[0]
            print(code)
            if code == "FORWARD":
                self.motor.forward()
                time.sleep(dist_per_leypress / self.motor.DIST_PER_SEC)
                self.motor.stop()
            elif code == "LEFT_FORWARD":
                # 30 degree turn left, then stop
                self.motor.forwardRight()
                time.sleep(self.motor.SEC_PER_TURN / 360.0 * 30.0)
                self.motor.stop()
            elif code == "RIGHT_FORWARD":
                # 30 degree turn right, then stop
                self.motor.forwardLeft()
                time.sleep(self.motor.SEC_PER_TURN / 360.0 * 30.0)
                self.motor.stop()
            elif code == "LEFT":
                self.motor.forwardRight()
                time.sleep(self.motor.SEC_PER_TURN / 360.0 * 90.0)
                self.motor.stop()
            elif code == "RIGHT":
                self.motor.forwardLeft()
                time.sleep(self.motor.SEC_PER_TURN / 360.0 * 90.0)
                self.motor.stop()
            elif code == "BACKWARD":
                self.motor.backward()
                time.sleep(dist_per_leypress / self.motor.DIST_PER_SEC)
                self.motor.stop()
            elif code == "LEFT_BACKWARD":
                # 30 degree turn left back, then stop
                self.motor.backwardRight()
                time.sleep(self.motor.SEC_PER_TURN / 360.0 * 30.0)
                self.motor.stop()
            elif code == "RIGHT_BACKWARD":
                # 30 degree turn right back, then stop
                self.motor.backwardLeft()
                time.sleep(self.motor.SEC_PER_TURN / 360.0 * 30.0)
                self.motor.stop()
            elif code == "STOP":
                self.motor.stop()
            elif code == "LINE_FOLLOW_ON":
                self.lineFollowModeOn()