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

using namespace std;

cBall::cBall()
{
    randomize();
    launchPower_ = 50;
    launchAngle_ = 0;
    numUpdatesMovingSlow_ = 0;
    stopped_ = false;
    
    timeLeftSmoking_ = 0;
    smokingTime_ = 3;
    isInHole_ = false;
    timeInHole_ = 0;
    
}

void cBall::randomize(int level)
{    
    numUpdatesMovingSlow_ = 0;
    stopped_ = false;
    isInHole_ = false;
    timeInHole_ = 0;
    timeLeftSmoking_ = 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;
    
    //
    switch(level)
    {
    case 1: position_ = cVector( -125.391 + (a-.5)*4, 23.6807, 17.3575 + (b-.5)*4); break;
    case 2: position_ = cVector( -24.6113 + (a-.5)*4, -15.9142, -100.546 + (b-.5)*4); break;
	case 3: position_ = cVector(-109.1 + (a-.5)*4, 15, -42 + (b-.5)); break;
	case 4: position_ = cVector(-138 + (a-.5)*4, 20, -160+ (b-.5)); break;
    default: position_ = cVector( (a-.5)*50, 150 + (b-.5)*6, (c-.5)*50);
    }
    velocity_ = cVector(0,0,0);
    
    //velocity_ = cVector((d-.5)*50, 0, (f-.5)*50);
    
    r_  = a;
    g_ = b;
    b_ = c;
    radius_ = .5; 
}

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

bool cBall::isInHole() { return isInHole_; } 

float cBall::getTimeInHole() { return timeInHole_; }

void cBall::update(float dt, cStaticTriangleMesh& staticObjects)
{
    
    //cout << position_ << endl;
    
    // detect if ball is in hole
    cVector holePosition;
    float holeRadius = 2;
    switch(level_)
    {
    case 1: holePosition = cVector(28.6923, -2.58441, 39.114); break;
    case 2: holePosition = cVector(60.5945, -33.9109, 159.748); break;
	case 3: holePosition = cVector(88.9361, -2.57943, -44.1391); break;
	case 4: holePosition = cVector(127.707, 7.72946, -132.746); break;
    }
    
    cVector ballToHole = position_ - holePosition;
    if(ballToHole.length() < holeRadius )
    {
    	isInHole_ = true;
        timeInHole_ += dt;
        return;
    }

    if(stopped_) return;
    if(dt == 0 ) return; //nothing to do
    
    if(position_.y_ < -50)
    {
        // the ball fell out of the course
        position_ = lastValidPosition_;
        velocity_ = cVector(0,0,0);
    }
    
    // stop the balll
    float slownessToStop = 2.6;
    if(velocity_.length() < slownessToStop )
    	++numUpdatesMovingSlow_;
    
    float maxUpdatesMovingSlowToStop = 100;
    if(numUpdatesMovingSlow_ >= maxUpdatesMovingSlowToStop)
        stopped_ = true;
        
    if(stopped_) cout << "stopped" << endl;
    
    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);
    
    int material = -1;
    // modifies parameters
    collideVertices(vlist, position_, endPos, closestIntersection, normal, material);
    collideEdges(elist, position_, endPos, closestIntersection, normal, material);
    collideFaces(tlist, position_, endPos, closestIntersection, normal, material);
    
    // leave a smoke trail
    if(timeLeftSmoking_ > 0)
    {
        cSmokeRingParticle *p = ps_->addSmokeRingParticle();
        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;
        p->velocity_ = .1 * cVector(a-.5,2,b-.5);
        p->position_ = position_ + radius_ * 2 * cVector(c-.5, d-.5, e-.5);
        p->r_ = timeLeftSmoking_ / smokingTime_; 
        p->g_ = timeLeftSmoking_ / smokingTime_; 
        p->b_ = timeLeftSmoking_ / smokingTime_; 
        p->size_ = radius_ + a *3;
        p->maxAge_ = 3 + b*4;
        timeLeftSmoking_ -= dt;
    }
    
    // there was no intersection with anything
    if(closestIntersection.intersected_ == false)
    {
        position_ = endPos;
        velocity_ = endVelocity;
        //sliding_ = false;
        return;
    }
    
    if(normal.x_ == 0 && normal.y_ == 0 && normal.z_ == 0)
    {
        cout << "error, got 0 normal, using random normal" << endl;
		normal = cVector(rand()%100 - 50,rand()%100 - 50,rand()%100 - 50);
		normal.normalize();
    }
    
    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_;
    

    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) ; 
    
    float materialDamp = 1;
    if(level_ == 4 && material == 1) materialDamp = .3; // snow damping
    
    velocity_ =  (materialDamp * .5 + .45 * (1 - fabs(normalizedVelocityDotNormal) )) * reflectedVelocity; // damping

    // volcano level lava bounce
    if(material == 0 && level_ == 2)
    {
        cout << "lava bounce" << endl;
        velocity_ *= .5;
        float a = ((float)(rand()%100))/100.0f;
        float b = ((float)(rand()%100))/100.0f;
        velocity_ += cVector((a-.5) * 50,70, (b-.5) * 50 );
        timeLeftSmoking_ = smokingTime_;
        
        for(int i = 0; i < 100; ++i)
        {
            float c = ((float)(rand()%100))/100.0f;
            float d = ((float)(rand()%100))/100.0f;
            float e = ((float)(rand()%100))/100.0f;
            
            cSplashParticle* p = ps_->addSplash();
            p->position_ = position_;
            p->velocity_ = 15 * cVector(c-.5, d*5, e-.5);
        }
    }
    
    // maze level bumpers
    if((material == 3 ) && level_ == 3)
    {
        cout << "bumper" << endl;
        velocity_ += 30 * velocity_ / velocity_.length();
    }
}



void cBall::collideFaces(	vector<cTriangle*>& triangles,
                                cVector& position_, cVector& newPosition,
                                cIntersectionResult &closestIntersection, 
                                cVector &normal,
                                int &material)
{	
   // 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();
            
           
                
            continue;
        }

        if( result.beta_ < closestIntersection.beta_ )
        {
            closestIntersection = result;
            normal = triangles[i]->getNormal();
            
            material = triangles[i]-> materialIndex_;
        }
    }
}

void cBall::collideEdges(	vector<cEdge*>& edges,
                                cVector& position_, cVector& newPosition,
                                cIntersectionResult &closestIntersection, 
                                cVector &normal,
                                int &material)
{
    // 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;
            
            material = edges[i]->f0_-> materialIndex_;
        }
    }
}

void cBall::collideVertices(	vector<cVertex*>& vertices,
                                cVector& position_, cVector& newPosition,
                                cIntersectionResult &closestIntersection, 
                                cVector &normal,
                                int &material)
{
    // 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;
            
            material = vertices[i]->triangles_[0]->materialIndex_;
        }
    }
}

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
    glDisable(GL_TEXTURE_2D);	
    glColor3f(r_,g_,b_);

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

    glPopMatrix();
    glEnable(GL_TEXTURE_2D);	
}

void cBall::drawLaunchArrow()
{
    if(!stopped_) return;
    
    cVector dir(.5*launchPower_*cos(launchAngle_), .1, .5*launchPower_ * sin(launchAngle_));
    dir -= dir.dot(launchNormal_) / launchNormal_.dot(launchNormal_) * launchNormal_;
    
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);			// Enable Blending
    glBlendFunc(GL_SRC_ALPHA, GL_ONE);	
    glDepthMask(GL_FALSE);
        
    glColor3f(1,1,1);
    for(float i = 0; i < 1; i += .1)
    {
        glColor4f(1,1,1, 1 - i);


        glPushMatrix();
        cVector center = position_ + cVector(0,radius_*2,0) + dir * i ;
        glTranslatef(center.x_,center.y_,center.z_);
        
        glutSolidSphere(radius_ - radius_ * i,10,10);
    
        glPopMatrix();
    }
    
    glEnable(GL_DEPTH_TEST);
    glDisable(GL_BLEND);	
    glDepthMask(GL_TRUE);
}

void cBall::launch()
{    
    if(!stopped_) return;
    lastValidPosition_ = position_;
    
    cVector dir(launchPower_*cos(launchAngle_), .1, launchPower_ * sin(launchAngle_));
    dir -= dir.dot(launchNormal_) / launchNormal_.dot(launchNormal_) * launchNormal_;
    stopped_ = false;
    numUpdatesMovingSlow_ = 0;
    
    velocity_ +=  dir;
}
