#include <math.h></math.h>

#include <string.h></string.h>

#include <helianthus.h></helianthus.h>

#define ROAD_SEGMENTS  128

#define SEGMENTS_WIDTH 25

typedef struct {

  double x, y;

} Point;

typedef struct {

  double m00, m01, m10, m11;

} Matrix;

typedef struct {

  Point p0;

  union {

    Matrix mat;

    struct { Point dir, norm; };

  };

  Matrix matInv;

} Segment;

typedef struct {

  double radius;

  Point pos;

  double angle;

  Point velocity;

  double angVel;

  double force;

  int collision;

  Point norm;

} Wheel;

typedef struct {

  Wheel w0, w1;

  double len;

  Point center;

  double angle;

} Car;

Car car;

int roadSegments;

double roadk0, roadk1;

Segment road[ROAD_SEGMENTS];

void matrixInvert(const Matrix *src, Matrix *dst) {

  double kd = src->m00*src->m11 - src->m01*src->m10;

  kd = fabs(kd) > 1e-10 ? 1/kd : 0;

  dst->m00 =  kd*src->m11;

  dst->m01 = -kd*src->m01;

  dst->m10 = -kd*src->m10;

  dst->m11 =  kd*src->m00;

}

void matrixMultPoint(const Matrix *m, const Point *src, Point *dst) {

  dst->x = src->x*m->m00 + src->y*m->m10;

  dst->y = src->x*m->m01 + src->y*m->m11;

}

void segmentInit(Segment *s, const Point *p0, const Point *p1) {

  s->p0.x = p0->x;

  s->p0.y = p0->y;

  s->dir.x = p1->x - p0->x;

  s->dir.y = p1->y - p0->y;

  double kl = sqrt(s->dir.x*s->dir.x + s->dir.y*s->dir.y);

  kl = kl > 1e-10 ? 1/kl : 0;

  s->norm.x =  kl*s->dir.y;

  s->norm.y = -kl*s->dir.x;

  matrixInvert(&s->mat, &s->matInv);

}

void wheelCollideSegment(Wheel *wheel, Segment *s) {

  Point pc, p = { wheel->pos.x - s->p0.x, wheel->pos.y - s->p0.y };

  matrixMultPoint(&s->matInv, &p, &pc);

  if (pc.x >= 0 && pc.x <= 1 && pc.y < wheel->radius && pc.y > -wheel->radius) {

    pc.y = wheel->radius;

    matrixMultPoint(&s->mat, &pc, &p);

    ++wheel->collision;

    wheel->pos.x = p.x + s->p0.x;

    wheel->pos.y = p.y + s->p0.y;

    wheel->norm.x = s->norm.x;

    wheel->norm.y = s->norm.y;

    return;

  }

  double kd = p.x*p.x + p.y*p.y;

  if (kd > 1e-10 && kd < wheel->radius*wheel->radius) {

    kd = 1/sqrt(kd);

    ++wheel->collision;

    wheel->pos.x = p.x*kd*wheel->radius + s->p0.x;

    wheel->pos.y = p.y*kd*wheel->radius + s->p0.y;

    wheel->norm.x = p.x*kd;

    wheel->norm.y = p.y*kd;

    return;

  }

}

void wheelUpdate(Wheel *wheel, double dt) {

  wheel->pos.x += wheel->velocity.x * dt;

  wheel->pos.y += wheel->velocity.y * dt;

  wheel->angle += wheel->angVel * dt;

  wheel->collision = 0;

  for(int i = 0; i < roadSegments; ++i)

    wheelCollideSegment(wheel, &road[i]);

  wheel->velocity.y += 200*dt;

  if (wheel->collision) {

    double dx = -wheel->norm.y;

    double dy =  wheel->norm.x;

    double v = wheel->velocity.x*dx  + wheel->velocity.y*dy;

    v += wheel->force*dt;

    wheel->velocity.x = v*dx;

    wheel->velocity.y = v*dy;

    wheel->angVel = v/wheel->radius;

  } else {

    wheel->angVel += wheel->force/wheel->radius*dt;

    double maxAngVel = 100;

    if (wheel->angVel >  maxAngVel) wheel->angVel =  maxAngVel; else

    if (wheel->angVel < -maxAngVel) wheel->angVel = -maxAngVel;

  }

}

void wheelDraw(Wheel *wheel) {

  saveState();

  strokeWidth(0.1*wheel->radius);

  stroke(COLOR_BLACK);

  fill(COLOR_GREEN);

  circle(wheel->pos.x, wheel->pos.y, wheel->radius);

  circle(wheel->pos.x + 0.5*wheel->radius*cos(wheel->angle),

         wheel->pos.y + 0.5*wheel->radius*sin(wheel->angle), wheel->radius*0.25);

  restoreState();

}

void carUpdate(Car *car, double dt) {

  Point c = { (car->w0.pos.x + car->w1.pos.x)/2,

              (car->w0.pos.y + car->w1.pos.y)/2 };

  car->center.x = c.x;

  car->center.y = c.y;

  Point d = { car->w1.pos.x - car->w0.pos.x,

              car->w1.pos.y - car->w0.pos.y };

  double kl = d.x*d.x + d.y*d.y;

  kl = kl > 1e-10 ? 1/sqrt(kl) : 0;

  Point n = { d.x*kl, d.y*kl };

  car->angle = atan2(n.y, n.x);

  car->w0.pos.x = c.x - n.x*car->len/2;

  car->w0.pos.y = c.y - n.y*car->len/2;

  car->w1.pos.x = c.x + n.x*car->len/2;

  car->w1.pos.y = c.y + n.y*car->len/2;

  Point np = { -n.y, n.x };

  Point cv = { (car->w0.velocity.x + car->w1.velocity.x)/2,

               (car->w0.velocity.y + car->w1.velocity.y)/2 };

  double v0 = (car->w0.velocity.x - cv.x)*np.x

            + (car->w0.velocity.y - cv.y)*np.y;

  car->w0.velocity.x = cv.x + v0*np.x;

  car->w0.velocity.y = cv.y + v0*np.y;

  double v1 = (car->w1.velocity.x - cv.x)*np.x

            + (car->w1.velocity.y - cv.y)*np.y;

  car->w1.velocity.x = cv.x + v1*np.x;

  car->w1.velocity.y = cv.y + v1*np.y;

  wheelUpdate(&car->w0, dt);

  wheelUpdate(&car->w1, dt);

}

void carDraw(Car *car) {

  saveState();

  translate(car->center.x, car->center.y);

  rotate(car->angle/PI*180);

  strokeWidth(0.02*car->len);

  stroke(COLOR_BLACK);

  fill(COLOR_BLUE);

  double l = car->len;

  moveTo(0, 0);

  lineTo( l*0.75, 0);

  lineTo( l*0.75, -l*0.25);

  lineTo( l*0.50, -l*0.25);

  lineTo( l*0.25, -l*0.50);

  lineTo(-l*0.25, -l*0.50);

  lineTo(-l*0.50, -l*0.25);

  lineTo(-l*0.75, -l*0.25);

  lineTo(-l*0.75, 0);

  closePath();

  restoreState();

  wheelDraw(&car->w0);

  wheelDraw(&car->w1);

}

void roadAddSegemnt(double segmentWidth) {

  if (!roadSegments) {

    Point p0 = { 0, 0 };

    Point p1 = { 0, segmentWidth };

    segmentInit(&road[0], &p0, &p1);

    ++roadSegments;

    return;

  }

  if (roadSegments >= ROAD_SEGMENTS) {

    memmove(road, road + 1, sizeof(*road)*(ROAD_SEGMENTS - 1));

    --roadSegments;

  }

  double ky = 0;

  for(int i = 0; i < 5; ++i) {

    roadk0 += (randomFloat()*2 - 1 - roadk0)*0.05;

    roadk1 += (roadk0 - roadk1)*0.25;

    ky += roadk1;

  }

  for(int i = 0; i < roadSegments; ++i)

    road[i].p0.x -= segmentWidth;

  car.w0.pos.x -= segmentWidth;

  car.w1.pos.x -= segmentWidth;

  Segment *prev = &road[roadSegments-1];

  Point p0 = { prev->p0.x + prev->dir.x,

               prev->p0.y + prev->dir.y };

  Point p1 = { p0.x + segmentWidth,

               p0.y + segmentWidth*0.5*ky };

  segmentInit(&road[roadSegments], &p0, &p1);

  ++roadSegments;

}

void init() {

  for(int i = 0; i < ROAD_SEGMENTS; ++i)

    roadAddSegemnt(SEGMENTS_WIDTH);

  car.w0.pos.x  = car.w1.pos.x  = road[ROAD_SEGMENTS/2].p0.x;

  car.w0.pos.y  = car.w1.pos.y  = road[ROAD_SEGMENTS/2].p0.y - 100;

  car.w0.radius = car.w1.radius = 20;

  car.w1.pos.x += 1;

  car.len = 100;

}

void draw() {

  double dt = windowGetFrameTime();

  double w = windowGetWidth();

  double h = windowGetHeight();

  double fk = 100;

  double force = 0;

  if (keyDown("left"))  force -= 1000;

  if (keyDown("right")) force += 1000;

  car.w0.force = car.w1.force = force;

  if (car.w0.angVel > 1e-10 && car.w0.force > 0) car.w0.force *= 1/(fabs(car.w0.angVel)/fk + 1);

  if (car.w0.angVel < 1e-10 && car.w0.force < 0) car.w0.force *= 1/(fabs(car.w0.angVel)/fk + 1);

  if (car.w1.angVel > 1e-10 && car.w1.force > 0) car.w1.force *= 1/(fabs(car.w1.angVel)/fk + 1);

  if (car.w1.angVel < 1e-10 && car.w1.force < 0) car.w1.force *= 1/(fabs(car.w1.angVel)/fk + 1);

  while(car.w0.pos.x - road[0].p0.x > 512 && road[roadSegments-1].p0.x - car.w0.pos.x < w)

    roadAddSegemnt(SEGMENTS_WIDTH);

  carUpdate(&car, dt);

  saveState();

  translate(w/2, h/2);

  translate(-car.w0.pos.x, -car.w0.pos.y);

  saveState();

  moveTo(road[0].p0.x, road[0].p0.y);

  for(int i = 1; i < roadSegments; ++i)

    lineTo(road[i].p0.x, road[i].p0.y);

  strokePath();

  restoreState();

  carDraw(&car);

  restoreState();

}

int main() {

  windowSetResizable(TRUE);

  windowSetVariableFrameRate();

  windowSetInit(&init);

  windowSetDraw(&draw);

  windowRun();

  return 0;

}