alt_sensor=peripheral.wrap("left")
angle_sensor=peripheral.wrap("back")
redstone=peripheral.wrap("top")

redstone2=peripheral.wrap("redstone_relay_1")

nav_sensor=peripheral.wrap("navigation_table_1")


angles=angle_sensor.getAngles()
alt=alt_sensor.getHeight()
pitch=angles[1]
roll=angles[2]
nav_angle=90-nav_sensor.getRelativeAngle()



wanted_alt=180

wanted_pitch=0
pitch_range=6
wanted_roll=0
roll_range=4
yaw_range=10





function set_yaw(yaw_val)
    left="bottom"
    right="right"
    if yaw_val==-1 then
        redstone2.setOutput(left,true)    --up
        redstone2.setOutput(right,false)
    end
    if yaw_val==0 then
        redstone2.setOutput(left,false)
        redstone2.setOutput(right,false)
    end
    if yaw_val==1 then
        redstone2.setOutput(left,false)
        redstone2.setOutput(right,true)
    end
end



function set_pitch(pitch_val)
    if pitch_val==1 then
        redstone.setOutput("left",true)    --up
        redstone.setOutput("right",false)
    end
    if pitch_val==0 then
        redstone.setOutput("right",false)
        redstone.setOutput("left",false)
    end
    if pitch_val==-1 then
        redstone.setOutput("left",false)
        redstone.setOutput("right",true)
    end
end

function set_roll(roll_val)
    if roll_val==1 then
        redstone.setOutput("back",true)    --right
        redstone.setOutput("front",false)
    end
    if roll_val==0 then
        redstone.setOutput("back",false)
        redstone.setOutput("front",false)
    end
    if roll_val==-1 then
        redstone.setOutput("back",false)    --left
        redstone.setOutput("front",true)
    end
end



function get_sensor_data()
    angles=angle_sensor.getAngles()
    alt=alt_sensor.getHeight()
    pitch=angles[1]
    roll=angles[2]
    nav_angle=90-nav_sensor.getRelativeAngle()
end

function calculate_pitch()
    wanted_pitch=(alt-wanted_alt)/10
    wanted_pitch=math.max(wanted_pitch,-30)
    wanted_pitch=math.min(wanted_pitch,30)


    if math.abs(wanted_pitch-pitch)>pitch_range then
        if wanted_pitch-pitch<0 then
            set_pitch(1)
        else
            set_pitch(-1)
        end
    else
        set_pitch(0)
    end
end


function calculate_roll()
    if math.abs(wanted_roll-roll)>roll_range then
        if wanted_roll-roll<0 then
            set_roll(1)
        else
            set_roll(-1)
        end
    else
        set_roll(0)
    end
end

function calculate_yaw()
    if math.abs(nav_angle)>yaw_range then
        if nav_angle<0 then
            set_yaw(-1)
        else
            set_yaw(1)
        end
    else
        set_yaw(0)
    end
end


while true do
    get_sensor_data()
    calculate_pitch()
    calculate_roll()
    calculate_yaw()
    print("Alt",alt)
    print("pitch",pitch)
    print("roll",roll)
    print("Wanted Pitch",wanted_pitch)
    print(nav_angle)
    sleep(0)
end