y2019/actors/auto_splines.cc

#include "y2019/actors/auto_splines.h"

#include "frc971/control_loops/control_loops_generated.h"

namespace y2019 {
namespace actors {

void MaybeFlipSpline(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
    bool is_left) {

  flatbuffers::Vector<float> *spline_y =
      GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);

  if (!is_left) {
    for (size_t i = 0; i < spline_y->size(); i++) {
      spline_y->Mutate(i, -spline_y->Get(i));
    }
  }
}

// Path off of level 2 to the far side of the rocket with a panel
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToFarRocket(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(2.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(2.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(4.0);
    velocity_constraint_builder.add_start_distance(0.0);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({1.14763818102, 1.66, 3.10, 4.05,
                                           4.45, 5.11, 5.77, 6.71, 7.27, 7.19,
                                           6.57});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({1.30261224364, 1.30217320136, 1.39,
                                           1.47, 1.56346705393, 1.69, 1.81,
                                           1.97, 2.18, 2.84, 3.33});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(2);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from the far side of the rocket to the loading station to pickup
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHP(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(3.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(2.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(4.5);
    velocity_constraint_builder.add_start_distance(0.0);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>(
          {6.6, 7.511, 6.332, 4.590, 1.561, 0.179});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>(
          {3.391, 2.826, 1.384, 3.395 - 0.20, 3.429 - 0.20, 3.434 - 0.20});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from the human player station to the far side of the rocket with a panel
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToFarRocket(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(3.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(3.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(4.0);
    velocity_constraint_builder.add_start_distance(7.0);
    velocity_constraint_builder.add_end_distance(15.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({0.895115737979, 2.9155615909,
                                           5.02361983866, 6.40346237218,
                                           7.1260656844, 7.83907559509});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({3.43030859063, 3.44230565037,
                                           2.8824369646, 2.81000389973,
                                           3.08853311072, 2.6933085577});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from the far side of the rocket to close to the loading station
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToNearHP(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  // TODO(theo): Add some real constraints.
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({6.51652191988, 6.83156293562,
                                           5.74513904409, 2.2337653586,
                                           1.94766705864, 0.727526876557});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({3.2465107468, 2.88277456846,
                                           1.93458779243, 3.44064777429,
                                           3.44377880106, 3.43326367284});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from level 2 to 2nd cargo ship bay with a hatch panel
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToSecondCargoShipBay(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(2.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(10.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(1.6);
    velocity_constraint_builder.add_start_distance(4.0);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, voltage_constraint_offset,
               velocity_constraint_offset});

  constexpr double kLess = 0.06;
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>(
          {1.0, 2.53944573074, 5.75526086906, 6.52583747973 - kLess,
           7.12318661548 - kLess, 7.22595029399 - kLess});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({1.5, 1.48, 2.05178220103,
                                           2.56666687655, 1.79340280288,
                                           1.16170693058});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from 2nd cargo ship bay to loading station
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SecondCargoShipBayToHP(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(4.0);
    velocity_constraint_builder.add_start_distance(0.0);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({7.22595029399, 7.1892447864,
                                           6.5373977907, 5.55997590982,
                                           1.22953437637, 0.32521840905});
  constexpr double kYShift = 0.1;
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>(
          {1.2, 1.44543230529, 2.00646674662, 3.43762336271 - kYShift,
           3.44125430793 - kYShift, 3.4360348159 - kYShift});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from loading station to 3rd cargo ship bay with a hatch panel
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToThirdCargoShipBay(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint2_offset;

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(10.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(3.5);
    velocity_constraint_builder.add_start_distance(0.0);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint2_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint2_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint2_builder.add_value(2.0);
    velocity_constraint2_builder.add_start_distance(6.0);
    velocity_constraint2_builder.add_end_distance(10.0);
    velocity_constraint2_offset = velocity_constraint2_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {voltage_constraint_offset, velocity_constraint_offset,
               velocity_constraint2_offset});

  constexpr double kEndMove = 0.25;
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({0.75, 1.112, 5.576, 7.497 - kEndMove,
                                           7.675 - kEndMove, 7.768 - kEndMove});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>(
          {3.431, 3.434, 2.712, 2.874, 1.786, 1.168});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

// Path from 3rd cargo ship bay to near the loading station
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::ThirdCargoShipBayToNearHP(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(0.5);
    velocity_constraint_builder.add_start_distance(0.0);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({7.75823205276, 7.58356294646,
                                           5.95536035287, 2.12377989323,
                                           1.29347361128, 0.598613577531});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({1.16791407107, 1.94564064915,
                                           2.54565614767, 3.43728005786,
                                           3.43775494434, 3.43119598027});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HabToFarRocketTest(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
    bool is_left) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(2.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(2.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(1.7);
    velocity_constraint_builder.add_start_distance(0.0);
    velocity_constraint_builder.add_end_distance(0.8);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({1.14763818102, 2.53944573074,
                                           3.74586892131, 5.22352745444,
                                           6.70255737219, 7.35784750785});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({1.30261224364, 1.28295363394,
                                           1.27450357714, 2.89953366429,
                                           3.10734391012, 2.90125929705});
  MaybeFlipSpline(builder, spline_y_offset, is_left);

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHPTest(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(1.5);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(1.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(0.5);
    velocity_constraint_builder.add_start_distance(9.5);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({6.53, 7.8, 7.8, 4.0, 2.0, 0.4});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({3.47, 3.0, 1.5, 3.0, 3.4, 3.4});

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToNearRocketTest(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(1.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(1.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(11.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder velocity_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    velocity_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VELOCITY);
    velocity_constraint_builder.add_value(0.5);
    velocity_constraint_builder.add_start_distance(2.7);
    velocity_constraint_builder.add_end_distance(10.0);
    velocity_constraint_offset = velocity_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset, velocity_constraint_offset});

  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({1.5, 2.0, 3.0, 4.0, 4.5, 5.12});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({3.4, 3.4, 3.4, 3.0, 3.0, 3.43});

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
  flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
  flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;

  {
    frc971::Constraint::Builder longitudinal_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    longitudinal_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
    longitudinal_constraint_builder.add_value(1.0);
    longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder lateral_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    lateral_constraint_builder.add_constraint_type(
        frc971::ConstraintType_LATERAL_ACCELERATION);
    lateral_constraint_builder.add_value(1.0);
    lateral_constraint_offset = lateral_constraint_builder.Finish();
  }

  {
    frc971::Constraint::Builder voltage_constraint_builder =
        builder->MakeBuilder<frc971::Constraint>();
    voltage_constraint_builder.add_constraint_type(
        frc971::ConstraintType_VOLTAGE);
    voltage_constraint_builder.add_value(6.0);
    voltage_constraint_offset = voltage_constraint_builder.Finish();
  }

  flatbuffers::Offset<
      flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
      constraints_offset =
          builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
              {longitudinal_constraint_offset, lateral_constraint_offset,
               voltage_constraint_offset});

  const float startx = 0.4;
  const float starty = 3.4;
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
                                           0.6f + startx, 0.4f + startx,
                                           0.4f + startx, 1.0f + startx});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
                                           starty - 0.3f, starty - 0.7f,
                                           starty - 1.0f, starty - 1.0f});

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_constraints(constraints_offset);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
      builder->fbb()->CreateVector<float>(
          {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
  flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
      builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});

  frc971::MultiSpline::Builder multispline_builder =
      builder->MakeBuilder<frc971::MultiSpline>();

  multispline_builder.add_spline_count(1);
  multispline_builder.add_spline_x(spline_x_offset);
  multispline_builder.add_spline_y(spline_y_offset);

  return multispline_builder.Finish();
}

}  // namespace actors
}  // namespace y2019