from typing import Optional, List, Dict, Tuple, Any
import os
import json
import string
import logging

from PyQt5.QtGui import QVector2D as Vector2, QVector3D as Vector3, QMatrix4x4 as Matrix4x4, QQuaternion as Quaternion
from north_c9.controller import C9Controller
from north_utils.encoding import convert_base
from north_utils.dict import AttrDict


logger = logging.getLogger(__name__)

SpacingType = Tuple[int, int]
LocationGridType = List[List['Location']]
LocationListType = List['Location']
TrayLocationsType = Dict[str, 'Location']


class ComponentError(Exception):
    pass


class ComponentChildNotFoundError(ComponentError):
    pass


class LocationError(Exception):
    pass


class LocationChildExistsError(LocationError):
    pass


class LocationChildNotFoundError(LocationError):
    pass


class ComponentSettingsError(Exception):
    pass


class ComponentSettingsJsonError(ComponentSettingsError):
    pass


class Location:
    """
    An object representing a position and rotation in 3D space.

    Location objects store the position and rotation in local coordinates which are translated on-the-fly to "world"
    or "root" coordinates for ease of use. Internally a 4x4 transformation matrix is constructed based on the parent-
    child relationship of the location which is used to transform positions between local and world coordinates sytems.

    Positions are stored as a Vector3 (or QVector3d) object and rotations are stored as a Quaternion (or QQuaternion).
    The lower level classes are from the PyQt4 library and handle all of the linear algebra needed.

    Note: Quaternions aren't the most intuitive way to work with rotations, so the Location class tries to use Euler
    angles (x / pitch, y / roll and z / yaw) wherever possible. The Quaternion class used provides `toEulerAngles` and
    `Quaternion.fromEulerAngles(x, y, z)` methods to convert to and from Euler angles when needed.
    """
    root: 'Location' = None

    @classmethod
    def find_root(cls) -> 'Location':
        """
        Returns the root (top-most) location, creating one if needed

        :return: root location
        """
        if cls.root is None:
            cls.root = Location(is_root=True)

        return cls.root

    @classmethod
    def location_from_origin_and_x_vector(cls, origin: Vector3, x_vector: Vector3, parent: Optional['Location']=None) -> 'Location':
        """
        Create a new location using the given origin as a position, with the rotation calculated from the given x axis vector

        :param origin: origin vector to use as position
        :param x_vector: x axis vector used to calculate rotation
        :param parent: [optional] parent location for newly craeted location
        :return: location
        """
        z = Vector3(0, 0, 1)  # Assume we haven't rotated our z vector at all
        x = (x_vector - origin).normalized()
        y = Vector3.crossProduct(z, x)
        rotation = Quaternion.fromAxes(x, y, z)

        return Location(local_position=origin, local_rotation=rotation, parent=parent)

    def __init__(self, x: Optional[float]=None, y: Optional[float]=None, z: Optional[float]=None,
                 rx: Optional[float]=None, ry: Optional[float]=None, rz: Optional[float]=None,
                 position: Optional[Vector3]=None, rotation: Optional[Quaternion]=None,
                 local_position: Optional[Vector3]=None, local_rotation: Optional[Quaternion]=None,
                 component: Optional['Component']=None, parent: Optional['Location']=None, children: Optional['List']=None,
                 is_root: bool=False, located: bool=True, use_probe: bool=False) -> None:
        """
        :param x: x position in world coordinates
        :param y: y position in world coordinates
        :param z: z position in world coordinates
        :param rx: x / pitch rotation in world coordinates
        :param ry: y / roll rotation in world coordinates
        :param rz: z / yaw rotation in world coordinates
        :param position: position vector in world coordinates
        :param rotation: rotation quaternion in world coordinates
        :param local_position: position vector in local coordinates
        :param local_rotation: rotation quaternion in local coordinates
        :param component: component that is referencing this location
        :param parent: location that this location's local coordinates are relative to
        :param children: list of child locations
        :param located: whether or not this location has been "located" by the robot (is not a placeholder)
        :param use_probe: use the robot probe when "locating" (driving the robot to) this location
        """
        self._parent: Location = None
        self.parent = Location.find_root() if parent is None and not is_root else parent
        self.children: List['Location'] = children if children is not None else []
        self.root: Optional[Location] = self.parent.root if self.parent is not None else None
        self._component: Optional[Component] = None
        self.component = component
        self.transform = Matrix4x4()
        self.inverted_transform = Matrix4x4()
        self.located = located
        self.use_probe = use_probe

        if local_position is not None:
            self.local_position = Vector3(local_position)
        elif self.parent is not None and position is not None:
            self.local_position = self.parent.world_position_to_local(position)
        elif self.parent is not None and (x is not None or y is not None or z is not None):
            self.local_position = self.parent.world_position_to_local(Vector3(x or 0, y or 0, z or 0))
        else:
            self.local_position = Vector3(x or 0, y or 0, z or 0)

        if local_rotation is not None:
            self.local_rotation = Quaternion(local_rotation)
        elif self.parent is not None and rotation is not None:
            self.local_rotation = self.parent.world_rotation_to_local(rotation)
        elif self.parent is not None and (rx is not None or ry is not None or rz is not None):
            self.local_rotation = self.parent.world_rotation_to_local(Quaternion.fromEulerAngles(rx or 0, ry or 0, rz or 0))
        else:
            self.local_rotation = Quaternion.fromEulerAngles(rx or 0, ry or 0, rz or 0)

        self.update_transform()

    def __repr__(self) -> str:
        return f'{self.__class__.__name__}({self.x}, {self.y}, {self.z}, {self.rx}, {self.ry}, {self.rz})'

    @property
    def parent(self) -> 'Location':
        """ Get or set the parent location """
        return self._parent

    @parent.setter
    def parent(self, value: 'Location'):
        self._parent = value
        if self._parent is not None:
            self._parent.add_child(self)

    def update_transform(self):
        self.transform = Matrix4x4()
        self.transform.translate(self.local_position)
        self.transform.rotate(self.local_rotation)
        self.inverted_transform = self.transform.inverted()[0]

    @property
    def world_to_local_transform(self):
        """ Calculate a transformation matrix for transforming from the world to the local reference frame """
        if self.parent is None:
            return self.transform

        return self.parent.world_to_local_transform * self.inverted_transform

    @property
    def local_to_world_transform(self):
        """ Calculate a transformation matrix for transforming from the local to the world reference frame """
        if self.parent is None:
            return self.inverted_transform

        return self.parent.local_to_world_transform * self.transform

    @property
    def component(self) -> Optional['Component']:
        """ Get or set the Component instance associated with this Location """
        return self._component

    @component.setter
    def component(self, component: 'Component'):
        self._component = component

    @property
    def position(self) -> Vector3:
        """ Get or set the position of this location using the world reference frame """
        if self.parent is None:
            return self.local_position

        return self.parent.local_position_to_world(self.local_position)

    @position.setter
    def position(self, position: Vector3):
        if self.parent is None:
            self.local_position = position
        else:
            self.local_position = self.parent.world_position_to_local(position)

    @property
    def x(self):
        """ Get or set the x position of this location in the world reference frame """
        return self.position[0]

    @x.setter
    def x(self, value: float):
        self.local_position.setX(self.world_position_to_local(Vector3(value, 0, 0)).x())

    @property
    def y(self):
        """ Get or set the y position of this location in the world reference frame """
        return self.position[1]

    @y.setter
    def y(self, value: float):
        self.local_position.setY(self.world_position_to_local(Vector3(0, value, 0)).y())

    @property
    def xy(self) -> Dict[str, float]:
        """ Get a dictionary with the x and y position for this location """
        return {
            'x': self.x,
            'y': self.y
        }

    @property
    def z(self):
        """ Get or set the y position of this location in the world reference frame """
        return self.position[2]

    @z.setter
    def z(self, value: float):
        self.local_position.setZ(self.world_position_to_local(Vector3(0, 0, value)).z())

    @property
    def rotation(self):
        """ Get or set the rotation of this location in the world reference frame """
        if self.parent is None:
            return self.local_rotation

        return self.parent.local_rotation_to_world(self.local_rotation)

    @rotation.setter
    def rotation(self, rotation: Quaternion):
        if self.parent is None:
            self.local_rotation = rotation
        else:
            self.local_rotation = self.world_rotation_to_local(rotation)

    @property
    def rx(self):
        """ Get the rotation around the x axis in the world reference frame """
        return self.rotation.getEulerAngles()[0]

    @property
    def ry(self):
        """ Get the rotation around the y axis in the world reference frame """
        return self.rotation.getEulerAngles()[1]

    @property
    def rz(self):
        """ Get the rotation around the z axis in the world reference frame """
        return self.rotation.getEulerAngles()[2]

    def copy(self, **kwargs):
        """
        Create and return a copy of this location instance

        :param kwargs: optional kwargs to change on the copied location
        :return: copied location
        """
        location_copy = self.__class__(local_position=self.local_position, local_rotation=self.local_rotation, parent=self.parent,
                                       component=self._component, children=self.children, located=self.located, use_probe=self.use_probe)

        for name, value in kwargs.items():
            setattr(location_copy, name, value)

        return location_copy

    def copy_from_list(self, location_list: List[float], parent: Optional['Location']=None) -> 'Location':
        """
        Create and return a copy of this location instance using the given list as arguments to the constructor

        :param location_list: list of arguments for Location constructor
        :param parent: optional parent for location copy
        :return: copied location
        """
        return self.copy().update_from_list(location_list, parent)

    def to_list(self) -> List[float]:
        """
        Create a list with the current location's x, y, z, rx, ry and rz that can be used with the copy and update from list methods
        :return: list of Location constructor arguments
        """
        pos = self.position
        rot = self.rotation.toEulerAngles()
        return [pos[0], pos[1], pos[2], rot[0], rot[1], rot[2]]

    def update_from_list(self, location_list: List[float], parent: Optional['Location']=None) -> 'Location':
        """
        Update the current location using a location list

        :param location_list: list of x, y, z, rx, ry and rz elements
        :param parent: optional parent for location copy
        :return: self
        """
        if parent is not None:
            self.parent = parent

        self.position = Vector3(*location_list[0:3])
        self.rotation = Quaternion.fromEulerAngles(*location_list[3:6])
        self.located = True

        self.update_transform()

        return self

    def world_position_to_local(self, position: Vector3) -> Vector3:
        """
        Convert a position in the world coordinate system to this component's local coordinate system

        :param position: world position vector
        :return: local position vector
        """
        if self.parent is None:
            return position

        return self.world_to_local_transform * position

    def local_position_to_world(self, position: Vector3) -> Vector3:
        """
        Convert a position in this component's local coordinate system to the world coordinate system

        :param position: local position vector
        :return: world position vector
        """
        if self.parent is None:
            return position

        return self.local_to_world_transform * position

    def world_rotation_to_local(self, rotation: Quaternion) -> Quaternion:
        """
        Convert a rotation in the world coordinate system to this component's local coordinate system

        :param position: world rotatation quaternion
        :return: local rotation quaternion
        """
        if self.parent is None:
            return rotation

        return self.parent.rotation.inverted() * rotation

    def local_rotation_to_world(self, rotation: Quaternion) -> Quaternion:
        """
        Convert a rotation this component's local coordinate system to the world coordinate system

        :param position: world rotatation quaternion
        :return: local rotation quaternion
        """
        if self.parent is None:
            return rotation

        return self.parent.rotation * rotation

    def add_child(self, location: 'Location'):
        """
        Add a child location, replacing the given location's parent if needed

        :param location: child location
        """
        if location in self.children:
            raise LocationChildExistsError(f'Location child already exists')

        if location.parent is not None:
            location.parent.remove_child(location, ignore_missing=True)

        self.children.append(location)

        location._parent = self

    def remove_child(self, location: 'Location', ignore_missing: bool=False):
        """
        Remove the given location from this location's list of children, along with the location parent

        :param location: child location to remove
        :param ignore_missing: don't raise an exception if location is not a child
        :raises LocationChildNotFoundError: raised if location is not a child
        """
        if location not in self.children:
            if ignore_missing:
                return

            raise LocationChildNotFoundError(f'Unable to remove, child not found')

        self.children.remove(location)
        location._parent = None


class LocationPlaceholder(Location):
    """
    A location instance that can be used as a "placeholder" location that can be located by driving a robot to it.

    LocationPlaceholder instances can be used as class variables in Component classes. These locations will be updated
    from data in a components.json file if one exists, or can be located by driving a robot using the Component.locate_all
    method. These updated placeholder location will be added as insance attributes to the newly-created component.

    Usage:

        class MyComponent(Component):
            location_one = PlaceholderLocation()
            location_two = PlaceholderLocation()
    """
    pass


class ComponentSettings:
    """
    Loads and saves component settings to a component.json file
    """
    data: Dict[str, Dict[str, Any]] = {}

    @classmethod
    def load_settings(cls, filepath: str) -> Dict[str, Any]:
        if filepath in cls.data:
            return cls.data[filepath]

        try:
            with open(filepath, 'r') as settings_file:
                cls.data[filepath] = json.loads(settings_file.read())
        except json.decoder.JSONDecodeError as err:
            raise ComponentSettingsJsonError(err)
        except FileNotFoundError:
            cls.data[filepath] = {}

        return cls.data[filepath]

    @classmethod
    def save_settings(cls, filepath: str, settings_key: str, settings: Dict[str, Any]):
        if filepath not in cls.data:
            cls.data[filepath] = {}

        cls.data[filepath][settings_key] = settings

        with open(filepath, 'w') as settings_file:
            settings_file.write(json.dumps(cls.data[filepath], indent=2))

    def __init__(self, component: 'Component', filename: Optional[str]=None, path: Optional[str]=None):
        if filename is None and path is None and component.parent is not None:
            self.filepath = component.parent.settings.filepath
        else:
            self.filepath = os.path.join(path or '.', filename or 'components.json')

        self.component = component
        self.data: Dict[str, Any] = None

        self.load()

    def load(self):
        component_settings = self.load_settings(self.filepath)
        self.data = component_settings.get(self.component.path, {})

    def save(self):
        self.save_settings(self.filepath, self.component.path, self.data)


class Component:
    """
    WARNING: This class is being deprecated for a simpler workflow based around hard-coded locations

    Represents a real-world component with a location and optional placeholder locations. This class is meant to be
    sub-classed for use in your own components.

    Child classes can override the `load_settings` method to update attributes from saved settings, along with the
    `get_settings` method to return extra settings to be saved (always call the super-class method for these two).
    The `locate` method can be overridden for custom behaviour when locating locations in this component by driving a robot.

    The `Component.locate_all` method is available to locate all placeholder locations and unlocated components by
    driving a robot. This should be called after creating your component instances, but before performing any movements.
    """

    @classmethod
    def locate_all(cls, controller: C9Controller, root: Optional[Location]=None):
        """
        Locate all components attached to the root recursively. This will prompt the user to drive the robot to any
        placeholder location that isn't in a components.json file.

        :param controller:
        :param root:
        """
        root = root if root is not None else Location.find_root()

        for location in root.children:
            if location.component is not None:
                location.component.locate(controller, stop=False)

            cls.locate_all(controller, location)

    def __init__(self, name: str, location: Optional[Location]=None, parent: Optional['Component']=None, use_probe: bool=False, settings: Optional['ComponentSettings']=None):
        """
        Creates a new Component instance

        :param name: component instance name
        :param location: component instance location
        :param parent: component instance parent
        :param use_probe: use the robot probe when "locating" locations for this component
        :param settings: optional ComponentSettings instance
        """
        self.logger = logger.getChild(self.__class__.__name__)
        self.logger.warning(f'Component class is deprecated and will be removed in the future')

        self.name = name
        self._location: Location = None
        self.placeholders: Dict[str, LocationPlaceholder] = {}
        self.location = location if location is not None else Location(located=False)
        self.location.use_probe = use_probe
        if parent is not None:
            self.location.parent = parent.location

        self.location.component = self
        self.settings = settings if settings is not None else ComponentSettings(self)
        self.load_settings()

        # create instances for location placeholders
        for name, placeholder in self.__class__.__dict__.items():  # loop through all the class attributes
            if isinstance(placeholder, LocationPlaceholder):
                try:
                    location_list: List[float] = self.settings.data[name]
                    print(f"{name}.location={location_list}")
                    location = placeholder.copy_from_list(location_list, parent=self.location)
                except KeyError:
                    location = placeholder.copy(located=False, parent=self.location)

                setattr(self, name, location)
                self.placeholders[name] = location

    @property
    def path(self):
        if self.parent is None:
            return self.name

        return f'{self.parent.path}.{self.name}'

    @property
    def location(self) -> Location:
        return self._location

    @location.setter
    def location(self, location: Location):
        self._location = location
        self._location.component = self

    @property
    def parent(self):
        return self.location.parent.component if self.location.parent is not None else None

    @property
    def children(self):
        return [location.component for location in self.location.children if location.component is not None]

    def load_settings(self):
        if 'location' in self.settings.data:
            print(f"{self.name}.location={self.settings.data['location']}")
            self.location.update_from_list(self.settings.data['location'])

    def get_settings(self) -> Dict[str, Any]:
        return {
            'location': self.location.to_list(),
            **{name: placeholder.to_list() for name, placeholder in self.placeholders.items()}
        }

    def save_settings(self):
        self.settings.data.update(self.get_settings())
        self.settings.save()

    def add_child(self, component: 'Component'):
        self.location.add_child(component.location)

    def remove_child(self, component: 'Component'):
        self.location.remove_child(component.location)

    def locate(self, controller: C9Controller, stop: bool=True, if_needed: bool=True):
        if if_needed and self.location.located:
            return

        position = controller.joystick.record_position_vector(f'Please drive robot to the {self.name} location and press Record / Start', use_probe=self.location.use_probe)
        self.location = Location(position=position, parent=self.location.parent)
        self.locate_placeholders(controller)
        self.save_settings()

        if stop:
            controller.joystick.stop_moving()

    def locate_placeholders(self, controller: C9Controller):
        for name, placeholder in self.placeholders.items():
            if not placeholder.located:
                placeholder.position = controller.joystick.record_position_vector(f'Please drive robot to the {self.name} {name} location and press Record / Start', use_probe=placeholder.use_probe)
                placeholder.located = True


class Grid(Component):
    """
    A component that contains a grid of locations. Useful for vial racks, pipette holders, or anything else with a grid.

    Grid locations are sorted similar to Excel by default, with columns labelled A-Z from left-to-right along with rows
    numbered in descending order top-to-bottom.

    Example grid labels:

        A1 B1 C1
        A2 B2 C2
    """

    COLUMN_ROW = 0
    ROW_COLUMN = 1

    @staticmethod
    def to_grid_index(row: int, column: int) -> str:
        """
        Create a grid index label from the given row and column

        :param row: row number, starting at 0
        :param column: colum number, starting at 0
        :return: grid index label
        """
        column_string = convert_base(column, string.ascii_uppercase)

        return f'{column_string}{row + 1}'

    def __init__(self, name: str, location: Optional[Location]=None, rows: int=5, columns: int=5, spacing: Tuple[int, int]=(10, 10), **kwargs) -> None:
        """
        Create a new grid instance.

        :param name: component instance name
        :param location: optional component location
        :param rows: number of grid rows, defaults to 5
        :param columns: number of grid columns, defaults to 5
        :param spacing: 2-element tuple of row and column spacing, defaults to (10, 10)
        :param kwargs: optional Component keyword arguments
        """
        Component.__init__(self, name, location, **kwargs)
        self.logger.warning(f'The Grid class is deprecated, please use the simpler Tray classes')

        self.locations: Dict[str, Location] = AttrDict()
        self.grid: List[List[Location]] = []
        self.rows = rows
        self.columns = columns
        self.spacing = spacing

        self.generate_grid_and_locations()

    def generate_grid_and_locations(self):
        for column in range(0, self.columns):
            self.grid.append([])

            for row in range(0, self.rows):
                location = self.calculate_location(row, column)
                index = self.to_grid_index(row, column)
                self.locations[index] = location
                self.grid[column].append(location)

    def calculate_location(self, row, column):
        row_index = self.rows - row - 1
        x = column * self.spacing[0]
        y = row_index * self.spacing[1]

        return Location(local_position=Vector3(x, y, 0), parent=self.location)

    def grid_indexes(self, order: int=ROW_COLUMN, reverse_columns: bool=False, reverse_rows: bool=False):
        """
        Return a list containing all the grid indexes in this grid

        :param order: the order of indexes (Grid.ROW_COLUMN or Grid.COLUMN_ROW), defaults to Grid.ROW_COLUMN
        :param reverse_columns: reverses the order of columns if True
        :param reverse_rows: reverses the order of rows if True
        :return: list of grid indexes
        """
        indexes = []
        rows = list(range(0, self.rows))
        columns = list(range(0, self.columns))

        if reverse_columns:
            columns.reverse()

        if reverse_rows:
            rows.reverse()

        if order == self.ROW_COLUMN:
            for row in rows:
                for col in columns:
                    indexes.append(self.to_grid_index(row, col))
        else:
            for col in columns:
                for row in rows:
                    indexes.append(self.to_grid_index(row, col))

        return indexes

    def load_settings(self):
        Component.load_settings(self)

        if 'spacing' in self.settings.data:
            self.spacing = tuple(self.settings.data['spacing'])

    def get_settings(self):
        return {
            **Component.get_settings(self),
            'spacing': self.spacing,
        }

    def locate(self, controller: C9Controller, stop: bool=True, if_needed: bool=True):
        """
        Ask the user to locate the grid by moving the robot to 3 locations that define the bounds of the grid.

        :param controller: C9Controller instance
        :param stop: stop using the joystick after locating if True (defaults to True)
        :param if_needed: only locate placeholder locations if they are needed (defaults to True)
        """
        if if_needed and self.location.located:
            return

        y = controller.joystick.record_position_vector(f'Drive robot to the "A" {self.name} grid index location and press Record / Start', use_probe=self.location.use_probe)
        origin = controller.joystick.record_position_vector(f'Drive robot to the "B" {self.name} grid index location and press Record / Start', use_probe=self.location.use_probe)
        x = controller.joystick.record_position_vector(f'Drive robot to the "C" {self.name} grid index location and press Record / Start', use_probe=self.location.use_probe)

        self.spacing = ((x - origin).length() / (self.columns - 1), (y - origin).length() / (self.rows - 1))
        self.location = Location.location_from_origin_and_x_vector(origin, x, parent=self.location.parent)
        self.locate_placeholders(controller)
        self.generate_grid_and_locations()
        self.save_settings()

        if stop:
            controller.joystick.stop_moving()


class Tray:
    """
    A tray is a simple base class used to manage groups of locations. Each tray has a location along with a dict mapping
    indexes to locations. It also has a `build_indexes()` method that returns a list of indexes for iteration, along
    with a `gripper_rotation(index)` method that returns a suggested gripper rotation for an index, or None.

    Subclasses should reimplement the `build_indexes` and `gripper_rotation` methods as needed.
    """

    def __init__(self, locations: Optional[TrayLocationsType]=None):
        """
        :param locations: a list of locations for this tray
        """
        self.locations: TrayLocationsType = {} if locations is None else locations

    def build_indexes(self) -> List[str]:
        """
        Build a list of index strings, useful for looping through tray locations

        :return: list of index strings
        """
        return list(self.locations.keys())

    def gripper_rotation(self, index: str) -> Optional[float]:
        """
        Get the preferred gripper rotation for the given tray index. This can be overridden in subclasses to rotate the
        gripper out of the way of trays.

        :return: gripper rotation in degrees, or None
        """
        return None


class GridTray(Tray):
    """
    A Tray that groups locations in a grid.

    Grid locations are sorted similar to Excel by default, with columns labelled A-Z from left-to-right along with rows
    numbered in descending order top-to-bottom.

    Example grid labels:

        A1 B1 C1
        A2 B2 C2
    """

    COLUMN_ROW = 0
    ROW_COLUMN = 1

    @staticmethod
    def to_grid_index(row: int, column: int) -> str:
        """
        Create a grid index label from the given row and column

        :param row: row number, starting at 0
        :param column: colum number, starting at 0
        :return: grid index label
        """
        column_string = convert_base(column, string.ascii_uppercase)

        return f'{column_string}{row + 1}'

    def __init__(self, location: Location, rows: int, columns: int, spacing: Tuple[float, float], order: int=ROW_COLUMN,
                 reverse_columns: bool=False, reverse_rows: bool=True):
        """
        :param location: origin (bottom-left) location of the GridTray (this location also defines the tray rotation)
        :param rows: the number of rows in the grid
        :param columns: the number of columns in the grid
        :param spacing: a tuple specifying the spacing between the locations in this grid [(row spacing, column spacing)]
        :param order: the default order to use when generating a list of indexes, defaults to row then column
        :param reverse_columns: reverse the column order in the generated list of indexes, defaults to False
        :param reverse_rows: reverse the row order in the generated list of indexes, defaults to True
        """
        Tray.__init__(self)
        self.location = location
        self.rows = rows
        self.columns = columns
        self.spacing = spacing
        self.order = order
        self.reverse_columns = reverse_columns
        self.reverse_rows = reverse_rows
        self.locations: Dict[str, Location] = {}
        self.grid: List[List[Location]] = []
        self.generate_grid_and_locations()

    def generate_grid_and_locations(self):
        for column in range(0, self.columns):
            self.grid.append([])

            for row in range(0, self.rows):
                location = self.calculate_location(row, column)
                index = self.to_grid_index(row, column)
                self.locations[index] = location
                self.grid[column].append(location)

    def calculate_location(self, row, column) -> Location:
        """
        Calculate the location for the given row and column

        :return: location instance for the given row and column
        """
        row_index = self.rows - row - 1
        x = column * self.spacing[0]
        y = row_index * self.spacing[1]

        return Location(local_position=Vector3(x, y, 0), parent=self.location)

    def gripper_rotation(self, index: str):
        return -self.location.rz

    def build_indexes(self):
        """
        Return a list containing all the grid indexes in this grid

        :param order: the order of indexes (Grid.ROW_COLUMN or Grid.COLUMN_ROW), defaults to Grid.ROW_COLUMN
        :param reverse_columns: reverses the order of columns if True
        :param reverse_rows: reverses the order of rows if True
        :return: list of grid indexes
        """
        indexes = []
        rows = list(range(0, self.rows))
        columns = list(range(0, self.columns))

        if self.reverse_columns:
            columns.reverse()

        if self.reverse_rows:
            rows.reverse()

        if self.order == self.ROW_COLUMN:
            for row in rows:
                for col in columns:
                    indexes.append(self.to_grid_index(row, col))
        else:
            for col in columns:
                for row in rows:
                    indexes.append(self.to_grid_index(row, col))

        return indexes
