
#include "cBall.h"
#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <vector>

using namespace std;

cBall::cBall()
{
    randomize();
    launchPower_ = 10;
    launchAngle_ = 0;
}

void cBall::randomize()
{    
    numUpdatesMovingSlow_ = 0;
    
    lastPosition_ = cVector(1000000,1000000,1000000);
    float a = ((float)(rand()%100))/100.0f;
    float b = ((float)(rand()%100))/100.0f;
    float c = ((float)(rand()%100))/100.0f;
    float d = ((float)(rand()%100))/100.0f;
    float e = ((float)(rand()%100))/100.0f;
    float f = ((float)(rand()%100))/100.0f;
    
    //position_ = cVector( (a-.5)*50, 50 + (b-.5)*6, (c-.5)*50);
    position_ = cVector( 0, 50 , 0);
    velocity_ = cVector((d-.5)*50, 0, (f-.5)*50);
    
    r_  = a;
    g_ = b;
    b_ = c;
    radius_ = 2*b + .3; 

}

cBall::~cBall() { /* Nothing to do */ }


void cBall::update(float dt, cStaticTriangleMesh& staticObjects)
{
    //cout << position_ << endl;
    if(position_.y_ < -50) randomize();
    
    if(dt == 0 ) return; //nothing to do
    
    cIntersectionResult closestIntersection(false, 1); // no intersection yet
    cVector normal(0,0,0);               // the normal at the closest point of intersection
        
    cVector g(0, -50, 0);
    cVector endPos = position_ + velocity_ * dt;// * dt + .5 * dt * dt * g;
    cVector endVelocity = velocity_ + dt * g;
    
    // build lists of triangles, edges and vertices that could collide with the ball
    vector<cTriangle*> tlist;
    vector<cEdge*> elist;
    vector<cVertex*> vlist;
    staticObjects.buildCollisionLists(position_, endPos, radius_, tlist,  vlist, elist);
    
    float bouciness = 1;
    // modifies parameters
    collideVertices(vlist, position_, endPos, closestIntersection, normal, bouciness);
    collideEdges(elist, position_, endPos, closestIntersection, normal, bouciness);
    collideFaces(tlist, position_, endPos, closestIntersection, normal, bouciness);
    
    
    // there was no intersection with anything
    if(closestIntersection.intersected_ == false)
    {
        numUpdatesMovingSlow_ = 0;
        position_ = endPos;
        velocity_ = endVelocity;
        //sliding_ = false;
        return;
    }
    
    if(normal.x_ == 0 && normal.y_ == 0 && normal.z_ == 0)
    {
        cout << "error, got 0 normal" << endl;
        return;
    }
    
    normal.normalize();
    launchNormal_ = (normal + 50 * launchNormal_) / 51; ;
    
    // if beta is more than betaFudge, move beta - betaFudge
    // since beta is the miniumum possible, there can be no collision on 
    // this path. otherwise, dont move at all
    //float betaFudge = .05;
    //if(closestIntersection.beta_ > betaFudge)
    //    position_ += (closestIntersection.beta_-betaFudge) *  (endPos - position_);    
    float normalFudge = .0001;
    position_ += closestIntersection.beta_ *  (endPos - position_) + normalFudge * normal;
    
    cVector incidentVelocity = velocity_ + g * dt * closestIntersection.beta_;
    
    if(incidentVelocity.dot(normal) < 0 )
    { 
            cVector reflectedVelocity = incidentVelocity - 2 * (normal.dot(incidentVelocity)) * normal;
            // make damping proportional to angle of incidence.
            // low angle of incidence gives low damping
            cVector normalizedVelocity = reflectedVelocity;
            normalizedVelocity.normalize();
            float normalizedVelocityDotNormal = normalizedVelocity.dot(normal) ; 
            velocity_ = bouciness * (.5 + .45 * (1 - fabs(normalizedVelocityDotNormal) )) * reflectedVelocity; // damping
    }
    else 
        velocity_ = incidentVelocity;
}



void cBall::collideFaces(	vector<cTriangle*>& triangles,
                                cVector& position_, cVector& newPosition,
                                cIntersectionResult &closestIntersection, 
                                cVector &normal,
                                float &bounciness)
{	
   // find closest face intersection and update the normal
    for(int i = 0; i < triangles.size(); ++i)
    {
        cIntersectionResult result = intersectsFace(position_, newPosition, *triangles[i]);
        
        if(!result.intersected_) continue;
        
        if(result.beta_ == closestIntersection.beta_ )
        {
            closestIntersection = result;
            normal += triangles[i]->getNormal();
            
            bounciness = 1;
            if( triangles[i]-> materialIndex_ == 100)
            { 
                cout << "spriong!" << endl;
                bounciness = 3;
            }
                
            continue;
        }

        if( result.beta_ < closestIntersection.beta_ )
        {
            closestIntersection = result;
            normal = triangles[i]->getNormal();
            
            //cout << triangles[i]-> materialIndex_ << endl;
            if( triangles[i]-> materialIndex_ == 100)
            { 
                cout << "spriong!" << endl;
                bounciness = 3;
            }
        }
    }
}

void cBall::collideEdges(	vector<cEdge*>& edges,
                                cVector& position_, cVector& newPosition,
                                cIntersectionResult &closestIntersection, 
                                cVector &normal,
                                float &bounciness)
{
    // find closest edge intersection
    for(int i = 0; i < edges.size(); ++i)
    {
        cIntersectionResult result = intersectsEdge(position_, newPosition, *edges[i]);
        
        if(!result.intersected_) continue;
        
        cVector incident = newPosition - position_;
        cVector sumForwardNormals;
        
        if(result.beta_ <= closestIntersection.beta_)
            edges[i]->getSumForwardFacingNormal(incident, sumForwardNormals);
        
        if(result.beta_ == closestIntersection.beta_)
        {
            normal += sumForwardNormals;
            closestIntersection = result;
        }
        else if(result.beta_ < closestIntersection.beta_)
        {
            normal = sumForwardNormals;
            closestIntersection = result;
        }
    }
}

void cBall::collideVertices(	vector<cVertex*>& vertices,
                                cVector& position_, cVector& newPosition,
                                cIntersectionResult &closestIntersection, 
                                cVector &normal,
                                float &bounciness)
{
    // find closest vertex intersection and update the normal
    for(int i = 0; i < vertices.size(); ++i)
    {
        cIntersectionResult result = intersectsVertex(position_, newPosition, *vertices[i]);
        
        if(!result.intersected_) continue;
        
        cVector incident = newPosition - position_;
        cVector sumForwardNormals;
        
        if(result.beta_ <= closestIntersection.beta_)
            vertices[i]->getSumForwardFacingNormal(incident, sumForwardNormals);
        
        if(result.beta_ == closestIntersection.beta_)
        {
            normal += sumForwardNormals;
            closestIntersection = result;
        }
        else if(result.beta_ < closestIntersection.beta_)
        {
            normal = sumForwardNormals;
            closestIntersection = result;
        }
    }
}

cIntersectionResult cBall::intersectsFace(cVector &start, cVector &end, cTriangle &face)
{   
    // get the standardized triangle and
    // transform the start and end vectors into standardized space 
    cStandardizedTriangle standard = face.getStandardized();
    float* transform = face.getStandardVertexTranform();
    cVector standardStart = start.matrixMultiply(transform);
    cVector standardEnd = end.matrixMultiply(transform);
    
    float fudgyRadius = radius_ - .0001;
    
    // no collision occurs if the ball is within distance radius_ from the xz plane
    if( standardStart.y_ < fudgyRadius || standardStart.y_ <= standardEnd.y_ ) return cIntersectionResult(false, 1);
    //if(standardStart.y_ <= standardEnd.y_) return cIntersectionResult(false, 1);
    
    // calculate beta such that the center of the ball is radius above the xz plane
    float beta = (fudgyRadius - standardStart.y_) / (standardEnd.y_ - standardStart.y_);
    
    // if beta is not within [0,1] no intersection occurs
    if( beta < 0 || 1 < beta ) return cIntersectionResult(false, 1);
    
    // otherwise, compute projection of ball's position at y = radius onto the xz plane
    cVector qPrime = standardStart + beta * (standardEnd - standardStart);
    qPrime.y_ = 0;
    
    // check the edge v2-v0
    if( qPrime.z_ <= 0 ) return cIntersectionResult(false, 1);   // check that z is positive
    
    // check the edge v0-v1
    if( (standard.v1_.x_ == 0) && (qPrime.x_ <= 0) ) // if edge on the z-axis
        return cIntersectionResult(false, 1); 
    
    if( qPrime.x_  <= (standard.v1_.x_ * (qPrime.z_ / standard.v1_.z_)) )
        return cIntersectionResult(false, 1);
    
    // check edge v1-v2
    if( qPrime.x_ >= standard.v2_.x_ +  (standard.v1_.x_ - standard.v2_.x_) * (qPrime.z_ - standard.v2_.z_) / (standard.v1_.z_ - standard.v2_.z_))
        return cIntersectionResult(false, 1);
    
    return cIntersectionResult(true, beta);
}

cIntersectionResult cBall::intersectsEdge(cVector &start, cVector &end, cEdge &edge)
{
   // get the standardized edge and
    // transform the start and end vectors into standardized space 
   float* transform = edge.getStandardEdgeTransform();
    cVector standardStart = start.matrixMultiply(transform);
    cVector standardEnd = end.matrixMultiply(transform);

    // checking if velocity is parallel to the edge
    if( standardStart.y_ == standardEnd.y_ && standardEnd.z_ == standardStart.z_)
        return cIntersectionResult(false,1);
    
    float A = (standardEnd.y_- standardStart.y_)*(standardEnd.y_- standardStart.y_) + (standardEnd.z_-standardStart.z_)*(standardEnd.z_-standardStart.z_);
    float B = 2 * ((standardStart.y_ * (standardEnd.y_- standardStart.y_)) + (standardStart.z_ * (standardEnd.z_- standardStart.z_))) ;
    float C = (standardStart.y_*standardStart.y_) + (standardStart.z_*standardStart.z_) - (radius_*radius_);
   cIntersectionResult result = quadratic(A,B,C);
   
   if( !result.intersected_) return result;
   
   //checking to see if beta is actually on the edge not just x-axis
   if(standardStart.x_ + result.beta_* (standardEnd.x_ - standardStart.x_) < edge.standardizedX_ &&
      standardStart.x_ + result.beta_* (standardEnd.x_ - standardStart.x_) > 0)
      return result;
   
   return cIntersectionResult(false, 1);

}

cIntersectionResult cBall::intersectsVertex(cVector &start, cVector &end, cVertex &vertex)
{ 
    // the ball is moving toward the vertex, solve this quadratic
    float A = (end.x_-start.x_)*(end.x_-start.x_) + (end.y_-start.y_)*(end.y_-start.y_) +
              (end.z_-start.z_)*(end.z_-start.z_);

    float B = 2*( (end.x_-start.x_)*(start.x_-vertex.x_) + (end.y_-start.y_)*(start.y_-vertex.y_) +
              (end.z_-start.z_)*(start.z_-vertex.z_));
   
    float C = (start.x_-vertex.x_)*(start.x_-vertex.x_)+(start.y_-vertex.y_)*(start.y_-vertex.y_)
            +(start.z_-vertex.z_)*(start.z_-vertex.z_)- (radius_*radius_);
    
    return quadratic(A,B,C);
}

cIntersectionResult cBall::quadratic(float A, float B, float C)
{
    float discr = (B*B) - (4*A*C);
    if( discr < 0 ) return cIntersectionResult(false, 1);
    
    discr = sqrt(discr);
    float beta1 = (-1*B + discr)/ (2 * A);
    float beta2 = (-1*B - discr)/(2 * A);
     
    float minBeta = 1; 
    bool inRange = false;
    if(beta2 >= 0 && beta2 < 1.0) { minBeta = beta2; inRange = true; }
    if(beta1 >= 0 && beta2 < 1.0)
    {
        inRange = true;
        if(beta1 < minBeta) minBeta = beta1;
    }
    
    if(inRange) 
        return cIntersectionResult(true, minBeta);
    else return cIntersectionResult(false, 1);
}

void cBall::draw()
{
    // since we are now texturemapping everything else, opengl probably needs to be told
    // that it is no longer drawing a non-texture mapped object.... if only i knew or cared right now
    
    glColor3f(r_,g_,b_);

    glPushMatrix();
    glTranslatef(position_.x_,position_.y_,position_.z_);
    
    glutSolidSphere(radius_,10,10);

    glPopMatrix();
}

void cBall::drawLaunchArrow()
{
    
    
    cVector dir(.5*launchPower_*cos(launchAngle_), .1, .5*launchPower_ * sin(launchAngle_));
    dir -= dir.dot(launchNormal_) / launchNormal_.dot(launchNormal_) * launchNormal_;
    
    glColor3f(0,0,0);
        glBegin(GL_LINES);
        glVertex3f(position_.x_,position_.y_,position_.z_);
        glVertex3f(position_.x_ + dir.x_,position_.y_ + dir.y_,position_.z_ + dir.z_);
    glEnd();
}

void cBall::launch()
{    
    cVector dir(launchPower_*cos(launchAngle_), .1, launchPower_ * sin(launchAngle_));
    dir -= dir.dot(launchNormal_) / launchNormal_.dot(launchNormal_) * launchNormal_;
    
    velocity_ +=  dir;
}
