// SquareGrid.hpp
//
//     Created on: 30. 01. 2018
//         Author: Jan Uhlik
//    Modified by:
//
// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library

#ifndef ALIB2_SQUAREGRID_HPP
#define ALIB2_SQUAREGRID_HPP

#include <alib/vector>
#include <alib/set>
#include <alib/pair>
#include <alib/map>
#include <alib/tuple>
#include <cmath>
#include <functional>
#include <object/Object.h>
#include <core/normalize.hpp>

#include <common/Normalize.hpp>
#include <grid/common/GridFunction.hpp>
#include <grid/common/GridDirection.hpp>
#include <grid/GridFeatures.hpp>
#include <grid/GridInterface.hpp>

namespace grid {

template<typename TCoordinate, typename TEdge>
class SquareGrid : public GridInterface<TCoordinate, TEdge> {
// ---------------------------------------------------------------------------------------------------------------------

 public:
  using coordinate_type = TCoordinate;
  using edge_type = TEdge;
  using node_type = ext::pair<TCoordinate, TCoordinate>;
  using direction_type = SquareGridDirections;

// ---------------------------------------------------------------------------------------------------------------------

 protected:
  TCoordinate m_height;
  TCoordinate m_width;
  ext::set<node_type> m_obstacles;

// =====================================================================================================================
// Constructor, Destructor, Operators
 public:
  explicit SquareGrid(TCoordinate height, TCoordinate width);

// ---------------------------------------------------------------------------------------------------------------------

  const ext::set<node_type> &getObstacleList() const &;
  ext::set<node_type> &&getObstacleList() &&;

// ---------------------------------------------------------------------------------------------------------------------

// =====================================================================================================================
// GridBase interface
 public:

  void operator>>(std::ostream &ostream) const override;

  explicit operator std::string() const override;

// =====================================================================================================================
// SquareGrid interface

 public:
  TCoordinate getHeight() const;
  TCoordinate getWidth() const;
  virtual void resize(TCoordinate height, TCoordinate width);

// ---------------------------------------------------------------------------------------------------------------------

  virtual void addObstacle(const node_type &n);
  virtual void addObstacle(node_type &&n);
  virtual void addObstacle(TCoordinate &&x, TCoordinate &&y);

// ---------------------------------------------------------------------------------------------------------------------

  virtual ext::pair<bool, node_type> step(const node_type &n, direction_type direction) const;
  virtual ext::pair<bool, TEdge> stepEdge(const node_type &n, direction_type direction) const;

// ---------------------------------------------------------------------------------------------------------------------

  virtual bool isObstacle(const node_type &n) const;
  virtual bool isNode(const node_type &n) const;

// ---------------------------------------------------------------------------------------------------------------------

  virtual bool isValidDirection(direction_type direction) const = 0;

// ---------------------------------------------------------------------------------------------------------------------

  virtual std::string toStringAs(const ext::map<node_type, char> &map) const;

// ---------------------------------------------------------------------------------------------------------------------

 protected:
  virtual bool checkCoordinates(const node_type &coordinate) const;
  virtual void throwCoordinates(const node_type &coordinate) const;

// ---------------------------------------------------------------------------------------------------------------------

  virtual TEdge createEdge(const node_type &a, const node_type &b) const = 0;

// ---------------------------------------------------------------------------------------------------------------------

// =====================================================================================================================
// GraphInterface interface

 public:
  size_t nodeCount() const override;
  size_t edgeCount() const override;

// ---------------------------------------------------------------------------------------------------------------------

  ext::set<node_type> getNodes() const override;
  ext::vector<TEdge> getEdges() const override;

// ---------------------------------------------------------------------------------------------------------------------

  ext::set<node_type> successors(const node_type &n) const override;
  ext::vector<TEdge> successorEdges(const node_type &n) const override;
  ext::set<node_type> predecessors(const node_type &n) const override;
  ext::vector<TEdge> predecessorEdges(const node_type &n) const override;


// ---------------------------------------------------------------------------------------------------------------------

};

// =====================================================================================================================

template<typename TCoordinate, typename TEdge>
SquareGrid<TCoordinate, TEdge>::SquareGrid(TCoordinate height, TCoordinate width)
    : m_height(height), m_width(width), m_obstacles(ext::set<node_type>()) {
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
const ext::set<typename SquareGrid<TCoordinate, TEdge>::node_type> &SquareGrid<TCoordinate,
                                                                               TEdge>::getObstacleList() const &{
  return m_obstacles;
}

template<typename TCoordinate, typename TEdge>
ext::set<typename SquareGrid<TCoordinate, TEdge>::node_type> &&SquareGrid<TCoordinate,
                                                                          TEdge>::getObstacleList() &&{
  return std::move(m_obstacles);
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
TCoordinate SquareGrid<TCoordinate, TEdge>::getHeight() const {
  return m_height;
}

template<typename TCoordinate, typename TEdge>
TCoordinate SquareGrid<TCoordinate, TEdge>::getWidth() const {
  return m_width;
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
void SquareGrid<TCoordinate, TEdge>::resize(TCoordinate height, TCoordinate width) {
  if (m_width <= width && m_height <= height) {
    m_width = width;
    m_height = height;
    return;
  }

  m_width = width;
  m_height = height;

  ext::set<node_type> new_obstacles;

  for (auto i: m_obstacles) {
    if (checkCoordinates(i)) {
      new_obstacles.insert(std::move(i));
    }
  }

  m_obstacles = new_obstacles;
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
void SquareGrid<TCoordinate, TEdge>::addObstacle(const node_type &n) {
  throwCoordinates(n);
  m_obstacles.insert(n);
}

template<typename TCoordinate, typename TEdge>
void SquareGrid<TCoordinate, TEdge>::addObstacle(node_type &&n) {
  throwCoordinates(n);

  m_obstacles.insert(std::forward<node_type>(n));
}

template<typename TCoordinate, typename TEdge>
void SquareGrid<TCoordinate, TEdge>::addObstacle(TCoordinate &&x, TCoordinate &&y) {
  addObstacle(ext::make_pair(std::forward<TCoordinate>(x), std::forward<TCoordinate>(y)));
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
ext::pair<bool, typename SquareGrid<TCoordinate, TEdge>::node_type>
SquareGrid<TCoordinate, TEdge>::step(const SquareGrid::node_type &n,
                                     SquareGrid::direction_type direction) const {
  node_type v = GridFunction::squareGridDirectionVector<TCoordinate>(direction);
  node_type tmp(n.first + v.first, n.second + v.second);
  bool validity = checkCoordinates(tmp) && !isObstacle(tmp) && isValidDirection(direction);

  return ext::make_pair(validity, tmp); // Bool if node is in grid
}

template<typename TCoordinate, typename TEdge>
ext::pair<bool, TEdge>
SquareGrid<TCoordinate, TEdge>::stepEdge(const SquareGrid::node_type &n,
                                         SquareGrid::direction_type direction) const {
  node_type v = GridFunction::squareGridDirectionVector<TCoordinate>(direction);
  node_type tmp(n.first + v.first, n.second + v.second);
  bool validity = checkCoordinates(tmp) && !isObstacle(tmp) && isValidDirection(direction);

  return ext::make_pair(validity, createEdge(n, tmp)); // Bool if node is in grid
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
bool SquareGrid<TCoordinate, TEdge>::isObstacle(const SquareGrid::node_type &n) const {
  return m_obstacles.find(n) != m_obstacles.end();
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
bool SquareGrid<TCoordinate, TEdge>::isNode(const SquareGrid::node_type &n) const {
  return !isObstacle(n);
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
std::string SquareGrid<TCoordinate, TEdge>::toStringAs(const ext::map<node_type,
                                                                      char> &map) const {
  std::string os;

  os += '+';
  for (TCoordinate i = 0; i < m_width; ++i) {
    os += "—";
  }
  os += "+\n";

  for (TCoordinate i = 0; i < m_height; ++i) {
    os += '|';

    for (TCoordinate j = 0; j < m_width; ++j) {
      node_type tmp = ext::make_pair(i, j);

      if (m_obstacles.find(tmp) != m_obstacles.end()) {
        os += "#";
      } else {

        if (map.find(tmp) != map.end()) {
          os += map.at(tmp);
        } else {
          os += ".";
        }
      }

    }

    os += "|\n";
  }

  os += '+';
  for (TCoordinate i = 0; i < m_width; ++i) {
    os += "—";
  }
  os += "+\n";

  return os;
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
bool SquareGrid<TCoordinate, TEdge>::checkCoordinates(const node_type &coordinate) const {
  return coordinate.first >= 0 &&
      coordinate.first < m_height &&
      coordinate.second >= 0 &&
      coordinate.second < m_width;
}

template<typename TCoordinate, typename TEdge>
void SquareGrid<TCoordinate, TEdge>::throwCoordinates(const node_type &coordinate) const {
  if (!checkCoordinates(coordinate)) {
    throw std::out_of_range("Coordinates are out of range");
  }
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
size_t SquareGrid<TCoordinate, TEdge>::nodeCount() const {
  return m_height * m_width - m_obstacles.size();
}

template<typename TCoordinate, typename TEdge>
size_t SquareGrid<TCoordinate, TEdge>::edgeCount() const {
  unsigned long cnt = 0;

  ext::set<direction_type> directions = {SquareGridDirections::south, SquareGridDirections::east};
  if (isValidDirection(SquareGridDirections::south_east)) {
    directions.insert(SquareGridDirections::south_east);
  }

  if (isValidDirection(SquareGridDirections::south_west)) {
    directions.insert(SquareGridDirections::south_west);
  }

  for (TCoordinate i = 0; i < m_height; ++i) {
    for (TCoordinate j = 0; j < m_width; ++j) {
      node_type n = ext::make_pair(i, j);

      for (const auto &direction : directions) {
        node_type v = GridFunction::squareGridDirectionVector<TCoordinate>(direction);
        node_type tmp = ext::make_pair(n.first + v.first, n.second + v.second);

        if (checkCoordinates(tmp) && !isObstacle(tmp)) {
          ++cnt;
        }
      }

    }
  }
  return cnt;
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
ext::set<typename SquareGrid<TCoordinate, TEdge>::node_type>
SquareGrid<TCoordinate, TEdge>::getNodes() const {
  ext::set<node_type> set;

  for (TCoordinate i = 0; i < m_height; ++i) {
    for (TCoordinate j = 0; j < m_width; ++j) {
      node_type tmp = ext::make_pair(i, j);

      if (!isObstacle(tmp)) {
        set.insert(std::move(tmp));
      }
    }
  }

  return set;
}

template<typename TCoordinate, typename TEdge>
ext::vector<TEdge> SquareGrid<TCoordinate, TEdge>::getEdges() const {
  ext::vector<TEdge> vec;

  ext::set<direction_type> directions = {SquareGridDirections::south, SquareGridDirections::east};
  if (isValidDirection(SquareGridDirections::south_east)) {
    directions.insert(SquareGridDirections::south_east);
  }

  if (isValidDirection(SquareGridDirections::south_west)) {
    directions.insert(SquareGridDirections::south_west);
  }

  for (TCoordinate i = 0; i < m_height; ++i) {
    for (TCoordinate j = 0; j < m_width; ++j) {
      node_type n = ext::make_pair(i, j);

      for (const auto &direction : directions) {
        node_type v = GridFunction::squareGridDirectionVector<TCoordinate>(direction);
        node_type tmp = ext::make_pair(n.first + v.first, n.second + v.second);

        if (checkCoordinates(tmp) && !isObstacle(tmp)) {
          vec.push_back(createEdge(n, tmp));
        }
      }

    }
  }

  return vec;
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
ext::set<typename SquareGrid<TCoordinate, TEdge>::node_type>
SquareGrid<TCoordinate,
           TEdge>::successors(const SquareGrid::node_type &n) const {
  throwCoordinates(n);

  ext::set<node_type> set;

  if (m_obstacles.find(n) != m_obstacles.end()) {
    return set;
  }

  for (const auto &direction : SQUARE_GRID_DIRECTIONS) {
    if (!isValidDirection(direction)) continue;

    node_type v = GridFunction::squareGridDirectionVector<TCoordinate>(direction);
    node_type tmp = ext::make_pair(n.first + v.first, n.second + v.second);

    if (checkCoordinates(tmp) && !isObstacle(tmp)) {
      set.insert(std::move(tmp));
    }
  }

  return set;
}

template<typename TCoordinate, typename TEdge>
ext::vector<TEdge>
SquareGrid<TCoordinate, TEdge>::successorEdges(const SquareGrid::node_type &n) const {
  throwCoordinates(n);

  ext::vector<TEdge> vec;

  if (m_obstacles.find(n) != m_obstacles.end()) {
    return vec;
  }

  for (const auto &direction : SQUARE_GRID_DIRECTIONS) {
    if (!isValidDirection(direction)) continue;
    node_type v = GridFunction::squareGridDirectionVector<TCoordinate>(direction);
    node_type tmp = ext::make_pair(n.first + v.first, n.second + v.second);

    if (checkCoordinates(tmp) && !isObstacle(tmp)) {
      vec.push_back(createEdge(n, tmp));
    }
  }

  return vec;
}

template<typename TCoordinate, typename TEdge>
ext::set<typename SquareGrid<TCoordinate, TEdge>::node_type>
SquareGrid<TCoordinate, TEdge>::predecessors(const node_type &n) const {
  return this->successors(n);
}

template<typename TCoordinate, typename TEdge>
ext::vector<TEdge>
SquareGrid<TCoordinate, TEdge>::predecessorEdges(const node_type &n) const {
  return this->successorEdges(n);
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TCoordinate, typename TEdge>
void SquareGrid<TCoordinate, TEdge>::operator>>(std::ostream &ostream) const {
  ostream << "(" << this->name() << std::endl;
  ostream << '+';
  for (TCoordinate i = 0; i < m_width; ++i) {
    ostream << "—";
  }
  ostream << "+\n";

  for (TCoordinate i = 0; i < m_height; ++i) {
    ostream << '|';

    for (TCoordinate j = 0; j < m_width; ++j) {
      node_type tmp = ext::make_pair(i, j);

      if (m_obstacles.find(tmp) != m_obstacles.end()) {
        ostream << "#";
      } else {
        ostream << ".";
      }

    }

    ostream << "|\n";
  }

  ostream << '+';
  for (TCoordinate i = 0; i < m_width; ++i) {
    ostream << "—";
  }
  ostream << "+\n";

  ostream << ")";
}

template<typename TCoordinate, typename TEdge>
SquareGrid<TCoordinate, TEdge>::operator std::string() const {
  std::stringstream ss;
  ss << "(" << this->name() << std::endl;
  ss << '+';
  for (TCoordinate i = 0; i < m_width; ++i) {
    ss << "—";
  }
  ss << "+\n";

  for (TCoordinate i = 0; i < m_height; ++i) {
    ss << '|';

    for (TCoordinate j = 0; j < m_width; ++j) {
      node_type tmp = ext::make_pair(i, j);

      if (m_obstacles.find(tmp) != m_obstacles.end()) {
        ss << "#";
      } else {
        ss << ".";
      }

    }

    ss << "|\n";
  }

  ss << '+';
  for (TCoordinate i = 0; i < m_width; ++i) {
    ss << "—";
  }
  ss << "+\n";

  ss << ")";
  return std::move(ss).str();
}

// ---------------------------------------------------------------------------------------------------------------------

// =====================================================================================================================

} // namespace grid
#endif //ALIB2_SQUAREGRID_HPP
