use Libraries.Robots.Lego.Button
use Libraries.Robots.Lego.Motor
use Libraries.Robots.Lego.TouchSensor
use Libraries.Robots.Lego.Screen
use Libraries.Robots.Lego.ColorSensor
use Libraries.Robots.Lego.Sound

action Main
    Button button
    Screen screen

    repeat until button:IsButtonPressed(button:ESCAPE_BUTTON)
        screen:OutputCenterLarge("Ready", 3)
        button:WaitForButtonPress()
        screen:Clear()

        if button:IsButtonPressed(button:LEFT_BUTTON)
            screen:OutputCenterLarge("Turn Left", 3)
            Turn360(true)
        elseif button:IsButtonPressed(button:RIGHT_BUTTON)
            screen:OutputCenterLarge("Turn Right", 3)
            Turn360(false)
        elseif button:IsButtonPressed(button:UP_BUTTON)
            screen:OutputCenterLarge("Stop on Black", 3)
            StopOnColor("black")
        elseif button:IsButtonPressed(button:DOWN_BUTTON)
            screen:OutputCenter("Drive Until Touch", 3)
            DriveUntilTouch()
        elseif button:IsButtonPressed(button:CENTER_BUTTON)
            screen:OutputCenter("Beep", 3)
            Beep()
        end

        screen:Clear()
    end
end

action DriveOneByInches(number wheel, number inch, text letter)
    //wheel: diameter of wheel
    //inch: how many inches you want to travel
    //letter: which wheel moves
    Motor motor
    wheel = wheel * 3.14
    number degree = 360 / wheel
    degree = degree * inch
    motor:RotateByDegrees(letter, degree)
end

action TurnByDegrees(number degree, number diameter, number wheel)
    //degree: number of degrees being turned from front
    //diameter: distance from wheel a to wheel b
    //wheel: circumference of wheel
    Motor motor
    number circumference = diameter * 3.14
    number leftWheel = circumference * (degree/360)//inches
    number rightWheel = -(circumference * (degree/360))//inches
    DriveOneByInches(1.75, leftWheel, "B")
    DriveOneByInches(1.75, rightWheel, "C")
    motor:Wait("B")
    motor:Wait("C")
end

action DriveUntilTouch()
    // goes straight until touch sensor enacted, then stops
    Motor motor
    TouchSensor touch

    touch:SetPort(3)
    motor:RotateForward("B")
    motor:RotateForward("C")
    repeat until touch:IsPressed()
    end
    motor:Stop("B")
    motor:Stop("C")
end

action Turn360(boolean left)
    number wheel = 1.75
    number diameter = 3.5
    number degrees = 360
    if left = false
        degrees = -360
    end
    
    TurnByDegrees(degrees, diameter, wheel)
end

action StopOnColor(text stopcolor)
    Motor motor
    ColorSensor color

    color:SetPort(2)
    text colorname = color:GetColor()
    motor:RotateBackward("B")
    motor:RotateBackward("C")
    repeat until colorname = stopcolor
        colorname = color:GetColor()
    end
    motor:Stop("B")
    motor:Stop("C")
end

action Beep()
    Sound sound

    sound:BeepSequenceUp()
end