/**
 * Implementation of a spheroid
 **/

#include "spheroid.hpp"
#include <iostream>

Spheroid::Spheroid(const Point& _center, double _min_p, double _p, double _q) :
    ImplicitSurface(_center), min_p(_min_p),
    init_p(_p), p(_p), q(_q), stiffness(0)
{
    _compute_volume();
}

void Spheroid::_compute_volume() {
    V = (4./3.) * PI * p * q * q;
}



void Spheroid::update_radius() {
    q = sqrt((3./4.) * V / PI / p);
}


void Spheroid::update_center_pos(Point& _center) {
    center = _center;
}

void Spheroid::check_ground_collision(const Ground* ground) {
    double height = (*ground)(center.x, center.z);
    if (((center.y -p) <= height) || (p < init_p)) {
        p = fmin(init_p, center.y - height);
        update_radius();
    }
}


Cuboid Spheroid::max_bounding_box() const {
    double max_radius = sqrt((3./4.) * V / PI / min_p);
    double max_height = init_p;
    Point _bd1(max_radius, max_height, max_radius);
    Point _bd2(-max_radius, -max_height, -max_radius);
    return Cuboid(_bd1, _bd2);
}


double Spheroid::operator() (double _x, double _y, double _z) const {
            return (pow(_x, 2) / pow(q, 2)
                  + pow(_y, 2) / pow(p, 2)
                  + pow(_z, 2) / pow(q, 2) -1);
}

Point Spheroid::location_hint() const {
    return Point(0, p, 0);
}