/**
   Updates some of the path weights for reverse steering calculations
  */
void AIVehicleC::UpdatePathWeights(Ogre::Real goalDegrees, Ogre::Real goalDist)
{
     mPathData.clear();
     
     Ogre::Vector3 thisPos = this->mpWorldObject->GetPosition();
     
     Ogre::Vector3 playerPos  = WorldManagerInst->GetPlayer()->GetVehicle()->GetPosition();

     Ogre::Real playerDist = abs(playerPos.distance(thisPos));

     for (uint32_t i=0; i<NUMBER_OF_STEERING_ANGLES; i++)
     {
          mSteeringWeights[i]=0;
     }

     //ACCOUNT FOR OTHER AI OBJECTS
     for (uint32_t i=0; i<AIManagerInst->GetObjects().size(); i++)
     {                  
          if (strcmp(AIManagerInst->GetObjects()[i]->GetTypeName(), "AIVehicleC") == 0)
          {
               AIVehicleC *otherVehicle = (AIVehicleC*)AIManagerInst->GetObjects()[i];
               if (otherVehicle!=this)
               {
                    Ogre::Vector3 otherPos = otherVehicle->mpWorldObject->GetPosition();
                    Ogre::Real thisRadius = otherVehicle->GetColRadius();
                    Ogre::Real     otherDist = thisPos.distance(otherPos);
                    if (otherDist<OBSTACLE_CONSIDER_DISTANCE && otherDist<goalDist && otherVehicle->GetState()!=AI_DESTROYED) //Obstacle is close enough to worry about, but not behind our goal
                    {
                         aiPathData_t pathData;
                     pathData.locations = otherPos;
                     pathData.distances = otherDist;
                     pathData.critical = true;
                     pathData.weights = DEFAULT_OTHER_AI_WEIGHT;
                     pathData.radii = thisRadius + mColRadius;

                     mPathData.push_back(pathData);                         
               }                                            
          }
     }
     
     //ACCOUNT FOR PLAYER
     if(!((WorldManagerInst->GetPlayer()->GetVehicle())&&(WorldManagerInst->GetPlayer()->GetVehicle()->GetChassis())) || mSuicidal)
     {
          //no player
     }
     else
     {
          Ogre::Vector3 playerPos =  WorldManagerInst->GetPlayer()->GetVehicle()->GetPosition();
          Ogre::Real playerDist = thisPos.distance(playerPos);
          if (playerDist<(OBSTACLE_CONSIDER_DISTANCE))  
          {

             aiPathData_t pathData;
             pathData.locations = playerPos;
             pathData.distances = playerDist;
             pathData.critical = true;
             pathData.weights = DEFAULT_DONT_RAM_PLAYER_WEIGHT;
             pathData.radii = DEFAULT_PLAYER_RADIUS + mColRadius;

             mPathData.push_back(pathData);
          }
     }
     //ACCOUNT FOR AIOBSTACLES if close enough for manuvers

     if (AIObstacleManagerInst->GetNumPoints()>0 || playerDist<OBSTACLE_CON_DIST)
     {
          int32_t numPoints = AIObstacleManagerInst->GetNumPoints();
          for (int32_t i=1; i<numPoints; i++)
          {
             Ogre::Vector3 obstPos;
               AIObstacleC *currObst = AIObstacleManagerInst->GetVectorizedObstacle(i);
             if(currObst!=0)
             {
                   obstPos = currObst->GetPosition();

                    Ogre::Real thisRadius = AIObstacleManagerInst->GetVectorizedObstacle(i)->GetRadius();//GetProperty("radius")->GetAsFloat();
                    Ogre::Real obstDist = thisPos.distance(obstPos);

                    if (obstDist <(OBSTACLE_CONSIDER_DISTANCE) && obstDist<goalDist)  
                    {          
                     aiPathData_t pathData;
                     pathData.locations = obstPos;
                     pathData.distances = obstDist;
                     pathData.critical = false;
                     pathData.weights = DEFAULT_OBSTACLE_WEIGHT;
                     pathData.radii = thisRadius + mColRadius;

                     mPathData.push_back(pathData);
                    }     
             }                                   
          }
     }

     Ogre::Real threatRange;

     if (mBackup)
     {
          threatRange = SAFE_RESUME_RANGE;
     }
     else
     {
          threatRange =  IMMINENT_THREAT_RANGE;
     }

     //default flags to false
     mWait=false;
     mWreckDanger=false;
     if (mBackupTimer<=0)
     {          
          mBackup=false;
     }

     //WEIGHT FOR AVOIDING OBSTACLES
     for (uint32_t i=0; i<mPathData.size(); i++)
     {
          //VECTOR ALGORITHM

          //Vector to obstacle
          Ogre::Vector3 bMinusA = mPathData[i].locations - thisPos;

          //only consider x-z plane 
          bMinusA.y = 0;

          //normalized version of b minus a
          Ogre::Vector3 d = bMinusA.normalisedCopy();

          //y axis
          Ogre::Vector3 q(0,1,0);

          //v is perpendicular to d
          Ogre::Vector3 v= bMinusA.crossProduct(q);

          //normalize v, the perpendicular radial vector
          v.normalise();

          //maximum danger angle, radius scalar value times v plus b-a vector to obstacle
          Ogre::Vector3 p = bMinusA + (v * mPathData[i].radii);

          //normalize pointer to limit of danger
          p.normalise();
          
          //the dot product of normalized vector to obstacle and p gives us a scalar representation of the maximum danger angle
          Ogre::Real w = d.dotProduct(p);

          //correct for close-up objects
          
          if (mPathData[i].radii>mPathData[i].distances)
          {
               w=0;
          }
          else if ((mPathData[i].radii*2)>=mPathData[i].distances && mPathData[i].critical)
          {
               Ogre::Real errorFactor = (mPathData[i].distances - mPathData[i].radii)/mPathData[i].radii;
               w = w*errorFactor;
          }

          //accout for imminent dangers 

          if ((abs(mPathData[i].distances)-mPathData[i].radii)<threatRange)
          {
               //Something is close! . . . but are we headed towards it?
               if (mHeading.dotProduct(d)>=w && mRecklessTimer>0)
               {
                    mWreckDanger=true;
                    mBackup=true;
                    mBackupTimer=BACKUP_TIME;
               }
     
               Vector3 backHeadhing = -mHeading;

               if (backHeadhing.dotProduct(d)>=w)
               {
                    if (mWreckDanger)
                    {
                         if (mRoadRageTime<=0)
                         {
                              mWait=true;
                         }
                    }
                    else
                    {
                         mWreckDanger=true;
                    }
               }
          }

          //for all of our possible directions
          for (int32_t j=0; j<NUMBER_OF_STEERING_ANGLES; j++)
          {
               //the current steering angle
               Ogre::Radian testAngle(Degree((Ogre::Real)(j*STEERING_STEP_DEGREES)));
               //vector representation of that angle
               Ogre::Vector3 testVector(sin(testAngle.valueRadians()),0,cos(testAngle.valueRadians()));
               testVector.normalise();
               
             //The closer the test vector is to the vector to the obstacle, the greater the dot product of the two will be
               //If the dot product is greater than the dot product of the vector to the obstacle and the maximum danger angle,
               //then the test vector must be pointing within the area of the obstacle.  Mark this area with the weight of the 
               //obstacle!
               if (testVector.dotProduct(d) >= w)
               {
                    mSteeringWeights[j] -= mPathData[i].weights; 
               }
          }          
     }     
     
     //WEIGHT FOR DIRECTION WE WANT TO GO IN
     
     int32_t goalIndexRise = (int32_t)( goalDegrees/STEERING_STEP_DEGREES + 0.5);
     
     while (goalIndexRise>=NUMBER_OF_STEERING_ANGLES)
     {
          goalIndexRise -= NUMBER_OF_STEERING_ANGLES;
     }

     while(goalIndexRise<0)
     {
          goalIndexRise += NUMBER_OF_STEERING_ANGLES;
     }
              
     int32_t goalIndexFall = goalIndexRise;
     Ogre::Real goalWeightVal = GOAL_DIRECTION_WEIGHT;
     mSteeringWeights[goalIndexRise] += goalWeightVal;

     for (int32_t i=0; i<(floor(NUMBER_OF_STEERING_ANGLES/2.0f)); i++)
     {
          goalWeightVal -= 1.0;
          goalIndexRise++;
          goalIndexFall--;
          
          while (goalIndexRise>=NUMBER_OF_STEERING_ANGLES)
          {
               goalIndexRise -= NUMBER_OF_STEERING_ANGLES;
          }
          while (goalIndexRise<0)
          {
               goalIndexRise += NUMBER_OF_STEERING_ANGLES;
          }

          while (goalIndexFall>=NUMBER_OF_STEERING_ANGLES)
          {
               goalIndexFall -= NUMBER_OF_STEERING_ANGLES;
          }
          while (goalIndexFall<0)
          {
               goalIndexFall += NUMBER_OF_STEERING_ANGLES;
          }
                  
          mSteeringWeights[goalIndexRise] +=  goalWeightVal;
          mSteeringWeights[goalIndexFall] +=  goalWeightVal;
     }
}