ompl::base::Cost ompl::base::OptimizationObjective::getCost(const Path &path) const
{
    // Cast path down to a PathGeometric
    const geometric::PathGeometric *pathGeom = dynamic_cast<const geometric::PathGeometric*>(&path);

    // Give up if this isn't a PathGeometric or if the path is empty.
    if (!pathGeom)
    {
        OMPL_ERROR("Error: Cost computation is only implemented for paths of type PathGeometric.");
        return this->identityCost();
    }
    else
    {
        std::size_t numStates = pathGeom->getStateCount();
        if (numStates == 0)
        {
            OMPL_ERROR("Cannot compute cost of an empty path.");
            return this->identityCost();
        }
        else
        {
            // Compute path cost by accumulating the cost along the path
            Cost cost(this->identityCost());
            for (std::size_t i = 1; i < numStates; ++i)
            {
                const State *s1 = pathGeom->getState(i-1);
                const State *s2 = pathGeom->getState(i);
                cost = this->combineCosts(cost, this->motionCost(s1, s2));
            }

            return cost;
        }
    }
}

bool ompl::app::RigidBodyGeometry::addEnvironmentMesh(const std::string &env)
{
    assert(!env.empty());
    std::size_t p = importerEnv_.size();
    importerEnv_.resize(p + 1);
    importerEnv_[p] = std::make_shared<Assimp::Importer>();

    const aiScene* envScene = importerEnv_[p]->ReadFile(env.c_str(),
                                                        aiProcess_Triangulate            |
                                                        aiProcess_JoinIdenticalVertices  |
                                                        aiProcess_SortByPType            |
                                                        aiProcess_OptimizeGraph          |
                                                        aiProcess_OptimizeMeshes);
    if (envScene != nullptr)
    {
        if (!envScene->HasMeshes())
        {
            OMPL_ERROR("There is no mesh specified in the indicated environment resource: %s", env.c_str());
            importerEnv_.resize(p);
        }
    }
    else
    {
        OMPL_ERROR("Unable to load environment scene: %s", env.c_str());
        importerEnv_.resize(p);
    }

    if (p < importerEnv_.size())
    {
        computeGeometrySpecification();
        return true;
    }
    
        return false;
}

void ompl::base::PlannerDataStorage::store(const PlannerData &pd, std::ostream &out)
{
    const SpaceInformationPtr &si = pd.getSpaceInformation();
    if (!out.good())
    {
        OMPL_ERROR("Failed to store PlannerData: output stream is invalid");
        return;
    }
    if (!si)
    {
        OMPL_ERROR("Failed to store PlannerData: SpaceInformation is invalid");
        return;
    }
    try
    {
        boost::archive::binary_oarchive oa(out);

        // Writing the header
        Header h;
        h.marker = OMPL_PLANNER_DATA_ARCHIVE_MARKER;
        h.vertex_count = pd.numVertices();
        h.edge_count = pd.numEdges();
        si->getStateSpace()->computeSignature(h.signature);
        oa << h;

        storeVertices(pd, oa);
        storeEdges(pd, oa);
    }
    catch (boost::archive::archive_exception &ae)
    {
        OMPL_ERROR("Failed to store PlannerData: %s", ae.what());
    }
}

bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
{
    if (!goal_)
    {
        OMPL_ERROR("Goal undefined");
        return false;
    }

    for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
    {
        const State *start = startStates_[i];
        if (start && si_->isValid(start) && si_->satisfiesBounds(start))
        {
            double dist;
            if (goal_->isSatisfied(start, &dist))
            {
                if (startIndex)
                    *startIndex = i;
                if (distance)
                    *distance = dist;
                return true;
            }
        }
        else
        {
            OMPL_ERROR("Initial state is in collision!");
        }
    }

    return false;
}

const ompl::base::StateValidityCheckerPtr& ompl::app::RigidBodyGeometry::allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision)
{
    if (validitySvc_)
        return validitySvc_;

    GeometrySpecification geom = getGeometrySpecification();

    switch (ctype_)
    {
#if OMPL_HAS_PQP
        case PQP:
            if (mtype_ == Motion_2D)
                validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
            else
                validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
            break;
#endif
        case FCL:
            if (mtype_ == Motion_2D)
                validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
            else
                validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
            break;

        default:
            OMPL_ERROR("Unexpected collision checker type (%d) encountered", ctype_);
    };

    return validitySvc_;
}

void ompl::base::SpaceInformation::printProperties(std::ostream &out) const
{
    out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
    out << "  - signature: ";
    std::vector<int> sig;
    stateSpace_->computeSignature(sig);
    for (std::size_t i = 0 ; i < sig.size() ; ++i)
        out << sig[i] << " ";
    out << std::endl;
    out << "  - dimension: " << stateSpace_->getDimension() << std::endl;
    out << "  - extent: " << stateSpace_->getMaximumExtent() << std::endl;
    if (isSetup())
    {
        bool result = true;
        try
        {
            stateSpace_->sanityChecks();
        }
        catch(Exception &e)
        {
            result = false;
            out << std::endl << "  - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl;
            OMPL_ERROR(e.what());
        }
        if (result)
            out << "  - sanity checks for state space passed" << std::endl;
        out << "  - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
        out << "  - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl;
        double uniform, near, gaussian;
        samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
        out << "  - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
    }
    else
        out << "Call setup() before to get more information" << std::endl;
}

 Plane2DEnvironment(const char *ppm_file)
 {
     bool ok = false;
     try
     {
         ppm_.loadFile(ppm_file);
         ok = true;
     }
     catch(ompl::Exception &ex)
     {
         OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
     }
     if (ok)
     {
         ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
         space->addDimension(0.0, ppm_.getWidth());
         space->addDimension(0.0, ppm_.getHeight());
         maxWidth_ = ppm_.getWidth() - 1;
         maxHeight_ = ppm_.getHeight() - 1;
         lightning_.reset(new ot::Lightning(ob::StateSpacePtr(space)));
         lightning_->setFilePath("lightning.db");
         // set state validity checking for this space
         lightning_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid, this, _1));
         space->setup();
         lightning_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
         vss_ = lightning_->getSpaceInformation()->allocValidStateSampler();
     }
 }

void ompl::geometric::RRTstar::setSampleRejection(const bool reject)
{
    if (static_cast<bool>(opt_) == true)
    {
        if (opt_->hasCostToGoHeuristic() == false)
        {
            OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
        }
    }

    // This option is mutually exclusive with setSampleRejection, assert that:
    if (reject == true && useInformedSampling_ == true)
    {
        OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
    }

    // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which we only want to do if one is already allocated.
    if (reject != useRejectionSampling_)
    {
        // Store the setting
        useRejectionSampling_ = reject;

        // If we currently have a sampler, we need to make a new one
        if (sampler_ || infSampler_)
        {
            // Reset the samplers
            sampler_.reset();
            infSampler_.reset();

            // Create the sampler
            allocSampler();
        }
    }
}

ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
{
    switch (plannerType)
    {
        case PLANNER_BITSTAR:
            return boost::make_shared<og::BITstar>(si);
            break;
        case PLANNER_CFOREST:
            return boost::make_shared<og::CForest>(si);
            break;
        case PLANNER_FMTSTAR:
            return boost::make_shared<og::FMT>(si);
            break;
        case PLANNER_PRMSTAR:
            return boost::make_shared<og::PRMstar>(si);;
            break;
        case PLANNER_RRTSTAR:
            return boost::make_shared<og::RRTstar>(si);;
            break;
        default:
            OMPL_ERROR("Planner-type enum is not implemented in allocation function.");
            return ob::PlannerPtr();
            break;
    }
}

    Plane2DEnvironment(const char *ppm_file)
    {
        bool ok = false;
        try
        {
            ppm_.loadFile(ppm_file);
            ok = true;
        }
        catch(ompl::Exception &ex)
        {
            OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
        }
        if (ok)
        {
            auto *space = new ob::RealVectorStateSpace();
            space->addDimension(0.0, ppm_.getWidth());
            space->addDimension(0.0, ppm_.getHeight());
            maxWidth_ = ppm_.getWidth() - 1;
            maxHeight_ = ppm_.getHeight() - 1;
            ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));

            // set state validity checking for this space
            ss_->setStateValidityChecker(std::bind(&Plane2DEnvironment::isStateValid, this, std::placeholders::_1));
            space->setup();
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
            //      ss_->setPlanner(ob::PlannerPtr(new og::RRTConnect(ss_->getSpaceInformation())));
        }
    }

bool ompl::tools::ThunderDB::addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime)
{
    // Error check
    if (!spars_)
    {
        OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
        insertionTime = 0;
        return false;
    }

    // Prevent inserting into database
    if (!saving_enabled_)
    {
        OMPL_WARN("ThunderDB: Saving is disabled so not adding path");
        return false;
    }

    bool result;
    double seconds = 120;  // 10; // a large number, should never need to use this
    ompl::base::PlannerTerminationCondition ptc = ompl::base::timedPlannerTerminationCondition(seconds, 0.1);

    // Benchmark runtime
    time::point startTime = time::now();
    {
        result = spars_->addPathToRoadmap(ptc, solutionPath);
    }
    insertionTime = time::seconds(time::now() - startTime);

    OMPL_INFORM("SPARSdb now has %d states", spars_->getNumVertices());

    // Record this new addition
    numPathsInserted_++;

    return result;
}

bool ompl::control::World::operator[](unsigned int i) const
{
    auto p = props_.find(i);
    if (p == props_.end())
        OMPL_ERROR("Proposition %u is not set in world", i);
    return p->second;
}

double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
{
    if (cellSizes_.size() > dim)
        return cellSizes_[dim];
    OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
    return 0.0;
}

    Plane2DEnvironment(const char *ppm_file)
    {
        bool ok = false;
        try
        {
            ppm_.loadFile(ppm_file);
            ok = true;
        }
        catch(ompl::Exception &ex)
        {
            OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
        }
        if (ok)
        {
            auto space(std::make_shared<ob::RealVectorStateSpace>());
            space->addDimension(0.0, ppm_.getWidth());
            space->addDimension(0.0, ppm_.getHeight());
            maxWidth_ = ppm_.getWidth() - 1;
            maxHeight_ = ppm_.getHeight() - 1;
            ss_ = std::make_shared<og::SimpleSetup>(space);

            // set state validity checking for this space
            ss_->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });
            space->setup();
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
            //      ss_->setPlanner(std::make_shared<og::RRTConnect>(ss_->getSpaceInformation()));
        }
    }

bool ompl::tools::Lightning::saveIfChanged()
{
    if (filePath_.empty())
    {
        OMPL_ERROR("No file path has been specified, unable to save experience DB");
        return false;
    }
    return experienceDB_->saveIfChanged(filePath_);
}

bool ompl::tools::LightningDB::load(const std::string &fileName)
{
    // Error checking
    if (fileName.empty())
    {
        OMPL_ERROR("Empty filename passed to save function");
        return false;
    }
    if ( !boost::filesystem::exists( fileName ) )
    {
        OMPL_WARN("Database file does not exist: %s", fileName.c_str());
        return false;
    }

    // Load database from file, track loading time
    time::point start = time::now();

    OMPL_INFORM("Loading database from file: %s", fileName.c_str());

    // Open a binary input stream
    std::ifstream iStream(fileName.c_str(), std::ios::binary);

    // Get the total number of paths saved
    double numPaths = 0;
    iStream >> numPaths;

    // Check that the number of paths makes sense
    if (numPaths < 0 || numPaths > std::numeric_limits<double>::max())
    {
        OMPL_WARN("Number of paths to load %d is a bad value", numPaths);
        return false;
    }

    // Start loading all the PlannerDatas
    for (std::size_t i = 0; i < numPaths; ++i)
    {
        // Create a new planner data instance
        ompl::base::PlannerDataPtr plannerData(new ompl::base::PlannerData(si_));

        // Note: the StateStorage class checks if the states match for us
        plannerDataStorage_.load(iStream, *plannerData.get());

        // Add to nearest neighbor tree
        nn_->add(plannerData);
    }

    // Close file
    iStream.close();

    double loadTime = time::seconds(time::now() - start);
    OMPL_INFORM("Loaded database from file in %f sec with %d paths", loadTime, nn_->size());
    return true;
}

 void configurePlannerRange(double &range, const std::string &context)
 {
     if (range < std::numeric_limits<double>::epsilon())
     {
         base::SpaceInformationPtr si = wsi_.lock();
         if (si)
         {
             range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
             OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range);
         }
         else
             OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.", context.c_str());
     }
 }

ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
{
    switch (plannerType)
    {
        case PLANNER_BITSTAR:
        {
            return std::make_shared<og::BITstar>(si);
            break;
        }
        case PLANNER_CFOREST:
        {
            return std::make_shared<og::CForest>(si);
            break;
        }
        case PLANNER_FMTSTAR:
        {
            return std::make_shared<og::FMT>(si);
            break;
        }
        case PLANNER_INF_RRTSTAR:
        {
            return std::make_shared<og::InformedRRTstar>(si);
            break;
        }
        case PLANNER_PRMSTAR:
        {
            return std::make_shared<og::PRMstar>(si);
            break;
        }
        case PLANNER_RRTSTAR:
        {
            return std::make_shared<og::RRTstar>(si);
            break;
        }
        default:
        {
            OMPL_ERROR("Planner-type enum is not implemented in allocation function.");
            return ob::PlannerPtr(); // Address compiler warning re: no return value.
            break;
        }
    }
}

void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
    if (ks.solve(maxTime))
    {
        std::ofstream out(outputFile.c_str());
        oc::PathControl path(ks.getSolutionPath());
        path.interpolate();
        if (!path.check())
            OMPL_ERROR("Path is invalid");
        writeParams(out);
        path.printAsMatrix(out);
        if (!ks.haveExactSolutionPath())
            OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
                ks.getProblemDefinition()->getSolutionDifference());
        OMPL_INFORM("Output saved in %s", outputFile.c_str());
    }

#if 0
    // Get the planner data, save the ship's (x,y) coordinates to one file and
    // the edge information to another file. This can be used for debugging
    // purposes; plotting the tree of states might give you some idea of
    // a planner's strategy.
    ob::PlannerData pd(ks.getSpaceInformation());
    ks.getPlannerData(pd);
    std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
    double* coords;
    unsigned numVerts = pd.numVertices();
    std::vector<unsigned int> edgeList;

    for (unsigned int i = 0; i < numVerts; ++i)
    {
        coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
        vertexFile << coords[0] << ' ' << coords[1] << '\n';

        pd.getEdges(i, edgeList);
        for (unsigned int j = 0; j < edgeList.size(); ++j)
            edgeFile << i << ' ' << edgeList[j] << '\n';
    }
#endif
}

void ompl::ProlateHyperspheroid::setTransverseDiameter(double transverseDiameter)
{
    if (transverseDiameter < dataPtr_->minTransverseDiameter_)
    {
        OMPL_ERROR("%g < %g", transverseDiameter, dataPtr_->minTransverseDiameter_);
        throw Exception("Transverse diameter cannot be less than the distance between the foci.");
    }

    // Store and update if changed
    if (dataPtr_->transverseDiameter_ != transverseDiameter)
    {
        // Mark as out of date
        dataPtr_->isTransformUpToDate_ = false;

        // Store
        dataPtr_->transverseDiameter_ = transverseDiameter;

        // Update the transform
        updateTransformation();
    }
    // No else, the diameter didn't change
}