#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile

import sys


def fun_1(ev3):
    pass


def fun_2(a,b):
    return 1

def move_1(mo_forward_1, mo_forward_2, mo_le_ri, mo_up_down):
    wait(2000)
    #move forward
    mo_forward_1.run_target(500, 1250,wait=False)
    mo_forward_2.run_target(500, 1250)

    #move arm
    mo_le_ri.run_target(200,-50)

    #move sensors
    mo_up_down.run_target(500,2500)
    wait(2000)
    mo_up_down.run_target(500,0)

    #move arm
    mo_le_ri.run_target(200,0)

    #move backwards
    mo_forward_1.run_target(500, 0,wait=False)
    mo_forward_2.run_target(500, 0)

    


def move_2():
    pass
def move_3():
    pass




ev3 = EV3Brick()

motor_forward_1 = Motor(Port.A) 
motor_forward_2 = Motor(Port.D)
motor_le_ri = Motor(Port.B, Direction.COUNTERCLOCKWISE, [8, 56]) #with gear for angle
motor_up_down = Motor(Port.C)

#n = int(sys.argv[1])

#if n == 1:
move_1(motor_forward_1,motor_forward_2,motor_le_ri,motor_up_down)