class Shape {
public:
    enum Type {SPHERE, BOX} type;
    // sphere data
    float radius;
    // box data
    vec3 halfSize; // vertices are (+-halfsize[0], +-halfSize[1], +-halfSize[2])
    std::vector<vec3> collisionSamples;
    // do not use constructor, call Shape::makeSphere or Shape::makeBox
    Shape();
    static Shape makeSphere(float radius);
    static Shape makeBox(vec3 halfSize);
    mat3 moment(); // returns moment of inertia assuming unit mass
    void draw(); // draws the shape in body space
    bool collisionTest(vec3 p, float &d, vec3 &n); // checks if point in shape
    // collisionTest takes as input a point p in body space,
    // stores signed distance from the surface in d (-ve inside, +ve outside),
    // stores the outward normal in n,
    // returns true if d < 0 (point is inside shape).
};

mat3 Shape::moment() {

    // Implement this yourself!
    // Should return the matrix M such that mass*M = I_body
    
    if (type == SPHERE) {

        // TODO

    } else { // type == BOX

        // TODO

    }
}

// Pre-implemented functions

Shape::Shape():
    radius(0), halfSize(0,0,0) {
}

Shape Shape::makeSphere(float radius) {
    Shape shape;
    shape.type = SPHERE;
    shape.radius = radius;
    return shape;
}

Shape Shape::makeBox(vec3 halfSize) {
    Shape shape;
    shape.type = BOX;
    shape.halfSize = halfSize;
    vec3 o = -halfSize;
    vec3 x = vec3(2*halfSize[0],0,0);
    vec3 y = vec3(0,2*halfSize[1],0);
    vec3 z = vec3(0,0,2*halfSize[2]);
    shape.collisionSamples.push_back(o);
    shape.collisionSamples.push_back(o + x);
    shape.collisionSamples.push_back(o + y);
    shape.collisionSamples.push_back(o + z);
    shape.collisionSamples.push_back(o + x + y);
    shape.collisionSamples.push_back(o + y + z);
    shape.collisionSamples.push_back(o + z + x);
    shape.collisionSamples.push_back(o + x + y + z);
    float res = 0.1;
    int nx = ceil(2*halfSize[0]/res);
    for (int i = 1; i < nx; i++) {
        float t = (float)i/nx;
        shape.collisionSamples.push_back(o + t*x);
        shape.collisionSamples.push_back(o + t*x + y);
        shape.collisionSamples.push_back(o + t*x + z);
        shape.collisionSamples.push_back(o + t*x + y + z);
    }
    int ny = ceil(2*halfSize[1]/res);
    for (int i = 1; i < ny; i++) {
        float t = (float)i/ny;
        shape.collisionSamples.push_back(o + t*y);
        shape.collisionSamples.push_back(o + t*y + x);
        shape.collisionSamples.push_back(o + t*y + z);
        shape.collisionSamples.push_back(o + t*y + x + z);
    }
    int nz = ceil(2*halfSize[2]/res);
    for (int i = 1; i < nz; i++) {
        float t = (float)i/nz;
        shape.collisionSamples.push_back(o + t*z);
        shape.collisionSamples.push_back(o + t*z + x);
        shape.collisionSamples.push_back(o + t*z + y);
        shape.collisionSamples.push_back(o + t*z + x + y);
    }
    return shape;
}

void Shape::draw() {
    if (type == SPHERE) {
        drawSphere(vec3(0,0,0), radius);
    } else { // type == BOX
        drawBox(-halfSize, halfSize);
    }
}

inline int sgn(float x) {return (x<0) ? -1 : (x>0) ? 1 : 0;}

bool Shape::collisionTest(vec3 p, float &d, vec3 &n) {
    if (type == SPHERE) {
        d = p.norm() - radius;
        n = p.normalized();
        return (d < 0);
    } else { // type == BOX
        d = -1e6;
        if (std::abs(p[0]) - halfSize[0] > d) {
            d = std::abs(p[0]) - halfSize[0];
            n = vec3(sgn(p[0]),0,0);
        }
        if (std::abs(p[1]) - halfSize[1] > d) {
            d = std::abs(p[1]) - halfSize[1];
            n = vec3(0,sgn(p[1]),0);
        }
        if (std::abs(p[2]) - halfSize[2] > d) {
            d = std::abs(p[2]) - halfSize[2];
            n = vec3(0,0,sgn(p[2]));
        }
        if (d > 0) {
            vec3 c = vec3(std::min(std::max(p[0], -halfSize[0]), halfSize[0]),
                          std::min(std::max(p[1], -halfSize[1]), halfSize[1]),
                          std::min(std::max(p[2], -halfSize[2]), halfSize[2]));
            d = (p - c).norm();
            n = (p - c).normalized();
        }
        return (d < 0);
    }
}