package com.thegongoliers.pathFollowing;

import com.thegongoliers.annotations.TestedBy;
import com.thegongoliers.math.PathWaypoint;
import edu.wpi.first.wpilibj.command.CommandGroup;

import java.util.List;

@TestedBy(team = "5112", year = "2018")
public class FollowPathCommand extends CommandGroup {

    public FollowPathCommand(Path path){
        generatePath(path);
    }

    public FollowPathCommand(SmartDriveTrainSubsystem drivetrain, List<PathWaypoint> waypoints){
        Path path = new Path(drivetrain);
        double x = 0, y = 0, angle = 90;
        for (PathWaypoint waypoint: waypoints){
            double angleToPosition = calculateAngle(x, y, waypoint.getX(), waypoint.getY());
            double rotation = angleToPosition - angle;
            angle += rotation;
            path.addRotation(rotation);

            double distance = calculateDistance(x, y, waypoint.getX(), waypoint.getY());
            path.addStraightAway(distance);

            double finalAngle = waypoint.getHeading() - angle;
            angle += finalAngle;
            path.addRotation(finalAngle);
        }
        generatePath(path);
    }

    private void generatePath(Path path){
        for (PathTaskCommand command: path){
            addSequential(command);
        }
    }

    /**
     * Calculates the distance between two points.
     * @param x1 The starting x position.
     * @param y1 The starting y position.
     * @param x2 The ending x position.
     * @param y2 The ending y position.
     * @return The distance between the two points.
     */
    static double calculateDistance(double x1, double y1, double x2, double y2){
        return Math.sqrt(Math.pow(y2 - y1, 2) + Math.pow(x2 - x1, 2));
    }

    /**
     * Calculates the angle between two points in degrees.
     * @param x1 The starting x position.
     * @param y1 The starting y position.
     * @param x2 The ending x position.
     * @param y2 The ending y position.
     * @return The angle between the two points in degrees.
     */
    static double calculateAngle(double x1, double y1, double x2, double y2){
        return Math.toDegrees(Math.atan2(y2 - y1, x2 - x1));
    }

}

@@ -305,79 +305,396 @@
     
-public FollowPathCommand(Path path)%7B%0A        generatePath(path);%0A    %7D%0A
+/**%0A     * Creates a command to follow a path.%0A     * @param path The path to follow.%0A     */%0A    public FollowPathCommand(Path path)%7B%0A        generatePath(path);%0A    %7D%0A%0A    /**%0A     * Creates a command to follow a path.%0A     * @param drivetrain The robot drivetrain.%0A     * @param waypoints The waypoints in the path (assumes the robot starts at (0, 0) and is facing 90 degrees).%0A     */
 %0A   
@@ -910,32 +910,80 @@
 nt: waypoints)%7B%0A
+            // Rotate to face the next waypoint%0A
             doub
@@ -1182,24 +1182,66 @@
 rotation);%0A%0A
+            // Drive to the next waypoint%0A
             
@@ -1361,16 +1361,140 @@
 tance);%0A
+            x = waypoint.getX();%0A            y = waypoint.getY();%0A%0A            // Face the correct direction of the waypoint
 %0A       

Fix bug in follow path command where x and y weren't updated