#include "trapezoidal.h"
// trapezoidal impl, not fuzzed

struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time) {
  float max_vel = trapezoidal->max_vel;
  float max_acc = trapezoidal->max_acc;
  float dist = trapezoidal->dist;

  struct Setpoint setpoint = {0.0, 0.0, 0.0};

  float time_accelerating = max_vel / max_acc;
  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;

	if (time < 0.0) {
		return setpoint;
	}

  if (2.0 * distance_while_accelerating > dist) {
    // triangular
    float peak_velocity_time = sqrt(dist/max_acc);
    float peak_velocity = max_acc * peak_velocity_time;

    if (time < peak_velocity_time) {
      // accelerating
      setpoint.acceleration = max_acc;
      setpoint.velocity = max_acc * time;
      setpoint.position = 0.5 * max_acc * time * time;
    } else if (time < peak_velocity_time * 2.0) {
      // slowing down
      float time_decay = time - peak_velocity_time;
      setpoint.acceleration = -max_acc;
      setpoint.velocity = peak_velocity - (time_decay * max_acc);
      setpoint.position = (0.5 * max_acc * peak_velocity_time * peak_velocity_time) // acceleration phase
        + peak_velocity * time_decay 
        - 0.5 * max_acc * time_decay * time_decay;
    } else {
      setpoint.acceleration = 0.0;
      setpoint.velocity = 0;
      setpoint.position = dist;
      setpoint.complete = true;
    }
  } else {
    // trapezoidal
    float cruise_distance = dist - 2.0 * distance_while_accelerating;
    float cruise_time = cruise_distance / max_vel;
    float total_time = 2 * time_accelerating + cruise_time;
    if (time < time_accelerating) {
      // still accelerating
      setpoint.acceleration = max_acc;
      setpoint.velocity = time * max_acc;
      setpoint.position = 0.5 * max_acc * time * time;
    } else if (time < time_accelerating + cruise_time) {
      // cruising
      setpoint.acceleration = 0.0;
      setpoint.velocity = max_vel;
      setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
    } else if (time < total_time) {
      // slowing down
      setpoint.acceleration = -max_acc;
      float time_decay = time - (time_accelerating + cruise_time);
      setpoint.velocity = max_vel - (time_decay * max_acc);
      setpoint.position = distance_while_accelerating + cruise_distance
        + (max_vel * time_decay)
        - 0.5 * max_acc * time_decay * time_decay;
    } else {
      //done
      setpoint.acceleration = 0.0;
      setpoint.velocity = 0.0;
      setpoint.position = dist;
      setpoint.complete = true;
    }
  }

  return setpoint;
}

float trapezoidal_time(struct Trapezoidal* trapezoidal) {
  float max_vel = trapezoidal->max_vel;
  float max_acc = trapezoidal->max_acc;
  float dist = trapezoidal->dist;

  float time_accelerating = max_acc / max_vel;
  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;

  if (2.0 * distance_while_accelerating > dist) {
    // triangular
    float peak_velocity_time = sqrt(dist/max_acc);
    return peak_velocity_time * 2.0;
  } else {
    // trapezoidal
    float cruise_distance = dist - 2.0 * distance_while_accelerating;
    float cruise_time = cruise_distance / max_vel;
    float total_time = 2 * time_accelerating + cruise_time;
    return total_time;
  }
}