"""
WraptMor.py A program that estimates ligament fiber insertion-to-insertion length and wrapping around bone surfaces.

Copyright 2020 William Zaylor and Jason P. Halloran

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.
"""

import numpy as np
import vtk
import scipy.spatial
import scipy.optimize


def getLength(pointA, pointB, surface, nominalNormal):
    """
    Get the length (including wrapping) between pointA and pointB.
    This function uses optimization to adjust the normal to the plane that is used to cut the surface.

    This function uses optimization to adjust the angle of a plane that used to cut the surface.
    The ``nominalNormal`` is used to determine the reference position (i.e. zero angle) for this plane.
    The ``nominalNormal`` is used to define a reference vector that is actually normal to the line connecting ``pointA`` and ``pointB``.

    This function returns the length (with wrapping) from pointA to pointB.
    Additionally, this function also returns the coordinates of the points that define wrapping, starting with pointA and ending with pointB

    :param pointA: array 1x3, The coordinates of pointA
    :param pointB: array 1x3, The coordinates of pointB
    :param surface: vtkPolyData, The surface that is being used to determine the wrapping length
    :param nominalNormal: array-like 1x3, A unit vector that is used to determine the reference vector for the optimization. This does not have to be perpendicular to the line connecting ``pointA`` and ``pointB``. See the documentation above for more detail
    :return: [float, array mx3], The length from pointA to pointB, and the points along this line, starting with pointA [pointA, wrapPoint0, wrapPoint1, ..., wrapPointm-2, pointB]
    """
    cutNormal = getCutNormal(pointA, pointB, surface, nominalNormal)
    length, wrappingPoints = getWrappingLength(pointA, pointB, surface, cutNormal)
    return length, wrappingPoints

def getLengthMultibody(pointA, pointB, surfaceA, surfaceB, nominalNormal, boundingBoxA=None, boundingBoxB=None, locatorA=None, locatorB=None):
    """
    Get the length (including wrapping) between pointA and pointB for wrapping around surfaceA and surfaceB.
    This function uses optimization to adjust the normal to the plane that is used to cut the surface.

    This function uses optimization to adjust the angle of a plane that used to cut the surface.
    The ``nominalNormal`` is used to determine the reference position (i.e. zero angle) for this plane.
    The ``nominalNormal`` is used to define a reference vector that is actually normal to the line connecting ``pointA`` and ``pointB``.

    The order of the wrapping points for surfaceA and surfaceB will start with the closest point to bodyA and end with the furthest point from bodyA.
    Note that these points include ``pointA`` and ``pointB``.
    i.e. starting with pointA [pointA, wrapPoint0, wrapPoint1, ..., wrapPointm-1], [wrapPointm, wrapPointm+1, ..., pointB]

    :param pointA: array 1x3, The coordinates of pointA
    :param pointB: array 1x3, The coordinates of pointB
    :param surfaceA: vtkPolyData, One of the surfaces that is being used to determine the wrapping length
    :param surfaceB: vtkPolyData, One of the surfaces that is being used to determine the wrapping length
    :param nominalNormal: array-like 1x3, A unit vector that is used to determine the reference vector for the optimization. This does not have to be perpendicular to the line connecting ``pointA`` and ``pointB``. See the documentation above for more detail
    :param boundingBoxA: vtkBoundingBox, The bounding box for surfaceA. This is included as an input for efficiency.
    :param boundingBoxB: vtkBoundingBox, The bounding box for surfaceB. This is included as an input for efficiency.
    :param locatorA: vtkCellLocator, The locator for surfaceA. This is included as an input for efficiency.
    :param locatorB: vtkCellLocator, The locator for surfaceB. This is included as an input for efficiency.
    :return: [float, array mx3, array nx3], [length, surfaceAWrapPoints, surfaceBWrapPoints] The length from pointA to pointB, the wrapping points on surfaceA, and the wrapping points on surfaceB. See documentation above for more information on the wrapping points.
    """
    # To improve the efficiency of the code, define the bounding body and locator for surfaceA and surfaceB
    # If boundingBoxA is None, then assume that the other variables are also None
    if boundingBoxA is None:
        boundingBoxA = vtk.vtkBoundingBox()
        boundingBoxA.AddBounds(surfaceA.GetBounds())
        boundingBoxB = vtk.vtkBoundingBox()
        boundingBoxB.AddBounds(surfaceB.GetBounds())

        locatorA = vtk.vtkCellLocator()
        locatorA.SetDataSet(surfaceA)
        locatorA.BuildLocator()

        locatorB = vtk.vtkCellLocator()
        locatorB.SetDataSet(surfaceB)
        locatorB.BuildLocator()

    # Combine the two surfaces into one polydata object
    appendFilter = vtk.vtkAppendPolyData()
    appendFilter.AddInputData(surfaceA)
    appendFilter.AddInputData(surfaceB)
    appendFilter.Update()
    combinedSurfaces = appendFilter.GetOutput()

    cutNormal = getCutNormalMultibody(pointA, pointB, surfaceA, surfaceB, nominalNormal, boundingBoxA=boundingBoxA, boundingBoxB=boundingBoxB, locatorA=locatorA, locatorB=locatorB, combinedSurfaces=combinedSurfaces)
    length, wrappingPointsA, wrappintPointsB = getWrappingLengthMultiBody(pointA, pointB, surfaceA, surfaceB, cutNormal, boundingBoxA=boundingBoxA, boundingBoxB=boundingBoxB, locatorA=locatorA, locatorB=locatorB, combinedSurfaces=combinedSurfaces)
    return length, wrappingPointsA, wrappintPointsB

def getCutNormal(pointA, pointB, surface, nominalNormal):
    """
    Get the cut normal that defines the minimum length (including wrapping) between pointA and pointB.
    This function uses optimization to adjust the normal to the plane that is used to cut the surface.

    This function uses optimization to adjust the angle of a plane that used to cut the surface.
    The ``nominalNormal`` is used to determine the reference position (i.e. zero angle) for this plane.
    The ``nominalNormal`` is used to define a reference vector that is actually normal to the line connecting ``pointA`` and ``pointB``.

    :param pointA: array 1x3, The coordinates of pointA
    :param pointB: array 1x3, The coordinates of pointB
    :param surface: vtkPolyData, The surface that is being used to determine the wrapping length
    :param nominalNormal: array-like 1x3, A unit vector that is used to determine the reference vector for the optimization. This does not have to be perpendicular to the line connecting ``pointA`` and ``pointB``. See the documentation above for more detail
    :return: array 1x3, The normal that yields the minimum length between the give pointA and pointB whith wrapping around the surface.
    """
    lineVector = (pointB - pointA)/np.linalg.norm(pointB - pointA)
    nominalNormal = nominalNormal/np.linalg.norm(nominalNormal)

    nominalProjection = np.dot(lineVector, nominalNormal)*lineVector

    # This vector is normal to the line created by connecting pointA and pointB
    referenceVector = (nominalNormal - nominalProjection)/np.linalg.norm(nominalNormal - nominalProjection)

    args = (pointA, pointB, surface, referenceVector, lineVector)
    cutAngle = _getCutAngle(args)
    cutNormal = _getCutNormal(referenceVector, lineVector, cutAngle)
    return cutNormal

def getCutNormalMultibody(pointA, pointB, surfaceA, surfaceB, nominalNormal, boundingBoxA=None, boundingBoxB=None, locatorA=None, locatorB=None, combinedSurfaces=None):
    """
    Get the cut normal that defines the minimum length (including wrapping) between pointA and pointB.
    This function uses optimization to adjust the normal to the plane that is used to cut the surface.

    This function uses optimization to adjust the angle of a plane that used to cut the surface.
    The ``nominalNormal`` is used to determine the reference position (i.e. zero angle) for this plane.
    The ``nominalNormal`` is used to define a reference vector that is actually normal to the line connecting ``pointA`` and ``pointB``.

    :param pointA: array 1x3, The coordinates of pointA
    :param pointB: array 1x3, The coordinates of pointB
    :param surfaceA: vtkPolyData, The surface that is being used to determine the wrapping length
    :param surfaceB: vtkPolyData, The surface that is being used to determine the wrapping length
    :param nominalNormal: array-like 1x3, A unit vector that is used to determine the reference vector for the optimization. This does not have to be perpendicular to the line connecting ``pointA`` and ``pointB``. See the documentation above for more detail
    :param boundingBoxA: vtkBoundingBox, The bounding box for surfaceA. This is included as an input for efficiency. This input is passed into _getMinLengthObjective, which is in turn passed into getWrappingLengthMultiBody
    :param boundingBoxB: vtkBoundingBox, The bounding box for surfaceB. This is included as an input for efficiency. This input is passed into _getMinLengthObjective, which is in turn passed into getWrappingLengthMultiBody
    :param locatorA: vtkCellLocator, The locator for surfaceA. This is included as an input for efficiency. This input is passed into _getMinLengthObjective, which is in turn passed into getWrappingLengthMultiBody
    :param locatorB: vtkCellLocator, The locator for surfaceB. This is included as an input for efficiency. This input is passed into _getMinLengthObjective, which is in turn passed into getWrappingLengthMultiBody
    :param combinedSurfaces: vtkPolyData, The combination of surfaceA and surfaceB into one surface/polyData. This input is used to increase efficiency.
    :return: array 1x3, The normal that yields the minimum length between the give pointA and pointB whith wrapping around the surface.
    """
    lineVector = (pointB - pointA)/np.linalg.norm(pointB - pointA)
    nominalNormal = nominalNormal/np.linalg.norm(nominalNormal)

    nominalProjection = np.dot(lineVector, nominalNormal)*lineVector

    # This vector is normal to the line created by connecting pointA and pointB
    referenceVector = (nominalNormal - nominalProjection)/np.linalg.norm(nominalNormal - nominalProjection)

    args = (pointA, pointB, surfaceA, surfaceB, referenceVector, lineVector, boundingBoxA, boundingBoxB, locatorA, locatorB, combinedSurfaces)
    cutAngle = _getCutAngle(args)
    cutNormal = _getCutNormal(referenceVector, lineVector, cutAngle)
    return cutNormal

def _getCutAngle(args):
    """
    This helper function iteratively solves for the cut angle that yields the minimum length.
    :param args: tuple 1x5 or 1x12, if 1x5 (pointA, pointB, surface, referenceVector, lineVector). If 1x12, (pointA, pointB, surfaceA, surfaceB, referenceVector, lineVector, boundingBoxA, boundingBoxB, locatorA, locatorB, combinedSurfaces, clippedCombinedSurfaces)
    :return: float, The angle that defines the minimum distance between the insertion points, including wrapping.
    """
    changeInAngle = 5.
    angles = np.arange(0., 180. + changeInAngle, changeInAngle)
    lengths = np.zeros_like(angles)
    for i in range(len(angles)):
        lengths[i] = _getMinLengthObjective([angles[i]], *args)

    minAngleIndex = np.argmin(lengths)
    minAngle = angles[minAngleIndex]

    bounds = [[minAngle - changeInAngle, minAngle + changeInAngle]]
    options = {'eps': 0.1}
    results = scipy.optimize.minimize(_getMinLengthObjective, [minAngle], method='SLSQP',jac=False, options=options, bounds=bounds, args=args)
    minimizer = results['x']
    return minimizer

def _getMinLengthObjective(angle, *args):
    """
    The objective function that is used to evaluate the length given a specific angle.
    args = (pointA, pointB, surface, referenceVector, lineVector)
    or
    args = (pointA, pointB, surfaceA, surfaceB, referenceVector, lineVector, multibody, boundingBoxA, boundingBoxB, locatorA, locatorB)
    See the documentation for getMinLength to find descriptions for ``pointA``, ``pointB``, and ``surface``

    ``referenceVector`` is perpendicular to ``lineVector``, and it is used to define the plane normal that is used for cutting.
    If ``angle``=0, then the plane cut normal is parallel with the ``referenceVector``.

    :param angle: float, The angle (degrees) that is used to change the position of the normal that is used to cut the surface.
    :param args: tuple, (pointA, pointB, surface, nominalCutNormal), or (pointA, pointB, surfaceA, surfaceB, referenceVector, lineVector, multibody, boundingBoxA, boundingBoxB, locatorA, locatorB).
    :return:
    """
    if len(args)==5: # Assume that multibody length is not being used
        pointA, pointB, surface, referenceVector, lineVector = args
        cutNormal = _getCutNormal(referenceVector, lineVector, angle[0])
        length, null = getWrappingLength(pointA, pointB, surface, cutNormal)

    elif len(args)==11: # Assume that multibody length is being used
        pointA, pointB, surfaceA, surfaceB, referenceVector, lineVector, boundingBoxA, boundingBoxB, locatorA, locatorB, combinedSurfaces = args
        cutNormal = _getCutNormal(referenceVector, lineVector, angle[0])
        length, null0, null1 = getWrappingLengthMultiBody(pointA, pointB, surfaceA, surfaceB, cutNormal, boundingBoxA=boundingBoxA, boundingBoxB=boundingBoxB, locatorA=locatorA, locatorB=locatorB, combinedSurfaces=combinedSurfaces)
    return length

def _getCutNormal(referenceVector, lineVector, angle):
    """
    Rotate the given ``referenceVector`` about the ``lineVector`` by the given angle
    :param referenceVector: array-like 1x3, The vector that defines the vector that is being rotated
    :param lineVector: array-like 1x3, The vector that defines the axis that is being rotated about
    :param angle: float, The angle (degrees) that the ``referenceVector`` is rotated about the ``lineVector``
    :return: array 1x3, The vector that defines the rotated ``referenceVector``
    """
    transform = vtk.vtkTransform()
    transform.RotateWXYZ(angle, lineVector)
    cutNormal = np.array(transform.TransformVector(referenceVector))
    return cutNormal

def getWrappingLengthMultiBody(pointA, pointB, surfaceA, surfaceB, cutNormal, boundingBoxA=None, boundingBoxB=None, locatorA=None, locatorB=None, combinedSurfaces=None):
    """
    Determine the wrapping length between pointA and pointB for wrapping around two surfaces.
    The given ``cutNormal`` is used to define the plane that is cut through both surfaces.

    .. NOTE:: It is assumed that pointA and pointB can lie on the plane defined by the cutNormal

    This function returns three values
        0) length: float, The length (with wrapping around both surfaces) from ``pointA`` to ``pointB``
        1) wrapPointsA: array mx3, The coordinates of the points that are wrapping around surfaceA
        1) wrapPointsB: array nx3, The coordinates of the points that are wrapping around surfaceB

    :param pointA: array 1x3, The coordinates of pointA
    :param pointB: array 1x3, The coordinates of pointB
    :param surfaceA: vtkPolyData, The surface that is being used to determine the wrapping length
    :param surfaceB: vtkPolyData, The surface that is being used to determine the wrapping length
    :param cutNormal: array-like 1x3, The normal that defines plane's normal that is being used to cut the surface. Note that this should allow for pointA and pointB to lie on this plane.
    :param boundingBoxA: vtkBoundingBox, The bounding box for surfaceA. This is included as an input for efficiency. This input is passed into _separateWrappingPoints
    :param boundingBoxB: vtkBoundingBox, The bounding box for surfaceB. This is included as an input for efficiency. This input is passed into _separateWrappingPoints
    :param locatorA: vtkCellLocator, The locator for surfaceA. This is included as an input for efficiency. This input is passed into _separateWrappingPoints
    :param locatorB: vtkCellLocator, The locator for surfaceB. This is included as an input for efficiency. This input is passed into _separateWrappingPoints
    :param combinedSurfaces: vtkPolyData, The combination of surfaceA and surfaceB into one surface/polyData. This input is used to increase efficiency.
    :return: [float, array mx3, array nx3], [length, wrapPointsA, wrapPointsB]. See the documentation above for more information
    """
    if combinedSurfaces is None:
        # Combine the two surfaces into one polydata object
        appendFilter = vtk.vtkAppendPolyData()
        appendFilter.AddInputData(surfaceA)
        appendFilter.AddInputData(surfaceB)
        appendFilter.Update()
        combinedSurfaces = appendFilter.GetOutput()

    # Get the wrapping length and wrapping points without regard to which body the points lie on.
    length, combinedWrapPoints = getWrappingLength(pointA, pointB, combinedSurfaces, cutNormal, multiBody=True)

    # 'combinedWrapPoints' contains 'pointA' and 'pointB', where 'pointA' is the first index, and 'pointB' is the last index.
    # These insertion points may be outside of the surface, and may cause issues in _separateWrappingPoints.
    # For this reason, if 'combinedWrapPoints' includes more than two points (i.e. more than 'pointA' and 'pointB'), then 'pointA' and 'pointB' are removed from 'combinedPoints', and added accordingly to 'wrapPointsA' and 'wrapPointsB'.
    # Otherwise, if 'combinedWrapPoints' only has two points (i.e. 'pointA' and 'pointB'), the the first one is assigned to bodyA, and the second is assigned to bodyB without using '_separateWrappingPoints'.
    if combinedWrapPoints.shape[0] > 2:
        combinedWrapPoints = np.delete(combinedWrapPoints, [0, len(combinedWrapPoints)-1], axis=0) # Remove 'pointA' and 'pointB' from 'combinedWrapPoints'
        wrapPointsA, wrapPointsB = _separateWrappingPoints(surfaceA, surfaceB, combinedWrapPoints, boundingBoxA=boundingBoxA, boundingBoxB=boundingBoxB, locatorA=locatorA, locatorB=locatorB) # Separate 'combinedWrapPoints' into points along bodyA and bodyB
        # If 'wrapPointsA' is None, then define 'wrapPointsA' as 'pointA'.
        if wrapPointsA is None: # If there are no points that correspond to surfaceA
            wrapPointsA = np.array([pointA])  # Make wrapPointsA the correct shape array (1,3) instead of (3,)
        else: # Otherwise put 'pointA' as the first row in 'wrapPointsA'
            wrapPointsA = np.insert(wrapPointsA, 0, pointA, axis=0)
        # If 'wrapPointsB' is None, then define 'wrapPointsB' as 'pointB'.
        if wrapPointsB is None: # If there are no points that correspond to surfaceB
            wrapPointsB = np.array([pointB]) # Make wrapPointsB the correct shape array (1,3) instead of (3,)
        else:  # Otherwise put 'pointB' as the last row in 'wrapPointsB'
            wrapPointsB = np.insert(wrapPointsB, len(wrapPointsB), pointB, axis=0)
    elif combinedWrapPoints.shape[0] == 2: # If only 'pointA' and 'pointB' are in combinedWrapPoints
        wrapPointsA = np.array([pointA]) # Make wrapPointsA the correct shape array (1,3) instead of (3,)
        wrapPointsB = np.array([pointB]) # Make wrapPointsB the correct shape array (1,3) instead of (3,)
    else:
        raise Exception('combinedWrapPoints is not the correct size/shape')

    return length, wrapPointsA, wrapPointsB

def getWrappingLength(point0, point1, surface, cutNormal, multiBody=False):
    """
    Determine the wrapping length between point0 and point1.
    The given ``cutNormal`` is used to define the plane that is cut through the surface.

    .. NOTE:: It is assumed that point0 and point1 can lie on the plane defined by the cutNormal

    This function returns the length (with wrapping) from point0 to point1.
    Additionally, this function also returns the coordinates of the points that define wrapping, starting with point0 and ending with point1

    :param point0: array 1x3, The coordinates of point0
    :param point1: array 1x3, The coordinates of point1
    :param surface: vtkPolyData, The surface that is being used to determine the wrapping length
    :param cutNormal: array-like 1x3, The normal that defines plane's normal that is being used to cut the surface. Note that this should allow for pointA and pointB to lie on this plane.
    :param multiBody: bool, If False, then it is assumed that wrapping is around the surface nearest ``point1``. Otherwise it is assumed that there is wrapping around the parts of the surface near ``point0`` and ``point1``.
    :return: [float, array mx3], The length from point0 to point1, and the points along this line, starting with point0 [point0, wrapPoint0, wrapPoint1, ..., wrapPointm-2, point1]
    """
    cutPlane = vtk.vtkPlane()
    cutPlane.SetOrigin(point0)
    cutPlane.SetNormal(cutNormal)
    cutter = vtk.vtkCutter()
    cutter.SetInputData(surface)
    cutter.SetCutFunction(cutPlane)
    cutter.Update()
    cutterOutline = cutter.GetOutput()

    if cutPlane.DistanceToPlane(point1) > 1.e-5:
        # Raise an error if the point1 does not lie on the cutPlane
        message = "**WARNING** point1 does not appear to lie on the plane that has been defined."
        raise ValueError(message)

    elif multiBody is True and cutPlane.DistanceToPlane(point0) > 1.e-5:
        # Raise an error if the point0 does not lie on the cutPlane
        message = "**WARNING** point0 does not appear to lie on the plane that has been defined."
        raise ValueError(message)

    if int(cutterOutline.GetNumberOfPoints()) == 0:
        length = np.linalg.norm(point0 - point1)
        wrapPoints = np.array([point0, point1])
    else:
        # Add pointA and pointB to the cutterOutline to include these values in the convex hull calculation
        point0CutterOutlineId = int(cutterOutline.GetNumberOfPoints())
        point1CutterOutlineId = int(cutterOutline.GetNumberOfPoints() + 1)
        cutterOutline.GetPoints().InsertNextPoint(point0)
        cutterOutline.GetPoints().InsertNextPoint(point1)

        # Get the pointIds of the points that make the convex hull around the cutterOutline (which now includes pointA and pointB)
        hullIds = getConvexHullPointIds(cutterOutline, cutNormal)

        length, wrapPoints = _getWrappingLength(cutterOutline, hullIds, point0CutterOutlineId, point1CutterOutlineId)
    if length is None and wrapPoints is None: # If the outputs of this are invalid, then assign large values to cause obvious issues downstream
        length = 1e6
        wrapPoints = np.array([[1.e6, 0., 0.], [-1.e6, 0., 0.]])
    return length, wrapPoints

def _getWrappingLength(outline, hullIds, pointAId, pointBId):
    """
    This helper function is used to determine the length between pointA and pointB given the outline and indicies that define the convex hull (which includes pointA and pointB).

    This function returns the length (with wrapping) from pointA to pointB.
    Additionally, this function also returns the coordinates of the points that define wrapping, starting with pointA and ending with pointB

    :param outline: vtkUnstructuredGrid, The line sequence that defines the potential wrapping path
    :param hullIds: array-like 1xn, The indicies of the points in ``outline`` (and pointA and pointB) that defines the convex hull around the ``outline``
    :param pointAId: int, The pointId of pointA. This should be in the set of ``hullIds``, and this indicates pointA in ``outline``
    :param pointBId: int, The pointId of pointB. This should be in the set of ``hullIds``, and this indicates pointB in ``outline``
    :return: [float, array mx3], The length from pointA to pointB, and the points along this line, starting with pointA [pointA, wrapPoint0, wrapPoint1, ..., wrapPointm-2, pointB]
    """
    numIds = len(hullIds)

    # "Walk" from pointA to pointB
    length0 = 0.
    wrappingPoints0 = []

    try:
        pointAHullIndex = np.where(hullIds == pointAId)[0][0]  # The index in hullIds where pointA is located.
    except IndexError: # Catch the case where pointAId is not part of the convex hull
        return None, None # Return None to deal with outside this function
    stop = False
    currentPoint = np.asarray(outline.GetPoint(hullIds[pointAHullIndex])) # Initialize the currentPoint as pointA.
    for i in range(len(hullIds)):
        currentHullIndex = (i+pointAHullIndex)%numIds # Use modulus division to allow the index to 'wrap' around the list
        nextHullIndex = (currentHullIndex+1)%numIds # Use modulus division to allow the index to 'wrap' around the list
        # If the nextHullIndex is pointBId, then stop the loop. Note, the length between the currentPoint and nextPoint will still be defined.
        if hullIds[nextHullIndex] == pointBId:
            stop = True

        nextPoint = np.asarray(outline.GetPoint(hullIds[nextHullIndex])) # Define the coordinates of the next point in the convex hull
        length0 += np.linalg.norm(nextPoint - currentPoint) # Add the distance between the currentPoint and the nextPoint to the length0 value
        wrappingPoints0.append(currentPoint) # Append the current point to the wrappingPoints0 list

        # Initialize values for the next iteration of the loop
        currentPoint = nextPoint # Define the currentPoint as this iteration's nextPoint
        if stop is True: break
    wrappingPoints0.append(nextPoint) # Append pointB to the end of wrappingPoints0

    # "Walk" from pointB to pointA
    length1 = 0.
    wrappingPoints1 = []
    try:
        pointBHullIndex = np.where(hullIds == pointBId)[0][0]  # The index in hullIds where pointB is located.
    except IndexError: # Catch the case where pointAId is not part of the convex hull
        return None, None # Return None to deal with outside this function
    stop = False
    currentPoint = np.asarray(outline.GetPoint(hullIds[pointBHullIndex]))  # Initialize the currentPoint as pointB.
    for i in range(len(hullIds)):
        currentHullIndex = (i + pointBHullIndex) % numIds  # Use modulus division to allow the index to 'wrap' around the list
        nextHullIndex = (currentHullIndex + 1) % numIds  # Use modulus division to allow the index to 'wrap' around the list
        # If the nextHullIndex is pointAId, then stop the loop. Note, the length between the currentPoint and nextPoint will still be defined.
        if hullIds[nextHullIndex] == pointAId:
            stop = True

        nextPoint = np.asarray(outline.GetPoint(hullIds[nextHullIndex]))  # Define the coordinates of the next point in the convex hull
        length1 += np.linalg.norm(nextPoint - currentPoint)  # Add the distance between the currentPoint and the nextPoint to the length1 value
        wrappingPoints1.append(currentPoint)  # Append the current point to the wrappingPoints1 list

        # Initialize values for the next iteration of the loop
        currentPoint = nextPoint  # Define the currentPoint as this iteration's nextPoint
        if stop is True: break
    wrappingPoints1.append(nextPoint) # Append pointB to the end of wrappingPoints0

    if length0 < length1:
        desiredLength = length0
        wrapPoints = np.asarray(wrappingPoints0)
    else:
        desiredLength = length1
        wrapPoints = np.flip(np.asarray(wrappingPoints1), axis=0) # Note that the row order of points1 should be reversed to have the first index be pointA

    return desiredLength, wrapPoints

def getConvexHullPointIds(outline, outlineNormal, debug=False):
    """
    Get the point Ids that define the convex hull of the given outline.
    It is assumed that the points in the outline are coplanar
    :param outline: vtkUnstructuredGrid, The outline that convex hull will be drawn around.
    :param outlineNormal: array-like 1x3, The normal to the plane that the coplanar points lie on.
    :param debug: bool, If True, then a plot of the convex hull is generated.
    :return: list, A list of the point Ids that are on the outside of the convex hull.
    """
    points = np.zeros((outline.GetNumberOfPoints(), 3))
    for i in range(outline.GetNumberOfPoints()):
        points[i] = outline.GetPoint(i)
    points = _getCutPointsOnPlane(points, outlineNormal)
    hull = scipy.spatial.ConvexHull(points)
    # Plot for debugging
    if debug is True:
        import matplotlib.pyplot as plt
        plt.plot(points[:, 0], points[:, 1], 'o', markersize=0.5)
        for simplex in hull.simplices:
            plt.plot(points[simplex, 0], points[simplex, 1], 'k-')
        plt.plot(points[hull.vertices, 0], points[hull.vertices, 1], 'r--', marker='o', markersize=3, lw=2)
        plt.plot(points[-2, 0], points[-2, 1], 'gx')
        plt.plot(points[-1, 0], points[-1, 1], 'gx')
        plt.show()
    return hull.vertices


def _getCutPointsOnPlane(points, cutNormal):
    """
    This helper function transforms the given set of coplanar 3D coordinates and transforms them to a 2D coordiante system.

    :param points: array nx3, The coordinates of n coplanar points.
    :param cutNormal: array 1x3, The unit vector of the normal of the plane that contains the given points.
    :return: array nx2, The coordinates of the given points on a 2D coordinate system.
    """
    cutNormal = cutNormal/np.linalg.norm(cutNormal)
    xAxis = (points[1] - points[0])/np.linalg.norm(points[1] - points[0])
    yAxis = np.cross(cutNormal, xAxis)
    yAxis = yAxis/np.linalg.norm(yAxis)

    transform = np.identity(3)
    transform[:,0] = xAxis
    transform[:,1] = yAxis
    transform[:,2] = cutNormal
    transform = np.asmatrix(transform)
    newCoordiantes = np.zeros_like(points)
    for i in range(len(points)):
        newCoordiante = transform.T*points[i].reshape((3,1))
        newCoordiantes[i] = newCoordiante.reshape((3,))
    return newCoordiantes[:,:2]

def _separateWrappingPoints(surfaceA, surfaceB, combinedPoints, boundingBoxA=None, boundingBoxB=None, locatorA=None, locatorB=None):
    """
    Define which of the given ``combinedPoints`` correspond to surfaceA and surfaceB.

    This function has two levels that are used to sort the ``combinedPoints``
        1) Check if a point is in either surfaceA's and surfaceB's bounding box. If only one of them contains the point, then it is assigned to the surface that contains the point.
        2) If both surfaces' bounding box contains the point, then the point is assigned to the surface that it is closest to.

    This function returns two values:
        0) pointsA: array mx3, The combinedPoints that correspond to surfaceA
        1) pointsB: array nx3, The combinedPoints that correspond to surfaceB

    :param surfaceA: vtkPolyData, The surface that is being used to determine which of the given ``combinedPoints`` correspond to this surface.
    :param surfaceB: vtkPolyData, The surface that is being used to determine which of the given ``combinedPoints`` correspond to this surface.
    :param combinedPoints: array lx3, The points that are being separated. Note that l = m + n
    :param boundingBoxA: vtkBoundingBox, The bounding box for surfaceA. This is included as an input for efficiency.
    :param boundingBoxB: vtkBoundingBox, The bounding box for surfaceB. This is included as an input for efficiency.
    :param locatorA: vtkCellLocator, The locator for surfaceA. This is included as an input for efficiency.
    :param locatorB: vtkBoundingBox, The locator for surfaceA. This is included as an input for efficiency.
    :return: [array mx3, array nx3], The ``combinedPoints`` that correspond to surfaceA and surfaceB, respectively. If there are no points correspond to a surface, the the approprite variable will be returned as None.
    """
    # Define a bounding box around surfaceA and surfaceB
    if boundingBoxA is None:
        boundingBoxA = vtk.vtkBoundingBox()
        boundingBoxA.AddBounds(surfaceA.GetBounds())
    if boundingBoxB is None:
        boundingBoxB = vtk.vtkBoundingBox()
        boundingBoxB.AddBounds(surfaceB.GetBounds())

    # Initialize a list of the points that are assigned to surfaceA and surfaceB
    containedPointsA = []
    containedPointsB = []

    containedA = False
    containedB = False
    for i in range(len(combinedPoints)):
        if boundingBoxA.ContainsPoint(combinedPoints[i]) == 1:
            containedA = True
        if boundingBoxB.ContainsPoint(combinedPoints[i]) == 1:
            containedB = True

        # If both bounding boxes contain the point, then find which surface the point is closest to, and change containedA or containedB accordingly
        if containedA is True and containedB is True:
            if locatorA is None: # If the locators have not been built yet
                locatorA = vtk.vtkCellLocator()
                locatorA.SetDataSet(surfaceA)
                locatorA.BuildLocator()

                locatorB = vtk.vtkCellLocator()
                locatorB.SetDataSet(surfaceB)
                locatorB.BuildLocator()
            # Find the distance from combinedPoint[i] to surfaceA and surfaceB (note there are quite a few unused inputs that need to be defined anyway).
            distA = vtk.mutable(0)
            distB = vtk.mutable(0)
            cell = vtk.vtkGenericCell()
            cellId = vtk.mutable(0)
            subId = vtk.mutable(0)
            locatorA.FindClosestPoint(combinedPoints[i], np.zeros(3), cell, cellId, subId, distA)
            locatorB.FindClosestPoint(combinedPoints[i], np.zeros(3), cell, cellId, subId, distB)

            # If the point is closer to surfaceA
            if distB > distA:
                containedB = False # Note if this line is executed, then containedA is True
            else:
                containedA = False # Note if this line is executed, then containedB is True

        if containedA is True:
            # If containedB is True, then there is a bug, and the script should exit.
            if containedB is True:
                import sys
                message = '**WARNING** In _separateWrappingPoints located in MCLS_003/src/Ligaments/SurfaceWrappingOptimization.py\nA bug occurred, and a wrapping point has been assigned to surfaceA and surfaceB. The script has exited.'
                print(message)
                sys.exit(1)
            containedPointsA.append(combinedPoints[i])
        elif containedB is True:
            # Note, no bug check is needed.
            containedPointsB.append(combinedPoints[i])
        else:
            message = 'A bug occurred, and a wrapping point has been not assigned to either surfaceA and surfaceB. The script has exited.\ncontainedA = {}\ncontainedB = {}'.format(containedA, containedB)
            raise Exception(message)

        # Set containedA and containedB to False for the next iteration
        containedA = False
        containedB = False

    if not containedPointsA: # If containedPointsA is an empty list
        desiredPointsA = None
    else:
        desiredPointsA = np.asarray(containedPointsA)

    if not containedPointsB: # If containedPointsB is an empty list
        desiredPointsB = None
    else:
        desiredPointsB = np.asarray(containedPointsB)

    return desiredPointsA, desiredPointsB