#include "Phys.h"
#include "Primitives.h"

void cleanPhys(struct Phys *phys) {
  cleanVector(&phys->position);
  cleanVector(&phys->velocity);
  cleanVector(&phys->direction);
  cleanVector(&phys->acceleration);
  phys->mass = 0.0f;
  phys->inverse_mass = 0.0f;
  phys->radius = 0.0f;
}

int doPhys(struct Phys *phys) {

  return 0;
}

int turnToVector(struct Phys *phys, struct Vector vector, float speed) {
  Vector dir = normVector(subVector(phys->position, vector));
  phys->direction = normVector(subVector(phys->direction, mulVector(dir, speed)));
  return 0;
}

int faceVector(struct Phys *phys, struct Vector vector) {
  Vector dir = normVector(subVector(phys->position, vector));
  phys->direction = dir;
  return 0;
}

int addForce(struct Phys *phys, float force) {
  phys->acceleration = mulVector(phys->direction, force);
  phys->acceleration = divVector(phys->acceleration, phys->mass);
  phys->acceleration = addVector(phys->acceleration, mulVector(phys->velocity, -0.20));
  return 0;
}

int doVelocity(struct Phys *phys) {
  phys->position = addVector(phys->position, phys->velocity);
  phys->acceleration = mulVector(phys->acceleration, 0.5);
  phys->velocity = addVector(phys->velocity, phys->acceleration);
  cleanVector(&phys->acceleration);
  return 0;
}

int collPhysBox(struct Phys *phys, struct Box *box) {
  int cx, cy;
  if (phys->position.x < box->x) {
    cx = box->x;
  } else if (phys->position.x > box->x + box->w) {
    cx = box->x + box->w;
  } else {
    cx = phys->position.x;
  }
  if (phys->position.y < box->y) {
    cy = box->y;
  } else if (phys->position.y > box->y + box->h) {
    cy = box->y + box->h;
  } else {
    cy = phys->position.y;
  }
  int delta_x = phys->position.x - cx;
  int delta_y = phys->position.y - cy;
  double distance = delta_x * delta_x + delta_y * delta_y;
  if (distance < phys->radius * phys->radius) {
    return 1;
  }
  return 0;
}
int collPhysCircle(struct Phys *phys, struct Circle *circle) {
  double rad_sqr = phys->radius + circle->radius;
  rad_sqr *= rad_sqr;
  double delta_x = phys->position.x - circle->x;
  double delta_y = phys->position.y - circle->y;
  double distance = delta_x * delta_x + delta_y * delta_y;
  if (distance < rad_sqr) {
    return 1;
  }
  return 0;
}