#include <math.h>
#include <string.h>
#include <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;
}