import math

# point in 3D space defined relative to the robot base
class Point:
    x: float
    y: float
    z: float

    def __init__(self, x, y, z):
        self.x = x
        self.y = y
        self.z = z

    def str(self):
        return str(self.x) + ' ' + str(self.y) + ' ' + str(self.z)

    def __eq__(self, other):
        # Overrides the default == implementation
        if self.x == other.x and self.y == other.y and self.z == other.z:
            return 1
        else:
            return 0


# data needed to define an array of hardware in the x-y plane (for example 1 box where start is the left bottom corner and end is the right top corner)
class HardwareArray:
    start: Point
    end: Point
    x_count: int
    y_count: int
    iterator: int = 0

    # variables used for moving the arm in the coordinate space of the hardware array
    # precalculate them when the parameters start, end, x_count or y_count change
    _x_addx: float = 0
    _x_addy: float = 0
    _y_addx: float = 0
    _y_addy: float = 0
    _tilt_angle: float = 0  # in radians

    def __init__(self, start, end, x_count, y_count):
        self.start = start
        self.end = end
        self.x_count = x_count
        self.y_count = y_count
        self._precalc_parameters()

    def _precalc_parameters(self):
        # length of the x axis
        _x_length = self.end.x - self.start.x
        # length of the y axis
        _y_length = self.end.y - self.start.y
        # ratio of y box length to x box length
        # division that returns 0 if the denominator is 0
        _ratio = (self.x_count-1) and (self.y_count-1) / (self.x_count-1) or 0
        # hypotenuse length of the box
        _hypotenuse = math.sqrt(_x_length**2 + _y_length**2)
        # calculate the box size with all edge cases (point or line)
        if (self.x_count-1) != 0 and (self.y_count-1) != 0:
            _x_box_length = _hypotenuse / math.sqrt(1 + _ratio**2)
            _y_box_length = _x_box_length * _ratio
        elif (self.x_count-1) != 0:
            _x_box_length = _hypotenuse
            _y_box_length = 0
        elif (self.y_count-1) != 0:
            _x_box_length = 0
            _y_box_length = _hypotenuse
        else:
            _x_box_length = 0
            _y_box_length = 0
        # calculate the tilt angle of the box respective to the x axis
        self._tilt_angle = math.atan2(_y_length,_x_length) - math.atan2(_y_box_length,_x_box_length)
        # calculate how much we need to add to the x or y axes when incrementing x or y positions
        self._x_addx = (self.x_count - 1) and math.cos(self._tilt_angle)*_x_box_length/(self.x_count - 1) or 0
        self._x_addy = (self.y_count - 1) and -math.sin(self._tilt_angle)*_y_box_length/(self.y_count - 1) or 0
        self._y_addx = (self.x_count - 1) and math.sin(self._tilt_angle)*_x_box_length/(self.x_count - 1) or 0
        self._y_addy = (self.y_count - 1) and math.cos(self._tilt_angle)*_y_box_length/(self.y_count - 1) or 0

    def _interpolate(self, x_pos, y_pos):
        if x_pos >= self.x_count or y_pos >= self.y_count:
            return Point(0,0,0)

        # interpolate the xyz position
        return Point(self.start.x + self._x_addx*x_pos + self._x_addy*y_pos,
                     self.start.y + self._y_addx*x_pos + self._y_addy*y_pos,
                     self.start.z)

    # get a point in space for the x-y position in the hardware array
    def getPosition(self, x_pos, y_pos):
        return self._interpolate(x_pos, y_pos)
    
    # 
    def getPosition(self, pos):
        return self._interpolate(int(pos % self.x_count), int(pos / self.x_count))

    # 
    def getNextPosition(self):
        if self.iterator >= (self.x_count*self.y_count):
            return Point(0,0,0)

        result = self.getPosition(self.iterator)
        self.iterator += 1
        return result

    # 
    def iteratorEnd(self):
        if self.iterator >= (self.x_count*self.y_count):
            return True
        else:
            return False

    # 
    def iteratorReset(self):
        self.iterator = 0

# data for all the hardware the robot needs to interact with
NUM_OF_BOXES = 6
NUM_OF_COLUMNS = 2
NUM_OF_NIXIES_IN_COLUMN = 5
BOX_SIZE_X = 4
BOX_SIZE_Y = 5
NUM_OF_NIXIES_IN_BOX = BOX_SIZE_X*BOX_SIZE_Y

class HardwarePositions:
    _default_boxes = [HardwareArray(Point(-453.3,7.3,136), Point(10,10,10), BOX_SIZE_X, BOX_SIZE_Y),    # box number 0 (leftmost)
                      HardwareArray(Point(-467,-247,130), Point(20,20,20), BOX_SIZE_X, BOX_SIZE_Y),
                      HardwareArray(Point(10,10,10), Point(20,20,20), BOX_SIZE_X, BOX_SIZE_Y),
                      HardwareArray(Point(10,10,10), Point(20,20,20), BOX_SIZE_X, BOX_SIZE_Y),
                      HardwareArray(Point(10,10,10), Point(20,20,20), BOX_SIZE_X, BOX_SIZE_Y),
                      HardwareArray(Point(10,10,10), Point(20,20,20), BOX_SIZE_X, BOX_SIZE_Y)]    # box number 5 (rightmost)
    
    _default_columns = [HardwareArray(Point(159.4,128.5,165), Point(164.5,602.6,0), NUM_OF_NIXIES_IN_COLUMN, 1),  # column number 1 (top)
                        HardwareArray(Point(10,10,10), Point(20,20,20), NUM_OF_NIXIES_IN_COLUMN, 1)]  # column number 2 (bottom)
    
    _default_AOI = Point(159.4,128.5,165)  # induction heater position
    _default_induction = Point(159.4,128.5,165)  # induction heater position

    def __init__(self):
        self.loadDefaults()
    
    def loadDefaults(self):
        self.boxes = self._default_boxes
        self.columns = self._default_columns
        self.AOI = self._default_AOI
        self.induction = self._default_induction

    def getPosition(self, position, subposition):
        if position < NUM_OF_BOXES:
            # accessing box
            if subposition < NUM_OF_NIXIES_IN_BOX:
                return (self.boxes[position].getPosition(subposition), self.boxes[position]._tilt_angle)
        elif position < NUM_OF_BOXES+NUM_OF_COLUMNS:
            # accessing column
            if subposition < NUM_OF_NIXIES_IN_COLUMN:
                return (self.columns[position-NUM_OF_BOXES].getPosition(subposition), self.columns[position-NUM_OF_BOXES]._tilt_angle)
        elif position == 8:
            # accessing AOI
            return (self.AOI, 0)
        elif position == 9:
            # accessing induction
            return (self.induction, 0)
        # out of range
        return (Point(0,0,0), 0)
        