import math

import agxSDK


Key = agxSDK.GuiEventListener


LINEAR_ACTUATOR_INDIVIDUAL_WHEEL_MOTORS = [
    (Key.KEY_Up, Key.KEY_Down, [
        ("drivetrain_wheel_vel_front_left_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_front_right_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_mid_left_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_mid_right_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_rear_left_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_rear_right_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
    ]),
    (Key.KEY_Left, Key.KEY_Right, [
        ("steering_left_vel_input", -0.1),
        ("steering_right_vel_input", 0.1),
    ]),
    (Key.KEY_Page_Up, Key.KEY_Page_Down, [
        ("cargo_bed_left_vel_input", 0.20, -0.20),
        ("cargo_bed_right_vel_input", 0.20, -0.20),
    ]),
]


LINEAR_ACTUATOR_DRIVETRAIN = [
    (Key.KEY_Up, [
        ("drivetrain_throttle_input", 0.1, 1.0, 0.0),
    ], "ramp_reset"),
    (Key.KEY_Down, [
        ("drivetrain_front_left_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_front_right_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_mid_left_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_mid_right_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_rear_left_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_rear_right_brake_engage_input", 0.01, 1.0, 0.0),
    ], "ramp_reset"),
    (Key.KEY_Left, Key.KEY_Right, [
        ("steering_left_vel_input", -0.1),
        ("steering_right_vel_input", 0.1),
    ]),
    (Key.KEY_Page_Up, Key.KEY_Page_Down, [
        ("cargo_bed_left_vel_input", 0.20),
        ("cargo_bed_right_vel_input", 0.20),
    ]),
    (ord('a'), ord('z'), [
        ("drivetrain_gear_selection_input", 1, -2, 6, 0),
    ], "step"),
    (Key.KEY_F1, [("drivetrain_front_diff_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F2, [("drivetrain_mid_diff_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F3, [("drivetrain_rear_diff_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F5, [("drivetrain_mid_center_axle_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F6, [("drivetrain_rear_center_axle_lock_input", True, False, False)], "toggle"),
]


ANGLE_CONTROL_STEERING_STEP = math.radians(0.5)
ANGLE_CONTROL_STEERING_LIMIT = math.radians(45.0)
ANGLE_CONTROL_CARGO_BED_STEP = math.radians(-0.25)
ANGLE_CONTROL_CARGO_BED_LIMIT = math.radians(-70.0)


ANGLE_CONTROL_INDIVIDUAL_WHEEL_MOTORS = [
    (Key.KEY_Up, Key.KEY_Down, [
        ("drivetrain_wheel_vel_front_left_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_front_right_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_mid_left_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_mid_right_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_rear_left_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
        ("drivetrain_wheel_vel_rear_right_input", 1.0 * (55.9 / 3.35), -1.0 * (16.0 / 3.35)),
    ]),
    (Key.KEY_Page_Up, [
        ("cargo_bed_angle_input", ANGLE_CONTROL_CARGO_BED_STEP, ANGLE_CONTROL_CARGO_BED_LIMIT),
    ], "ramp"),
    (Key.KEY_Page_Down, [
        ("cargo_bed_angle_input", -ANGLE_CONTROL_CARGO_BED_STEP, 0.0),
    ], "ramp"),
    (Key.KEY_Left, [
        ("steering_angle_input", -ANGLE_CONTROL_STEERING_STEP, -ANGLE_CONTROL_STEERING_LIMIT),
    ], "ramp"),
    (Key.KEY_Right, [
        ("steering_angle_input", ANGLE_CONTROL_STEERING_STEP, ANGLE_CONTROL_STEERING_LIMIT),
    ], "ramp"),
]


ANGLE_CONTROL_DRIVETRAIN = [
    (Key.KEY_Up, [
        ("drivetrain_throttle_input", 0.1, 1.0, 0.0),
    ], "ramp_reset"),
    (Key.KEY_Down, [
        ("drivetrain_front_left_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_front_right_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_mid_left_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_mid_right_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_rear_left_brake_engage_input", 0.01, 1.0, 0.0),
        ("drivetrain_rear_right_brake_engage_input", 0.01, 1.0, 0.0),
    ], "ramp_reset"),
    (ord('a'), ord('z'), [
        ("drivetrain_gear_selection_input", 1, -2, 6, 0),
    ], "step"),
    (Key.KEY_F1, [("drivetrain_front_diff_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F2, [("drivetrain_mid_diff_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F3, [("drivetrain_rear_diff_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F5, [("drivetrain_mid_center_axle_lock_input", True, False, False)], "toggle"),
    (Key.KEY_F6, [("drivetrain_rear_center_axle_lock_input", True, False, False)], "toggle"),
    (Key.KEY_Page_Up, [
        ("cargo_bed_angle_input", ANGLE_CONTROL_CARGO_BED_STEP, ANGLE_CONTROL_CARGO_BED_LIMIT),
    ], "ramp"),
    (Key.KEY_Page_Down, [
        ("cargo_bed_angle_input", -ANGLE_CONTROL_CARGO_BED_STEP, 0.0),
    ], "ramp"),
    (Key.KEY_Left, [
        ("steering_angle_input", -ANGLE_CONTROL_STEERING_STEP, -ANGLE_CONTROL_STEERING_LIMIT),
    ], "ramp"),
    (Key.KEY_Right, [
        ("steering_angle_input", ANGLE_CONTROL_STEERING_STEP, ANGLE_CONTROL_STEERING_LIMIT),
    ], "ramp"),
]
