import epuck2
import time

MIC_THRESHOLD = 150
PROX_THRESHOLD = 2000
GYRO_RAW2DPS = (250.0/32768.0)

mic_counter = 0
prox_counter = 0
tree_dist_counter = 0
demo_state = 0
steps_left_start = 0
turn_angle_deg = 0

def flash_leds():
    epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 0,0,0)
    epuck2.set_leds(0x01)
    time.sleep(0.02)
    epuck2.set_leds(0x00)
    epuck2.set_all_rgb(100,0,0, 0,0,0, 0,0,0, 0,0,0)
    time.sleep(0.02)
    epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 0,0,0)
    epuck2.set_leds(0x02)
    time.sleep(0.02)
    epuck2.set_leds(0x00)
    epuck2.set_all_rgb(0,0,0, 100,0,0, 0,0,0, 0,0,0)            
    time.sleep(0.02)
    epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 0,0,0)
    epuck2.set_leds(0x04)
    time.sleep(0.02)
    epuck2.set_leds(0x00)
    epuck2.set_all_rgb(0,0,0, 0,0,0, 100,0,0, 0,0,0)            
    time.sleep(0.02)
    epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 0,0,0)
    epuck2.set_leds(0x08)
    time.sleep(0.02)
    epuck2.set_leds(0x00)
    epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 100,0,0)
    time.sleep(0.02)

while 1:

    if demo_state == 0: # waiting for clapping to start
        mic = epuck2.get_mic_volume()
        print("mic volumes:" + str(mic) + ",counter:", str(mic_counter))
        if (mic[0] or mic[1] or mic[2] or mic[3])  > MIC_THRESHOLD:
            mic_counter += 1
            if mic_counter >= 2:
                demo_state = 1
                print("Clap detected!")
        else:
            mic_counter -= 1
            if mic_counter < 0:
                mic_counter = 0
    
    elif demo_state == 1:  # Led animation
        for i in range(5):
            flash_leds()
        epuck2.set_leds(0x00)
        epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 0,0,0)
        demo_state = 2
    
    elif demo_state == 2:  # Wait for hand in front of proximities sensors
        prox = epuck2.get_proximity()
        print("prox values:" + str(prox) + ",counter:", str(prox_counter))
        if (prox[0] > PROX_THRESHOLD) and (prox[7] > PROX_THRESHOLD):
            prox_counter += 1
            time.sleep(0.05)
            if prox_counter > 20: # after 1 second
                data = epuck2.get_all_sensors()
                steps_left_start = data[34]
                epuck2.set_motors_speed(200, 200)
                demo_state = 3
                print("Proximity detected! Left steps start:", steps_left_start)
        else:
            prox_counter = 0

    elif demo_state == 3:  # Go forward for a while
        data = epuck2.get_all_sensors()
        steps_left = data[34]
        print("Left steps:", steps_left)
        if (steps_left - steps_left_start) > 800:
            epuck2.set_motors_speed(0,0)
            demo_state = 4
            print("Reached target pos, stopping.")
    
    elif demo_state == 4:  # Wait for the christmas tree to be placed at the correct distance
        dist = epuck2.get_distance()
        if dist < 220 and dist > 180: # between 18 and 22 cm
            epuck2.set_all_rgb(0,100,0, 0,100,0, 0,100,0, 0,100,0)
            epuck2.set_leds(0x10)
            tree_dist_counter += 1
            time.sleep(0.1)
            if tree_dist_counter > 30: # after 3 second
                demo_state = 5
                epuck2.set_motors_speed(150,-150)
                print("Tree detected at correct distance!")
        else:
            epuck2.set_all_rgb(100,0,0, 100,0,0, 100,0,0, 100,0,0)
            epuck2.set_leds(0x20)
            tree_dist_counter = 0
    
    elif demo_state == 5:  # Rotate 180 degrees
        gyro = epuck2.get_gyro_raw()
        turn_angle_deg += gyro[2] * GYRO_RAW2DPS * 0.05  # dt = 50 ms
        time.sleep(0.05)
        if abs(turn_angle_deg) >= 180.0:
            epuck2.set_motors_speed(0,0)
            epuck2.play_sound(0x40) # Play christmas sound
            demo_state = 6
            print("Rotation completed.")

    elif demo_state == 6:  # Wait for button press to restart
        if epuck2.button_pressed():
            mic_counter = 0
            prox_counter = 0
            tree_dist_counter = 0
            demo_state = 0
            steps_left_start = 0
            turn_angle_deg = 0
            epuck2.set_all_rgb(0,0,0, 0,0,0, 0,0,0, 0,0,0)
            epuck2.set_leds(0x00)
            epuck2.play_sound(0x20) # stop sound
            print("Restarting demo.")