#include "Ball.hpp"
#include <iostream>
#include <cmath>

bool bouncing = true;
double stop_bouncing;

Ball::Ball(const Point& _center, double _min_height, double _v_x, double _v_z,
        double _p, double _q) :
    Center(_center),
    surface(_center, _min_height, _p, _q),
    radius(_p),
    init_h(_center.y),
    min_height(_min_height),
    bounce_number(0.0),
    crt_time(0),
    A(_center.y),
    B(0),
    U(sqrt(2 * G_CTE * _center.y)),
    T(sqrt(2.0 * _center.y / G_CTE)),
    v_x(_v_x),
    v_z(_v_z)
{
}

void Ball::_compute_pos(double dt) {
    if (bouncing) {
        Center.y = fmax(
            min_height,
            A + B * crt_time - (G_CTE / 2) * crt_time * crt_time + min_height
        );
    } else {
        double n_time = crt_time - stop_bouncing;
        Center.y = ((radius * (1.0 - exp(-n_time)) + radius)/2.) + (radius -
                ((radius * (1- exp(-n_time)) + radius)/2.)) * sin(5. * n_time);
        v_x *= 0.99;
        v_z *= 0.99;
    }
    Center.x += dt * v_x;
    Center.z += dt * v_z;
    surface.update_center_pos(Center);
    surface.check_horizontal_plan_collision(0.0);
}

void Ball::_compute_T_n() {
    double update = (2. * U / G_CTE);
    T += update;
}
void Ball::_compute_B_n() {
    B = G_CTE * T + U;
}

void Ball::_compute_A_n() {
    A = - G_CTE * T * T / 2 - (U * T);
}

void Ball::_compute_U_n() {
    U *= 0.8;
}

void Ball::_compute_v_x(Point normal) {
    double factor = normal.x / (4*(normal.x + normal.z));
    v_x *= (0.5 + factor);
}

void Ball::_compute_v_z(Point normal) {
    double factor = normal.z / (4*(normal.x + normal.z));
    v_z *= (0.5 + factor);
}

void Ball::update_pos(double dt) {
    crt_time += dt;
    if ((bouncing) && (crt_time >= T)) {
        double old_t = T;
        double max_t;
        Point normal = surface.getNormalVector();
        bounce_number += 1;
        _compute_U_n();
        _compute_A_n();
        _compute_B_n();
        _compute_T_n();
        _compute_v_x(normal);
        _compute_v_z(normal);
        max_t = (T - old_t)/2.0 + old_t;

        if (((A + B * max_t - (G_CTE / 2) * max_t * max_t + min_height) < radius)) {
            stop_bouncing = crt_time;
            bouncing = false;
        }
    }
    _compute_pos(dt);
}

std::ostream& operator<< (std::ostream &out, Ball const& data) {
    Point center = data.getCenter();
    out << "t:" << data.access_crt_time() << ":";
    out << "T:" << data.accessT() << ":";
    out << center.x << ':';
    out << center.z << ':';
    out << center.y << ':';
    return out;
}