physics.h

#pragma once

#include "precomp.h"

#include "graphics.h"
#include "kernel.h"
#include "timing.h"


using Vec2 = cpVect;


struct Physics {
  Physics(const Physics &) = delete;
  auto operator=(const Physics &) -> Physics & = delete;
  Physics(const Physics &&) = delete;
  auto operator=(const Physics &&) -> Physics & = delete;

  explicit Physics(Timing &tim_);
  ~Physics();


private:
  template<typename T, auto Free, auto GetSpace, auto RemoveFromSpace>
  struct Wrapper {
    Wrapper(const Wrapper &) = delete;
    auto operator=(const Wrapper &) -> Wrapper & = delete;

    ~Wrapper() {
      reset();
    }

    Wrapper(Wrapper &&other) noexcept
        : ptr(std::exchange(other.ptr, nullptr)) {
      if (ptr) {
        ptr->userData = this;
      }
    }

    auto operator=(Wrapper &&other) noexcept -> Wrapper & {
      if (this != &other) {
        reset(std::exchange(other.ptr, nullptr));
      }
      return *this;
    }

  private:
    friend struct Physics;

    T *ptr = nullptr;

    explicit Wrapper(T *ptr_)
        : ptr(ptr_) {
      if (ptr) {
        ptr->userData = this;
      }
    }

    auto reset(T *ptr_ = nullptr) -> void {
      if (ptr) {
        if constexpr (std::is_same_v<T, cpBody>) {
          cpBodyEachConstraint(
              ptr,
              [](cpBody *, cpConstraint *constraint, void *) {
                if (auto sp = cpConstraintGetSpace(constraint)) {
                  cpSpaceRemoveConstraint(sp, constraint);
                }
              },
              nullptr);
          cpBodyEachShape(
              ptr,
              [](cpBody *, cpShape *shape, void *) {
                if (auto sp = cpShapeGetSpace(shape)) {
                  cpSpaceRemoveShape(sp, shape);
                }
              },
              nullptr);
        }
        if (auto sp = GetSpace(ptr)) {
          RemoveFromSpace(sp, ptr);
        }
        Free(ptr);
      }
      ptr = ptr_;
      if (ptr) {
        ptr->userData = this;
      }
    }
  };

public:
  struct Body : Wrapper<cpBody, cpBodyFree, cpBodyGetSpace, cpSpaceRemoveBody> {
    using Wrapper::Wrapper;

    enum class Type {
      Dynamic,
      Kinematic,
      Static,
    };

    [[nodiscard]] auto getType() const -> Type {
      switch (cpBodyGetType(ptr)) {
      case CP_BODY_TYPE_DYNAMIC:
        return Type::Dynamic;
      case CP_BODY_TYPE_KINEMATIC:
        return Type::Kinematic;
      case CP_BODY_TYPE_STATIC:
        return Type::Static;
      }
      return Type::Static;
    }
    auto setType(Type type) -> Body && {
      switch (type) {
      case Type::Dynamic:
        cpBodySetType(ptr, CP_BODY_TYPE_DYNAMIC);
        break;
      case Type::Kinematic:
        cpBodySetType(ptr, CP_BODY_TYPE_KINEMATIC);
        break;
      case Type::Static:
        cpBodySetType(ptr, CP_BODY_TYPE_STATIC);
        break;
      }
      return std::move(*this);
    }

    [[nodiscard]] auto getMass() const -> double {
      return cpBodyGetMass(ptr);
    }
    auto setMass(double mass) -> Body && {
      cpBodySetMass(ptr, mass);
      return std::move(*this);
    }

    [[nodiscard]] auto getMoment() const -> double {
      return cpBodyGetMoment(ptr);
    }
    auto setMoment(double moment) -> Body && {
      cpBodySetMoment(ptr, moment);
      return std::move(*this);
    }

    [[nodiscard]] auto getPosition() const -> Vec2 {
      return cpBodyGetPosition(ptr);
    }
    auto setPosition(Vec2 position) -> Body && {
      cpBodySetPosition(ptr, position);
      return std::move(*this);
    }

    [[nodiscard]] auto getVelocity() const -> Vec2 {
      return cpBodyGetVelocity(ptr);
    }
    auto setVelocity(Vec2 velocity) -> Body && {
      cpBodySetVelocity(ptr, velocity);
      return std::move(*this);
    }

    [[nodiscard]] auto toWorld(Vec2 point) const -> Vec2 {
      return cpBodyLocalToWorld(ptr, point);
    }
    [[nodiscard]] auto toLocal(Vec2 point) const -> Vec2 {
      return cpBodyWorldToLocal(ptr, point);
    }

    [[nodiscard]] auto getEntity() const -> Entity {
      return ent;
    }
    auto setEntity(Entity ent_) -> Body && {
      ent = ent_;
      return std::move(*this);
    }

    auto draw(Graphics &gfx) const -> void;

  private:
    Entity ent = Kernel::null;
  };

  struct Constraint
      : Wrapper<cpConstraint, cpConstraintFree, cpConstraintGetSpace, cpSpaceRemoveConstraint> {
    using Wrapper::Wrapper;

    [[nodiscard]] auto getMaxBias() const -> double {
      return cpConstraintGetMaxBias(ptr);
    }
    auto setMaxBias(double maxBias) -> Constraint && {
      cpConstraintSetMaxBias(ptr, maxBias);
      return std::move(*this);
    }

    [[nodiscard]] auto getMaxForce() const -> double {
      return cpConstraintGetMaxForce(ptr);
    }
    auto setMaxForce(double maxForce) -> Constraint && {
      cpConstraintSetMaxForce(ptr, maxForce);
      return std::move(*this);
    }
  };

  struct Shape : Wrapper<cpShape, cpShapeFree, cpShapeGetSpace, cpSpaceRemoveShape> {
    using Wrapper::Wrapper;

    [[nodiscard]] auto getRadius() const -> double {
      switch (ptr->klass->type) {
      case CP_CIRCLE_SHAPE:
        return cpCircleShapeGetRadius(ptr);
      case CP_SEGMENT_SHAPE:
        return cpSegmentShapeGetRadius(ptr);
      case CP_POLY_SHAPE:
        return cpPolyShapeGetRadius(ptr);
      default:
        return 0;
      }
    }
    auto setRadius(double radius) -> Shape && {
      switch (ptr->klass->type) {
      case CP_CIRCLE_SHAPE:
        cpCircleShapeSetRadius(ptr, radius);
        break;
      case CP_SEGMENT_SHAPE:
        cpSegmentShapeSetRadius(ptr, radius);
        break;
      case CP_POLY_SHAPE:
        cpPolyShapeSetRadius(ptr, radius);
        break;
      default:
        break;
      }
      return std::move(*this);
    }

    [[nodiscard]] auto getNumVertices() const -> int {
      return cpPolyShapeGetCount(ptr);
    }
    [[nodiscard]] auto getVertex(int index) const -> Vec2 {
      return cpPolyShapeGetVert(ptr, index);
    }

    auto getBody() -> Body & {
      return *static_cast<Body *>(cpBodyGetUserData(cpShapeGetBody(ptr)));
    }
    [[nodiscard]] auto getBody() const -> const Body & {
      return *static_cast<Body *>(cpBodyGetUserData(cpShapeGetBody(ptr)));
    }

    struct BoundingBox {
      double minX, minY, maxX, maxY;
    };
    [[nodiscard]] auto getBoundingBox() const -> BoundingBox {
      auto bb = cpShapeGetBB(ptr);
      return BoundingBox { bb.l, bb.b, bb.r, bb.t };
    }
  };


  [[nodiscard]] auto createDynamic(double mass, double moment) -> Body {
    return wrap(cpBodyNew(mass, moment));
  }

  [[nodiscard]] auto createStatic() -> Body {
    return wrap(cpBodyNewStatic());
  }

  [[nodiscard]] auto createPivot(Body &a, Body &b, Vec2 anchorA, Vec2 anchorB) -> Constraint {
    return wrap(cpPivotJointNew2(a.ptr, b.ptr, anchorA, anchorB));
  }

  [[nodiscard]] auto createCircle(Body &body, double radius, Vec2 offset = { 0, 0 }) -> Shape {
    return wrap(cpCircleShapeNew(body.ptr, radius, offset));
  }

  [[nodiscard]] auto createBox(Body &body, double width, double height, double radius = 0)
      -> Shape {
    return wrap(cpBoxShapeNew(body.ptr, width, height, radius));
  }

  [[nodiscard]] auto createPoly(Body &body, const std::vector<Vec2> &verts) -> Shape {
    return wrap(cpPolyShapeNew(body.ptr, int(verts.size()), verts.data(), { 1, 0, 0, 1, 0, 0 }, 0));
  }


  [[nodiscard]] auto getBackground() -> Body & {
    return background;
  }


  template<typename F>
  auto segmentQuery(Vec2 start, Vec2 end, double radius, F &&f) -> void {
    cpSpaceSegmentQuery(
        space, start, end, radius, CP_SHAPE_FILTER_ALL,
        [](cpShape *shape, cpVect point, cpVect normal, double alpha, void *data) {
          auto f = *static_cast<F *>(data);
          f(*static_cast<Shape *>(cpShapeGetUserData(shape)), point, normal, alpha);
        },
        &f);
  }


  auto reindex(Body &body) -> void {
    cpSpaceReindexShapesForBody(space, body.ptr);
  }


  auto frame() -> void {
    tim.prof("PhysicsFrame", [&]() {
      cpSpaceStep(space, tim.dt());
    });
  }


private:
  Timing &tim;

  cpSpace *space;

  Body background;


  [[nodiscard]] auto wrap(cpBody *ptr) -> Body {
    return Body(cpSpaceAddBody(space, ptr));
  }

  [[nodiscard]] auto wrap(cpConstraint *ptr) -> Constraint {
    return Constraint(cpSpaceAddConstraint(space, ptr));
  }

  [[nodiscard]] auto wrap(cpShape *ptr) -> Shape {
    return Shape(cpSpaceAddShape(space, ptr));
  }
};