"""
A very simply autopilot designed to simplify testing of planes by providing an
existing framework for getting a plane in the air.
"""

from .io import Protocol
from . import packets


class TakeOffMixin:
    _take_off_state = None
    _take_off_heading = None

    def take_off(self):
        self.take_off_started()

    def take_off_got_data_packet(self, packet, address):
        if self._take_off_state is None:
            return False

        if self._take_off_state == 'started':
            _, _, true_heading, _ = packet.read_pitch_roll_headings()
            self._take_off_heading = true_heading
            print('Landing strip heading is:', true_heading)
            self._take_off_throttle()
            self._take_off_state = 'throttle'
        elif self._take_off_state == 'throttle':
            _, _, true_heading, _ = packet.read_pitch_roll_headings()
            diff = self._take_off_heading - true_heading

            print(diff)

            p = packets.DataPacket()
            p.write_joystick_elevator_aileron_rudder(rudder=diff * 3, elevator=0.1)
            self.send_packet(p)

        return True

    def _take_off_throttle(self):
        self.send_packet(packets.CommandPacket('sim/flight_controls/brakes_toggle_regular'))

        packet = packets.DataPacket()
        packet.write_throttle_command(1)
        self.send_packet(packet)

    def take_off_started(self):
        self._take_off_state = 'started'

    def take_off_finished(self):
        self._take_off_state = None

@@ -132,16 +132,38 @@
 r.%0A%22%22%22%0A%0A
+from . import packets%0A
 from .io
@@ -181,32 +181,39 @@
 tocol%0Afrom .
+packets
  import 
 packets%0A%0A%0Acl
@@ -200,21 +200,19 @@
  import 
-packe
+uni
 ts%0A%0A%0Acla
@@ -833,33 +833,36 @@
 %0A            _, 
-_
+roll
 , true_heading, 
@@ -973,127 +973,551 @@
     
-print(diff)%0A%0A            p = packets.DataPacket()%0A            p.write_joystick_elevator_aileron_rudder(rudder=diff * 3,
+rudder = diff * 5%0A            elevator = 0%0A            aileron = 0%0A%0A            lift, _, _ = packet.read_aero_forces()%0A%0A            # TODO calculate this based on weight of craft%0A            if lift %3E= 5000 * units.newton:%0A                elevator = 0.25%0A                aileron = -roll.to(units.radians).magnitude%0A%0A            p = packets.DataPacket()%0A            p.write_joystick_elevator_aileron_rudder(rudder=rudder,%0A                                                     aileron=aileron,%0A                                                    
  ele
@@ -1526,11 +1526,16 @@
 tor=
-0.1
+elevator
 )%0A  

Implement proper take off behaviour