Waypoint.h

#pragma once
 
#include <frc/geometry/Translation2d.h>
#include <frc/geometry/Rotation2d.h>
#include <wpi/json.h>
#include <optional>
#include "pathplanner/lib/util/FlippingUtil.h"
 
namespace pathplanner {
 
#define AUTO_CONTROL_DISTANCE_FACTOR 0.4
 
class Waypoint {
public:
    std::optional<frc::Translation2d> prevControl;
    frc::Translation2d anchor;
    std::optional<frc::Translation2d> nextControl;
 
    constexpr Waypoint(std::optional<frc::Translation2d> prevControl,
            frc::Translation2d anchor,
            std::optional<frc::Translation2d> nextControl) : prevControl(
            prevControl), anchor(anchor), nextControl(nextControl) {
    }
 
    Waypoint flip() const;
 
    static Waypoint autoControlPoints(frc::Translation2d anchor,
            frc::Rotation2d heading,
            std::optional<frc::Translation2d> prevAnchor,
            std::optional<frc::Translation2d> nextAnchor);
 
    static Waypoint fromJson(const wpi::json &waypointJson);
};
}