/*
  This code is hereby released to the public domain.
  ~aaaaaa123456789, 2020-04-08, last updated 2020-06-06
*/

#include <stdlib.h>
#include <math.h>

struct point {
  short x, y;
};

// points, count: vertices
// position: relative position within the line, between 0 and 1
// speeding_distance: distance required to go from no speed to max and vice-versa
struct point compute_point (const struct point * points, unsigned count, double position, double speeding_distance) {
  if (!(points && count)) return (struct point) {.x = 0, .y = 0};
  if (!-- count) return *points;
  if (position <= 0) return *points;
  if (position >= 1) return points[count];
  if (speeding_distance < 0.01) speeding_distance = 0.01;
  double acceleration = 4 / speeding_distance;
  double * segment_length = malloc(sizeof *segment_length * count);
  unsigned p;
  for (p = 0; p < count; p ++)
    segment_length[p] = sqrt((points[p + 1].x - points[p].x) * (points[p + 1].x - points[p].x) +
                             (points[p + 1].y - points[p].y) * (points[p + 1].y - points[p].y));
  double * corner_speed = malloc(sizeof *corner_speed * (count + 1));
  *corner_speed = corner_speed[count] = 0;
  // speed at each corner: 1 - cos(angle). Easy to calculate using a dot product, without any actual trigonometry.
  for (p = 1; p < count; p ++)
    corner_speed[p] = 1 - ((points[p + 1].x - points[p].x) * (points[p - 1].x - points[p].x) +
                           (points[p + 1].y - points[p].y) * (points[p - 1].y - points[p].y)) /
                          (segment_length[p] * segment_length[p - 1]);
  double * segment_time = malloc(sizeof *segment_time * count);
  unsigned char * segment_type = malloc(sizeof *segment_type * count);
  double total_time = 0;
  double initial, final, peak, time, length; // work with local variables for simplicity
  for (p = 0; p < count; p ++) {
    initial = corner_speed[p];
    final = corner_speed[p + 1];
    length = segment_length[p];
    peak = sqrt((initial * initial + final * final) / 2 + length * acceleration);
    if (peak >= 2) {
      // speed capped at 2; the peak value is just a calculation aid now
      segment_type[p] = 0;
      time = (peak * peak - 2 * (initial + final) + 4) / (acceleration * 2);
    } else if ((peak >= initial) && (peak >= final)) {
      segment_type[p] = 1;
      time = (peak * 2 - initial - final) / acceleration;
    } else if (initial <= final) {
      // not enough distance to reach the final speed, so we lower it
      segment_type[p] = 2;
      final = sqrt(initial * initial + 2 * acceleration * length);
      corner_speed[p + 1] = final;
      time = length * 2 / (initial + final);
    } else {
      // not enough distance to stop this time, but we must stop faster or we'll overrun the segment
      segment_type[p] = 3;
      time = length * 2 / (initial + final); // same as above
    }
    segment_time[p] = time;
    total_time += time;
  }
  position *= total_time;
  for (p = 0; (p < count) && (segment_time[p] <= position); position -= segment_time[p ++]);
  unsigned char type = 4;
  if (p < count) {
    initial = corner_speed[p];
    final = corner_speed[p + 1];
    time = segment_time[p];
    length = segment_length[p];
    type = segment_type[p];
    position /= time;
  }
  // everything's in a local variable now, so get rid of those allocated arrays
  free(corner_speed);
  free(segment_time);
  free(segment_length);
  free(segment_type);
  double first_split, second_split;
  switch (type) {
    case 0:
      // two splits: when peak speed is reached and when slowdown begins
      first_split = (2 - initial) / acceleration;
      second_split = time - (2 - final) / acceleration;
      break;
    case 1:
      // only one split
      first_split = ((final - initial) / acceleration + time) / 2;
      second_split = first_split;
      break;
    case 2:
      // no splits (the split would come after the end point); assign an arbitrary large value
      first_split = time * 16;
      second_split = first_split;
      break;
    case 3:
      // uses a different formula, so compute the position directly (and avoid recomputing later)
      position *= (2 * initial + position * (final - initial)) / (initial + final);
      break;
    default:
      // only possible if position >= 1 due to rounding errors; just stay at the line's end point
      return points[count];
  }
  if (type < 3)
    if (position <= (first_split / time))
      position *= time * (initial + time * position * acceleration / 2) / length;
    else if (position >= (second_split / time))
      position = 1 + (position - 1) * time * (final - time * acceleration * (position - 1) / 2) / length;
    else
      position = (2 * (time * position - first_split) + (4 - initial * initial) / (2 * acceleration)) / length;
  // this relies on implicit float to short conversion
  // FIXME: point wraps with large negative speeds (see https://www.youtube.com/watch?v=kpk2tdsPh0A#t=632)
  return (struct point) {
    .x = round(points[p].x * (1 - position) + points[p + 1].x * position),
    .y = round(points[p].y * (1 - position) + points[p + 1].y * position)
  };
}