#include <unistd.h>

#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 "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/profiled_subsystem_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 "frc971/input/redundant_joystick_data.h"
#include "frc971/zeroing/wrap.h"
#include "y2023_bot3/constants.h"
#include "y2023_bot3/control_loops/drivetrain/drivetrain_base.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"

using frc971::CreateProfileParameters;
using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
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 Side = frc971::control_loops::drivetrain::RobotSide;
using y2023_bot3::control_loops::superstructure::PivotGoal;
using y2023_bot3::control_loops::superstructure::RollerGoal;

namespace y2023_bot3 {
namespace input {
namespace joysticks {

namespace superstructure = y2023_bot3::control_loops::superstructure;

struct ButtonData {
  ButtonLocation button;
};

namespace {
// XBox controller
const ButtonLocation kSpit(3, 1);          // A
const ButtonLocation kSpitHigh(3, 4);      // Y
const ButtonLocation kPickup(3, 8);        // M4
const ButtonLocation kPickupBack(3, 7);    // M3
const ButtonLocation kScore(3, 9);         // M1
const ButtonLocation kScoreBack(3, 10);    // M2
const ButtonLocation kScoreMid(3, 5);      // LB
const ButtonLocation kScoreMidBack(3, 6);  // RB
}  // namespace
class Reader : public ::frc971::input::ActionJoystickInput {
 public:
  Reader(::aos::EventLoop *event_loop)
      : ::frc971::input::ActionJoystickInput(
            event_loop,
            ::y2023_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
            ::frc971::input::DrivetrainInputReader::InputType::kPistol, {}),
        superstructure_goal_sender_(
            event_loop->MakeSender<superstructure::Goal>("/superstructure")),
        superstructure_status_fetcher_(
            event_loop->MakeFetcher<superstructure::Status>(
                "/superstructure")) {}

  void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }

  bool has_scored_ = false;

  void HandleTeleop(
      const ::frc971::input::driver_station::Data &data) override {
    (void)data;
    superstructure_status_fetcher_.Fetch();
    if (!superstructure_status_fetcher_.get()) {
      AOS_LOG(ERROR, "Got no superstructure status message.\n");
      return;
    }

    std::optional<double> place_index = std::nullopt;
    (void)place_index;

    {
      auto builder = superstructure_goal_sender_.MakeBuilder();

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

      RollerGoal roller_goal = RollerGoal::IDLE;
      PivotGoal pivot_goal = PivotGoal::NEUTRAL;

      if (data.IsPressed(kSpit)) {
        roller_goal = RollerGoal::SPIT;
      } else if (data.IsPressed(kSpitHigh)) {
        roller_goal = RollerGoal::SPIT_HIGH;
      }

      if (data.IsPressed(kScore)) {
        pivot_goal = PivotGoal::SCORE_LOW_FRONT;
      } else if (data.IsPressed(kScoreBack)) {
        pivot_goal = PivotGoal::SCORE_LOW_BACK;
      } else if (data.IsPressed(kScoreMid)) {
        pivot_goal = PivotGoal::SCORE_MID_FRONT;
      } else if (data.IsPressed(kScoreMidBack)) {
        pivot_goal = PivotGoal::SCORE_MID_BACK;
      } else if (data.IsPressed(kPickup)) {
        pivot_goal = PivotGoal::PICKUP_FRONT;
        roller_goal = RollerGoal::INTAKE_CUBE;
      } else if (data.IsPressed(kPickupBack)) {
        pivot_goal = PivotGoal::PICKUP_BACK;
        roller_goal = RollerGoal::INTAKE_CUBE;
      }

      superstructure_goal_builder.add_roller_goal(roller_goal);
      superstructure_goal_builder.add_pivot_goal(pivot_goal);
      if (builder.Send(superstructure_goal_builder.Finish()) !=
          aos::RawSender::Error::kOk) {
        AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
      }
    }
  }

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

  ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
};

}  // namespace joysticks
}  // namespace input
}  // namespace y2023_bot3

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());
  ::y2023_bot3::input::joysticks::Reader reader(&event_loop);

  event_loop.Run();

  return 0;
}