#include <unistd.h>

#include <chrono>
#include <cmath>
#include <cstdio>
#include <cstring>

#include "aos/actions/actions.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "aos/vision/events/udp.h"
#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/input/action_joystick_input.h"
#include "frc971/input/driver_station_data.h"
#include "frc971/input/drivetrain_input.h"
#include "frc971/input/joystick_input.h"
#include "y2019/camera_log_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/target_selector_generated.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
#include "y2019/vision.pb.h"

using aos::events::ProtoTXUdpSocket;
using frc971::CreateProfileParameters;
using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::drivetrain::LocalizerControl;
using frc971::input::driver_station::ButtonLocation;
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
using y2019::control_loops::superstructure::CreateSuctionGoal;
using y2019::control_loops::superstructure::SuctionGoal;

namespace chrono = ::std::chrono;

namespace y2019 {
namespace input {
namespace joysticks {

namespace superstructure = y2019::control_loops::superstructure;

using google::protobuf::StringPrintf;

struct ElevatorWristPosition {
  double elevator;
  double wrist;
};

const ButtonLocation kSuctionBall(4, 2);
const ButtonLocation kSuctionHatch(3, 15);
const ButtonLocation kDeployStilt(4, 1);
const ButtonLocation kHalfStilt(4, 3);
const ButtonLocation kFallOver(3, 16);

const ButtonLocation kRocketForwardLower(5, 1);
const ButtonLocation kRocketForwardMiddle(5, 2);
const ButtonLocation kRocketForwardUpper(5, 4);
const ButtonLocation kCargoForward(5, 3);

const POVLocation kRocketBackwardUnpressed(5, -1);
const POVLocation kRocketBackwardLower(5, 180);
const POVLocation kRocketBackwardMiddle(5, 90);
const POVLocation kRocketBackwardUpper(5, 0);
const POVLocation kCargoBackward(5, 270);

const ButtonLocation kPanelSwitch(5, 7);
const ButtonLocation kCargoSwitch(5, 8);

const ButtonLocation kBallHPIntakeForward(5, 6);
const ButtonLocation kBallHPIntakeBackward(5, 5);
const JoystickAxis kBallOutake(5, 3);
const JoystickAxis kBallIntake(5, 4);

const ButtonLocation kPanelHPIntakeForward(5, 6);
const ButtonLocation kPanelHPIntakeBackward(5, 5);

const ButtonLocation kRelease(2, 4);
// Reuse quickturn for the cancel button.
const ButtonLocation kCancelAutoMode(2, 3);
const ButtonLocation kReleaseButtonBoard(3, 4);
const ButtonLocation kResetLocalizerLeftForwards(3, 10);
const ButtonLocation kResetLocalizerLeftBackwards(3, 9);

const ButtonLocation kResetLocalizerRightForwards(3, 8);
const ButtonLocation kResetLocalizerRightBackwards(3, 7);

const ButtonLocation kResetLocalizerLeft(3, 11);
const ButtonLocation kResetLocalizerRight(3, 13);

const ButtonLocation kNearCargoHint(3, 3);
const ButtonLocation kMidCargoHint(3, 5);
const ButtonLocation kFarCargoHint(3, 6);

const JoystickAxis kCargoSelectorY(5, 6);
const JoystickAxis kCargoSelectorX(5, 5);

const ButtonLocation kCameraLog(3, 14);

const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};

const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};

const ElevatorWristPosition kPanelForwardLowerPos{0.0, M_PI / 2.0};
const ElevatorWristPosition kPanelBackwardLowerPos{0.0, -M_PI / 2.0};

const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0};
const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0};

const ElevatorWristPosition kPanelForwardUpperPos{1.51, M_PI / 2.0};
const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0};

const ElevatorWristPosition kPanelCargoForwardPos{0.0, M_PI / 2.0};
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};

const ElevatorWristPosition kBallForwardLowerPos{0.21, 1.27};
const ElevatorWristPosition kBallBackwardLowerPos{0.43, -1.99};

const ElevatorWristPosition kBallForwardMiddlePos{0.925, 1.21};
const ElevatorWristPosition kBallBackwardMiddlePos{1.19, -1.98};

const ElevatorWristPosition kBallForwardUpperPos{1.51, 0.961};
const ElevatorWristPosition kBallBackwardUpperPos{1.44, -1.217};

const ElevatorWristPosition kBallCargoForwardPos{0.59, 1.2};
const ElevatorWristPosition kBallCargoBackwardPos{0.868265, -2.1};

const ElevatorWristPosition kBallHPIntakeForwardPos{0.55, 1.097};
const ElevatorWristPosition kBallHPIntakeBackwardPos{0.89, -2.018};

const ElevatorWristPosition kBallIntakePos{0.309, 2.13};

class Reader : public ::frc971::input::ActionJoystickInput {
 public:
  Reader(::aos::EventLoop *event_loop)
      : ::frc971::input::ActionJoystickInput(
            event_loop,
            ::y2019::control_loops::drivetrain::GetDrivetrainConfig(),
            ::frc971::input::DrivetrainInputReader::InputType::kPistol,
            {.run_teleop_in_auto = true,
             .cancel_auto_button = kCancelAutoMode}),
        target_selector_hint_sender_(
            event_loop->MakeSender<
                ::y2019::control_loops::drivetrain::TargetSelectorHint>(
                "/drivetrain")),
        localizer_control_sender_(
            event_loop->MakeSender<LocalizerControl>("/drivetrain")),
        camera_log_sender_(
            event_loop->MakeSender<::y2019::CameraLog>("/camera")),
        superstructure_goal_fetcher_(
            event_loop->MakeFetcher<superstructure::Goal>("/superstructure")),
        superstructure_goal_sender_(
            event_loop->MakeSender<superstructure::Goal>("/superstructure")),
        superstructure_position_fetcher_(
            event_loop->MakeFetcher<superstructure::Position>(
                "/superstructure")),
        superstructure_status_fetcher_(
            event_loop->MakeFetcher<superstructure::Status>(
                "/superstructure")) {
    const uint16_t team = ::aos::network::GetTeamNumber();
    superstructure_goal_fetcher_.Fetch();
    if (superstructure_goal_fetcher_.get()) {
      grab_piece_ = superstructure_goal_fetcher_->has_suction()
                        ? superstructure_goal_fetcher_->suction()->grab_piece()
                        : false;
      switch_ball_ =
          superstructure_goal_fetcher_->has_suction()
              ? (superstructure_goal_fetcher_->suction()->gamepiece_mode() == 0)
              : true;
    }
    video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
        StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
  }

  void AutoEnded() override {
    AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
    grab_piece_ = true;
    switch_ball_ = false;
  }

  void HandleTeleop(
      const ::frc971::input::driver_station::Data &data) override {
    ::aos::monotonic_clock::time_point monotonic_now =
        ::aos::monotonic_clock::now();
    superstructure_position_fetcher_.Fetch();
    superstructure_status_fetcher_.Fetch();
    if (!superstructure_status_fetcher_.get() ||
        !superstructure_position_fetcher_.get()) {
      AOS_LOG(ERROR, "Got no superstructure status or position packet.\n");
      return;
    }

    CHECK(superstructure_status_fetcher_->has_stilts());

    if (!superstructure_status_fetcher_->has_piece()) {
      last_not_has_piece_ = monotonic_now;
    }

    auto main_superstructure_goal_builder =
        superstructure_goal_sender_.MakeBuilder();

    flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;

    {
      flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
          elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
              *main_superstructure_goal_builder.fbb(), 0.0,
              CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
                                      0.0, 0.0));

      flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
          intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
              *main_superstructure_goal_builder.fbb(), -1.2);

      flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
          wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
              *main_superstructure_goal_builder.fbb(), 0.0,
              CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
                                      0.0, 0.0));

      flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
          stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
              *main_superstructure_goal_builder.fbb(), 0.0,
              CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
                                      0.0, 0.0));

      flatbuffers::Offset<SuctionGoal> suction_offset =
          CreateSuctionGoal(*main_superstructure_goal_builder.fbb(), false, 0);

      superstructure::Goal::Builder superstructure_goal_builder =
          main_superstructure_goal_builder.MakeBuilder<superstructure::Goal>();

      superstructure_goal_builder.add_elevator(elevator_offset);
      superstructure_goal_builder.add_intake(intake_offset);
      superstructure_goal_builder.add_wrist(wrist_offset);
      superstructure_goal_builder.add_stilts(stilts_offset);
      superstructure_goal_builder.add_suction(suction_offset);
      superstructure_goal_builder.add_roller_voltage(0.0);

      superstructure_goal_offset = superstructure_goal_builder.Finish();
    }
    superstructure::Goal *mutable_superstructure_goal = CHECK_NOTNULL(
        GetMutableTemporaryPointer(*main_superstructure_goal_builder.fbb(),
                                   superstructure_goal_offset));

    {
      auto builder = target_selector_hint_sender_.MakeBuilder();
      control_loops::drivetrain::TargetSelectorHint::Builder
          target_selector_hint_builder =
              builder
                  .MakeBuilder<control_loops::drivetrain::TargetSelectorHint>();
      if (data.IsPressed(kNearCargoHint)) {
        target_selector_hint_builder.add_suggested_target(
            control_loops::drivetrain::SelectionHint::NEAR_SHIP);
      } else if (data.IsPressed(kMidCargoHint)) {
        target_selector_hint_builder.add_suggested_target(
            control_loops::drivetrain::SelectionHint::MID_SHIP);
      } else if (data.IsPressed(kFarCargoHint)) {
        target_selector_hint_builder.add_suggested_target(
            control_loops::drivetrain::SelectionHint::FAR_SHIP);
      } else {
        const double cargo_joy_y = data.GetAxis(kCargoSelectorY);
        const double cargo_joy_x = data.GetAxis(kCargoSelectorX);
        if (cargo_joy_y > 0.5) {
          target_selector_hint_builder.add_suggested_target(
              control_loops::drivetrain::SelectionHint::NEAR_SHIP);
        } else if (cargo_joy_y < -0.5) {
          target_selector_hint_builder.add_suggested_target(
              control_loops::drivetrain::SelectionHint::FAR_SHIP);
        } else if (::std::abs(cargo_joy_x) > 0.5) {
          target_selector_hint_builder.add_suggested_target(
              control_loops::drivetrain::SelectionHint::MID_SHIP);
        } else {
          target_selector_hint_builder.add_suggested_target(
              control_loops::drivetrain::SelectionHint::NONE);
        }
      }
      if (builder.Send(target_selector_hint_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to send target selector hint.\n");
      }
    }

    if (data.PosEdge(kResetLocalizerLeft)) {
      auto builder = localizer_control_sender_.MakeBuilder();
      // Start at the left feeder station.
      LocalizerControl::Builder localizer_control_builder =
          builder.MakeBuilder<LocalizerControl>();
      localizer_control_builder.add_x(0.6);
      localizer_control_builder.add_y(3.4);
      localizer_control_builder.add_keep_current_theta(true);

      if (builder.Send(localizer_control_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to reset localizer.\n");
      }
    }

    if (data.PosEdge(kResetLocalizerRight)) {
      auto builder = localizer_control_sender_.MakeBuilder();
      // Start at the right feeder station.
      LocalizerControl::Builder localizer_control_builder =
          builder.MakeBuilder<LocalizerControl>();
      localizer_control_builder.add_x(0.6);
      localizer_control_builder.add_y(-3.4);
      localizer_control_builder.add_keep_current_theta(true);

      if (builder.Send(localizer_control_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to reset localizer.\n");
      }
    }

    if (data.PosEdge(kResetLocalizerLeftForwards)) {
      auto builder = localizer_control_sender_.MakeBuilder();
      // Start at the left feeder station.
      LocalizerControl::Builder localizer_control_builder =
          builder.MakeBuilder<LocalizerControl>();
      localizer_control_builder.add_x(0.4);
      localizer_control_builder.add_y(3.4);
      localizer_control_builder.add_theta(0.0);

      if (builder.Send(localizer_control_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to reset localizer.\n");
      }
    }

    if (data.PosEdge(kResetLocalizerLeftBackwards)) {
      auto builder = localizer_control_sender_.MakeBuilder();
      // Start at the left feeder station.
      LocalizerControl::Builder localizer_control_builder =
          builder.MakeBuilder<LocalizerControl>();
      localizer_control_builder.add_x(0.4);
      localizer_control_builder.add_y(3.4);
      localizer_control_builder.add_theta(M_PI);

      if (builder.Send(localizer_control_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to reset localizer.\n");
      }
    }

    if (data.PosEdge(kResetLocalizerRightForwards)) {
      auto builder = localizer_control_sender_.MakeBuilder();
      // Start at the right feeder station.
      LocalizerControl::Builder localizer_control_builder =
          builder.MakeBuilder<LocalizerControl>();
      localizer_control_builder.add_x(0.4);
      localizer_control_builder.add_y(-3.4);
      localizer_control_builder.add_theta(0.0);

      if (builder.Send(localizer_control_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to reset localizer.\n");
      }
    }

    if (data.PosEdge(kResetLocalizerRightBackwards)) {
      auto builder = localizer_control_sender_.MakeBuilder();
      // Start at the right feeder station.
      LocalizerControl::Builder localizer_control_builder =
          builder.MakeBuilder<LocalizerControl>();
      localizer_control_builder.add_x(0.4);
      localizer_control_builder.add_y(-3.4);
      localizer_control_builder.add_theta(M_PI);

      if (builder.Send(localizer_control_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Failed to reset localizer.\n");
      }
    }

    if (data.PosEdge(kRelease) &&
        monotonic_now >
            last_release_button_press_ + ::std::chrono::milliseconds(500)) {
      if (superstructure_status_fetcher_->has_piece()) {
        release_mode_ = ReleaseButtonMode::kRelease;
      } else {
        release_mode_ = ReleaseButtonMode::kBallIntake;
      }
    }

    if (data.IsPressed(kRelease)) {
      last_release_button_press_ = monotonic_now;
    }

    AOS_LOG(INFO, "has_piece: %d\n",
            superstructure_status_fetcher_->has_piece());
    if (data.IsPressed(kSuctionBall)) {
      grab_piece_ = true;
    } else if (data.IsPressed(kSuctionHatch)) {
      grab_piece_ = true;
    } else if ((release_mode_ == ReleaseButtonMode::kRelease &&
                data.IsPressed(kRelease)) ||
               data.IsPressed(kReleaseButtonBoard) ||
               !superstructure_status_fetcher_->has_piece()) {
      grab_piece_ = false;
      AOS_LOG(INFO, "releasing due to other thing\n");
    }

    if (data.IsPressed(kRocketBackwardUnpressed)) {
      elevator_wrist_pos_ = kStowPos;
    }
    mutable_superstructure_goal->mutable_intake()->mutate_unsafe_goal(-1.2);
    mutable_superstructure_goal->mutate_roller_voltage(0.0);

    const bool kDoBallIntake =
        (!climbed_ && release_mode_ == ReleaseButtonMode::kBallIntake &&
         data.IsPressed(kRelease)) ||
        data.GetAxis(kBallIntake) > 0.9;
    const bool kDoBallOutake = data.GetAxis(kBallOutake) > 0.9;

    if (data.IsPressed(kPanelSwitch)) {
      switch_ball_ = false;
    } else if (data.IsPressed(kCargoSwitch)) {
      switch_ball_ = true;
    }

    if (switch_ball_) {
      if (superstructure_status_fetcher_->has_piece()) {
        mutable_superstructure_goal->mutable_wrist()
            ->mutable_profile_params()
            ->mutate_max_acceleration(20);
      }

      // Go to intake position and apply vacuum
      if (data.IsPressed(kBallHPIntakeForward)) {
        grab_piece_ = true;
        elevator_wrist_pos_ = kBallHPIntakeForwardPos;
      } else if (data.IsPressed(kBallHPIntakeBackward)) {
        grab_piece_ = true;
        elevator_wrist_pos_ = kBallHPIntakeBackwardPos;
      }

      // Go to elevator/wrist position. Overrides intake position if pressed so
      // we can re-grab the ball.
      if (data.IsPressed(kRocketForwardLower)) {
        elevator_wrist_pos_ = kBallForwardLowerPos;
      } else if (data.IsPressed(kRocketBackwardLower)) {
        elevator_wrist_pos_ = kBallBackwardLowerPos;
      } else if (data.IsPressed(kRocketForwardMiddle)) {
        elevator_wrist_pos_ = kBallForwardMiddlePos;
      } else if (data.IsPressed(kRocketBackwardMiddle)) {
        elevator_wrist_pos_ = kBallBackwardMiddlePos;
      } else if (data.IsPressed(kRocketForwardUpper)) {
        elevator_wrist_pos_ = kBallForwardUpperPos;
      } else if (data.IsPressed(kRocketBackwardUpper)) {
        elevator_wrist_pos_ = kBallBackwardUpperPos;
      } else if (data.IsPressed(kCargoForward)) {
        elevator_wrist_pos_ = kBallCargoForwardPos;
      } else if (data.IsPressed(kCargoBackward)) {
        elevator_wrist_pos_ = kBallCargoBackwardPos;
      }
    } else {
      if (data.IsPressed(kPanelHPIntakeForward)) {
        grab_piece_ = true;
        elevator_wrist_pos_ = kPanelHPIntakeForwrdPos;
      } else if (data.IsPressed(kPanelHPIntakeBackward)) {
        grab_piece_ = true;
        elevator_wrist_pos_ = kPanelHPIntakeBackwardPos;
      }

      // Go to elevator/wrist position. Overrides intake position if pressed so
      // we can re-grab the panel.
      if (data.IsPressed(kRocketForwardLower)) {
        elevator_wrist_pos_ = kPanelForwardLowerPos;
      } else if (data.IsPressed(kRocketBackwardLower)) {
        elevator_wrist_pos_ = kPanelBackwardLowerPos;
      } else if (data.IsPressed(kRocketForwardMiddle)) {
        elevator_wrist_pos_ = kPanelForwardMiddlePos;
      } else if (data.IsPressed(kRocketBackwardMiddle)) {
        elevator_wrist_pos_ = kPanelBackwardMiddlePos;
      } else if (data.IsPressed(kRocketForwardUpper)) {
        elevator_wrist_pos_ = kPanelForwardUpperPos;
      } else if (data.IsPressed(kRocketBackwardUpper)) {
        elevator_wrist_pos_ = kPanelBackwardUpperPos;
      } else if (data.IsPressed(kCargoForward)) {
        elevator_wrist_pos_ = kPanelCargoForwardPos;
      } else if (data.IsPressed(kCargoBackward)) {
        elevator_wrist_pos_ = kPanelCargoBackwardPos;
      }
    }

    if (switch_ball_) {
      if (kDoBallOutake ||
          (kDoBallIntake &&
           monotonic_now < last_not_has_piece_ + chrono::milliseconds(200))) {
        mutable_superstructure_goal->mutable_intake()->mutate_unsafe_goal(0.83);
      }

      if (kDoBallIntake && !superstructure_status_fetcher_->has_piece()) {
        elevator_wrist_pos_ = kBallIntakePos;
        mutable_superstructure_goal->mutate_roller_voltage(9.0);
        grab_piece_ = true;
      } else {
        if (kDoBallOutake) {
          mutable_superstructure_goal->mutate_roller_voltage(-6.0);
        } else {
          mutable_superstructure_goal->mutate_roller_voltage(0.0);
        }
      }
    }

    constexpr double kDeployStiltPosition = 0.5;

    if (data.IsPressed(kFallOver)) {
      mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.71);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_velocity(0.65);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_acceleration(1.25);
    } else if (data.IsPressed(kHalfStilt)) {
      was_above_ = false;
      mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.345);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_velocity(0.65);
    } else if (data.IsPressed(kDeployStilt) || was_above_) {
      mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(
          kDeployStiltPosition);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_velocity(0.65);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_acceleration(2.0);
    } else {
      mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.005);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_velocity(0.65);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_acceleration(2.0);
    }

    if (superstructure_status_fetcher_->stilts()->position() > 0.1) {
      elevator_wrist_pos_ = kClimbPos;
      climbed_ = true;
      mutable_superstructure_goal->mutable_wrist()
          ->mutable_profile_params()
          ->mutate_max_acceleration(10);
      mutable_superstructure_goal->mutable_elevator()
          ->mutable_profile_params()
          ->mutate_max_acceleration(6);
    }

    // If we've been asked to go above deploy and made it up that high, latch
    // was_above.
    if (mutable_superstructure_goal->stilts()->unsafe_goal() >
            kDeployStiltPosition &&
        superstructure_status_fetcher_->stilts()->position() >=
            kDeployStiltPosition) {
      was_above_ = true;
    } else if ((superstructure_position_fetcher_->platform_left_detect() &&
                superstructure_position_fetcher_->platform_right_detect()) &&
               !data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
      was_above_ = false;
    }

    if (superstructure_status_fetcher_->stilts()->position() >
            kDeployStiltPosition &&
        mutable_superstructure_goal->stilts()->unsafe_goal() ==
            kDeployStiltPosition) {
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_velocity(0.30);
      mutable_superstructure_goal->mutable_stilts()
          ->mutable_profile_params()
          ->mutate_max_acceleration(0.75);
    }

    if ((release_mode_ == ReleaseButtonMode::kRelease &&
         data.IsPressed(kRelease)) ||
        data.IsPressed(kReleaseButtonBoard)) {
      grab_piece_ = false;
      AOS_LOG(INFO, "Releasing due to button\n");
    }

    if (switch_ball_) {
      CHECK_NOTNULL(mutable_superstructure_goal->mutable_suction())
          ->mutate_gamepiece_mode(0);
    } else {
      CHECK_NOTNULL(mutable_superstructure_goal->mutable_suction())
          ->mutate_gamepiece_mode(1);
    }

    vision_control_.set_flip_image(elevator_wrist_pos_.wrist < 0);

    CHECK_NOTNULL(mutable_superstructure_goal->mutable_suction())
        ->mutate_grab_piece(grab_piece_);

    mutable_superstructure_goal->mutable_elevator()->mutate_unsafe_goal(
        elevator_wrist_pos_.elevator);
    mutable_superstructure_goal->mutable_wrist()->mutate_unsafe_goal(
        elevator_wrist_pos_.wrist);

    if (main_superstructure_goal_builder.Send(superstructure_goal_offset) !=
        aos::RawSender::Error::kOk) {
      AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
    }

    if (monotonic_now >
        last_vision_control_ + ::std::chrono::milliseconds(50)) {
      video_tx_->Send(vision_control_);
      last_vision_control_ = monotonic_now;
    }

    {
      auto builder = camera_log_sender_.MakeBuilder();
      (void)builder.Send(
          CreateCameraLog(*builder.fbb(), data.IsPressed(kCameraLog)));
    }
  }

 private:
  ::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
      target_selector_hint_sender_;

  ::aos::Sender<LocalizerControl> localizer_control_sender_;

  ::aos::Sender<::y2019::CameraLog> camera_log_sender_;

  ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;

  ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;

  ::aos::Fetcher<superstructure::Position> superstructure_position_fetcher_;
  ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;

  // Bool to track if we've been above the deploy position.  Once this bool is
  // set, don't let the stilts retract until we see the platform.
  bool was_above_ = false;

  // Current goals here.
  ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
  bool grab_piece_ = false;

  bool switch_ball_ = false;

  bool climbed_ = false;

  enum class ReleaseButtonMode {
    kBallIntake,
    kRelease,
  };

  ReleaseButtonMode release_mode_ = ReleaseButtonMode::kRelease;
  aos::monotonic_clock::time_point last_release_button_press_ =
      aos::monotonic_clock::min_time;

  VisionControl vision_control_;
  ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
  ::aos::monotonic_clock::time_point last_vision_control_ =
      ::aos::monotonic_clock::time_point::min();

  // Time at which we last did not have a game piece.
  ::aos::monotonic_clock::time_point last_not_has_piece_ =
      ::aos::monotonic_clock::time_point::min();
};

}  // namespace joysticks
}  // namespace input
}  // namespace y2019

int main(int argc, char **argv) {
  ::aos::InitGoogle(&argc, &argv);

  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
      aos::configuration::ReadConfig("aos_config.json");

  ::aos::ShmEventLoop event_loop(&config.message());
  ::y2019::input::joysticks::Reader reader(&event_loop);

  event_loop.Run();

  return 0;
}