/**
 * Processes any left-clicks for picking a target,
 * or right-clicks to scroll the globe.
 * @param action Pointer to an action.
 */
void SelectDestinationState::globeClick(Action *action)
{
	double lon, lat;
	int mouseX = (int)floor(action->getAbsoluteXMouse()), mouseY = (int)floor(action->getAbsoluteYMouse());
	_globe->cartToPolar(mouseX, mouseY, &lon, &lat);

	// Ignore window clicks
	if (mouseY < 28)
	{
		return;
	}

	// Clicking on a valid target
	if (action->getDetails()->button.button == SDL_BUTTON_LEFT)
	{
		std::vector<Target*> v = _globe->getTargets(mouseX, mouseY, true);
		if (v.empty())
		{
			Waypoint *w = new Waypoint();
			w->setLongitude(lon);
			w->setLatitude(lat);
			v.push_back(w);
		}
		_game->pushState(new MultipleTargetsState(_game, v, _craft, 0));
	}
}

bool Enemy::initWithTheGame(HelloWorld* game)
{
	bool bRet = false;
	do 
	{
		attackedBy = CCArray::createWithCapacity(5);
		attackedBy->retain();
		maxHp = 40;
		currentHp = maxHp;
		active = false;
		walkingSpeed = 0.5;

		_theGame = game;
		_mySprite = CCSprite::create("enemy.png");
		this->addChild(_mySprite);

		Waypoint *waypoint = (Waypoint*)_theGame->getWaypoints()->objectAtIndex(_theGame->getWaypoints()->count() - 1);
		destinationWaypoint = waypoint->getNextWaypoint();
		CCPoint pos = waypoint->getMyPosition();
		myPosition = pos;
		_mySprite->setPosition(pos);
		_theGame->addChild(this);

		this->scheduleUpdate();

		bRet = true;
	} while (0);

	return bRet;
}

Waypoint *psPathNetwork::FindRandomWaypoint(int group, csVector3& v,iSector *sector, float range, float * found_range)
{
    csList<Waypoint*>::Iterator iter(waypointGroups[group]);
    Waypoint *wp;

    csArray<Waypoint*> nearby;
    csArray<float> dist;
 
    while (iter.HasNext())
    {
        wp = iter.Next();

        float dist2 = world->Distance(v,sector,wp->loc.pos,wp->GetSector(engine));
        
        if (range < 0 || dist2 < range)
        {
            nearby.Push(wp);
            dist.Push(dist2);
        }
    }

    if (nearby.GetSize()>0)  // found one or more closer than range
    {
        size_t pick = psGetRandom((uint32)nearby.GetSize());
        
        if (found_range) *found_range = sqrt(dist[pick]);
        
        return nearby[pick];
    }


    return NULL;
}

/**
 * Remove a buoy
 * @param tile TileIndex been queried
 * @param flags operation to perform
 * @pre IsBuoyTile(tile)
 * @return cost or failure of operation
 */
CommandCost RemoveBuoy(TileIndex tile, DoCommandFlag flags)
{
	/* XXX: strange stuff, allow clearing as invalid company when clearing landscape */
	if (!Company::IsValidID(_current_company) && !(flags & DC_BANKRUPT)) return_cmd_error(INVALID_STRING_ID);

	Waypoint *wp = Waypoint::GetByTile(tile);

	if (HasStationInUse(wp->index, false, _current_company)) return_cmd_error(STR_ERROR_BUOY_IS_IN_USE);
	/* remove the buoy if there is a ship on tile when company goes bankrupt... */
	if (!(flags & DC_BANKRUPT)) {
		CommandCost ret = EnsureNoVehicleOnGround(tile);
		if (ret.Failed()) return ret;
	}

	if (flags & DC_EXEC) {
		wp->facilities &= ~FACIL_DOCK;

		InvalidateWindowData(WC_WAYPOINT_VIEW, wp->index);

		/* We have to set the water tile's state to the same state as before the
		 * buoy was placed. Otherwise one could plant a buoy on a canal edge,
		 * remove it and flood the land (if the canal edge is at level 0) */
		MakeWaterKeepingClass(tile, GetTileOwner(tile));

		wp->rect.AfterRemoveTile(wp, tile);

		wp->UpdateVirtCoord();
		wp->delete_ctr = 0;
	}

	return CommandCost(EXPENSES_CONSTRUCTION, _price[PR_CLEAR_WAYPOINT_BUOY]);
}

Waypoint *ScriptFunctions::convertWaypoint(Aurora::NWScript::Object *o) {
	Waypoint *waypoint = dynamic_cast<Waypoint *>(o);
	if (!waypoint || (waypoint->getID() == kObjectIDInvalid))
		return 0;

	return waypoint;
}

Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint)
{
    Waypoint *waypoint = NULL;
    int count = objMngr->getNumInstances(Waypoint::OBJID);

    if (index < count) {
        // reuse object
        qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count;
        waypoint = Waypoint::GetInstance(objMngr, index);
        if (newWaypoint) {
            newWaypoint->deleteLater();
        }
    } else if (index < count + 1) {
        // create "next" object
        qDebug() << "ModelUAVProxy::createWaypoint - created waypoint instance :" << index;
        // TODO is there a limit to the number of wp?
        waypoint = newWaypoint ? newWaypoint : new Waypoint;
        waypoint->initialize(index, waypoint->getMetaObject());
        objMngr->registerObject(waypoint);
    } else {
        // we can only create the "next" object
        // TODO fail in a clean way :(
    }
    return waypoint;
}

/**
 * Rename a waypoint.
 * @param tile unused
 * @param flags type of operation
 * @param p1 id of waypoint
 * @param p2 unused
 * @param text the new name or an empty string when resetting to the default
 * @return the cost of this operation or an error
 */
CommandCost CmdRenameWaypoint(TileIndex tile, DoCommandFlag flags, uint32 p1, uint32 p2, const char *text)
{
	Waypoint *wp = Waypoint::GetIfValid(p1);
	if (wp == NULL) return CMD_ERROR;

	if (wp->owner != OWNER_NONE) {
		CommandCost ret = CheckOwnership(wp->owner);
		if (ret.Failed()) return ret;
	}

	bool reset = StrEmpty(text);

	if (!reset) {
		if (Utf8StringLength(text) >= MAX_LENGTH_STATION_NAME_CHARS) return CMD_ERROR;
		if (!IsUniqueWaypointName(text)) return_cmd_error(STR_ERROR_NAME_MUST_BE_UNIQUE);
	}

	if (flags & DC_EXEC) {
		free(wp->name);
		wp->name = reset ? NULL : strdup(text);

		wp->UpdateVirtCoord();
	}
	return CommandCost();
}

/**
 * Confirms the selected target for the craft.
 * @param action Pointer to an action.
 */
void ConfirmDestinationState::btnOkClick(Action *)
{
	Waypoint *w = dynamic_cast<Waypoint*>(_target);
	if (w != 0 && w->getId() == 0)
	{
		w->setId(_game->getSavedGame()->getId("STR_WAYPOINT"));
		_game->getSavedGame()->getWaypoints()->push_back(w);
	}
	_craft->setDestination(_target);
	_craft->setStatus("STR_OUT");
	if(_craft->getInterceptionOrder() == 0)
	{
		int maxInterceptionOrder = 0;
		for(std::vector<Base*>::iterator baseIt = _game->getSavedGame()->getBases()->begin(); baseIt != _game->getSavedGame()->getBases()->end(); ++baseIt)
		{
			for(std::vector<Craft*>::iterator craftIt = (*baseIt)->getCrafts()->begin(); craftIt != (*baseIt)->getCrafts()->end(); ++craftIt)
			{
				if((*craftIt)->getInterceptionOrder() > maxInterceptionOrder)
				{
					maxInterceptionOrder = (*craftIt)->getInterceptionOrder();
				}
			}
		}
		_craft->setInterceptionOrder(++maxInterceptionOrder);
	}
	_game->popState();
	_game->popState();
}

/**
 *	Make up a cache of all the WaypointSets that any of the waypoints in
 *	our own set are adjacent to.
 */
void WaypointSet::cacheAdjacentSets() const
{
	std::set<WaypointSet*>	adjSetsSet;

	Waypoints::const_iterator wit;
	Waypoint* pWaypoint;

	for(wit = waypoints_.begin(); wit != waypoints_.end(); wit++)
	{
		pWaypoint = *wit;
		if (pWaypoint == NULL) continue;

		int vc = pWaypoint->vertexCount();
		for (int i = 0; i < vc; i++)
		{
			WaypointSet * pWPSet = pWaypoint->adjacentWaypointSet( i );
			if (pWPSet != NULL) adjSetsSet.insert( pWPSet );
		}
	}

	adjacentSets_.clear();
	std::set<WaypointSet*>::iterator sit;
	for (sit = adjSetsSet.begin(); sit != adjSetsSet.end(); sit++)
	{
		adjacentSets_.push_back( *sit );
	}

	adjacentSetsCurrent_ = true;
}

bool Heuristic::collides(const Waypoint &w0, const Waypoint &w1, ThreadData* data) const {
	const Real dist = getDist(w0, w1);
	const U32 size = (U32)Math::round(dist/desc.collisionDesc.pathDistDelta) +1;
	Real p[2];
	Waypoint w;

#ifdef _HEURISTIC_PERFMON
	++perfCollisionPath;
	perfCollisionSegs += size - 1;
#endif

	// test for collisions in the range (w0, w1) - excluding w0 and w1
	for (U32 i = 1; i < size; ++i) {
		p[0] = Real(i)/Real(size);
		p[1] = REAL_ONE - p[0];

		// lineary interpolate coordinates
		for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j)
			w.cpos[j] = p[0]*w0.cpos[j] + p[1]*w1.cpos[j];

		// skip reference pose computation
		w.setup(controller, false, true);

		// test for collisions
		if (collides(w, data))
			return true;
	}

	return false;
}

/**
 *	This method returns the waypoint whose centre is nearest to the given
 *	position. It should generally be used in the case where there is no
 *	enclosing waypoint.
 *
 *	@param position	Position to check
 *	@param foundDistSquared	The squared distance to the closest point on
 *							the waypoint, or FLT_MAX if NULL was returned
 *
 *	@return The nearest waypoint, or NULL (if no waypoints in this chunk).
 */ 
Waypoint* WaypointSet::findClosestWaypoint( const Vector3& position,
	float & foundDistSquared )
{
	Waypoints::iterator waypointIter;
	Waypoint* pWaypoint;
	Waypoint* pBestWaypoint = NULL;
	float bestDistanceSquared = FLT_MAX;
	float distanceSquared;

	for(waypointIter = waypoints_.begin();
		waypointIter != waypoints_.end();
		waypointIter++)
	{
		pWaypoint = *waypointIter;
		if (!pWaypoint) continue;

		distanceSquared = (pWaypoint->centre() - position).lengthSquared();

		if(distanceSquared < bestDistanceSquared)
		{
			bestDistanceSquared = distanceSquared;
			pBestWaypoint = pWaypoint;
		}
	}

	foundDistSquared = bestDistanceSquared;
	return pBestWaypoint;
}

/**
 *	Loose any bindings to the given chunk
 */
void WaypointSet::loose( WaypointChunk & achunk )
{
	Waypoints::iterator wit;
	Waypoint* pWaypoint;

	for(wit = waypoints_.begin(); wit != waypoints_.end(); wit++)
	{
		pWaypoint = *wit;
		if (pWaypoint == NULL) continue;

		int vc = pWaypoint->vertexCount();
		for (int i = 0; i < vc; i++)
		{
			// check the chunk is right
			if (pWaypoint->adjacentChunkID(i) == achunk.chunkID())
			{
				// also check the chunk ptr, in case there are two waypoint
				// chunks of the same name (can happen with ref counting)
				WaypointSet * pWPSet = pWaypoint->adjacentWaypointSet( i );
				if (pWPSet != NULL && &pWPSet->chunk() == &achunk)
				{
					pWaypoint->adjacentWaypointSet( i, NULL );
				}
			}
		}
	}

	adjacentSetsCurrent_ = false;
}

OGRFeature* GTMWaypointLayer::GetNextFeature()
{
    if( bError )
        return nullptr;

    while (poDS->hasNextWaypoint())
    {
        Waypoint* poWaypoint = poDS->fetchNextWaypoint();
        if (poWaypoint == nullptr)
        {
            CPLError(CE_Failure, CPLE_AppDefined,
                     "Could not read waypoint. File probably corrupted");
            bError = true;
            return nullptr;
        }

        OGRFeature* poFeature = new OGRFeature( poFeatureDefn );
        double altitude = poWaypoint->getAltitude();
        if (altitude == 0.0)
            poFeature->SetGeometryDirectly(new OGRPoint
                                           (poWaypoint->getLongitude(),
                                            poWaypoint->getLatitude()));
        else
            poFeature->SetGeometryDirectly(new OGRPoint
                                           (poWaypoint->getLongitude(),
                                            poWaypoint->getLatitude(),
                                            altitude));

        if (poSRS)
            poFeature->GetGeometryRef()->assignSpatialReference(poSRS);
        poFeature->SetField( NAME, poWaypoint->getName());
        poFeature->SetField( COMMENT, poWaypoint->getComment());
        poFeature->SetField( ICON, poWaypoint->getIcon());

        GIntBig wptdate = poWaypoint->getDate();
        if (wptdate != 0)
        {
            struct tm brokendownTime;
            CPLUnixTimeToYMDHMS(wptdate, &brokendownTime);
            poFeature->SetField( DATE,
                                 brokendownTime.tm_year + 1900,
                                 brokendownTime.tm_mon + 1,
                                 brokendownTime.tm_mday,
                                 brokendownTime.tm_hour,
                                 brokendownTime.tm_min,
                                 static_cast<float>(brokendownTime.tm_sec));
        }

        poFeature->SetFID( nNextFID++ );
        delete poWaypoint;
        if( (m_poFilterGeom == nullptr
             || FilterGeometry( poFeature->GetGeometryRef() ) )
            && (m_poAttrQuery == nullptr
                || m_poAttrQuery->Evaluate( poFeature )) )
            return poFeature;

        delete poFeature;
    }
    return nullptr;
}

void NPC::GoTo(Vector3 destination)
{
	path.clear();

	Waypoint* currLocation = new Waypoint(position, Waypoint::sizeH, Waypoint::sizeV);
	Waypoint* targetLocation = new Waypoint(destination, Waypoint::sizeH, Waypoint::sizeV);

	currLocation->position.y = Waypoint::sizeV / 2;
	targetLocation->position.y = Waypoint::sizeV / 2;

	if (currLocation->CheckLink(*targetLocation)){//if there is a clear path between location and destination
		path.push_back(targetLocation);
	}
	else{ //else follow Dijkstra

		currLocation->LinkWaypoints();
		targetLocation->LinkWaypoints();

		for (vector<Waypoint*>::iterator it = (targetLocation->reachableWaypoints).begin(); it != (targetLocation->reachableWaypoints).end(); ++it){
			(*it)->target = targetLocation;
		}

		path = Dijkstra(currLocation, targetLocation);

		if (path.size() > 0){
			path.pop_back(); // removes the last waypoint - it represents current position
		}
	}

	checkPoint = path.rbegin();
}

void WaypointLoader::printResults(std::ostream& sout)
{
   Waypoint waypoint;
   for (int i=0; i < nql; i++) {
      waypoint.setRecord( db->getRecord( ql[i]->idx ) ) ;
      waypoint.printRecord(sout);
   }
}

/**
 * Returns to the previous screen.
 * @param action Pointer to an action.
 */
void ConfirmDestinationState::btnCancelClick(Action *action)
{
	Waypoint *w = dynamic_cast<Waypoint*>(_target);
	if (w != 0 && w->getId() == 0)
	{
		delete w;
	}
	_game->popState();
}

/**
 * Build a buoy.
 * @param tile tile where to place the buoy
 * @param flags operation to perform
 * @param p1 unused
 * @param p2 unused
 * @param text unused
 * @return the cost of this operation or an error
 */
CommandCost CmdBuildBuoy(TileIndex tile, DoCommandFlag flags, uint32 p1, uint32 p2, const char *text)
{
	if (tile == 0) return_cmd_error(STR_ERROR_SITE_UNSUITABLE);

	WaterClass wc;
	if ((flags & DC_PASTE) && !(flags & DC_EXEC)) {
		/* When pasting a buoy, there may be no water yet (a canal will be placed when DC_EXE'ing).
		 * Ignore that there is no water so we can calculate the cost more precisely. */
		wc = WATER_CLASS_INVALID;
	} else {
		if (!HasTileWaterGround(tile)) return_cmd_error(STR_ERROR_SITE_UNSUITABLE);
		wc = GetWaterClass(tile);
	}

	if (IsBridgeAbove(tile)) return_cmd_error(STR_ERROR_MUST_DEMOLISH_BRIDGE_FIRST);

	if (!IsTileFlat(tile)) return_cmd_error(STR_ERROR_SITE_UNSUITABLE);

	/* Check if there is an already existing, deleted, waypoint close to us that we can reuse. */
	Waypoint *wp = FindDeletedWaypointCloseTo(tile, STR_SV_STNAME_BUOY, OWNER_NONE);
	if (wp == NULL && !Waypoint::CanAllocateItem()) return_cmd_error(STR_ERROR_TOO_MANY_STATIONS_LOADING);

	CommandCost cost(EXPENSES_CONSTRUCTION, _price[PR_BUILD_WAYPOINT_BUOY]);
	if (!IsWaterTile(tile)) {
		CommandCost ret = DoCommand(tile, 0, 0, flags | DC_AUTO, CMD_LANDSCAPE_CLEAR);
		if (ret.Failed()) return ret;
		cost.AddCost(ret);
	}

	if (flags & DC_EXEC) {
		if (wp == NULL) {
			wp = new Waypoint(tile);
		} else {
			/* Move existing (recently deleted) buoy to the new location */
			wp->xy = tile;
			InvalidateWindowData(WC_WAYPOINT_VIEW, wp->index);
		}
		wp->rect.BeforeAddTile(tile, StationRect::ADD_TRY);

		wp->string_id = STR_SV_STNAME_BUOY;

		wp->facilities |= FACIL_DOCK;
		wp->owner = OWNER_NONE;

		wp->build_date = _date;

		if (wp->town == NULL) MakeDefaultName(wp);

		assert(wc != WATER_CLASS_INVALID);
		MakeBuoy(tile, wp->index, wc);

		wp->UpdateVirtCoord();
		InvalidateWindowData(WC_WAYPOINT_VIEW, wp->index);
	}

	return cost;
}

/**
 * Initializes all the elements in the Confirm Destination window.
 * @param game Pointer to the core game.
 * @param craft Pointer to the craft to retarget.
 * @param target Pointer to the selected target (NULL if it's just a point on the globe).
 */
ConfirmDestinationState::ConfirmDestinationState(Game *game, Craft *craft, Target *target) : State(game), _craft(craft), _target(target)
{
	Waypoint *w = dynamic_cast<Waypoint*>(_target);
	_screen = false;

	// Create objects
	_window = new Window(this, 224, 72, 16, 64);
	_btnOk = new TextButton(50, 12, 68, 104);
	_btnCancel = new TextButton(50, 12, 138, 104);
	_txtTarget = new Text(212, 32, 22, 72);

	// Set palette
	if (w != 0 && w->getId() == 0)
	{
		_game->setPalette(_game->getResourcePack()->getPalette("BACKPALS.DAT")->getColors(Palette::blockOffset(6)), Palette::backPos, 16);
	}
	else
	{
		_game->setPalette(_game->getResourcePack()->getPalette("BACKPALS.DAT")->getColors(Palette::blockOffset(4)), Palette::backPos, 16);
	}

	add(_window);
	add(_btnOk);
	add(_btnCancel);
	add(_txtTarget);

	centerAllSurfaces();

	// Set up objects
	_window->setColor(Palette::blockOffset(15)-1);
	_window->setBackground(_game->getResourcePack()->getSurface("BACK12.SCR"));

	_btnOk->setColor(Palette::blockOffset(8)+5);
	_btnOk->setText(tr("STR_OK"));
	_btnOk->onMouseClick((ActionHandler)&ConfirmDestinationState::btnOkClick);
	_btnOk->onKeyboardPress((ActionHandler)&ConfirmDestinationState::btnOkClick, Options::keyOk);

	_btnCancel->setColor(Palette::blockOffset(8)+5);
	_btnCancel->setText(tr("STR_CANCEL_UC"));
	_btnCancel->onMouseClick((ActionHandler)&ConfirmDestinationState::btnCancelClick);
	_btnCancel->onKeyboardPress((ActionHandler)&ConfirmDestinationState::btnCancelClick, Options::keyCancel);

	_txtTarget->setColor(Palette::blockOffset(15)-1);
	_txtTarget->setBig();
	_txtTarget->setAlign(ALIGN_CENTER);
	_txtTarget->setVerticalAlign(ALIGN_MIDDLE);
	_txtTarget->setWordWrap(true);
	if (w != 0 && w->getId() == 0)
	{
		_txtTarget->setText(tr("STR_TARGET").arg(tr("STR_WAY_POINT")));
	}
	else
	{
		_txtTarget->setText(tr("STR_TARGET").arg(_target->getName(_game->getLanguage())));
	}
}

void floatingWaypoint(){
	btAlignedObjectArray<Waypoint*>* wayList = entManager->getWaypointList();
	Car* c = entManager->getCar(0);
	btTransform wayPointT1 = c->physicsObject->getWorldTransform();	
	entManager->createWaypoint("model/waypoint.obj", wayPointT1);	
	Waypoint* newWay = wayList->at(wayList->size()-1);
	newWay->setThrottle(controller1.getTriggers());
	Waypoint* prevWay = wayList->at(waypointIndex1);
	prevWay->addNextWaypoint(newWay);
	waypointIndex1 = newWay->getIndex();
}

bool ModelUavoProxy::objectsToModel()
{
    // build model from uav objects
    // the list of objects can end with "garbage" instances due to previous flightpath
    // they need to be ignored

    PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
    PathPlan::DataFields pathPlanData = pathPlan->getData();

    int waypointCount  = pathPlanData.WaypointCount;
    int actionCount    = pathPlanData.PathActionCount;

    // consistency check
    if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
        QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan way point count error !"));
        return false;
    }
    if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) {
        QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan path action count error !"));
        return false;
    }
    if (pathPlanData.Crc != computePathPlanCrc(waypointCount, actionCount)) {
        QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Path plan CRC error !"));
        return false;
    }

    int rowCount = myModel->rowCount();
    if (waypointCount < rowCount) {
        myModel->removeRows(waypointCount, rowCount - waypointCount);
    } else if (waypointCount > rowCount) {
        myModel->insertRows(rowCount, waypointCount - rowCount);
    }

    for (int i = 0; i < waypointCount; ++i) {
        Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
        Q_ASSERT(waypoint);
        if (!waypoint) {
            continue;
        }

        Waypoint::DataFields waypointData = waypoint->getData();
        waypointToModel(i, waypointData);

        PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction());
        Q_ASSERT(action);
        if (!action) {
            continue;
        }

        PathAction::DataFields actionData = action->getData();
        pathActionToModel(i, actionData);
    }
    return true;
}