// Path: java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java
// public class PoseChange 
// {    
//     
//     /**
//     * Creates a new PoseChange object with specified velocities.
//     */
//     public PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds)
//     {
//         this.dxy_mm = dxy_mm;
//         this.dtheta_degrees = dtheta_degrees;
//         this.dt_seconds = dtSeconds;
//     }
// 
//     /**
//     * Creates a new PoseChange object with zero velocities.
//     */
//     public PoseChange()
//     {
//         this.dxy_mm = 0;
//         this.dtheta_degrees = 0;
//         this.dt_seconds = 0;
//     }
// 
//     /**
//     * Updates this PoseChange object.
//     * @param dxy_mm new forward distance traveled in millimeters
//     * @param dtheta_degrees new angular rotation in degrees
//     * @param dtSeconds time in seconds since last velocities
//     */
//     public void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
//     {
//         double velocity_factor = (dtSeconds > 0) ?  (1 / dtSeconds) : 0;
//         
//         this.dxy_mm = dxy_mm * velocity_factor;
//         
//         this.dtheta_degrees = dtheta_degrees * velocity_factor;
//     }
// 
//     /**
//       * Returns a string representation of this PoseChange object.
//       */
//     public String toString()
//     {
//         return String.format("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
//             this.dxy_mm, this.dtheta_degrees, this.dt_seconds);
//     }
// 
//     /**
//       * Returns the forward component of this PoseChange object.
//       */
//     public double getDxyMm()
//     {
//         return this.dxy_mm;
//     }
// 
//     /**
//       * Returns the angular component of this PoseChange object.
//       */
//      public double getDthetaDegrees()
//     {
//         return this.dtheta_degrees;
//     }
// 
//     /**
//       * Returns the time component of this PoseChange object.
//       */
//     public double getDtSeconds()
//     {
//         return this.dt_seconds;
//     }
// 
//     /**
//       * Forward component of velocity, in mm to be divided by time in seconds.
//       */
//     protected double dxy_mm;
// 
//     /**
//       * Angular component of velocity, in mm to be divided by time in seconds.
//       */
// 
//      protected double dtheta_degrees;
// 
//     /**
//       *  Time in seconds between successive velocity measurements.
//       */
//      protected double dt_seconds;
// }


import edu.wlu.cs.levy.breezyslam.components.PoseChange;
     * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses       
     * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
     * @return poseChange object representing poseChange for these odometry values
     */

    public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
    {
        WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry);

        double dxy_mm = 0;
        double dtheta_degrees = 0;
        double dt_seconds = 0;

        if (this.timestamp_seconds_prev > 0)
        {             
            double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev;
            double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev;

            dxy_mm =  this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees));

            dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees);

            dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev;
        }

        // Store current odometry for next time
        this.timestamp_seconds_prev = odometry.timestamp_seconds;
        this.left_wheel_degrees_prev = odometry.left_wheel_degrees;
        this.right_wheel_degrees_prev = odometry.right_wheel_degrees;      

// Path: java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java
// public class PoseChange 
// {    
//     
//     /**
//     * Creates a new PoseChange object with specified velocities.
//     */
//     public PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds)
//     {
//         this.dxy_mm = dxy_mm;
//         this.dtheta_degrees = dtheta_degrees;
//         this.dt_seconds = dtSeconds;
//     }
// 
//     /**
//     * Creates a new PoseChange object with zero velocities.
//     */
//     public PoseChange()
//     {
//         this.dxy_mm = 0;
//         this.dtheta_degrees = 0;
//         this.dt_seconds = 0;
//     }
// 
//     /**
//     * Updates this PoseChange object.
//     * @param dxy_mm new forward distance traveled in millimeters
//     * @param dtheta_degrees new angular rotation in degrees
//     * @param dtSeconds time in seconds since last velocities
//     */
//     public void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
//     {
//         double velocity_factor = (dtSeconds > 0) ?  (1 / dtSeconds) : 0;
//         
//         this.dxy_mm = dxy_mm * velocity_factor;
//         
//         this.dtheta_degrees = dtheta_degrees * velocity_factor;
//     }
// 
//     /**
//       * Returns a string representation of this PoseChange object.
//       */
//     public String toString()
//     {
//         return String.format("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
//             this.dxy_mm, this.dtheta_degrees, this.dt_seconds);
//     }
// 
//     /**
//       * Returns the forward component of this PoseChange object.
//       */
//     public double getDxyMm()
//     {
//         return this.dxy_mm;
//     }
// 
//     /**
//       * Returns the angular component of this PoseChange object.
//       */
//      public double getDthetaDegrees()
//     {
//         return this.dtheta_degrees;
//     }
// 
//     /**
//       * Returns the time component of this PoseChange object.
//       */
//     public double getDtSeconds()
//     {
//         return this.dt_seconds;
//     }
// 
//     /**
//       * Forward component of velocity, in mm to be divided by time in seconds.
//       */
//     protected double dxy_mm;
// 
//     /**
//       * Angular component of velocity, in mm to be divided by time in seconds.
//       */
// 
//      protected double dtheta_degrees;
// 
//     /**
//       *  Time in seconds between successive velocity measurements.
//       */
//      protected double dt_seconds;
// }


// Path: java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java
import edu.wlu.cs.levy.breezyslam.components.PoseChange;

     * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses       
     * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
     * @return poseChange object representing poseChange for these odometry values
     */

    public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
    {
        WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry);

        double dxy_mm = 0;
        double dtheta_degrees = 0;
        double dt_seconds = 0;

        if (this.timestamp_seconds_prev > 0)
        {             
            double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev;
            double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev;

            dxy_mm =  this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees));

            dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees);

            dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev;
        }

        // Store current odometry for next time
        this.timestamp_seconds_prev = odometry.timestamp_seconds;
        this.left_wheel_degrees_prev = odometry.left_wheel_degrees;
        this.right_wheel_degrees_prev = odometry.right_wheel_degrees;      

        return new PoseChange(dxy_mm, dtheta_degrees, dt_seconds);