/***************************************************************************
 *   Copyright (C) 2005-2019 by the FIFE team                              *
 *   http://www.fifengine.net                                              *
 *   This file is part of FIFE.                                            *
 *                                                                         *
 *   FIFE is free software; you can redistribute it and/or                 *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library 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     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the                 *
 *   Free Software Foundation, Inc.,                                       *
 *   51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA          *
 ***************************************************************************/

// Standard C++ library includes
#include <algorithm>

// 3rd party library includes

// FIFE includes
// These includes are split up in two parts, separated by one empty line
// First block: files included from the FIFE root src directory
// Second block: files included from the same folder
#include "model/metamodel/grids/cellgrid.h"
#include "model/structures/layer.h"
#include "model/structures/cellcache.h"
#include "model/structures/cell.h"
#include "model/structures/map.h"
#include "pathfinder/route.h"
#include "util/math/fife_math.h"

#include "multilayersearch.h"

namespace FIFE {
	MultiLayerSearch::MultiLayerSearch(Route* route, const int32_t sessionId):
		RoutePatherSearch(route, sessionId),
		m_to(route->getEndNode()),
		m_from(route->getStartNode()),
		m_startCache(m_from.getLayer()->getCellCache()),
		m_endCache(m_to.getLayer()->getCellCache()),
		m_currentCache(NULL),
		m_startZone(m_startCache->getCell(m_from.getLayerCoordinates())->getZone()),
		m_endZone(m_endCache->getCell(m_to.getLayerCoordinates())->getZone()),
		m_startCoordInt(m_startCache->convertCoordToInt(m_from.getLayerCoordinates())),
		m_lastStartCoordInt(m_startCoordInt),
		m_destCoordInt(m_endCache->convertCoordToInt(m_to.getLayerCoordinates())),
		m_lastDestCoordInt(-1),
		m_next(0),
		m_foundLast(true) {

		// if end zone is invalid (static blocker) then change it
		if (!m_endZone) {
			Cell* endcell = m_endCache->getCell(m_to.getLayerCoordinates());
			const std::vector<Cell*>& neighbors = endcell->getNeighbors();
			for (std::vector<Cell*>::const_iterator it = neighbors.begin();
				it != neighbors.end(); ++it) {
				Zone* tmpzone = (*it)->getZone();
				if (tmpzone) {
					m_endZone = tmpzone;
					if (tmpzone == m_startZone) {
						break;
					}
				}
			}
		}
		Cell* startCell = m_startCache->getCell(m_from.getLayerCoordinates());

		// here we hope to find between targets
		// first test if target can be reached easy
		searchBetweenTargetsNeighbor();
		// if no betweenTargets could be found we check as second the whole map
		if (m_betweenTargets.empty()) {
			searchBetweenTargetsMap();
		}
		// if it is a protected cell it can have a second startzone
		if (m_betweenTargets.empty() && startCell->isZoneProtected()) {
			const std::vector<Cell*>& neighbors = startCell->getNeighbors();
			for (std::vector<Cell*>::const_iterator it = neighbors.begin();
				it != neighbors.end(); ++it) {
				Zone* tmpzone = (*it)->getZone();
				if (tmpzone) {
					if (tmpzone != m_startZone) {
						m_startZone = tmpzone;
						break;
					}
				}
			}
			searchBetweenTargetsNeighbor();
			if (m_betweenTargets.empty()) {
				searchBetweenTargetsMap();
			}
		}
		// failed to find between targets, no Path can be created
		if (m_betweenTargets.empty()) {
			setSearchStatus(search_status_failed);
			m_route->setRouteStatus(ROUTE_FAILED);
		}
	}

	MultiLayerSearch::~MultiLayerSearch() {
	}

	void MultiLayerSearch::createSearchFrontier(int32_t startInt, CellCache* cache) {
		// reset all
		m_sortedFrontier.clear();
		m_spt.clear();
		m_sf.clear();
		m_gCosts.clear();
		// fill with defaults
		m_sortedFrontier.pushElement(PriorityQueue<int32_t, double>::value_type(startInt, 0.0));
		int32_t max_index = cache->getMaxIndex();
		m_spt.resize(max_index, -1);
		m_sf.resize(max_index, -1);
		m_gCosts.resize(max_index, 0.0);
		m_next = 0;
	}

	void MultiLayerSearch::updateSearch() {
		if (m_sortedFrontier.empty()) {
			if (!m_foundLast || m_lastDestCoordInt == m_destCoordInt || getSearchStatus() == search_status_failed) {
				setSearchStatus(search_status_failed);
				m_route->setRouteStatus(ROUTE_FAILED);
				return;
			}
			if (m_betweenTargets.empty()) {
				TransitionInfo* trans = m_currentCache->getCell(m_currentCache->convertIntToCoord(m_lastDestCoordInt))->getTransition();
				if (trans) {
					m_lastStartCoordInt = m_endCache->convertCoordToInt(trans->m_mc);
				}
				m_currentCache = m_endCache;
				m_lastDestCoordInt = m_currentCache->getCell(m_to.getLayerCoordinates())->getCellId();
			} else {
				if (m_lastDestCoordInt != -1) {
					TransitionInfo* trans = m_currentCache->getCell(m_currentCache->convertIntToCoord(m_lastDestCoordInt))->getTransition();
					if (trans) {
						m_lastStartCoordInt = trans->m_layer->getCellCache()->convertCoordToInt(trans->m_mc);
					}
				}
				m_currentCache = m_betweenTargets.front()->getLayer()->getCellCache();
				m_lastDestCoordInt = m_betweenTargets.front()->getCellId();
				m_betweenTargets.pop_front();
				m_foundLast = false;
			}
			createSearchFrontier(m_lastStartCoordInt, m_currentCache);
		}

		PriorityQueue<int32_t, double>::value_type topvalue = m_sortedFrontier.getPriorityElement();
		m_sortedFrontier.popElement();
		m_next = topvalue.first;
		m_spt[m_next] = m_sf[m_next];
		// found destination
		if (m_destCoordInt == m_next && m_betweenTargets.empty()) {
			if (m_endCache == m_currentCache) {
				setSearchStatus(search_status_complete);
				m_route->setRouteStatus(ROUTE_SEARCHED);
				return;
			}
		}

		// found between target
		if (m_lastDestCoordInt == m_next) {
			calcPathStep();
			m_sortedFrontier.clear();
			m_foundLast = true;
			return;
		}

		ModelCoordinate destCoord = m_currentCache->convertIntToCoord(m_lastDestCoordInt);
		ModelCoordinate nextCoord = m_currentCache->convertIntToCoord(m_next);
		CellGrid* grid = m_currentCache->getLayer()->getCellGrid();
		Cell* nextCell = m_currentCache->getCell(nextCoord);
		if (!nextCell) {
			return;
		}
		int32_t cellZ = nextCell->getLayerCoordinates().z;
		int32_t maxZ = m_route->getZStepRange();
		bool zLimited = maxZ != -1;
		uint8_t blockerThreshold = m_ignoreDynamicBlockers ? 2 : 1;
		bool limitedArea = m_route->isAreaLimited();
		const std::vector<Cell*>& adjacents = nextCell->getNeighbors();
		if (adjacents.empty()) {
			return;
		}
		for (std::vector<Cell*>::const_iterator i = adjacents.begin(); i != adjacents.end(); ++i) {
			if (*i == NULL) {
				continue;
			}
			if ((*i)->getLayer()->getCellCache() != m_currentCache) {
				continue;
			}
			int32_t adjacentInt = (*i)->getCellId();
			if (m_sf[adjacentInt] != -1 && m_spt[adjacentInt] != -1) {
				continue;
			}
			if (zLimited && ABS(cellZ-(*i)->getLayerCoordinates().z) > maxZ) {
				continue;
			}
			bool blocker = (*i)->getCellType() > blockerThreshold;
			ModelCoordinate adjacentCoord = (*i)->getLayerCoordinates();
			if ((adjacentInt == m_next || blocker) && adjacentInt != m_destCoordInt) {
				if (blocker && m_multicell) {
					std::vector<Cell*>::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), *i);
					if (bc_it == m_ignoredBlockers.end()) {
						continue;
					}
				} else {
					continue;
				}
			}
			// search if there are blockers which could block multicell object
			if (m_multicell) {
				blocker = false;
				Location currentLoc(nextCell->getLayer());
				currentLoc.setLayerCoordinates(nextCell->getLayerCoordinates());
				Location adjacentLoc((*i)->getLayer());
				adjacentLoc.setLayerCoordinates((*i)->getLayerCoordinates());

				int32_t rotation = getAngleBetween(currentLoc, adjacentLoc);
				std::vector<ModelCoordinate> coords = grid->toMultiCoordinates(adjacentLoc.getLayerCoordinates(), m_route->getOccupiedCells(rotation));
				std::vector<ModelCoordinate>::iterator coord_it = coords.begin();
				for (; coord_it != coords.end(); ++coord_it) {
					Cell* cell = m_currentCache->getCell(*coord_it);
					if (cell) {
						if (cell->getCellType() > blockerThreshold) {
							std::vector<Cell*>::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), cell);
							if (bc_it == m_ignoredBlockers.end()) {
								blocker = true;
								break;
							}
						}
						if (limitedArea) {
							// check if cell is on one of the areas
							bool sameAreas = false;
							const std::list<std::string> areas = m_route->getLimitedAreas();
							std::list<std::string>::const_iterator area_it = areas.begin();
							for (; area_it != areas.end(); ++area_it) {
								if (m_currentCache->isCellInArea(*area_it, cell)) {
									sameAreas = true;
									break;
								}
							}
							if (!sameAreas) {
								blocker = true;
								break;
							}
						}
					} else {
						blocker = true;
						break;
					}
				}
				if (blocker) {
					continue;
				}
			} else if (limitedArea) {
				// check if cell is on one of the areas
				bool sameAreas = false;
				const std::list<std::string> areas = m_route->getLimitedAreas();
				std::list<std::string>::const_iterator area_it = areas.begin();
				for (; area_it != areas.end(); ++area_it) {
					if (m_currentCache->isCellInArea(*area_it, *i)) {
						sameAreas = true;
						break;
					}
				}
				if (!sameAreas) {
					continue;
				}
			}

			double gCost = m_gCosts[m_next];
			if (m_specialCost) {
				gCost += m_currentCache->getAdjacentCost(adjacentCoord ,nextCoord, m_route->getCostId());
			} else {
				gCost += m_currentCache->getAdjacentCost(adjacentCoord ,nextCoord);
			}
			double hCost = grid->getHeuristicCost(adjacentCoord, destCoord);
			if (m_sf[adjacentInt] == -1) {
				m_sortedFrontier.pushElement(PriorityQueue<int32_t, double>::value_type(adjacentInt, gCost + hCost));
				m_gCosts[adjacentInt] = gCost;
				m_sf[adjacentInt] = m_next;
			} else if (gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
				m_sortedFrontier.changeElementPriority(adjacentInt, gCost + hCost);
				m_gCosts[adjacentInt] = gCost;
				m_sf[adjacentInt] = m_next;
			}
		}
	}

	void MultiLayerSearch::calcPathStep() {
		int32_t current = m_lastDestCoordInt;
		int32_t end = m_lastStartCoordInt;
		Location newnode(m_currentCache->getLayer());
		Path path;
		// This assures that the agent always steps into the center of the target cell.
		newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current));
		path.push_back(newnode);
		while(current != end) {
			if (m_spt[current] < 0 ) {
				// This is when the size of m_spt can not handle the distance of the location
				setSearchStatus(search_status_failed);
				m_route->setRouteStatus(ROUTE_FAILED);
				break;
			}
			current = m_spt[current];
			newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current));
			path.push_front(newnode);
		}
		// set exact start coordinates
		if (m_path.empty()) {
			path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinatesRef());
		}
		m_path.insert(m_path.end(), path.begin(), path.end());
	}

	void MultiLayerSearch::calcPath() {
		int32_t current = m_lastDestCoordInt;
		int32_t end = m_lastStartCoordInt;
		Location newnode(m_currentCache->getLayer());
		Path path;
		// This assures that the agent always steps into the center of the target cell.
		newnode.setLayerCoordinates(
			m_currentCache->getCell(m_currentCache->convertIntToCoord(current))->getLayerCoordinates());
		path.push_back(newnode);
		while(current != end) {
			if (m_spt[current] < 0 ) {
				// This is when the size of m_spt can not handle the distance of the location
				setSearchStatus(search_status_failed);
				m_route->setRouteStatus(ROUTE_FAILED);
				break;
			}
			current = m_spt[current];
			newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current));
			path.push_front(newnode);
		}
		m_path.insert(m_path.end(), path.begin(), path.end());
		m_route->setPath(m_path);
	}

	void MultiLayerSearch::searchBetweenTargetsNeighbor() {
		std::vector<Cell*> cells = m_startCache->getTransitionCells(m_endCache->getLayer());
		if (!cells.empty()) {
			Location loc;
			Cell* cell = NULL;
			// find nearest portal (air-line distance)
			for (std::vector<Cell*>::iterator it = cells.begin(); it != cells.end(); ++it) {
				if ((*it)->getZone() != m_startZone) {
					continue;
				}
				TransitionInfo* trans = (*it)->getTransition();
				Cell* transTarget = trans->m_layer->getCellCache()->getCell(trans->m_mc);
				if (transTarget->getZone() != m_endZone) {
					continue;
				}
				if (!cell) {
					loc.setLayer((*it)->getLayer());
					loc.setLayerCoordinates((*it)->getLayerCoordinates());
					cell = *it;
					continue;
				}
				Location temp((*it)->getLayer());
				temp.setLayerCoordinates((*it)->getLayerCoordinates());
				Location loc1(cell->getTransition()->m_layer);
				loc1.setLayerCoordinates(cell->getTransition()->m_mc);
				Location loc2((*it)->getTransition()->m_layer);
				loc2.setLayerCoordinates((*it)->getTransition()->m_mc);

				double temp_distance = temp.getLayerDistanceTo(m_from) + loc2.getLayerDistanceTo(m_to);
				double current_distance = loc.getLayerDistanceTo(m_from) + loc1.getLayerDistanceTo(m_to);
				if (current_distance > temp_distance) {
					loc = temp;
					cell = *it;
				}
			}
			if (cell) {
				m_betweenTargets.push_back(cell);
			}
		}
	}

	void MultiLayerSearch::searchBetweenTargetsMap() {
		// in case no transition is there then return
		std::vector<Cell*> endTransCells;
		std::vector<Cell*> tmpTransCells = m_endCache->getTransitionCells();
		std::vector<Cell*>::iterator tcell_it = tmpTransCells.begin();
		for (; tcell_it != tmpTransCells.end(); ++tcell_it) {
			Zone* zone = (*tcell_it)->getZone();
			if (zone == m_endZone) {
				endTransCells.push_back(*tcell_it);
			}
		}
		if (endTransCells.empty()) {
			return;
		}
		// in case no transition is there then return
		std::vector<Cell*> startTransCells;
		tmpTransCells = m_startCache->getTransitionCells();
		tcell_it = tmpTransCells.begin();
		for (; tcell_it != tmpTransCells.end(); ++tcell_it) {
			Zone* zone = (*tcell_it)->getZone();
			if (zone == m_startZone) {
				startTransCells.push_back(*tcell_it);
			}
		}
		if (startTransCells.empty()) {
			return;
		}

		// fetch zones
		std::vector<Zone*> zones;
		const std::list<Layer*>& allLayers = m_from.getLayer()->getMap()->getLayers();
		std::list<Layer*>::const_iterator lay_it = allLayers.begin();
		for (; lay_it != allLayers.end(); ++lay_it) {
			CellCache* cache = (*lay_it)->getCellCache();
			if (cache) {
				const std::vector<Zone*>& tmp_zones = cache->getZones();
				zones.insert(zones.end(), tmp_zones.begin(), tmp_zones.end());
			}
		}

		// sort zones by iterator distance
		std::map<Zone*, int32_t> zoneDistanceMap;
		std::map<int32_t, Zone*> distanceZoneMap;
		for (std::vector<Zone*>::iterator zit = zones.begin(); zit != zones.end(); ++zit) {
			// pseudo distance
			int32_t distance = std::distance(zones.begin(), zit);
			zoneDistanceMap.insert(std::pair<Zone*, int32_t>(*zit, distance));
			distanceZoneMap.insert(std::pair<int32_t, Zone*>(distance, *zit));
		}
		// start zone
		int32_t startZone = zoneDistanceMap.find(m_startZone)->second;
		// target zone
		int32_t targetZone = zoneDistanceMap.find(m_endZone)->second;
		// Priority queue to sort zones
		PriorityQueue<int32_t, double> sortedfrontier;
		// add start zone
		sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(startZone, 0.0));
		// max size zones
		int32_t max_index = zones.size();
		// shortest tree
		std::vector<int32_t> spt(max_index, -1);
		// search frontier
		std::vector<int32_t> sf(max_index, -1);
		// costs
		std::vector<double> costs(max_index, 0.0);
		bool found = false;
		while (!found) {
			if (sortedfrontier.empty()) {
				break;
			}
			PriorityQueue<int32_t, double>::value_type topvalue = sortedfrontier.getPriorityElement();
			sortedfrontier.popElement();
			int32_t next = topvalue.first;
			spt[next] = sf[next];
			// found destination zone
			if (targetZone == next) {
				found = true;
				break;
			}
			Zone* nextZone = distanceZoneMap.find(next)->second;
			// look in zone for tranistions
			std::vector<Cell*> transCells = nextZone->getTransitionCells();
			if (transCells.empty()) {
				continue;
			}
			std::vector<Cell*>::iterator cell_it = transCells.begin();
			for (; cell_it != transCells.end(); ++cell_it) {
				// transition info
				TransitionInfo* trans = (*cell_it)->getTransition();
				// target transition cache
				CellCache* transCache = trans->m_layer->getCellCache();
				// target transition cell
				Cell* transCelle = transCache->getCell(trans->m_mc);
				// next zone as int
				int32_t nextInt =  zoneDistanceMap.find(transCelle->getZone())->second;
				if (nextInt == next && nextInt != targetZone) {
					continue;
				}
				// iterator distance as cost
				double cost = costs[next] + static_cast<double>(ABS(nextInt-startZone));
				if (sf[nextInt] == -1) {
					sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(nextInt, cost));
					costs[nextInt] = cost;
					sf[nextInt] = next;
				} else if (cost < costs[nextInt] && spt[nextInt] == -1) {
					sortedfrontier.changeElementPriority(nextInt, cost);
					costs[nextInt] = cost;
					sf[nextInt] = next;
				}
			}
		}
		// startZone to endZone could be solved
		if (found) {
			// fetch sorted zones
			int32_t current = targetZone;
			int32_t end = startZone;

			std::list<Zone*> betweenZones;
			Zone* tempZone = distanceZoneMap.find(current)->second;
			betweenZones.push_back(tempZone);
			while(current != end) {
				if (spt[current] < 0 ) {
					return;
				}
				current = spt[current];
				tempZone = distanceZoneMap.find(current)->second;
				betweenZones.push_front(tempZone);
			}
			// so here we fetch the transistions
			Location lastLoc = m_from;
			for (std::list<Zone*>::iterator lit = betweenZones.begin(); lit != betweenZones.end(); ++lit) {
				++lit;
				if (lit == betweenZones.end()) {
					break;
				}
				Zone* nextZone = *lit;
				--lit;
				Zone* currentZone = *lit;
				std::vector<Cell*> tempCells = currentZone->getTransitionCells();
				if (tempCells.empty()) {
					continue;
				}

				Cell* transCell = NULL;
				double nextCost = 0.0;
				Location newLoc = lastLoc;
				for (std::vector<Cell*>::iterator cit = tempCells.begin(); cit != tempCells.end(); ++cit) {
					Cell* nextCell = *cit;
					TransitionInfo* nextTrans = nextCell->getTransition();
					Cell* transTargetCell = nextTrans->m_layer->getCellCache()->getCell(nextTrans->m_mc);
					// skip wrong transitions
					if (nextZone != transTargetCell->getZone()) {
						continue;
					}
					// nearest transistion (air-line distance)
					Location tmpLoc((*cit)->getLayer());
					tmpLoc.setLayerCoordinates((*cit)->getLayerCoordinates());
					double locCost = lastLoc.getLayerDistanceTo(tmpLoc);
					if (!transCell || locCost < nextCost) {
						nextCost = locCost;
						transCell = *cit;
						newLoc.setLayer(transTargetCell->getLayer());
						newLoc.setLayerCoordinates(transTargetCell->getLayerCoordinates());
					}
				}
				// found valid transition cell
				if (transCell) {
					m_betweenTargets.push_back(transCell);
					lastLoc = newLoc;
				}
			}
		}
	}
}
