import pdb

import numpy as np
from matplotlib import cm
from moorpy.Catenary import catenary
from moorpy.nonlinear import nonlinear                                      
from moorpy.helpers import (unitVector, LineError, CatenaryError, 
                     rotationMatrix, makeTower, read_mooring_file, 
                     quiver_data_to_segments, printVec, printMat,
                     get_dynamic_matrices, get_dynamic_tension, get_modes, guyan_reduce)

from os import path

 
class Line():
    '''A class for any mooring line that consists of a single material'''

    def __init__(self, mooringSys, num, L, lineType, nSegs=100, cb=0, isRod=0, attachments = [0,0]):
        '''Initialize Line attributes

        Parameters
        ----------
        mooringSys : system object
            The system object that contains the point object
        num : int
            indentifier number
        L : float
            line unstretched length [m]
        lineType : dict
            dictionary containing the coefficients needed to describe the line (could reference an entry of System.lineTypes).
        nSegs : int, optional
            number of segments to split the line into. Used in MoorPy just for plotting. The default is 100.
        cb : float, optional
            line seabed friction coefficient (will be set negative if line is fully suspended). The default is 0.
        isRod : boolean, optional
            determines whether the line is a rod or not. The default is 0.

        Returns
        -------
        None.

        '''
        
        self.sys    = mooringSys       # store a reference to the overall mooring system (instance of System class)
        
        self.attached = attachments  # ID numbers of the Points at the Line ends [a,b] >>> NOTE: not fully supported <<<<

        self.number = num
        self.isRod = isRod
            
        self.L = L              # line unstretched length (may be modified if using nonlinear elasticity) [m]
        self.L0 = L             # line reference unstretched length [m]
        self.type = lineType    # dictionary of a System.lineTypes entry
        self.cost = {}          # empty dictionary to contain cost information
        
        if not self.isRod:
            self.EA = self.type['EA']  # use the default stiffness value for now (may be modified if using nonlinear elasticity) [N]
        
        self.nNodes = int(nSegs) + 1
        self.cb = float(cb)    # friction coefficient (will automatically be set negative if line is fully suspended)
        self.sbnorm = []    # Seabed Normal Vector (to be filled with a 3x1 normal vector describing seabed orientation)
        
        self.rA = np.zeros(3) # end coordinates
        self.rB = np.zeros(3)
        self.fA = np.zeros(3) # end forces
        self.fB = np.zeros(3)
        self.TA = 0  # end tensions [N]
        self.TB = 0
        self.KA = np.zeros([3,3])  # 3D stiffness matrix of end A [N/m]
        self.KB = np.zeros([3,3])  # 3D stiffness matrix of end B
        self.KBA= np.zeros([3,3])  # 3D stiffness matrix of cross coupling between ends
        
        self.HF = 0           # fairlead horizontal force saved for next solve
        self.VF = 0           # fairlead vertical force saved for next solve
        self.info = {}        # to hold all info provided by catenary
        
        self.qs = 1  # flag indicating quasi-static analysis (1). Set to 0 for time series data
        self.show = True      # a flag that will be set to false if we don't want to show the line (e.g. if results missing)
        self.color = 'k'
        self.lw=0.5
        
        self.fCurrent = np.zeros(3)  # total current force vector on the line [N]

    def loadData(self, dirname, rootname, sep='.MD.', id=0):
        '''Loads line-specific time series data from a MoorDyn output file'''
        
        self.qs = 0 # signals time series data
        
        if self.isRod==1:
            strtype='Rod'
        elif self.isRod==0:
            strtype='Line'
            
        if id==0:
            id = self.number
        
        filename = dirname+rootname+sep+strtype+str(id)+'.out'
        
        if path.exists(filename):


        # try:
        
            # load time series data
            data, ch, channels, units = read_mooring_file("", filename) # remember number starts on 1 rather than 0

            # get time info
            if ("Time" in ch):
                self.Tdata = data[:,ch["Time"]]
                self.dt = self.Tdata[1]-self.Tdata[0]
            else:
                raise LineError("loadData: could not find Time channel for mooring line "+str(self.number))
        
            
            nT = len(self.Tdata)  # number of time steps
            
            # check for position data <<<<<<
            
            self.xp = np.zeros([nT,self.nNodes])
            self.yp = np.zeros([nT,self.nNodes])
            self.zp = np.zeros([nT,self.nNodes])
            
            
            for i in range(self.nNodes):
                self.xp[:,i] = data[:, ch['Node'+str(i)+'px']]
                self.yp[:,i] = data[:, ch['Node'+str(i)+'py']]
                self.zp[:,i] = data[:, ch['Node'+str(i)+'pz']]
            
            '''
            if self.isRod==0:
                self.Te = np.zeros([nT,self.nNodes-1])   # read in tension data if available
                if "Seg1Te" in ch:
                    for i in range(self.nNodes-1):
                        self.Te[:,i] = data[:, ch['Seg'+str(i+1)+'Te']]
                        
                self.Ku = np.zeros([nT,self.nNodes])   # read in curvature data if available
                if "Node0Ku" in ch:
                    for i in range(self.nNodes):
                        self.Ku[:,i] = data[:, ch['Node'+str(i)+'Ku']]
            else:
                # read in Rod buoyancy force data if available
                if "Node0Box" in ch:
                    self.Bx = np.zeros([nT,self.nNodes])   
                    self.By = np.zeros([nT,self.nNodes])
                    self.Bz = np.zeros([nT,self.nNodes])
                    for i in range(self.nNodes):
                        self.Bx[:,i] = data[:, ch['Node'+str(i)+'Box']]
                        self.By[:,i] = data[:, ch['Node'+str(i)+'Boy']]
                        self.Bz[:,i] = data[:, ch['Node'+str(i)+'Boz']]

            if "Node0Ux" in ch:
                self.Ux = np.zeros([nT,self.nNodes])   # read in fluid velocity data if available
                self.Uy = np.zeros([nT,self.nNodes])
                self.Uz = np.zeros([nT,self.nNodes])
                for i in range(self.nNodes):
                    self.Ux[:,i] = data[:, ch['Node'+str(i)+'Ux']]
                    self.Uy[:,i] = data[:, ch['Node'+str(i)+'Uy']]
                    self.Uz[:,i] = data[:, ch['Node'+str(i)+'Uz']]
            
            #Read in tension data if available
            if "Seg1Ten" in ch:
                self.Ten = np.zeros([nT,self.nNodes-1])   
                for i in range(self.nNodes-1):
                    self.Ten[:,i] = data[:, ch['Seg'+str(i+1)+'Ten']]
            '''
            
            
            
            # --- Read in additional data if available ---

            # segment tension  <<< to be changed to nodal tensions in future MD versions
            if "Seg1Ten" in ch:
                self.Tendata = True
                self.Te = np.zeros([nT,self.nNodes-1])
                for i in range(self.nNodes-1):
                    self.Te[:,i] = data[:, ch['Seg'+str(i+1)+'Ten']]
            elif "Seg1Te" in ch:
                self.Tendata = True
                self.Te = np.zeros([nT,self.nNodes-1])
                for i in range(self.nNodes-1):
                    self.Te[:,i] = data[:, ch['Seg'+str(i+1)+'Te']]
            else:
                self.Tendata = False
                        
            # curvature at node
            if "Node0Ku" in ch:
                self.Kudata = True
                self.Ku = np.zeros([nT,self.nNodes])   
                for i in range(self.nNodes):
                    self.Ku[:,i] = data[:, ch['Node'+str(i)+'Ku']]
            else:
                self.Kudata = False
            
            # water velocity data 
            if "Node0Ux" in ch:  
                self.Udata = True
                self.Ux = np.zeros([nT,self.nNodes])
                self.Uy = np.zeros([nT,self.nNodes])
                self.Uz = np.zeros([nT,self.nNodes])
                for i in range(self.nNodes):
                    self.Ux[:,i] = data[:, ch['Node'+str(i)+'Ux']]
                    self.Uy[:,i] = data[:, ch['Node'+str(i)+'Uy']]
                    self.Uz[:,i] = data[:, ch['Node'+str(i)+'Uz']]
            else:
                self.Udata = False
                
            # buoyancy force data
            if "Node0Box" in ch:  
                self.Bdata = True
                self.Bx = np.zeros([nT,self.nNodes])
                self.By = np.zeros([nT,self.nNodes])
                self.Bz = np.zeros([nT,self.nNodes])
                for i in range(self.nNodes):
                    self.Bx[:,i] = data[:, ch['Node'+str(i)+'Box']]
                    self.By[:,i] = data[:, ch['Node'+str(i)+'Boy']]
                    self.Bz[:,i] = data[:, ch['Node'+str(i)+'Boz']]
            else:
                self.Bdata = False
                
            # hydro drag data
            if "Node0Dx" in ch: 
                self.Ddata = True
                self.Dx = np.zeros([nT,self.nNodes])   # read in fluid velocity data if available
                self.Dy = np.zeros([nT,self.nNodes])
                self.Dz = np.zeros([nT,self.nNodes])
                for i in range(self.nNodes):
                    self.Dx[:,i] = data[:, ch['Node'+str(i)+'Dx']]
                    self.Dy[:,i] = data[:, ch['Node'+str(i)+'Dy']]
                    self.Dz[:,i] = data[:, ch['Node'+str(i)+'Dz']]
            else:
                self.Ddata = False
                
            # weight data
            if "Node0Wx" in ch: 
                self.Wdata = True
                self.Wx = np.zeros([nT,self.nNodes])   # read in fluid velocity data if available
                self.Wy = np.zeros([nT,self.nNodes])
                self.Wz = np.zeros([nT,self.nNodes])
                for i in range(self.nNodes):
                    self.Wx[:,i] = data[:, ch['Node'+str(i)+'Wx']]
                    self.Wy[:,i] = data[:, ch['Node'+str(i)+'Wy']]
                    self.Wz[:,i] = data[:, ch['Node'+str(i)+'Wz']]
            else:
                self.Wdata = False
            
            
            
            # initialize positions (is this used?)
            self.xpi= self.xp[0,:]
            self.ypi= self.yp[0,:]
            self.zpi= self.zp[0,:]
            
            # calculate the dynamic LBot !!!!!!! doesn't work for sloped bathymetry yet !!!!!!!!!!
            for i in range(len(self.zp[0])):
                if np.max(self.zp[:,i]) > self.zp[0,0]:
                    inode = i
                    break
                else:
                    inode = i
            self.LBotDyn = (inode-1)*self.L/(self.nNodes-1)
            
            # get length (constant)
            #self.L = np.sqrt( (self.xpi[-1]-self.xpi[0])**2 + (self.ypi[-1]-self.ypi[0])**2 + (self.zpi[-1]-self.zpi[0])**2 )
            # ^^^^^^^ why are we changing the self.L value to not the unstretched length specified in MoorDyn?
            # moved this below the dynamic LBot calculation because I wanted to use the original self.L
            # >>> this is probably needed for Rods - should look into using for Rods only <<<
            
            # check for tension data <<<<<<<
            
            self.show = True
            
        else:
            self.Tdata = []
            self.show = False
            print(f"Error geting data for {'Rod' if self.isRod else 'Line'} {self.number}: {filename}")
            print("dirname: {} or rootname: {} is incorrect".format(dirname, rootname))
            
         
        # >>> this was another option for handling issues - maybe no longer needed <<<
        #except Exception as e:
        #    # don't fail if there's an issue finding data, just flag that the line shouldn't be shown/plotted
        #    print(f"Error geting data for {'Rod' if self.isRod else 'Line'} {self.number}: ")
        #    print(e)
        #    self.show = False
        
    
    def setL(self, L):
        '''Sets the line unstretched length [m], and saves it for use with
        static-dynamic stiffness adjustments. Also reverts to static 
        stiffness to avoid an undefined state of having changing the line
        length in a state with adjusted dynamic EA and L values.'''
        self.L = L
        self.L0 = L
        self.revertToStaticStiffness()


    def getTimestep(self, Time):
        '''Get the time step to use for showing time series data'''
        
        if Time < 0: 
            ts = int(-Time)  # negative value indicates passing a time step index
        else:           # otherwise it's a time in s, so find closest time step
            if len(self.Tdata) > 0:
                for index, item in enumerate(self.Tdata):                
                    ts = -1
                    if item > Time:
                        ts = index
                        break
                if ts==-1:
                    raise LineError(self.number, "getTimestep: requested time likely out of range")
            else:
                raise LineError(self.number, "getTimestep: zero time steps are stored")

        return ts
        
        

    def getLineCoords(self, Time, n=0, segmentTensions=False):
        '''Gets the updated line coordinates for drawing and plotting purposes.'''
        
        if n==0: n = self.nNodes   # <<< not used!
    
        # special temporary case to draw a rod for visualization. This assumes the rod end points have already been set somehow
        if self.qs==1 and self.isRod > 0:
        
            # make points for appropriately sized cylinder
            d = self.type['d_vol']
            Xs, Ys, Zs = makeTower(self.L, np.array([d/2, d/2]))   # add in makeTower method once you start using Rods
            
            # get unit vector and orientation matrix
            k = (self.rB-self.rA)/self.L
            Rmat = np.array(rotationMatrix(0, np.arctan2(np.hypot(k[0],k[1]), k[2]), np.arctan2(k[1],k[0])))
        
            # translate and rotate into proper position for Rod
            coords = np.vstack([Xs, Ys, Zs])
            newcoords = np.matmul(Rmat,coords)
            Xs = newcoords[0,:] + self.rA[0]
            Ys = newcoords[1,:] + self.rA[1]
            Zs = newcoords[2,:] + self.rA[2]
            
            return Xs, Ys, Zs, None
        
    
        # if a quasi-static analysis, just call the catenary function to return the line coordinates
        elif self.qs==1:
            
            self.staticSolve(profiles=1) # call with flag to tell Catenary to return node info
            
            #Xs = self.rA[0] + self.info["X"]*self.cosBeta 
            #Ys = self.rA[1] + self.info["X"]*self.sinBeta 
            #Zs = self.rA[2] + self.info["Z"]
            #Ts = self.info["Te"]
            Xs = self.Xs
            Ys = self.Ys
            Zs = self.Zs
            Ts = self.Ts
            return Xs, Ys, Zs, Ts
            
        # otherwise, count on read-in time-series data
        else:

            # figure out what time step to use
            ts = self.getTimestep(Time)
            
            # drawing rods
            if self.isRod > 0:
            
                k1 = np.array([ self.xp[ts,-1]-self.xp[ts,0], self.yp[ts,-1]-self.yp[ts,0], self.zp[ts,-1]-self.zp[ts,0] ]) / self.L # unit vector
                
                k = np.array(k1) # make copy
            
                Rmat = np.array(rotationMatrix(0, np.arctan2(np.hypot(k[0],k[1]), k[2]), np.arctan2(k[1],k[0])))  # <<< should fix this up at some point, MattLib func may be wrong
                
                # make points for appropriately sized cylinder
                d = self.type['d_vol']
                Xs, Ys, Zs = makeTower(self.L, np.array([d/2, d/2]))   # add in makeTower method once you start using Rods
                
                # translate and rotate into proper position for Rod
                coords = np.vstack([Xs, Ys, Zs])
                newcoords = np.matmul(Rmat,coords)
                Xs = newcoords[0,:] + self.xp[ts,0]
                Ys = newcoords[1,:] + self.yp[ts,0]
                Zs = newcoords[2,:] + self.zp[ts,0]
                
                return Xs, Ys, Zs, None
                
            # drawing lines
            else:
                
                # handle whether or not there is tension data
                try:  # use average to go from segment tension to node tensions <<< can skip this once MD is updated to output node tensions
                    if segmentTensions:
                        Te = self.Te[ts,:]  # return tensions of segments rather than averaging to get tensions of nodes
                    else:
                        Te = 0.5*(np.append(self.Te[ts,0], self.Te[ts,:]) +np.append(self.Te[ts,:], self.Te[ts,-1]))
                except: # otherwise return zeros to avoid an error (might want a warning in some cases?)
                    Te = np.zeros(self.nNodes)
                
                return self.xp[ts,:], self.yp[ts,:], self.zp[ts,:], Te
    
    
    def getCoordinate(self, s, n=100):
        '''Returns position and tension at a specific point along the line's unstretched length'''
        
        dr =  self.rB - self.rA                 
        LH = np.hypot(dr[0], dr[1])  
            
        Ss = np.linspace(0, self.L, n)
        Xs, Ys, Zs, Ts = self.getLineCoords(0.0, n=n)
        
        X = np.interp(s, Ss, Xs)*dr[0]/LH  #?
        Y = np.interp(s, Ss, Ys)*dr[1]/LH  #?
        Z = np.interp(s, Ss, Zs)
        T = np.interp(s, Ss, Ts)
        
        # <<< is this function used for anything?  Does it make sense?
        
        return X, Y, Z, T
        
    
    
    def drawLine2d(self, Time, ax, color="k", Xuvec=[1,0,0], Yuvec=[0,0,1], Xoff=0, Yoff=0, colortension=False, cmap='rainbow', plotnodes=[], plotnodesline=[], label="", alpha=1.0,linewidth=1):
        '''Draw the line on 2D plot (ax must be 2D)

        Parameters
        ----------
        Time : float
            time value at which to draw the line
        ax : axis
            the axis on which the line is to be drawn
        color : string, optional
            color identifier in one letter (k=black, b=blue,...). The default is "k".
        Xuvec : list, optional
            plane at which the x-axis is desired. The default is [1,0,0].
        Yuvec : list, optional
            plane at which the y-axis is desired. The default is [0,0,1].
        colortension : bool, optional
            toggle to plot the lines in a colormap based on node tensions. The default is False
        cmap : string, optional
            colormap string type to plot tensions when colortension=True. The default is 'rainbow'
        linewidth: float, optional
            sets the mooring lines linewidth in matplotlib. default is 1

        Returns
        -------
        linebit : list
            list of axes and points on which the line can be plotted

        '''
        
        linebit = []  # make empty list to hold plotted lines, however many there are
        
        if color == 'self':
            colorplot = self.color  # attempt to allow custom colors
        else:
            colorplot = color
        
        Xs, Ys, Zs, Ts = self.getLineCoords(Time)
        
        if self.isRod > 0:
            
            # apply any 3D to 2D transformation here to provide desired viewing angle
            Xs2d = Xs*Xuvec[0] + Ys*Xuvec[1] + Zs*Xuvec[2] 
            Ys2d = Xs*Yuvec[0] + Ys*Yuvec[1] + Zs*Yuvec[2] 
        
            for i in range(int(len(Xs)/2-1)):
                linebit.append(ax.plot(Xs2d[2*i:2*i+2]    ,Ys2d[2*i:2*i+2]    , lw=0.5, color=colorplot))  # side edges
                linebit.append(ax.plot(Xs2d[[2*i,2*i+2]]  ,Ys2d[[2*i,2*i+2]]  , lw=0.5, color=colorplot))  # end A edges
                linebit.append(ax.plot(Xs2d[[2*i+1,2*i+3]],Ys2d[[2*i+1,2*i+3]], lw=0.5, color=colorplot))  # end B edges
        
        # drawing lines...
        else:            
            if self.qs==0:
                self.rA = np.array([Xs[0], Ys[0], Zs[0]])
                self.rB = np.array([Xs[-1], Ys[-1], Zs[-1]])
            
            # apply any 3D to 2D transformation here to provide desired viewing angle
            Xs2d = Xs*Xuvec[0] + Ys*Xuvec[1] + Zs*Xuvec[2] + Xoff
            Ys2d = Xs*Yuvec[0] + Ys*Yuvec[1] + Zs*Yuvec[2] + Yoff
            
            if colortension:    # if the mooring lines want to be plotted with colors based on node tensions
                maxT = np.max(Ts); minT = np.min(Ts)
                for i in range(len(Xs)-1):          # for each node in the line
                    color_ratio = ((Ts[i] + Ts[i+1])/2 - minT)/(maxT - minT)  # ratio of the node tension in relation to the max and min tension
                    cmap_obj = cm.get_cmap(cmap)    # create a cmap object based on the desired colormap
                    rgba = cmap_obj(color_ratio)    # return the rbga values of the colormap of where the node tension is
                    linebit.append(ax.plot(Xs2d[i:i+2], Ys2d[i:i+2], color=rgba))
            else:
                linebit.append(ax.plot(Xs2d, Ys2d, lw=linewidth, color=color, label=label, alpha=alpha)) # previously had lw=1 (linewidth)
            
            if len(plotnodes) > 0:
                for i,node in enumerate(plotnodes):
                    if self.number==plotnodesline[i]:
                        linebit.append(ax.plot(Xs2d[node], Ys2d[node], 'o', color=color, markersize=5))   
            
        self.linebit = linebit # can we store this internally?
        
        self.X = np.array([Xs, Ys, Zs])
            
        return linebit

    

    def drawLine(self, Time, ax, color="k", endpoints=False, shadow=True, colortension=False, cmap_tension='rainbow'):
        '''Draw the line in 3D
        
        Parameters
        ----------
        Time : float
            time value at which to draw the line
        ax : axis
            the axis on which the line is to be drawn
        color : string, optional
            color identifier in one letter (k=black, b=blue,...). The default is "k".
        endpoints : bool, optional
            toggle to plot the end points of the lines. The default is False
        shadow : bool, optional
            toggle to plot the mooring line shadow on the seabed. The default is True
        colortension : bool, optional
            toggle to plot the lines in a colormap based on node tensions. The default is False
        cmap : string, optional
            colormap string type to plot tensions when colortension=True. The default is 'rainbow'
            
        Returns
        -------
        linebit : list
            list of axes and points on which the line can be plotted
        '''
        
        if not self.show:  # exit if this line isn't set to be shown
            return 0
        
        if color == 'self':
            color = self.color  # attempt to allow custom colors
            lw = self.lw
        elif color == None:
            color = [0.3, 0.3, 0.3]  # if no color, default to grey
            lw = 1
        else:
            lw = 1
        
        linebit = []  # make empty list to hold plotted lines, however many there are
        
        Xs, Ys, Zs, tensions = self.getLineCoords(Time)
        
        if self.isRod > 0:
            for i in range(int(len(Xs)/2-1)):
                linebit.append(ax.plot(Xs[2*i:2*i+2],Ys[2*i:2*i+2],Zs[2*i:2*i+2]            , color=color))  # side edges
                linebit.append(ax.plot(Xs[[2*i,2*i+2]],Ys[[2*i,2*i+2]],Zs[[2*i,2*i+2]]      , color=color))  # end A edges
                linebit.append(ax.plot(Xs[[2*i+1,2*i+3]],Ys[[2*i+1,2*i+3]],Zs[[2*i+1,2*i+3]], color=color))  # end B edges
            
            # scatter points for line ends 
            #if endpoints == True:
            #    linebit.append(ax.scatter([Xs[0], Xs[-1]], [Ys[0], Ys[-1]], [Zs[0], Zs[-1]], color = color))
        
        # drawing lines...
        else:
            if self.qs==0:
                self.rA = np.array([Xs[0], Ys[0], Zs[0]])
                self.rB = np.array([Xs[-1], Ys[-1], Zs[-1]])
            
            if colortension:    # if the mooring lines want to be plotted with colors based on node tensions
                maxT = np.max(tensions); minT = np.min(tensions)
                for i in range(len(Xs)-1):          # for each node in the line
                    color_ratio = ((tensions[i] + tensions[i+1])/2 - minT)/(maxT - minT)  # ratio of the node tension in relation to the max and min tension
                    cmap_obj = cm.get_cmap(cmap_tension)    # create a cmap object based on the desired colormap
                    rgba = cmap_obj(color_ratio)    # return the rbga values of the colormap of where the node tension is
                    linebit.append(ax.plot(Xs[i:i+2], Ys[i:i+2], Zs[i:i+2], color=rgba, zorder=100))
            else:
                linebit.append(ax.plot(Xs, Ys, Zs, color=color, lw=lw, zorder=100))
                
            if shadow:
                if self.sys.seabedMod == 0:
                    Zs = np.zeros_like(Xs)-self.sys.depth
                elif self.sys.seabedMod == 1:
                    Zs = self.sys.depth - self.sys.xSlope*Xs - self.sys.ySlope*Ys
                elif self.sys.seabedMod == 2:
                    Zs = np.zeros(len(Xs))
                    for i in range(len(Xs)):
                        z,_ = self.sys.getDepthFromBathymetry(Xs[i], Ys[i])
                        Zs[i] = -z

                ax.plot(Xs, Ys, Zs, color=[0.5, 0.5, 0.5, 0.2], lw=lw, zorder = 1.5) # draw shadow
            
            if endpoints == True:
                linebit.append(ax.scatter([Xs[0], Xs[-1]], [Ys[0], Ys[-1]], [Zs[0], Zs[-1]], color = color))
                
                    
            # draw additional data if available (should make this for rods too eventually - drawn along their axis nodes)
            if self.qs == 0:              
                ts = self.getTimestep(Time)
                
                if self.Tendata:
                    pass
                if self.Kudata:
                    pass        
                if self.Udata:
                    self.Ubits = ax.quiver(Xs, Ys, Zs, self.Ux[ts,:], self.Uy[ts,:], self.Uz[ts,:], color="blue")  # make quiver plot and save handle to line object
                if self.Bdata:
                    self.Bbits = ax.quiver(Xs, Ys, Zs, self.Bx[ts,:], self.By[ts,:], self.Bz[ts,:], color="red")
                if self.Ddata:
                    self.Dbits = ax.quiver(Xs, Ys, Zs, self.Dx[ts,:], self.Dy[ts,:], self.Dz[ts,:], color="green")
                if self.Wdata:
                    self.Wbits = ax.quiver(Xs, Ys, Zs, self.Wx[ts,:], self.Wy[ts,:], self.Wz[ts,:], color="orange")
                
                
        self.linebit = linebit # can we store this internally?
        
        self.X = np.array([Xs, Ys, Zs])
        
            
        return linebit
    
    
    def redrawLine(self, Time, colortension=False, cmap_tension='rainbow', drawU=True):  #, linebit):
        '''Update 3D line drawing based on instantaneous position'''
        
        linebit = self.linebit
        
        if self.isRod > 0:
            
            Xs, Ys, Zs, Ts = self.getLineCoords(Time)
            
            for i in range(int(len(Xs)/2-1)):
                        
                linebit[3*i  ][0].set_data(Xs[2*i:2*i+2],Ys[2*i:2*i+2])    # side edges (x and y coordinates)
                linebit[3*i  ][0].set_3d_properties(Zs[2*i:2*i+2])         #            (z coordinates)             
                linebit[3*i+1][0].set_data(Xs[[2*i,2*i+2]],Ys[[2*i,2*i+2]])           # end A edges
                linebit[3*i+1][0].set_3d_properties(Zs[[2*i,2*i+2]])                    
                linebit[3*i+2][0].set_data(Xs[[2*i+1,2*i+3]],Ys[[2*i+1,2*i+3]])   # end B edges
                linebit[3*i+2][0].set_3d_properties(Zs[[2*i+1,2*i+3]])
        
        # drawing lines...
        else:
        
            Xs, Ys, Zs, Ts = self.getLineCoords(Time)
            
            if colortension:
                self.rA = np.array([Xs[0], Ys[0], Zs[0]])       # update the line ends based on the MoorDyn data
                self.rB = np.array([Xs[-1], Ys[-1], Zs[-1]])
                maxT = np.max(Ts); minT = np.min(Ts)
                cmap_obj = cm.get_cmap(cmap_tension)               # create the colormap object
                
                for i in range(len(Xs)-1):  # for each node in the line, find the relative tension of the segment based on the max and min tensions
                    color_ratio = ((Ts[i] + Ts[i+1])/2 - minT)/(maxT - minT)
                    rgba = cmap_obj(color_ratio)
                    linebit[i][0]._color = rgba         # set the color of the segment to a new color based on its updated tension
                    linebit[i][0].set_data(Xs[i:i+2],Ys[i:i+2])     # set the x and y coordinates
                    linebit[i][0].set_3d_properties(Zs[i:i+2])      # set the z coorindates
            
            else:
                linebit[0][0].set_data(Xs,Ys)    # (x and y coordinates)
                linebit[0][0].set_3d_properties(Zs)         # (z coordinates) 
                    
            
        
            # draw additional data if available (should make this for rods too eventually - drawn along their axis nodes)
            if self.qs == 0:
                ts = self.getTimestep(Time)                    
                s = 0.0002
                
                if self.Tendata:
                    pass
                if self.Kudata:
                    pass        
                if self.Udata:
                    self.Ubits.set_segments(quiver_data_to_segments(Xs, Ys, Zs, self.Ux[ts,:], self.Uy[ts,:], self.Uz[ts,:], scale=10.))
                if self.Bdata:
                    self.Bbits.set_segments(quiver_data_to_segments(Xs, Ys, Zs, self.Bx[ts,:], self.By[ts,:], self.Bz[ts,:], scale=s))
                if self.Ddata:
                    self.Dbits.set_segments(quiver_data_to_segments(Xs, Ys, Zs, self.Dx[ts,:], self.Dy[ts,:], self.Dz[ts,:], scale=s))
                if self.Wdata:
                    self.Wbits.set_segments(quiver_data_to_segments(Xs, Ys, Zs, self.Wx[ts,:], self.Wy[ts,:], self.Wz[ts,:], scale=s))
                
                
        
        return linebit
        
        
    
    
    def setEndPosition(self, r, endB):
        '''Sets the end position of the line based on the input endB value.

        Parameters
        ----------
        r : array
            x,y,z coorindate position vector of the line end [m].
        endB : boolean
            An indicator of whether the r array is at the end or beginning of the line

        Raises
        ------
        LineError
            If the given endB value is not a 1 or 0

        Returns
        -------
        None.

        '''
        
        if endB == 1:
            self.rB = np.array(r, dtype=float)
        elif endB == 0:
            self.rA = np.array(r, dtype=float)
        else:
            raise LineError("setEndPosition: endB value has to be either 1 or 0")
        
        
    def staticSolve(self, reset=False, tol=0.0001, profiles=0):
        '''Solves static equilibrium of line. Sets the end forces of the line based on the end points' positions.

        Parameters
        ----------
        reset : boolean, optional
            Determines if the previous fairlead force values will be used for the catenary iteration. The default is False.

        tol : float
            Convergence tolerance for catenary solver measured as absolute error of x and z values in m.
            
        profiles : int
            Values greater than 0 signal for line profile data to be saved (used for plotting, getting distributed tensions, etc).

        Raises
        ------
        LineError
            If the horizontal force at the fairlead (HF) is less than 0

        Returns
        -------
        None.

        '''

        # deal with horizontal tension starting point
        if self.HF < 0:
            raise LineError("Line HF cannot be negative") # this could be a ValueError too...
            
        if reset==True:   # Indicates not to use previous fairlead force values to start catenary 
            self.HF = 0   # iteration with, and insteady use the default values.
        
        
        # ensure line profile information is computed if needed for computing current loads
        if self.sys.currentMod == 1 and profiles == 0:
            profiles = 1

        # get seabed depth and slope under each line end
        depthA, nvecA = self.sys.getDepthFromBathymetry(self.rA[0], self.rA[1])
        depthB, nvecB = self.sys.getDepthFromBathymetry(self.rB[0], self.rB[1])
        
        # deal with height off seabed issues
        if self.rA[2] < -depthA:
            self.rA[2] = -depthA
            self.cb = 0
            #raise LineError("Line {} end A is lower than the seabed.".format(self.number)) <<< temporarily adjust to seabed depth
        elif self.rB[2] < -depthB:
            raise LineError("Line {} end B is lower than the seabed.".format(self.number))
        else:
            self.cb = -depthA - self.rA[2]  # when cb < 0, -cb is defined as height of end A off seabed (in catenary)

        # Handle case of line above the water
        if self.rA[2] > 0 or self.rB[2] > 0:  # end B out of water  << or do this in catenary?
            ww = (self.type['m'] - np.pi/4*self.type['d_vol']**2 *self.sys.rho) * self.sys.g
            wa = (self.type['m']) * self.sys.g  # weight in air
            
            z_top = max(self.rA[2], self.rB[2])
            z_bot = min(self.rA[2], self.rB[2])
            
            # overwrite the weight based on the wet-dry weight ratio based on end z vals
            self.w = ww + (wa - ww) * ( -min(0, z_bot) )/( z_top - z_bot )
        else:
            # reset to the normal weight if the line hasn't left the water
            self.w = (self.type['m'] - np.pi/4*self.type['d_vol']**2 *self.sys.rho) * self.sys.g
            
            # >>> TODO: should replace all this with a more versatile catenary solve in future <<<
        
        # ----- Perform rotation/transformation to 2D plane of catenary -----
        
        dr =  self.rB - self.rA
        
        # if a current force is present, include it in the catenary solution
        if np.sum(np.abs(self.fCurrent)) > 0:
        
            # total line exernal force per unit length vector (weight plus current drag)
            w_vec = self.fCurrent/self.L + np.array([0, 0, -self.w])
            w_total = np.linalg.norm(w_vec)
            w_hat = w_vec/w_total
            
            # >>> may need to adjust to handle case of buoyant vertical lines <<<
            
            # get rotation matrix from gravity down to w_vec being down
            if w_hat[0] == 0 and w_hat[1] == 0: 
                if w_hat[2] < 0:
                    R_curr = np.eye(3,3)
                else:
                    R_curr = -np.eye(3,3)
            else:
                R_curr = RotFrm2Vect(w_hat, np.array([0, 0, -1]))  # rotation matrix to make w vertical
        
            # vector from A to B needs to be put into the rotated frame
            dr = np.matmul(R_curr, dr)  
        
        # if no current force, things are simple
        else:
            R_curr = np.eye(3,3)
            w_total = self.w
        
        
        # horizontal and vertical dimensions of line profile (end A to B)
        LH = np.linalg.norm(dr[:2])
        LV = dr[2]
        
        
        # apply a rotation about Z' to align the line profile with the X'-Z' plane
        theta_z = -np.arctan2(dr[1], dr[0])
        R_z = rotationMatrix(0, 0, theta_z)
        
        # overall rotation matrix (global to catenary plane)
        R = np.matmul(R_z, R_curr)   
        
        # figure out slope in plane (only if contacting the seabed)
        if self.rA[2] <= -depthA or self.rB[2] <= -depthB:
            #nvecA_prime = np.matmul(R, nvecA)
            #dz_dx = -nvecA_prime[0]*(1.0/nvecA_prime[2])  # seabed slope components
            #dz_dy = -nvecA_prime[1]*(1.0/nvecA_prime[2])  # seabed slope components
            
            if LH == 0:
                alpha = 0
            else:
                # Seabed slope along line direction (based on end A/B depth)
                dz_dx = (-depthB + depthA)/LH  
                # we only care about dz_dx since the line is in the X-Z plane in this rotated situation
                alpha = np.degrees(np.arctan(dz_dx))
            cb = self.cb
        else:
            if np.sum(np.abs(self.fCurrent)) > 0 or nvecA[2] < 1: # if there is current or seabed slope
                alpha = 0
                cb = min(0, dr[2]) - 100  # put the seabed out of reach (model limitation)
            else:  # otherwise proceed as usual (this is the normal case)
                alpha = 0
                cb = self.cb
        
        
        # ----- call catenary function or alternative and save results -----
        
        #If EA is found in the line properties we will run the original catenary function 
        if 'EA' in self.type:
            try:
                (fAH, fAV, fBH, fBV, info) = catenary(LH, LV, self.L, self.EA, 
                     w_total, CB=cb, alpha=alpha, HF0=self.HF, VF0=self.VF, Tol=tol, 
                     nNodes=self.nNodes, plots=profiles, depth=self.sys.depth)
            
            except CatenaryError as error:
                raise LineError(self.number, error.message)       
        #If EA isnt found then we will use the ten-str relationship defined in the input file 
        else:
            (fAH, fAV, fBH, fBV, info) = nonlinear(LH, LV, self.L, self.type['Str'], self.type['Ten'],np.linalg.norm(w_total)) 
    
    
        # save line profile coordinates in global frame (involves inverse rotation)
        if profiles > 0:
            # note: instantiating new arrays rather than writing directly to self.Xs 
            # seems to be necessary to avoid plots auto-updating to the current 
            # profile of the Line object.
            Xs = np.zeros(self.nNodes)
            Ys = np.zeros(self.nNodes)
            Zs = np.zeros(self.nNodes)
            # apply inverse rotation to node positions
            for i in range(0,self.nNodes):
                temp_array = np.array([info['X'][i], 0 ,info['Z'][i]])
                unrot_pos = np.matmul(temp_array, R)
                
                Xs[i] = self.rA[0] + unrot_pos[0]
                Ys[i] = self.rA[1] + unrot_pos[1]
                Zs[i] = self.rA[2] + unrot_pos[2]

            self.Xs = Xs
            self.Ys = Ys
            self.Zs = Zs
            self.Ts = info["Te"]
        
        # save fairlead tension components for use as ICs next iteration
        self.HF = info["HF"]
        self.VF = info["VF"]
        
        # save other important info
        self.LBot = info["LBot"]
        self.z_extreme = self.rA[2] + info["Zextreme"]
        self.info = info
        
        # save forces in global reference frame
        self.fA = np.matmul(np.array([fAH, 0, fAV]), R)
        self.fB = np.matmul(np.array([fBH, 0, fBV]), R)
        self.TA = np.linalg.norm(self.fA) # end tensions
        self.TB = np.linalg.norm(self.fB)
        
        # Compute transverse (out-of-plane) stiffness term
        if LH < 0.01*abs(LV):  # if line is nearly vertical (note: this theshold is unverified)
            Kt = 0.5*(fAV-fBV)/LV  # compute Kt based on vertical tension/span
        else:  # otherwise use the classic horizontal approach
            Kt = -fBH/LH
        
        
        # save 3d stiffness matrix in global orientation for both line ends (3 DOF + 3 DOF)
        self.KA  = from2Dto3Drotated(info['stiffnessA'],  Kt, R.T)  # reaction at A due to motion of A
        self.KB  = from2Dto3Drotated(info['stiffnessB'],  Kt, R.T)  # reaction at B due to motion of B
        self.KBA = from2Dto3Drotated(info['stiffnessBA'],-Kt, R.T)  # reaction at B due to motion of A
        
        
        # ----- calculate current loads if applicable, for use next time -----
        
        if self.sys.currentMod == 1: 

            U = self.sys.current  # 3D current velocity [m/s]  (could be changed to depth-dependent profile)
            
            fCurrent = np.zeros(3)  # total current force on line in x, y, z [N]        
            
            # Loop through each segment along the line and add up the drag forces.
            # This is in contrast to MoorDyn calculating for nodes.
            for i in range(self.nNodes-1):
                #For each segment find the tangent vector and then calculate the current loading
                dr_seg = np.array([self.Xs[i+1] - self.Xs[i], 
                                   self.Ys[i+1] - self.Ys[i], 
                                   self.Zs[i+1] - self.Zs[i]])  # segment vector
                ds_seg = np.linalg.norm(dr_seg)
                
                if ds_seg > 0:                   # only include if segment length > 0
                    q = dr_seg/ds_seg
                    # transverse and axial current velocity components
                    Uq = np.dot(U, q) * q
                    Up = U - Uq          
                    # transverse and axial drag forces on segment
                    dp = 0.5*self.sys.rho*self.type["Cd"]        *self.type["d_vol"]*ds_seg*np.linalg.norm(Up)*Up
                    dq = 0.5*self.sys.rho*self.type["CdAx"]*np.pi*self.type["d_vol"]*ds_seg*np.linalg.norm(Uq)*Uq
                    # add to total current force on line
                    fCurrent += dp + dq    
            
            self.fCurrent = fCurrent  # save for use next call
        else:
            self.fCurrent = np.zeros(3)  # if no current, ensure this force is zero


        # ----- plot the profile if requested -----
        if profiles > 1:
            import matplotlib.pyplot as plt
            plt.plot(self.info['X'], self.info['Z'])
            plt.show()
    

    def getTension(self, s):
        '''Returns tension at a given point along the line
        
        Parameters
        ----------
        
        s : scalar or array-like
            Value or array of values for the arc length along the line from end A to end B at which
            the information is desired. Positive values are arc length in m, negative values are a
            relative location where 0 is end A, -1 is end B, and -0.5 is the midpoint.
        
        Returns
        -------
        
        tension value(s)
        
        '''
        #if s < 0:
        #    s = -s*self.L            
        #if s > self.L:
        #    raise ValueError('Specified arc length is larger than the line unstretched length.')
        
        Te = np.interp(s, self.info['s'], self.info['Te'])
        
        return Te


    def getPosition(self, s):
        '''Returns position at a given point along the line
        
        Parameters
        ----------
        
        s : scalar or array-like
            Value or array of values for the arc length along the line from end A to end B at which
            the information is desired. Positive values are arc length in m, negative values are a
            relative location where 0 is end A, -1 is end B, and -0.5 is the midpoint.
        
        Returns
        -------
        
        position vector(s)
        
        '''
        
        # >>> should be merged with getLineCoords and getCoordinate functionality <<<
        
        x = np.interp(s, self.info['s'], self.info['X'])
        z = np.interp(s, self.info['s'], self.info['Z'])
        
        
        dr =  self.rB - self.rA                 
        LH = np.hypot(dr[0], dr[1])
        Xs = self.rA[0] + x*dr[0]/LH
        Ys = self.rA[1] + x*dr[1]/LH
        Zs = self.rA[2] + z
        
        return np.vstack([ Xs, Ys, Zs])

    
    def getCost(self):
        '''Fill in a cost dictionary and return the total cost for this Line object.'''
        self.cost = {}  # clear any old cost numbers
        self.cost['material'] = self.type['cost']*self.L0
        total_cost = sum(self.cost.values()) 
        return total_cost
    
    
    def attachLine(self, lineID, endB):
        pass


    def activateDynamicStiffness(self, display=0):
        '''Switch mooring line model to dynamic line stiffness value, 
        including potential unstretched line length adjustment, taking the 
        current state as the mean/static offset position to work from. 
        This only works when dynamic line properties are used.'''
        
        if self.type['EAd'] > 0:
            # switch to dynamic stiffness value
            EA_old = self.type['EA']
            EA_new = self.type['EAd'] + self.type['EAd_Lm']*np.mean([self.TA, self.TB])  # this implements the sloped Krd = alpha + beta*Lm
            self.EA = np.max([EA_new, EA_old])  # only if the dynamic stiffness is higher than the static stiffness, activate the dynamic stiffness
            
            # adjust line length to maintain current tension (approximate)
            self.L = self.L0 * (1 + self.TB/EA_old)/(1 + self.TB/EA_new)
            
        else:
            if display > 0:
                print(f'Line {self.number} has zero dynamic stiffness coefficient so activateDynamicStiffness does nothing.')
        
    
    def revertToStaticStiffness(self):
        '''Switch mooring line model to dynamic line stiffness
        values, including potential unstretched line length
        adjustment. This only works when dynamic line properties
        are used.'''
        
        # switch to static/default stiffness value
        self.EA = self.type['EA']
        
        # revert to original line length
        self.L = self.L0
  


    def updateLumpedMass(self, omegas, S_zeta, depth, kbot=0, cbot=0, seabed_tol=1e-4, RAO_A=None, RAO_B=None):
        '''Updates the inertia, added mass, damping, and stiffness matrices for the line using a lumped mass approach.

        The updated values are stored in:
        - self.Ml_allNodes, self.Al_allNodes, self.Bl_allNodes, and self.Kl_allNodes: 3N x 3N matrices, where N is the number of nodes (3 dofs for each line node)
        - self.Ml, self.Al, self.Bl, and self.Kl: 6 x 6 matrices that lump the matrices _allNodes at the extremities of the line. For example,
                self.Kl[:3, :3] is analogous to self.KA, self.Kl[3:, 3:] is analogous to self.KB, and self.Kl[:3, 3:] is analogous to self.KBA.

        Only the damping matrix depends on sea state and RAOs. Hence, this function can be called without the RAOs to compute the inertia, added mass, and 
        stiffness matrices. But do NOT trust the values of the damping matrix if the RAOs are not provided!!!

        Inputs
        ----------        
        omegas : ndarray
            Array of frequencies in rad/s.
        S_zeta : ndarray
            Wave spectrum array in m^2/(rad/s), must be of the same length as omegas.
        depth : float
            Water depth.
        kbot : float
            Vertical stiffness for points lying on the seabed.
        cbot : float
            Vertical damping for points lying on the seabed.
        seabed_tol : float, optional
            Distance from seabed within which a node is considered to be lying on the seabed, by default 1e-4 m.
        RAO_A : ndarray
            Translational RAOs for end node A given as a (m,3) array where m is the number of frequencies (must be equal to the length of omegas) .
        RAO_B : ndarray
            Translational RAOs for end node B given as a (m,3) array where m is the number of frequencies (must be equal to the length of omegas) .

        '''

        def lump_matrix(matrix, nodes2remove=None):
            # The matrices should be symmetrical, but they can be slightly off due to numerical errors.
            # Because we are going to invert them twice, we force them to be symmetrical to avoid amplifying the errors. 
            matrix = (matrix + matrix.T)/2

            # Empty nodes2remove is the same thing as not removing any nodes
            if nodes2remove is not None and nodes2remove.size==0:
                nodes2remove = None

            if nodes2remove is not None:
                # Convert nodes2remove to dofs
                dofs2remove = np.array([(node*3, node*3+1, node*3+2) for node in nodes2remove]).flatten()

                # Create a mask for the dofs to keep
                # dofs2remove = np.array([node*3 + 2 for node in nodes2remove])

                # Create a mask for the dofs to keep
                mask = np.ones(matrix.shape[0], dtype=bool)
                mask[dofs2remove] = False
                # Remove the rows and columns
                matrix = matrix[mask][:, mask]           

            n = matrix.shape[0]

            # If we are not removing the extremities, we fill the whole 6x6 matrix_inv_coupled
            if nodes2remove is None or (nodes2remove[0] !=0 and nodes2remove[-1] != self.nNodes-1):                
                keep = list(range(3)) + list(range(n-3, n))
                free = [i for i in range(n) if i not in keep]
                i_arrange = keep + free
                matrix_coupled = guyan_reduce(matrix[:,i_arrange][i_arrange,:], len(keep))
            
            # if we are removing the first node, we fill the bottom right 3x3 matrix
            if nodes2remove is not None and nodes2remove[0] == 0:
                keep = list(range(n-3, n))
                free = [i for i in range(n) if i not in keep]
                i_arrange = keep + free
                matrix_coupled = np.block([[np.zeros((3,3)), np.zeros((3,3))], [np.zeros((3,3)), guyan_reduce(matrix[:,i_arrange][i_arrange,:], len(keep))]])
            
            # if we are removing the last node, we fill the top left 3x3 matrix
            if nodes2remove is not None and nodes2remove[-1] == self.nNodes-1:
                keep = list(range(3))
                free = [i for i in range(n) if i not in keep]
                i_arrange = keep + free
                matrix_coupled = np.block([[guyan_reduce(matrix[:,i_arrange][i_arrange,:], len(keep)), np.zeros((3,3))], [np.zeros((3,3)), np.zeros((3,3))]])

            return matrix_coupled
       
        # Remove the nodes that are lying on the seabed
        if not hasattr(self, 'lineList'):
            X_mean,Y_mean,Z_mean,T_mean = self.getLineCoords(0.0,n=self.nNodes) # coordinates of line nodes and tension values
            depth = self.sys.depth
        else:
            X_mean, Y_mean, Z_mean = [np.zeros(self.nNodes) for _ in range(3)]
            depth = self.depth
            idxNodeA = 0
            for iline, line in enumerate(self.lineList):
                x, y, z, _ = line.getLineCoords(0.0)
                n = len(x)                
                if iline == 0:  # For the first line, include all nodes
                    X_mean[idxNodeA:idxNodeA+n] = x
                    Y_mean[idxNodeA:idxNodeA+n] = y
                    Z_mean[idxNodeA:idxNodeA+n] = z
                    idxNodeA += n
                else:  # For subsequent lines, exclude the first node to avoid repetition
                    X_mean[idxNodeA:idxNodeA+n-1] = x[1:]
                    Y_mean[idxNodeA:idxNodeA+n-1] = y[1:]
                    Z_mean[idxNodeA:idxNodeA+n-1] = z[1:]
                    idxNodeA += (n - 1)
        idx2remove = np.where(Z_mean <= -depth+1e-06)[0]
                
        # If we have the motions of the extremities of the lines, we can iterate the quadratic drag to compute the damping matrix.
        # Otherwise, we use the default values for the motion amplitude (unitary displacement for all nodes) to linearize the quadratic drag. This is clearly wrong, but affects only drag.
        if RAO_A is None and RAO_B is None:
            r_dynamic = np.ones((len(omegas), self.nNodes, 3))
            self.Ml_allNodes, self.Al_allNodes, self.Bl_allNodes, self.Kl_allNodes, _, _ = self.getDynamicMatrices(omegas, S_zeta, r_dynamic, depth, kbot=kbot, cbot=cbot, seabed_tol=seabed_tol)
        else:
            if RAO_A is None:
                RAO_A = np.zeros([len(omegas), 3])
            if RAO_B is None:
                RAO_B = np.zeros([len(omegas), 3])
            self.Ml_allNodes, self.Al_allNodes, self.Bl_allNodes, self.Kl_allNodes = self.dynamicSolve(omegas, S_zeta, RAO_A, RAO_B, depth, kbot, cbot, seabed_tol=seabed_tol, returnMatrices=True) # Dynamic solve calls get_dynamic_matrices internally

        self.Ml = lump_matrix(self.Ml_allNodes, nodes2remove=idx2remove)
        self.Al = lump_matrix(self.Al_allNodes, nodes2remove=idx2remove)
        self.Bl = lump_matrix(self.Bl_allNodes, nodes2remove=idx2remove)
        self.Kl = lump_matrix(self.Kl_allNodes, nodes2remove=idx2remove)       

    def getDynamicMatricesLumped(self):
        '''Return the lumped M,A,B,K matrices for the Line object.'''
        if not hasattr(self, 'Ml'):
            raise LineError("Call updateLumpedMass first")
        return self.Ml, self.Al, self.Bl, self.Kl

    def getDynamicMatrices(self, omegas, S_zeta, r_dynamic, depth, kbot=0, cbot=0, seabed_tol=1e-4):
        '''Compute M,A,B,K matrices for Line. See get_dynamic_matrices().'''
        return get_dynamic_matrices(self, omegas, S_zeta, r_dynamic, depth, kbot, cbot, seabed_tol=seabed_tol)    

    def dynamicSolve(self, omegas, S_zeta, RAO_A, RAO_B, depth, kbot=0, cbot=0, seabed_tol=1e-4, tol = 0.01, iters=100, w = 0.8, conv_time=False, returnMatrices=False):
        '''Compute complex amplitudes of line nodes. See get_dynamic_tension().'''
        return get_dynamic_tension(self, omegas, S_zeta, RAO_A, RAO_B, depth, kbot, cbot, seabed_tol=seabed_tol, tol=tol, iters=iters, w=w, conv_time=conv_time, returnMatrices=returnMatrices)
    
    
    def getModes(self,*args, **kwargs):
        '''Compute (and optionally plot) the line's mode shapes.
        See get_modes().'''
        return get_modes(self, *args, **kwargs)
    

def from2Dto3Drotated(K2D, Kt, R): 
    '''Initialize a line end's analytic stiffness matrix in the 
    plane of the catenary then rotate the matrix to be about the 
    global frame using [K'] = [R][K][R]^T
    
    Parameters
    ----------
    K2D : 2x2 matrix
        Planar stiffness matrix of line end [N/m]
    Kt : float
        Transverse (out-of-plane) stiffness term [N/m].
    R : 3x3 matrix
        Rotation matrix from global frame to the local
        X-Z plane of the line
        
    Returns
    -------
    3x3 stiffness matrix in global orientation [N/m].
    '''    
    K2 = np.array([[K2D[0,0], 0 , K2D[0,1]],
                   [  0     , Kt,   0     ],
                   [K2D[1,0], 0 , K2D[1,1]]])
    
    return np.matmul(np.matmul(R, K2), R.T)    


def RotFrm2Vect( A, B):
    '''Rodriguez rotation function, which returns the rotation matrix 
    that transforms vector A into Vector B.
    '''
    
    v = np.cross(A,B)
    ssc = np.array([[0, -v[2], v[1]],
                [v[2], 0, -v[0]],
                [-v[1], v[0], 0]])
         
    R =  np.eye(3,3) + ssc + np.matmul(ssc,ssc)*(1-np.dot(A,B))/(np.linalg.norm(v)*np.linalg.norm(v))            

    return R