class IdentifyGate(Task):
    """
    Finish when we can see a good enough amount of the gate
    """
    min_bbar_width = 0.15

    def on_first_run(self, vision):
        self.seen_cons_check = ConsistencyCheck(4, 5)

    def on_run(self, vision):
        self.seen_cons_check.add(vision.bottom is not None and \
                # (vision.left is not None or vision.right is not None) and \
                                 bbar_width_ratio(vision) >= self.min_bbar_width)
        if self.seen_cons_check.check():
            self.finish()

class align(Task):
    def update_data(self):
        self.pipe_results = shm.pipe_results.get()

    def on_first_run(self):
        self.update_data()

        self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5)
        self.alignment_checker = ConsistencyCheck(19, 20)

        pipe_found = self.pipe_results.heuristic_score > 0

        self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
                                     target=lambda self=self: get_camera_center(self.pipe_results),
                                     deadband=(20,20), px=0.003, py=0.003, dx=0.001, dy=0.001,
                                     valid=pipe_found)

        self.logi("Beginning to align to the pipe's heading")

    def on_run(self):
        self.update_data()

        self.align()
        self.center()

        if self.alignment_checker.check(self.align.finished):
            self.finish()

class IdentifyBins(Task):
    """Identifies which bin to drop markers into, centers over it

    Start: Both bins visible in downward cam
    Finish: Centered over chosen bin
    """
    def on_first_run(self, bin_group, heading=None, *args, **kwargs):
        self.logi("Centering over bins...")
        self.logv("Starting IdentifyBins task")
        self.task = DownwardTarget(px=0.0025, py=0.0025)
        self.align_checker = ConsistencyCheck(6, 6)
        # TODO start alignment task.
        self.init_time = self.this_run_time

        self.bin_group = bin_group

        if bin1.covered == FAST_RUN:
            self.target_bin = bin1
        else:
            self.target_bin = bin2

    def on_run(self, bin_group, heading=None):
        self.bin1_results = self.bin_group.get()
        target = get_camera_center(self.bin1_results)

        # TODO Increase deadband as time increases.
        self.task((self.bin1_results.x, self.bin1_results.y), target=target, deadband=(25, 25), valid=lambda: self.bin1_results.probability > 0.0)

        if self.task.finished:
            if heading is None:
              target_heading = shm.kalman.heading.get() + self.bin1_results.angle
            else:
              target_heading = heading()

            align_task = Heading(target_heading, deadband=0.5)
            align_task()
            if self.align_checker.check(align_task.finished):
                VelocityX(0)()
                VelocityY(0)()
                self.finish()
        else:
            self.align_checker.clear()

    def on_finish(self):
        self.logi("Centered!")
        self.logv('IdentifyBins task finished in {} seconds!'.format(
            self.this_run_time - self.init_time))

class CheckBinsInSight(Task):
    """ Checks if the desired bin is in sight of the camera
    Used in SearchBinsTask as MasterConcurrent's end condition"""
    def on_first_run(self, *args, **kwargs):
        self.logv("Checking if bins in sight")
        self.init_time = self.this_run_time
        self.seen_checker1 = ConsistencyCheck(6, 8)
        self.seen_checker2 = ConsistencyCheck(6, 8)

    def on_run(self):
        cover_results = cover.get()
        yellow_results = yellow1.get()
        if self.seen_checker1.check(cover_results.probability > 0.0) or \
           self.seen_checker2.check(yellow_results.probability > 0.0):
            self.finish()

    def on_finish(self):
        self.logv('CheckBinsInSight task finished in {} seconds!'.format(
            self.this_run_time - self.init_time))

class SearchFor(Task):
    def on_first_run(self, search_task, visible, consistent_frames=(3, 5)):
        self.found_checker = ConsistencyCheck(*consistent_frames)

    def on_run(self, search_task, visible, consistent_frames=(3, 5)):
        visible = call_if_function(visible)
        if self.found_checker.check(visible):
            self.finish()
        else:
            search_task()

class Buoy(Task):
    """
    Wrapper around the BuoyRam class that will specifically ram a red or green
    buoy
    """
    def __init__(self, buoy, *args, **kwargs):
        super().__init__(*args, **kwargs)
        # Instantiate the BuoyRam task
        self.buoy = buoy
        self.align_task = AlignTarget(self.location_validator,
                AssumeBuoy(self.location_validator),
                (self.buoy.center_x.get, self.buoy.center_y.get), self.buoy)
        self.ram_task = BuoyRam(self.location_validator,
                (self.buoy.center_x.get, self.buoy.center_y.get), self.buoy,
                self.collision_validator, self.align_task)
        self.seen_frames_checker = ConsistencyCheck(5, 5)
        self.last_percent_frame = 0
        self.PERCENT_FRAME_THRESHOLD = 10
        self.PERCENT_FRAME_DELTA_THRESHOLD = 10
        self.TIMEOUT = 100

    def on_first_run(self):
        self.logv("Starting {} task".format(self.__class__.__name__))

    def on_run(self):
        # Perform BuoyRam task
        if self.this_run_time - self.first_run_time > self.TIMEOUT:
            self.finish()
            self.loge("Buoy ({}) timed out!".format(self.buoy))
            return
        self.ram_task()
        if self.ram_task.finished:
            self.finish()

    def on_finish(self):
        self.logv("Buoy ({}) task finished in {} seconds!".format(
            self.buoy, self.this_run_time - self.first_run_time))
        Zero()()

    def location_validator(self):
        # TODO even more robust location validator
        return self.seen_frames_checker.check(self.buoy.probability.get() != 0)

    def collision_validator(self):
        # TODO even more robust collision validator
        if not shm.gpio.wall_1.get():
            self.logi("Detected buoy ram using touch sensor!")
            return True

        current = self.buoy.percent_frame.get()
        if current >= self.PERCENT_FRAME_THRESHOLD:
            if abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD:
                return True
            self.last_percent_frame = current
        return False

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.locator_task = LocateBuoy(self.location_validator)
        self.locator_task = AssumeBuoy(self.location_validator)
        self.align_task = AlignTarget(self.location_validator,
                self.locator_task, (yellow_buoy_results.top_x.get,
            yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)
        self.concurrent_align_task = AlignTarget(self.location_validator,
                self.locator_task, (yellow_buoy_results.top_x.get,
            yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)

        self.MAX_RAM_SPEED = 0.6
        self.ram_speed = self.MAX_RAM_SPEED
        self.ram_task = RamTarget(self.location_validator,
                self.collision_validator, self.locator_task,
                self.concurrent_align_task, ram_speed=lambda: self.ram_speed)

        self.ready_to_ram_checker = ConsistencyCheck(40, 40)
        self.seen_frames_checker = ConsistencyCheck(5, 5)

        SCUTTLE_TIME = 2.0
        BUOY_MOUNTING_DISTANCE = 0.55
        self.scuttle = Sequential(Log("Rising to prepare for scuttle"),
                                  RelativeToInitialDepth(-0.2, error=0.02),
                                  Log("Moving on top of Yellow Buoy"),
                                  MoveX(BUOY_MOUNTING_DISTANCE),
                                  Log("Beginning %0.2f second scuttle!" % SCUTTLE_TIME),
                                  MasterConcurrent(Timer(SCUTTLE_TIME), VelocityX(1.0), RelativeToInitialDepth(3.0, error=0.03)),
                                  Log("Scuttle complete. Returning to targeting position."),
                                  Depth(1.0)
                                  # GoToPosition(lambda: self.target_position[0], lambda: self.target_position[1], depth=lambda: self.target_depth)
                                  )

        self.tasks = Sequential(self.locator_task, self.align_task,
                                self.ram_task, self.scuttle)

        #self.last_percent_frame = 0
        #self.PERCENT_FRAME_DELTA_THRESHOLD = 10

        self.PERCENT_FRAME_THRESHOLD = 2
        self.PERCENT_FRAME_SLOWDOWN_THRESHOLD = 0.4
        self.TIMEOUT = 100

class ConsistentTask(Task):
    """
    Checks to make sure a non-finite task consistently is finished
    """
    def on_first_run(self, task, success=18, total=20, *args, **kwargs):
        self.cons_check = ConsistencyCheck(success, total)

    def on_run(self, task, *args, **kwargs):
        task()
        if self.cons_check.check(task.finished):
            self.finish()

    def on_first_run(self, altitude, *args, **kwargs):
        self.min_speed = 0.008
        self.min_delta = 0.1
        self.deque_size = 20

        self.success = False
        self.altitude_task = Altitude(altitude, p=0.3)
        self.stop_cons_check = ConsistencyCheck(3, 3)
        self.readings = deque()
        self.initial_altitude = shm.dvl.savg_altitude.get()
        self.last_altitude = self.initial_altitude

class align(Task):
    def on_first_run(self):
        self.align = PIDLoop(output_function=RelativeToCurrentHeading(), target=0, input_value=lambda: shm.pipe_results.angle.get(), negate=True, deadband=1)
        self.alignment_checker = ConsistencyCheck(19, 20)

        self.logi("Beginning to align to the pipe's heading")

    def on_run(self):
        self.align()

        if self.alignment_checker.check(self.align.finished):
            self.finish()

    def on_first_run(self):
        self.update_data()

        pipe_found = self.pipe_results.heuristic_score > 0

        self.centered_checker = ConsistencyCheck(18, 20)

        self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
                                     target=lambda self=self: get_camera_center(self.pipe_results),
                                     deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001,
                                     valid=pipe_found)
        self.logi("Beginning to center on the pipe")

    def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None):
        self.logi("Centering over bins...")
        self.logv("Starting IdentifyBins task")

        self.center_valid = False
        self.center_coords = (0, 0)

        self.task = DownwardTarget(px=0.0025, py=0.0025,
                                   point=lambda: self.center_coords,
                                   valid=lambda: self.center_valid)

        self.center_checker = ConsistencyCheck(15, 17)
        self.align_checker = ConsistencyCheck(6, 6)
        # TODO start alignment task.
        self.init_time = self.this_run_time

        self.uncovered_bin_vector = None
        self.seen_two = False

        cam = get_downward_camera()
        self.cover_tracker = Tracker(cam['width'] * 0.15)
        self.yellow_tracker = Tracker(cam['width'] * 0.15)

    def on_first_run(self, bin_group, heading=None, *args, **kwargs):
        self.logi("Centering over bins...")
        self.logv("Starting IdentifyBins task")
        self.task = DownwardTarget(px=0.0025, py=0.0025)
        self.align_checker = ConsistencyCheck(6, 6)
        # TODO start alignment task.
        self.init_time = self.this_run_time

        self.bin_group = bin_group

        if bin1.covered == FAST_RUN:
            self.target_bin = bin1
        else:
            self.target_bin = bin2

class AlignTarget(Task):
    """
    Aligns using ForwardTarget on a target coordinate, while ensuring that the
    target is visible
    """
    def __init__(self, validator, locator_task, target_coords, vision_group, forward_target_p=0.003, *args, **kwargs):
        """
        validator - a function that returns True when the target is visible and False
            otherwise.
        locator_task - a task that locates the target
        target_coords - the coordinates of the target with which to align
        """
        super().__init__(*args, **kwargs)
        self.validator = validator
        self.locator_task = locator_task
        def get_center():
            return get_forward_camera_center()

        self.target_task = ForwardTarget(target_coords, target=get_center,
                px=forward_target_p, dx=forward_target_p/3,
                py=forward_target_p, dy=forward_target_p/3, deadband=(30, 30))
        self.target_checker = ConsistencyCheck(10, 10)

        self.TIMEOUT = 60

    def on_first_run(self):
        self.logv("Starting {} task".format(self.__class__.__name__))

    def on_run(self):
        if self.this_run_time - self.first_run_time > self.TIMEOUT:
            self.finish()
            self.loge("{} timed out!".format(self.__class__.__name__))
            return
        self.logv("Running {}".format(self.__class__.__name__))
        if self.validator():
            self.target_task()
        else:
            self.locator_task()
        if self.target_checker.check(self.target_task.finished):
            self.finish()

    def on_finish(self):
        self.logv('{} task finished in {} seconds!'.format(
            self.__class__.__name__,
            self.this_run_time - self.first_run_time))

class AltitudeUntilStop(Task):
    """
    Attempt to move to the target altitude until the sub gets stuck
    """
    def on_first_run(self, altitude, *args, **kwargs):
        self.min_speed = 0.008
        self.min_delta = 0.1
        self.deque_size = 20

        self.success = False
        self.altitude_task = Altitude(altitude, p=0.3)
        self.stop_cons_check = ConsistencyCheck(3, 3)
        self.readings = deque()
        self.initial_altitude = shm.dvl.savg_altitude.get()
        self.last_altitude = self.initial_altitude

    def on_run(self, altitude, *args, **kwargs):
        if not self.altitude_task.has_ever_finished:
            self.altitude_task()
            current_altitude = shm.dvl.savg_altitude.get()
            self.readings.append((current_altitude, time.time()))
            if len(self.readings) > self.deque_size:
                self.readings.popleft()

            if abs(current_altitude - self.initial_altitude) >= self.min_delta and \
                    len(self.readings) >= self.deque_size:
                delta_altitude = self.readings[-1][0] - self.readings[0][0]
                delta_time = self.readings[-1][1] - self.readings[0][1]
                speed = abs(delta_altitude / delta_time)

                if self.stop_cons_check.check(speed < self.min_speed):
                    self.logi('Stopped changing altitude, finishing')
                    self.success = True
                    self.finish()
        else:
            self.loge('Bounding altitude reached')
            self.finish()

class center(Task):
    def update_data(self):
        self.pipe_results = shm.pipe_results.get()

    def on_first_run(self):
        self.update_data()

        pipe_found = self.pipe_results.heuristic_score > 0

        self.centered_checker = ConsistencyCheck(18, 20)

        self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
                                     target=get_downward_camera_center,
                                     deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001,
                                     valid=pipe_found)
        self.logi("Beginning to center on the pipe")

    def on_run(self):
        self.update_data()
        self.center()

        if self.centered_checker.check(self.center.finished):
            self.center.stop()
            self.finish()

 def __init__(self, seen_cons_check=(2, 3), unseen_cons_check=(5, 6)):
     self.last_obj = None
     self.tracking = False
     self.seen_cons_check = ConsistencyCheck(*seen_cons_check)
     self.unseen_cons_check = ConsistencyCheck(*unseen_cons_check)

class ScuttleYellowBuoy(Task):
    """
    Locates and scuttles a yellow buoy by dragging it down.

    Precondition: The yellow buoy is located at a position in front of the
    position of the submarine.

    Postcondition: The submarine will have dragged down the yellow buoy, and the
    submarine will be positioned above the yellow buoy.
    """
    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.locator_task = LocateBuoy(self.location_validator)
        self.locator_task = AssumeBuoy(self.location_validator)
        self.align_task = AlignTarget(self.location_validator,
                self.locator_task, (yellow_buoy_results.top_x.get,
            yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)
        self.concurrent_align_task = AlignTarget(self.location_validator,
                self.locator_task, (yellow_buoy_results.top_x.get,
            yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)

        self.MAX_RAM_SPEED = 0.6
        self.ram_speed = self.MAX_RAM_SPEED
        self.ram_task = RamTarget(self.location_validator,
                self.collision_validator, self.locator_task,
                self.concurrent_align_task, ram_speed=lambda: self.ram_speed)

        self.ready_to_ram_checker = ConsistencyCheck(40, 40)
        self.seen_frames_checker = ConsistencyCheck(5, 5)

        SCUTTLE_TIME = 2.0
        BUOY_MOUNTING_DISTANCE = 0.55
        self.scuttle = Sequential(Log("Rising to prepare for scuttle"),
                                  RelativeToInitialDepth(-0.2, error=0.02),
                                  Log("Moving on top of Yellow Buoy"),
                                  MoveX(BUOY_MOUNTING_DISTANCE),
                                  Log("Beginning %0.2f second scuttle!" % SCUTTLE_TIME),
                                  MasterConcurrent(Timer(SCUTTLE_TIME), VelocityX(1.0), RelativeToInitialDepth(3.0, error=0.03)),
                                  Log("Scuttle complete. Returning to targeting position."),
                                  Depth(1.0)
                                  # GoToPosition(lambda: self.target_position[0], lambda: self.target_position[1], depth=lambda: self.target_depth)
                                  )

        self.tasks = Sequential(self.locator_task, self.align_task,
                                self.ram_task, self.scuttle)

        #self.last_percent_frame = 0
        #self.PERCENT_FRAME_DELTA_THRESHOLD = 10

        self.PERCENT_FRAME_THRESHOLD = 2
        self.PERCENT_FRAME_SLOWDOWN_THRESHOLD = 0.4
        self.TIMEOUT = 100

    def on_first_run(self):
        self.logv("Starting {} task".format(self.__class__.__name__))

    def on_run(self):
        # Locate the yellow buoy
        # Align with the yellow buoy
        # Move forward until collision with the buoy (using RamTarget)
        # Descend to drag buoy downwards
        if self.this_run_time - self.first_run_time > self.TIMEOUT:
            self.finish()
            self.loge("{} timed out!".format(self.__class__.__name__))
            return
        self.tasks()
        if self.tasks.finished:
            self.finish()

    def on_finish(self):
        self.logv('{} task finished in {} seconds!'.format(
            self.__class__.__name__,
            self.this_run_time - self.first_run_time))
        Zero()()

    def location_validator(self):
        # TODO even more robust location validator
        return self.seen_frames_checker.check(yellow_buoy_results.probability.get() != 0)

    def collision_validator(self):
        # TODO even more robust collision validator
        current = yellow_buoy_results.percent_frame.get()

        aligned = self.concurrent_align_task.finished
        max_ram_speed = self.MAX_RAM_SPEED
        if not aligned:
          max_ram_speed /= 3

        self.ram_speed = scaled_speed(final_value=self.PERCENT_FRAME_THRESHOLD + 1,
                                      initial_value=self.PERCENT_FRAME_SLOWDOWN_THRESHOLD,
                                      final_speed=0.0, initial_speed=max_ram_speed,
                                      current_value=current)

        if self.ready_to_ram_checker.check(current >= self.PERCENT_FRAME_THRESHOLD and \
                                           aligned):
            self.target_position = get_sub_position()
            self.target_depth = shm.kalman.depth.get()
            self.logi("Close enough to yellow buoy to scuttle!")
            return True
            #if yellow_buoy_results.center_y.get() > (shm.camera.forward_height.get() - 10) \
#.........这里部分代码省略.........

class IdentifyBins(Task):
    """ Identifies which bin to drop markers into, centers over it """
    def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None):
        self.logi("Centering over bins...")
        self.logv("Starting IdentifyBins task")

        self.center_valid = False
        self.center_coords = (0, 0)

        self.task = DownwardTarget(px=0.0025, py=0.0025,
                                   point=lambda: self.center_coords,
                                   valid=lambda: self.center_valid)

        self.center_checker = ConsistencyCheck(15, 17)
        self.align_checker = ConsistencyCheck(6, 6)
        # TODO start alignment task.
        self.init_time = self.this_run_time

        self.uncovered_bin_vector = None
        self.seen_two = False

        cam = get_downward_camera()
        self.cover_tracker = Tracker(cam['width'] * 0.15)
        self.yellow_tracker = Tracker(cam['width'] * 0.15)

    def on_run(self, run_type, heading=None, uncovered_bin_vector=None):
        yellows = [TrackedBin(b.center_x, b.center_y) if b.probability > 0.0 else None for b in [yellow1.get(), yellow2.get()] ]
        cover_g = cover.get()
        covert = TrackedBin(cover_g.center_x, cover_g.center_y) if cover_g.probability > 0.0 else None

        self.consistent_bins = self.yellow_tracker.track(*yellows)
        self.consistent_cover = self.cover_tracker.track(covert, None)

        def calculate_bin_vector(bin1, bin2):
          body_frame = [(bin1.x, bin1.y), (bin2.x, bin2.y)]
          world_frame = [np.array(rotate(body, -shm.kalman.heading.get())) for body in body_frame]
          bin2bin = world_frame[1] - world_frame[0]
          return bin2bin / np.linalg.norm(bin2bin)

        if any(self.consistent_cover) and any(self.consistent_bins):
          if run_type == "cover":
            good_cover = self.consistent_cover[0]
            if good_cover is None:
              good_cover = self.consistent_cover[1]
            good_yellow = self.consistent_bins[0]
            if good_yellow is None:
              good_yellow = self.consistent_bins[1]

            bin2cover_hat = calculate_bin_vector(good_yellow, good_cover)

            if self.uncovered_bin_vector is None:
              # TODO Take average here.
              self.uncovered_bin_vector = bin2cover_hat
              self.logi("Discovered cover to bin world vector %0.2f %0.2f" % \
                        (self.uncovered_bin_vector[0], self.uncovered_bin_vector[1]))

        if run_type == "cover":
          cands = self.consistent_cover + self.consistent_bins
        else:
          if all(self.consistent_bins) and uncovered_bin_vector is not None:
            bin2bin = calculate_bin_vector(self.consistent_bins[0], self.consistent_bins[1])
            if bin2bin.dot(uncovered_bin_vector()) > 0:
              index = 1
            else:
              index = 0

            if not self.seen_two:
              self.seen_two = True
              self.uncovered_ind = index
              self.logi("Chose bin with index %d: current coords %d %d" % \
                        (self.uncovered_ind, self.consistent_bins[self.uncovered_ind].x, self.consistent_bins[self.uncovered_ind].y))
            else:
              if self.uncovered_ind == index:
                self.logv("Confirmed uncovered bin has index %d" % index)
              else:
                self.logi("WARNING: Detected new uncovered bin index %d!" % index)

          if not self.seen_two:
            self.logv("Did not find two yellow!")
            cands = self.consistent_bins + self.consistent_cover
          else:
            cands = [self.consistent_bins[self.uncovered_ind], self.consistent_bins[1 - self.uncovered_ind]] + self.consistent_cover

        for i, cand in enumerate(cands):
          if cand is not None:
            self.logv("Found good bin of index %d" % i)
            self.center_valid = True
            self.center_coords = cand.x, cand.y
            break
        else:
          self.logv("No good bins found to center on!")
          self.center_valid = False

        # Assumes cover and contours from same camera
        target = get_downward_camera_center()
        # TODO Increase deadband as time increases.
        self.task(target=target, deadband=(25, 25))

        if self.center_checker.check(self.task.finished):
          if run_type == "cover" or heading is None:
#.........这里部分代码省略.........

 def on_first_run(self, search_task, visible, consistent_frames=(3, 5)):
     self.found_checker = ConsistencyCheck(*consistent_frames)