#!/usr/bin/env python3

# Build loosely based on R3ptar robot code and building instructions:
# https://github.com/pybricks/pybricks-projects/tree/master/sets/mindstorms-ev3/home-main/r3ptar
# https://www.lego.com/cdn/product-assets/product.bi.additional.extra.pdf/31313_X_R3PTAR.pdf

# Main changes include:
# - Converting code from Pybricks Micropython to EV3Dev Python to allow using Mindsensors motor multiplexer, not supported in former
# - Adding 2 lift motors with linear actuators
# - Connecting lift motors to Mindsensors motor multiplexer
# - Moving 'move' motor under body
# - Adding wheels in front (omni) and back
# - Adding Wi-Fi 2-way connectivity to get started from Jeep and have it play sounds using its Bluetooth mic
# - Adding dinosaur sounds from https://www.dinosaurfact.net/Sounds.php and display image (in .bmp format) created from stock.

# Import modules
import math, random, socket, sys, time

from ev3dev2.button import Button
from ev3dev2.display import Display
from ev3dev2.led import Leds
from ev3dev2.motor import LargeMotor, MediumMotor, MoveSteering, MoveTank, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, GyroSensor, InfraredSensor, TouchSensor, UltrasonicSensor
from ev3dev2.sound import Sound

from PIL import Image # Display image

# Initialize brick
Brontie_button = Button()
Brontie_lcd = Display()
Brontie_led = Leds()
Brontie_sound = Sound()

# Initialize robot constants
axle_track = 110 # mm distance between middle of tire contact
wheel_diameter = 43.2 # mm

# Initialize motors
motor_turn = MediumMotor(OUTPUT_A)
motor_move = LargeMotor(OUTPUT_B)
motor_eat = LargeMotor(OUTPUT_D)
# motor_down = Motor(OUTPUT_D)

# Mindsensors motor multiplexer on port INPUT_1 using I2C at address x03
motor_lift_right = LargeMotor('in1:i2c3:M1') # Large motors need to be defined even though medium motor used
motor_lift_left = LargeMotor('in1:i2c3:M2')

# move_tank = MoveTank(OUTPUT_B, OUTPUT_C)
# move_steering = MoveSteering(OUTPUT_B, OUTPUT_C)

# Initialize sensors
# sensor_colour = ColorSensor(INPUT_1)
# sensor_gyro = GyroSensor(INPUT_2)
sensor_ir = InfraredSensor(INPUT_4)
# sensor_touch = TouchSensor(INPUT_3)
# sensor_us = UltrasonicSensor(INPUT_4)

# Initialize program constants
SPEED_LIFT = 750
# Motor speeds
SPEED_HIGH = 100
SPEED_LOW = 50
SPEED_MEDIUM = 75
# Motor durations
DURATION_HIGH = 4.000
DURATION_LOW = 1.000
DURATION_MEDIUM = 2.000
# Wait durations
WAIT_LIFT = 10
WAIT_WHILE = 0.1

# Declare program variables
# Flag for leaves not detected, not connected
leaves_not_detected = True
not_connected = True

# Declare functions
# Detect leaves
def detect_leaves():

    global leaves_not_detected
    heading, distance = sensor_ir.heading_and_distance(channel=1) # Tuple returned by IR sensor on channel 1
    output = 'Heading: ' + str(heading) + ' Distance: ' + str(distance)
    print(output)
    turn(heading)

    if distance is not None and distance < 30: # Threshold distance in cm
        print('Detecting leaves...')
        motor_move.stop() # Stop moving
        lift_dino()
        leaves_not_detected = False

# Eat leaves
def eat_leaves():
    # Play sound
    print('Eating leaves...')
    #Brontie_sound.play_file('Brontie_Moan.wav')
    send_message("MOAN")
    # Open mouth
    motor_eat.on_for_seconds(SPEED_LOW, DURATION_HIGH)
    # Wait 3 sec
    wait_eat_temp = WAIT_WHILE * 30
    time.sleep(wait_eat_temp)
    # Close mouth
    motor_eat.on_for_seconds(-SPEED_LOW, DURATION_HIGH)
    # Play sound
    #Brontie_sound.play_file('Brontie_Moan.wav')
    send_message("MOAN")

# Lift dinosaur
def lift_dino():
    # Initialise temp speed
    print('Lifting dino...')
    speed_temp = SPEED_LIFT
    # Run both lift motors for 10 sec up
    speed_temp = -speed_temp
    motor_lift_right.run_forever(speed_sp=speed_temp)
    motor_lift_left.run_forever(speed_sp=speed_temp)
    time.sleep(WAIT_LIFT)
    # Stop both motors
    motor_lift_right.stop()
    motor_lift_left.stop()
    eat_leaves()
    # Run both lift motors for 10 sec down
    speed_temp = -speed_temp
    motor_lift_right.run_forever(speed_sp=speed_temp)
    motor_lift_left.run_forever(speed_sp=speed_temp)
    time.sleep(WAIT_LIFT)
    # Stop both motors
    motor_lift_right.stop()
    motor_lift_left.stop()

# Send message to Jeep
def send_message(sound_name):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    try:
        s.connect(("jeep", 12346))  # port must match Jeep’s listener
        s.sendall(sound_name.encode())
        print("Message", sound_name, "sent to Jeep successfully.")
    except Exception as e:
        print("Connection to Jeep unsuccessful! Error:", e)
    finally:
        s.close()

# Turn according to IR sensor heading
# Heading is -25 to 25 (0, 12 noon    12, 1 pm    25, 2 pm    -12, 11 am  -25, 10 am)
def turn(heading):
    # forward 12 noon
    if (heading > -5) and (heading <= 5):
        print('Forward')
        motor_move.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
    # right 1 pm, turn slightly right
    elif (heading > 5) and (heading <= 15):
        print('Right 1 pm')
        motor_turn.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
        motor_move.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
    # right 2 pm, turn more right
    elif (heading > 15) and (heading <= 25):
        print('Right 2 pm')
        motor_turn.on_for_seconds(SPEED_MEDIUM, DURATION_MEDIUM)
        motor_move.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
    # left 11 am, turn slightly left
    elif (heading > -15) and (heading <= -5):
        print('Right 11 am')
        motor_turn.on_for_seconds(-SPEED_MEDIUM, DURATION_LOW)
        motor_move.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
    # left 10 am, turn more left
    elif (heading > -25) and (heading <= -15):
        print('Right 10 am')
        motor_turn.on_for_seconds(-SPEED_MEDIUM, DURATION_MEDIUM)
        motor_move.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
    # Heading out of range, play sound
    else:
        print('Out of range')
        #Brontie_sound.play_file('Brontie_Moan.wav')
        send_message("MOAN")

# Declare main program
# Set Brontie image
image = Image.open('Brontie.bmp') # EV3Dev uses bmp format, not png, and needs import of Image from PIL
Brontie_lcd.image.paste(image, (0, 0))
Brontie_lcd.update()
# Close mouth
motor_eat.on_for_seconds(-SPEED_LOW, DURATION_LOW)

# Set up comms to listen for START command
host = '0.0.0.0'  # Brontie
port = 12345

r = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
r.bind((host, port))
r.listen(5)  # Allow multiple connections
print("Waiting for connection from Jeep...")

while not_connected:
    conn, addr = r.accept()
    # Decode using UTF-8 for consistency
    msg = conn.recv(1024).decode('utf-8')

    if msg == "START":
        print("Message START received from Jeep successfully.")
        send_message("MOAN")
    # Close this connection, but keep listening
    conn.close()
    not_connected = False

# Play sound
#Brontie_sound.play_file('Brontie_Moan.wav')

motor_move.on_for_seconds(SPEED_LOW, DURATION_LOW)
# Shake body left & right
motor_turn.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
motor_turn.on_for_seconds(-SPEED_MEDIUM, DURATION_MEDIUM)
motor_turn.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
    
# Repeat until leaves are detected
while leaves_not_detected:

    # Move in snake like pattern
    for _ in range(2):
        motor_move.on_for_seconds(SPEED_LOW, DURATION_LOW)
        motor_turn.on_for_seconds(SPEED_MEDIUM, DURATION_LOW)
        SPEED_MEDIUM = -SPEED_MEDIUM
    # Detect leaves
    detect_leaves()
    # Wait to avoid excessive loop execution
    time.sleep(WAIT_WHILE)

# Play mooing sound 3 times every 5 sec
for _ in range(3):
    print('Continuing mooing...')
    #Brontie_sound.play_file('Brontie_Moan.wav')
    send_message("MOAN")
    wait_while_temp = WAIT_WHILE * 50
    time.sleep(wait_while_temp)