---

import math

DT = 0.2
MAX_STEPS = 12000
FAR = 9.0
PIT_LIMIT = 1.35

def clamp(value, low, high):
    return max(low, min(high, value))

def distance(value):
    # -1 от датчика меняем на 9.0, чтобы не ломать сравнения
    return value if value >= 0 else FAR

def angle(value):
    # перевод угла в [-pi, pi]
    return (value + math.pi) % (2 * math.pi) - math.pi

def cliff(value):
    return value < 0 or value > PIT_LIMIT

async def read_sensors():
    names = ('front', 'front_left', 'front_right', 'left', 'right', 'pit', 'pit_front_left', 'pit_front_right')
    values = {}
    for name in names:
        values[name] = await cargo_dist(name)
    values['gyro'] = await cargo_gyro()
    return values

print('Старт')
await cargo_sleep(0.5)
reference_yaw = (await cargo_gyro())['yaw']

steer_command = 0.0
drive_command = 0.0
pitch_smooth = 0.0
slope_boost = False
previous_yaw = reference_yaw
avoid_side = 0
avoid_timer = 0
log_phase = 0
log_ticks = 0
blocked_time = 0.0
reverse_timer = 0
stuck_time = 0.0
previous_front = FAR

try:
    for step in range(MAX_STEPS):
        s = await read_sensors()
        gyro = s['gyro']
        front = distance(s['front'])
        front_left = distance(s['front_left'])
        front_right = distance(s['front_right'])
        left = distance(s['left'])
        right = distance(s['right'])
        pitch = gyro['pitch']
        pit_center = cliff(s['pit'])
        pit_left = cliff(s['pit_front_left'])
        pit_right = cliff(s['pit_front_right'])
        
        # отсекаем ложные срабатывания ямы на кочках и спусках
        on_slope = abs(pitch) > 0.13
        confirmed_cliff = not on_slope and (pit_center and (pit_left or pit_right) or (pit_left and pit_right))
        
        yaw_error = angle(reference_yaw - gyro['yaw'])
        yaw_rate = angle(gyro['yaw'] - previous_yaw) / DT
        previous_yaw = gyro['yaw']
        
        bias = 0.14 if step < 200 and pitch < -0.08 else 0.0
        
        # PD-регулятор по курсу (D = 0.35 гасит занос)
        desired_steer = clamp(0.85 * yaw_error - 0.35 * yaw_rate + bias, -0.65, 0.65)
        
        # лимиты скорости по таймингам трассы
        desired_drive = 100 if step < 165 else 78 if step < 230 else 68 if step < 310 else 82 if step < 380 else 48
        mode = 'курс'
        
        if reverse_timer > 0:
            mode = 'реверс'
            desired_steer = -0.55 * avoid_side
            desired_drive = -100 if step < 165 else -70
            reverse_timer -= 1
        elif confirmed_cliff:
            mode = 'яма'
            if pit_left and (not pit_right):
                avoid_side = 1
            elif pit_right and (not pit_left):
                avoid_side = -1
            elif avoid_side == 0:
                avoid_side = 1 if right > left else -1
            desired_steer = 0.70 * avoid_side
            if step >= 165:
                desired_drive = min(desired_drive, 60)
            avoid_timer = 8
        elif step < 175:
            avoid_side = 0
            avoid_timer = 0
            
        # проезд бревен в три стадии после 420 шага
        elif log_phase == 1 or (log_phase == 0 and step > 420 and (front < 8.0) and (front_right < 3.5)):
            mode = 'бревна_1'
            if log_phase == 0:
                log_phase, log_ticks = (1, 10)
            desired_steer = 0.58
            desired_drive = 40
            log_ticks -= 1
            if log_ticks <= 0:
                log_phase, log_ticks = (2, 14)
        elif log_phase == 2:
            mode = 'бревна_2'
            desired_steer = clamp(0.9 * yaw_error - 0.3 * yaw_rate, -0.42, 0.42)
            if front_left < 1.8:
                desired_steer = max(desired_steer, 0.2)
            elif front_right < 3.4:
                desired_steer = min(desired_steer, -0.42)
            desired_drive = 38
            log_ticks -= 1
            if log_ticks <= 0:
                log_phase = 3
        elif log_phase == 3:
            mode = 'бревна_3'
            desired_steer = clamp(0.9 * yaw_error - 0.3 * yaw_rate, -0.28, 0.28)
            desired_drive = 65
            if front < 5.5:
                desired_drive = 38
            if front < 3.0:
                desired_drive = 24
                desired_steer = 0.45 if front_right >= front_left else -0.45
            elif front_right < 2.5:
                desired_steer = min(desired_steer, -0.38)
            elif front_left < 2.5:
                desired_steer = max(desired_steer, 0.38)
        
        elif (abs(yaw_error) > 0.55 and front > 5.5 and min(front_left, front_right) > 3.2):
            avoid_timer = 0
            
        # объезд препятствий
        elif ((front < 5.5 or min(front_left, front_right) < 3.2) if step < 165 else (front < 2.0 or (step >= 380 and min(front_left, front_right) < 3.0))):
            mode = 'объезд'
            left_clear = min(front_left, left + 1.0)
            right_clear = min(front_right, right + 1.0)
            if step < 380 or avoid_timer <= 0 or avoid_side == 0:
                if left_clear < 1.3 and right_clear > left_clear + 0.5:
                    avoid_side = 1
                elif right_clear < 1.3 and left_clear > right_clear + 0.5:
                    avoid_side = -1
                elif abs(left_clear - right_clear) < 0.8 and abs(yaw_error) > 0.15:
                    avoid_side = 1 if yaw_error > 0 else -1
                elif avoid_side == 0 or abs(left_clear - right_clear) > 1.5:
                    avoid_side = 1 if right_clear > left_clear else -1
                avoid_timer = 5  # держим маневр 5 тиков, чтобы не дергаться
            else:
                avoid_timer -= 1
            
            desired_steer = (0.65 if front < 3.0 else 0.42) * avoid_side
            
            if step >= 165:
                desired_drive = min(desired_drive, 50 if front >= 3.0 else 34)
                
        elif avoid_timer > 0:
            desired_steer *= 0.65  # плавный возврат руля после маневра
            avoid_timer -= 1
        else:
            avoid_side = 0
            
        # проверка на упор в стену и застревание
        if front < 0.85 and desired_drive > 0:
            blocked_time += DT
        else:
            blocked_time = max(0.0, blocked_time - DT)
            
        if front < 4.5 and abs(front - previous_front) < 0.08 and (abs(yaw_rate) < 0.025) and (abs(steer_command) > 0.3):
            stuck_time += DT
        else:
            stuck_time = 0.0
            
        previous_front = front
        
        if step >= 175 and (blocked_time >= 1.5 or stuck_time >= 1.5):
            reverse_timer = 6 if step >= 165 else 16
            blocked_time = 0.0
            stuck_time = 0.0
            
        # буст тяги в горку
        pitch_smooth = 0.8 * pitch_smooth + 0.2 * pitch
        if pitch_smooth < -0.06:
            slope_boost = True
        elif pitch_smooth > -0.02:
            slope_boost = False
            
        if slope_boost and step < 400 and desired_drive > 0 and not confirmed_cliff:
            desired_drive = max(desired_drive, 100 if front > 3.0 else 85)
            
        # поправка под левый столб на первых воротах
        if (175 <= step < 245 and not confirmed_cliff and
                desired_drive > 0 and pitch > -0.16 and yaw_error > -0.18 and
                front_left < 2.8 and front_right > front_left + 1.8 and
                left < 3.4):
            desired_steer = max(desired_steer, 0.16)
            desired_drive = min(desired_drive, 78)

        # сглаживание руля и газа
        steer_rate = 0.18 if step < 165 else 0.12
        steer_command += clamp(desired_steer - steer_command, -steer_rate, steer_rate)
        steer_command = clamp(steer_command, -0.8, 0.8)
        
        if desired_drive < 0:
            drive_command = desired_drive
        else:
            drive_command = clamp(drive_command + clamp(desired_drive - drive_command, -15, 20), 0, 100)
            
        cargo_steer(float(steer_command))
        cargo_drive(int(drive_command))
        
        if step % 8 == 0:
            print('шаг', step, mode, 'F', round(front, 1), 'FL', round(front_left, 1), 'FR', round(front_right, 1))
        
        await cargo_sleep(DT)

finally:
    cargo_stop()