source: code/branches/RacingBots_FS18/src/modules/gametypes/SpaceRaceController.cc @ 11913

/*
 *   ORXONOX - the hottest 3D action shooter ever to exist
 *                    > www.orxonox.net <
 *
 *
 *   License notice:
 *
 *   This program is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU General Public License
 *   as published by the Free Software Foundation; either version 2
 *   of the License, or (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 *  Created on: Oct 8, 2012findCheck
 *      Author: purgham
 */

#include <gametypes/SpaceRaceController.h>
#include "core/CoreIncludes.h"
#include "core/XMLPort.h"
#include "gametypes/SpaceRaceManager.h"
#include "collisionshapes/CollisionShape.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"


namespace orxonox
{
    RegisterClass(SpaceRaceController);

    const int ADJUSTDISTANCE = 500;
    const int MINDISTANCE = 5;
    /*
     * Idea: Find static Point (checkpoints the spaceship has to reach)
     */
    SpaceRaceController::SpaceRaceController(Context* context) :
        ArtificialController(context)
    {
        RegisterObject(SpaceRaceController);
        std::vector<RaceCheckPoint*> checkpoints;

        virtualCheckPointIndex = -2;
        if (ObjectList<SpaceRaceManager>().size() != 1)
            orxout(internal_warning) << "Expected 1 instance of SpaceRaceManager but found " << ObjectList<SpaceRaceManager>().size() << endl;
        for (SpaceRaceManager* manager : ObjectList<SpaceRaceManager>())
        {
            checkpoints = manager->getAllCheckpoints();
            nextRaceCheckpoint_ = manager->findCheckpoint(0);
        }

        OrxAssert(!checkpoints.empty(), "No Checkpoints in Level");
        checkpoints_ = checkpoints;
        staticRacePoints_ = findStaticCheckpoints(nextRaceCheckpoint_, checkpoints);
        // initialisation of currentRaceCheckpoint_
        currentRaceCheckpoint_ = nullptr;

        int i;
        for (i = -2; findCheckpoint(i) != nullptr; i--)     // WIESO?
        {
            continue;
        }

    }

    //------------------------------
    // functions for initialisation

    void SpaceRaceController::XMLPort(Element& xmlelement, XMLPort::Mode mode)
    {
        SUPER(SpaceRaceController, XMLPort, xmlelement, mode);
        XMLPortParam(ArtificialController, "accuracy", setAccuracy, getAccuracy, xmlelement, mode).defaultValues(100.0f);
        XMLPortObject(ArtificialController, WorldEntity, "waypoints", addWaypoint, getWaypoint, xmlelement, mode);
    }

    /*
     * called from constructor 'SpaceRaceController'
     * returns a vector of static Point (checkpoints the spaceship has to reach)
     */
    std::vector<RaceCheckPoint*> SpaceRaceController::findStaticCheckpoints(RaceCheckPoint* currentCheckpoint, const std::vector<RaceCheckPoint*>& allCheckpoints)
    {
        std::map<RaceCheckPoint*, int> zaehler; // counts how many times the checkpoint was reached (for simulation)
        for (RaceCheckPoint* checkpoint : allCheckpoints)
        {
            zaehler.insert(std::pair<RaceCheckPoint*, int>(checkpoint,0));
        }
        int maxWays = rekSimulationCheckpointsReached(currentCheckpoint, zaehler);

        std::vector<RaceCheckPoint*> returnVec;
        for (const auto& mapEntry : zaehler)
        {
            if (mapEntry.second == maxWays)
            {
                returnVec.push_back(mapEntry.first);
            }
        }
        return returnVec;
    }

    /*
     * called from 'findStaticCheckpoints'
     * return how many ways go from the given Checkpoint to the last Checkpoint (of the Game)
     */
    int SpaceRaceController::rekSimulationCheckpointsReached(RaceCheckPoint* currentCheckpoint, std::map<RaceCheckPoint*, int>& zaehler)
    {

        if (currentCheckpoint->isLast())
        {// last point reached

            zaehler[currentCheckpoint] += 1;
            return 1; // 1 Way form the last point to this one
        }
        else
        {
            int numberOfWays = 0; // counts number of ways from this Point to the last point
            for (int checkpointIndex : currentCheckpoint->getNextCheckpoints())
            {
                if (currentCheckpoint == findCheckpoint(checkpointIndex))
                {
                    orxout() << currentCheckpoint->getCheckpointIndex()<<endl;
                    continue;
                }
                if (findCheckpoint(checkpointIndex) == nullptr){
                    orxout(internal_warning) << "Problematic Point: " << checkpointIndex << endl;
                }
                else
                    numberOfWays += rekSimulationCheckpointsReached(findCheckpoint(checkpointIndex), zaehler);

            }
            zaehler[currentCheckpoint] += numberOfWays;
            return numberOfWays; // returns the number of ways from this point to the last one
        }
    }

    //-------------------------------------
    // functions for dynamic Way-search

    float SpaceRaceController::distanceSpaceshipToCheckPoint(RaceCheckPoint* CheckPoint)
    {
        if (this->getControllableEntity() != nullptr)
        {
            return (CheckPoint->getPosition()- this->getControllableEntity()->getPosition()).length();
        }
        return -1;
    }

    /*
     * called by: 'tick' or  'adjustNextPoint'
     * returns the next Checkpoint which the shortest way contains
     */
    RaceCheckPoint* SpaceRaceController::nextPointFind(RaceCheckPoint* raceCheckpoint)
    {
        float minDistance = 0;
        RaceCheckPoint* minNextRaceCheckPoint = nullptr;

        // find the next checkpoint with the minimal distance
        for (int checkpointIndex : raceCheckpoint->getNextCheckpoints())
        {
            RaceCheckPoint* nextRaceCheckPoint = findCheckpoint(checkpointIndex);
            float distance = recCalculateDistance(nextRaceCheckPoint, this->getControllableEntity()->getPosition());

            if (distance < minDistance || minNextRaceCheckPoint == nullptr)
            {
                minDistance = distance;
                minNextRaceCheckPoint = nextRaceCheckPoint;
            }

        }
        if(minNextRaceCheckPoint==nullptr){orxout()<<"minNextRaceCheckPoint=nullpointer line 175 SpaceRaceController index: "<<endl;}
        return minNextRaceCheckPoint;
    }

    /*
     * called from 'nextPointFind'
     * returns the distance between "currentPosition" and the next static checkpoint that can be reached from "currentCheckPoint"
     */
    float SpaceRaceController::recCalculateDistance(RaceCheckPoint* currentCheckPoint, const Vector3& currentPosition)
    {
        // find: looks if the currentCheckPoint is a staticCheckPoint (staticCheckPoint is the same as: static Point)
        if (std::find(staticRacePoints_.begin(), staticRacePoints_.end(), currentCheckPoint) != staticRacePoints_.end())
        {
            return (currentCheckPoint->getPosition() - currentPosition).length();
        }
        else
        {
            float minimum = std::numeric_limits<float>::max();
            for (int checkpointIndex : currentCheckPoint->getNextCheckpoints())
            {
                int dist_currentCheckPoint_currentPosition = static_cast<int> ((currentPosition- currentCheckPoint->getPosition()).length());

                minimum = std::min(minimum, dist_currentCheckPoint_currentPosition + recCalculateDistance(findCheckpoint(checkpointIndex), currentCheckPoint->getPosition()));
                // minimum of distanz from 'currentPosition' to the next static Checkpoint
            }
            return minimum;
        }
    }

    /*called by 'tick'
     *adjust chosen way of the Spaceship every "AdjustDistance" because spaceship could be displaced through an other one
     */
    RaceCheckPoint* SpaceRaceController::adjustNextPoint()
    {
        if (currentRaceCheckpoint_ == nullptr) // no Adjust possible

        {
            return nextRaceCheckpoint_;
        }
        if ((currentRaceCheckpoint_->getNextCheckpoints()).size() == 1) // no Adjust possible

        {
            return nextRaceCheckpoint_;
        }

        //Adjust possible

        return nextPointFind(currentRaceCheckpoint_);
    }

    RaceCheckPoint* SpaceRaceController::findCheckpoint(int index) const
    {
        for (RaceCheckPoint* checkpoint : this->checkpoints_){
            if (checkpoint->getCheckpointIndex() == index)
                return checkpoint;

        }

        orxout()<<"returned nullptr @line 231 SpaceRaceController"<<endl;
        return nullptr;
    }

    SpaceRaceController::~SpaceRaceController()
    {
        if (this->isInitialized())
        {
            for (int i =-1; i>virtualCheckPointIndex; i--)
                delete findCheckpoint(i);
        }
    }

    void SpaceRaceController::tick(float dt)
    {
        if (this->getControllableEntity() == nullptr || this->getControllableEntity()->getPlayer() == nullptr )
        {
            //orxout()<< this->getControllableEntity() << " in tick"<<endl;
            return;
        }
        //FOR virtual Checkpoints
        if(nextRaceCheckpoint_->getCheckpointIndex() < 0)
        {
            if( distanceSpaceshipToCheckPoint(nextRaceCheckpoint_) < 200)
            {
                currentRaceCheckpoint_=nextRaceCheckpoint_;
                nextRaceCheckpoint_ = nextPointFind(nextRaceCheckpoint_);
                lastPositionSpaceship=this->getControllableEntity()->getPosition();
                //orxout()<< "CP "<< currentRaceCheckpoint_->getCheckpointIndex()<<" chanched to: "<< nextRaceCheckpoint_->getCheckpointIndex()<<endl;
            }
        }

        if (nextRaceCheckpoint_->playerWasHere(this->getControllableEntity()->getPlayer()))
        {//Checkpoint erreicht

            currentRaceCheckpoint_ = nextRaceCheckpoint_;
            OrxAssert(nextRaceCheckpoint_, "next race checkpoint undefined");
            nextRaceCheckpoint_ = nextPointFind(nextRaceCheckpoint_);
            lastPositionSpaceship = this->getControllableEntity()->getPosition();
            //orxout()<< "CP "<< currentRaceCheckpoint_->getCheckpointIndex()<<" chanched to: "<< nextRaceCheckpoint_->getCheckpointIndex()<<endl;
        }

        else if ((lastPositionSpaceship-this->getControllableEntity()->getPosition()).length()/dt > ADJUSTDISTANCE)
        {
            nextRaceCheckpoint_ = adjustNextPoint();
            lastPositionSpaceship = this->getControllableEntity()->getPosition();
        }

        // Abmessung fuer MINDISTANCE gut;

        else if((lastPositionSpaceship - this->getControllableEntity()->getPosition()).length()/dt < MINDISTANCE )
        {
            this->moveToPosition(Vector3(rnd()*100, rnd()*100, rnd()*100));
            this->spin();
            orxout(user_status) << "Mindistance reached" << std::endl;
            return;
        }
        //orxout(user_status) << "dt= " << dt << ";  distance= " << (lastPositionSpaceship-this->getControllableEntity()->getPosition()).length() <<std::endl;
        lastPositionSpaceship = this->getControllableEntity()->getPosition();
       
        this->boostControl();
        if (nextRaceCheckpoint_ == nullptr) orxout() << "nextRaceCheckpoint_ equals to nullpointer look @line 334 SpaceRaceController.cc" << endl;
        this->moveToPosition(nextRaceCheckpoint_->getPosition());
        this->boostControl();
    }

    // True if a coordinate of 'pointToPoint' is smaller then the corresponding coordinate of 'groesse'
    bool SpaceRaceController::vergleicheQuader(const Vector3& pointToPoint, const Vector3& groesse)
    {
        if(std::abs(pointToPoint.x) < groesse.x)
            return true;
        if(std::abs(pointToPoint.y) < groesse.y)
            return true;
        if(std::abs(pointToPoint.z) < groesse.z)
            return true;
        return false;

    }

    bool SpaceRaceController::directLinePossible(RaceCheckPoint* racepoint1, RaceCheckPoint* racepoint2, const std::vector<StaticEntity*>& allObjects)
    {

        Vector3 cP1ToCP2 = (racepoint2->getPosition() - racepoint1->getPosition()) / (racepoint2->getPosition() - racepoint1->getPosition()).length(); //unit Vector
        Vector3 centerCP1 = racepoint1->getPosition();
        btVector3 positionObject;
        btScalar radiusObject;

        for (StaticEntity* object : allObjects)
        {
            for (int everyShape=0; object->getAttachedCollisionShape(everyShape) != nullptr; everyShape++)
            {
                btCollisionShape* currentShape = object->getAttachedCollisionShape(everyShape)->getCollisionShape();
                if(currentShape == nullptr)
                continue;

                currentShape->getBoundingSphere(positionObject,radiusObject);
                Vector3 positionObjectNonBT(positionObject.x(), positionObject.y(), positionObject.z());
                if((powf((cP1ToCP2.dotProduct(centerCP1-positionObjectNonBT)),2)-(centerCP1-positionObjectNonBT).dotProduct(centerCP1-positionObjectNonBT)+powf(radiusObject, 2))>0)
                {
                    return false;
                }

            }
        }
        return true;

    }
}