#   trajectory tracking aerobatic control
#   See README.md for usage
#   Written by Matthew Hampsey, Andy Palmer and Andrew Tridgell, with controller
#   assistance from Paul Riseborough, testing by Henry Wurzburg

# To use this schedule put the file on your microSD in the root directory 
# (not the APM directory) with name trick93.txt The schedule can then be 
# used in auto missions or in TRIKn_ID commands for tricks on a switch

# This schedule is set up to be flown Left to Right. The schedule starts downwind - so 
# the mission should be set up with WP's lined up on the flight line (150m out from the 
# pilot), and the mission should be triggered when the plane gets to the center marker 
# heading RIGHT to LEFT. Schedule direction is reversed with aerom_scale = -1 (and 
# remember to reverse the mission WP's as well). Note the required height is greater 
# than 400 feet - so only fly at an airfield where there is a 1500 foot clearance

# This is an example of a F3A F23 schedule. Some manouvers are flown over the center
# line of the runway. Please understand the behaviour by flying in SITL before flying this
# schedule with a real aircraft! Your aircraft requires adequate performance to complete 
# the schedule

name: F3AF23

function half_reverse_cuban_eight(r, arg2, arg3, arg4)
   local rabs = math.abs(r)
   return make_paths("half_reverse_cuban_eight", {
         { path_vertical_arc(r,  45),   roll_angle(0) },
         { path_straight(2*rabs/3),     roll_angle(0) },
         { path_straight(2*rabs/3),     roll_angle(180) },
         { path_straight(2*rabs/3),     roll_angle(0) },
         { path_vertical_arc(-r, 225),  roll_angle(0) },
   })
end

function f23_1(radius, arg2, arg3, arg4) -- loop
   return make_paths("f23_1", {
            { path_straight(radius/2),             roll_angle(0) },
            { path_straight(radius/2),             roll_angle(-270) },
            { path_straight(radius/2),             roll_angle(0) },     
            { path_vertical_arc(radius, 360),      roll_angle(360) },             
            { path_straight(radius/2),             roll_angle(0) },
            { path_straight(radius/2),             roll_angle(-270) },
            { path_straight(radius/2),             roll_angle(0), roll_ref=180 },          
      })
end

function f23_2(radius, height, arg3, arg4)  -- stall turn   -- Currently a humpty!
   return make_paths("f23_2", {
            { path_vertical_arc(radius, 90),          roll_angle(0) },
            { path_straight((height-2*radius)/3),     roll_angle(0) },
            { path_straight((height-2*radius)/3),     roll_angle(180) },
            { path_straight((height-2*radius)/3),     roll_angle(0) },
            { path_vertical_arc(-radius, 180),        roll_angle(0) },   
            { path_straight((height-2*radius)),       roll_angle(0) },    
            { path_vertical_arc(-radius, 90),         roll_angle(0) },            
      })
end

function f23_3(length, arg2, arg3, arg4)   -- eight point roll
   return make_paths("f23_3", {
            { path_straight(length/16),       roll_angle(45) },
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) },
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) },  
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) },
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) }, 
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) },
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) },
            { path_straight(length/16),       roll_angle(0) },
            { path_straight(length/16),       roll_angle(45) },  
            { path_straight(length/16),       roll_angle(0) },            
      })
end

function f23_4(radius, height, arg3, arg4)   -- sharks tooth 
   local l = (height - (2 * radius)) 
   local angle_l = l * 1.9                                -- WRONG
   return make_paths("f23_4", {
            { path_vertical_arc(radius, 90),               roll_angle(0) },
            { path_straight(l * 2/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(-90) },
            { path_straight(l * 1/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(-90) },
            { path_straight(l * 1/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(-90) },
            { path_straight(l * 2/9),                      roll_angle(0) },
            { path_vertical_arc(radius, 135),              roll_angle(0) },    
            { path_straight(angle_l/3),                    roll_angle(0) },
            { path_straight(angle_l/3),                    roll_angle(270) },
            { path_straight(angle_l/3),                    roll_angle(0) },
            { path_vertical_arc(-radius, 45),              roll_angle(0), roll_ref=180 },                                
      })
end

function f23_5(radius, height_gain, arg3, arg4)   -- square loop on corner
   local l = ((height_gain - (2 * radius)) * math.sin(math.rad(45)))     -- CHECK
   return make_paths("f23_5", {
            { path_vertical_arc(radius, 45),         roll_angle(0) },
            { path_straight(l/3),                    roll_angle(0) },
            { path_straight(l/3),                    roll_angle(90) },
            { path_straight(l/3),                    roll_angle(0) },
            { path_vertical_arc(radius, 90),         roll_angle(0) },  
            { path_straight(l/3),                    roll_angle(0) },
            { path_straight(l/3),                    roll_angle(180) },
            { path_straight(l/3),                    roll_angle(0) },
            { path_vertical_arc(radius, 90),         roll_angle(0) },  
            { path_straight(l/3),                    roll_angle(0) },
            { path_straight(l/3),                    roll_angle(180) },
            { path_straight(l/3),                    roll_angle(0) },
            { path_vertical_arc(radius, 90),         roll_angle(0) }, 
            { path_straight(l/3),                    roll_angle(0) },
            { path_straight(l/3),                    roll_angle(90) },
            { path_straight(l/3),                    roll_angle(0) },
            { path_vertical_arc(radius, 45),         roll_angle(0) , roll_ref=180 },              
      })
end

function f23_6(radius, height_gain, arg3, arg4)   -- humpty bump
   local l = (height_gain - 2*radius) 
   return make_paths("f23_6", {
            { path_vertical_arc(radius, 90),               roll_angle(0) },
            { path_straight(2 * l/7),                      roll_angle(0) },
            { path_straight(l/7),                          roll_angle(180) },
            { path_straight(l/7),                          roll_angle(0) },
            { path_straight(l/7),                          roll_angle(180) },
            { path_straight(2 * l/7),                      roll_angle(0) },
            { path_vertical_arc(-radius, 180),             roll_angle(360) },
            { path_straight(l/3),                          roll_angle(0) },
            { path_straight(l/3),                          roll_angle(360) },       -- should be a snap roll
            { path_straight(l/3),                          roll_angle(0) },
            { path_vertical_arc(-radius, 90),              roll_angle(0) , roll_ref=180 },          
      })
end

function f23_7(radius, arg2, arg3, arg4) -- horizontal eight
   return make_paths("f23_7", {
            { path_vertical_arc(radius, 270),             roll_angle(270) },
            { path_vertical_arc(-radius, 360),            roll_angle(360) },
            { path_vertical_arc(radius, 90),              roll_angle(90) },                 
      })
end

function f23_8(radius, height, arg3, arg4)  -- tear drop
   local l = (height - 2*radius)
   local angle_l = l * 0.74                                       -- WRONG
   return make_paths("f23_8", {
         { path_vertical_arc(radius, 90),           roll_angle(0) },
         { path_straight(l/6),                      roll_angle(0) },
         { path_straight(4 * l/6),                  roll_angle(360) },
         { path_straight(l/6),                      roll_angle(0) },         
         { path_vertical_arc(radius, 225),          roll_angle(0) },  
         { path_straight(2 * angle_l/7),            roll_angle(0) },
         { path_straight(angle_l/7),                roll_angle(90) },
         { path_straight(angle_l/7),                roll_angle(0) },
         { path_straight(angle_l/7),                roll_angle(90) },
         { path_straight(2 * angle_l/7),            roll_angle(0) },   
         { path_vertical_arc(-radius, 135),         roll_angle(0) },          
      })
end

function f23_9(radius, height, arg3, arg4)   -- 45 up
   local l = (height - (2 * radius) + (2 * radius * math.cos(math.rad(45)))) / math.cos(math.rad(45))
   return make_paths("f23_9", {
            { path_straight(radius/3),                    roll_angle(90) },
            { path_straight(radius/3),                    roll_angle(0) },
            { path_vertical_arc(radius, 45),              roll_angle(0) },
            { path_straight(l/3),                         roll_angle(0) },
            { path_straight(l/6),                         roll_angle(180) },
            { path_straight(l/6),                         roll_angle(-180) },
            { path_straight(l/3),                         roll_angle(0) },
            { path_vertical_arc(-radius, 45),             roll_angle(0) }, 
            { path_straight(radius/3),                    roll_angle(0) },
            { path_straight(radius/3),                    roll_angle(90) , roll_ref=180 },            
      })
end

function f23_10(radius, height, arg3, arg4)   -- figure z  
   local l = height - (2 * radius)
   -- local angle_l = (l + (2 * radius * math.cos(math.rad(45)))) / math.cos(math.rad(45)) -- WRONG
   local angle_l = l  * 1.1  
   return make_paths("f23_10", {
            { path_vertical_arc(-radius, 90),              roll_angle(0) },
            { path_straight(l/4),                          roll_angle(0) },
            { path_straight(l/4),                          roll_angle(180) },
            { path_straight(l/4),                          roll_angle(-180) },
            { path_straight(l/4),                          roll_angle(0) },
            { path_vertical_arc(-radius, 90),              roll_angle(0) },
            { path_straight(2 * l/7),                      roll_angle(0) },
            { path_straight(l/7),                          roll_angle(90) },
            { path_straight(l/7),                          roll_angle(0) },
            { path_straight(l/7),                          roll_angle(90) },
            { path_straight(2 * l/7),                      roll_angle(0) },  
            { path_vertical_arc(-radius, 135),             roll_angle(0) },         
            { path_straight(angle_l/6),                    roll_angle(0) },
            { path_straight(angle_l/3),                    roll_angle(360) },
            { path_straight(angle_l/6),                    roll_angle(0) },           
            { path_vertical_arc(radius, 135),              roll_angle(0) },                             
      })
end

function f23_11(radius, height, arg3, arg4)   -- heart
   local rabs = math.abs(radius)             
   local l =  height * 1.15                     -- WRONG                                 
   return make_paths("f23_11", {
            { path_vertical_arc(-radius, 45),              roll_angle(0) },
            { path_straight(l * 2/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(-90) },
            { path_straight(l * 1/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(-90) },
            { path_straight(l * 1/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(-90),  roll_ref=90 },
            { path_straight(l * 2/9),                      roll_angle(0) },
            { path_vertical_arc(radius, 180),              roll_angle(0) }, 
            { path_straight(l * 1/3),                      roll_angle(0) },
            { path_straight(l * 1/3),                      roll_angle(180) ,  roll_ref=-90 },
            { path_straight(l * 1/3),                      roll_angle(0) },
            { path_vertical_arc(radius, 90),               roll_angle(0) },
            { path_straight(l * 1/3),                      roll_angle(0) },
            { path_straight(l * 1/3),                      roll_angle(180) ,  roll_ref=90 },
            { path_straight(l * 1/3),                      roll_angle(0) },
            { path_vertical_arc(-radius, 180),             roll_angle(0) },            
            { path_straight(l * 2/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(90) },
            { path_straight(l * 1/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(90) },
            { path_straight(l * 1/9),                      roll_angle(0) },
            { path_straight(l * 1/9),                      roll_angle(90),  roll_ref=-90 },
            { path_straight(l * 2/9),                      roll_angle(0) },
            { path_vertical_arc(-radius, 45),              roll_angle(0) },           
      })
end

function f23_12(radius, height, arg3, arg4)   -- reverse top hat
   return make_paths("f23_12", {
            { path_vertical_arc(-radius, 90),         roll_angle(0) },
            { path_straight((height-2*radius)/3),     roll_angle(0) },
            { path_straight((height-2*radius)/3),     roll_angle(180) },
            { path_straight((height-2*radius)/3),     roll_angle(0) },
            { path_vertical_arc(-radius, 90),         roll_angle(0) },   
            { path_straight(radius/2),                roll_angle(0) }, 
            { path_vertical_arc(-radius, 90),         roll_angle(0) },  
            { path_straight((height-2*radius)/3),     roll_angle(0) },
            { path_straight((height-2*radius)/3),     roll_angle(180) },
            { path_straight((height-2*radius)/3),     roll_angle(0) }, 
            { path_vertical_arc(radius, 90),          roll_angle(0) , roll_ref=180 },            
      })
end

function f23_13(radius, height, arg3, arg4)  -- spin place holder
   local l = height - (2 * radius)
   return make_paths("f23_13", {
         { path_vertical_arc(-radius, 90),             roll_angle(0) },
         { path_straight(2 * l/7),                     roll_angle(0) },
         { path_straight(l/7),                         roll_angle(180) },
         { path_straight(l/7),                         roll_angle(0) },
         { path_straight(l/7),                         roll_angle(-180) },
         { path_straight(2 * l/7),                     roll_angle(0) }, 
         { path_vertical_arc(radius, 90),              roll_angle(0) },        
      })
end

function f23_14(r, arg2, arg3, arg4)   -- half cuban eight - needs a snap on the way down
   local rabs = math.abs(r)
   return make_paths("f23_14", {
         --{ path_straight((2*rabs*math.sqrt(2))/4),          roll_angle(0) },
         { path_straight((2*rabs*math.sqrt(2))/4),          roll_angle(180) },
         { path_straight((2*rabs*math.sqrt(2))/4),          roll_angle(-180) },
         { path_straight((2*rabs*math.sqrt(2))/4),          roll_angle(0) },
         { path_vertical_arc(r,  225),                      roll_angle(0) },
         { path_straight(2*rabs/3),                         roll_angle(0) },
         { path_straight(2*rabs/3),                         roll_angle(180) },
         { path_straight(2*rabs/3),                         roll_angle(0) },
         { path_vertical_arc(-r, 45),                       roll_angle(0) },
   })
end                            

function f23_15(radius, arg2, arg3, arg4)   -- rolling circle
   return make_paths("f23_15", { 
         { path_horizontal_arc(radius, 180),       roll_angle(-180),     thr_boost=true },
         { path_horizontal_arc(radius, 180),       roll_angle(180),      thr_boost=true },
      })
end

function f23_16(radius, height, arg3, arg4)   -- half square loop
   local l = height - (2 * radius)
   return make_paths("f23_16", {
         { path_vertical_arc(radius, 90),              roll_angle(0) },
         { path_straight(l/4),                         roll_angle(0) },
         { path_straight(l/4),                         roll_angle(180) },
         { path_straight(l/4),                         roll_angle(-180) },
         { path_straight(l/4),                         roll_angle(0) }, 
         { path_vertical_arc(radius, 90),              roll_angle(0)  , roll_ref=180},         
      })
end

function f23_17(radius, arg2, arg3, arg4)   -- down loop
   return make_paths("f23_17", {
            { path_vertical_arc(-radius, 90),       roll_angle(180) },
            { path_vertical_arc(-radius, 90),       roll_angle(0) },     -- needs a snap in here
            { path_vertical_arc(-radius, 90),       roll_angle(0) },              
            { path_vertical_arc(-radius, 90),       roll_angle(180) },
            
      })
end

function half_roll(arg1, arg2, arg3, arg4)   -- half roll for testing inverted manouvers
   return make_paths("half_roll", {
            { path_straight(40),             roll_angle(180) },
            { path_straight(10),             roll_angle(0) },   
      })
end

straight_roll 80 0
half_reverse_cuban_eight 70

align_center
message: Loop
f23_1 90

align_box 1
message: StallTurn
f23_2 40 200

align_center
message: RollCombination
f23_3 200

align_box 1
message: KnifeEdgeSharksTooth
f23_4 30 190

align_center
message: SquareLooponCorner
f23_5 40 200

align_box 1
message: Humpty
f23_6 40 200

align_center
message: HorizontalEight
f23_7 100

align_box 1
message: TearDrop
f23_8 40 200  
         
align_center
message: 45Up
f23_9 40 200

align_box 1
message: FigureZ
f23_10 40 200

align_center
message: Heart
f23_11 40 200

align_box 1
message: ReverseTopHat
f23_12 40 200

align_center
message: Spin
f23_13 40 200

align_box 1
message: HalfCuban
f23_14 70
  
align_center
message: RollingCircle
f23_15 100

align_box 1
message: HalfSquareLoop
f23_16 40 200

align_center
message: DownLoop
f23_17 100

straight_roll 50 0
half_roll 0
straight_roll 50 0