#Utility Functions
import copy
from collections import defaultdict
import glob
import os
import random
from stl import mesh

#Math Functions
import alphashape
from descartes import PolygonPatch
import math
import numpy as np
import scipy.linalg as ling
from scipy.spatial import Delaunay
from scipy.special import jn

#Drawing Functios
import matplotlib.pyplot
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection, Line3DCollection
from matplotlib.colors import LightSource
from matplotlib import cm

#Other Modules
from faser_math import fsr
from faser_utils.disp.disp import disp, progressBar


# Create an instance of a LightSource and use it to illuminate the surface.

def alpha_shape_3D(pos, alpha):
    """
    Compute the alpha shape (concave hull) of a set of 3D points.
    Parameters:
        pos - np.array of shape (n, 3) points.
        alpha - alpha value.
    return
        outer surface vertex indices, edge indices, and triangle indices
    """
    #Function found here https://stackoverflow.com/questions/26303878/alpha-shapes-in-3d
    tetra = Delaunay(pos)
    # Find radius of the circumsphere.
    # By definition, radius of the sphere fitting inside the tetrahedral needs
    # to be smaller than alpha value
    # http://mathworld.wolfram.com/Circumsphere.html
    tetrapos = np.take(pos, tetra.vertices, axis=0)
    normsq = np.sum(tetrapos**2, axis=2)[:,:,None]
    ones = np.ones((tetrapos.shape[0], tetrapos.shape[1], 1))
    a = np.linalg.det(np.concatenate((tetrapos, ones), axis=2))
    Dx = np.linalg.det(np.concatenate((normsq, tetrapos[:,:,[1, 2]], ones), axis=2))
    Dy = -np.linalg.det(np.concatenate((normsq, tetrapos[:,:,[0, 2]], ones), axis=2))
    Dz = np.linalg.det(np.concatenate((normsq, tetrapos[:,:,[0, 1]], ones), axis=2))
    c = np.linalg.det(np.concatenate((normsq, tetrapos), axis=2))
    r = np.sqrt(Dx**2+Dy**2+Dz**2-4*a*c)/(2*np.abs(a))

    # Find tetrahedrals
    tetras = tetra.vertices[r<alpha,:]
    # triangles
    TriComb = np.array([(0, 1, 2), (0, 1, 3), (0, 2, 3), (1, 2, 3)])
    Triangles = tetras[:,TriComb].reshape(-1, 3)
    Triangles = np.sort(Triangles, axis=1)
    # Remove triangles that occurs twice, because they are within shapes
    TrianglesDict = defaultdict(int)
    for tri in Triangles:TrianglesDict[tuple(tri)] += 1
    Triangles=np.array([tri for tri in TrianglesDict if TrianglesDict[tri] ==1])
    #edges
    EdgeComb=np.array([(0, 1), (0, 2), (1, 2)])
    Edges=Triangles[:,EdgeComb].reshape(-1, 2)
    Edges=np.sort(Edges, axis=1)
    Edges=np.unique(Edges, axis=0)

    Vertices = np.unique(Edges)
    return Vertices,Edges,Triangles

def DrawManipulability(J, tm, lenfactor, ax):
    """Short summary.

    Args:
        J (type): Description of parameter `J`.
        tm (type): Description of parameter `tm`.
        lenfactor (type): Description of parameter `lenfactor`.

    Returns:
        type: Description of returned object.

    """
    p = tm[0:3, 3]
    R = tm[0:3, 2]
    Aw = J[0:3,:] @ J[0:3,:].conj().transpose()
    Av = J[3:6,:] @ J[3:6,:].conj().transpose()

    weigv, weigd = ling.eig(Aw)
    weig = math.sqrt(np.diag(weigd))
    [veigv, veigd] = ling.eig(Av)
    veig = math.sqrt(np.diag(veigd))

    weigvs = weigv.copy()
    veigvs = veigv.copy()

    for i in range(0, 3):
        weigvs[0:3, i] = R @ weigv[0:3, i]
        veigvs[0:3, i] = R @ veigv[0:3, i]

    for i in range(0, 3):
        pw = p + lenfactor * weigvs[0:3, i] * weig[i]
        pv = p + lenfactor * veigvs[0:3, i] * veig[i]

        ax.plot3D(p, pw)
        ax.plot3D(p, pv)

def drawROM(arm, ares,  ax):
    """Short summary.

    Args:
        arm (type): Description of parameter `arm`.
        ares (type): Description of parameter `ares`.
        ax (type): Description of parameter `ax`.

    Returns:
        type: Description of returned object.

    """
    farthestx = []
    farthesty = []
    farthestz = []
    lmin = -180
    lmax = 180

    for i in range(50000):
        print(i/50000*100)
        t = arm.FK(np.random.rand(7) * 2 * np.pi - np.pi)
        ta = fsr.TMtoTAA(t)
        farthestx.append(ta[0])
        farthesty.append(ta[1])
        farthestz.append(ta[2])

    ax.scatter3D(farthestx, farthesty, farthestz, s = 2)

def DrawSTL(tm, fname, ax, scale = 1.0):
    """Short summary.

    Args:
        tm (type): Description of parameter `tm`.
        fname (type): Description of parameter `fname`.
        ax (type): Description of parameter `ax`.
        scale (type): Description of parameter `scale`.

    Returns:
        type: Description of returned object.

    """
    #make sure to install nuumpy-stl and not stl


    t_mesh = mesh.Mesh.from_file(fname)
    for i in range(len(t_mesh.x)):
        for j in range(3):
            t_mesp = fsr.TAAtoTM(np.array([t_mesh.x[i, j], t_mesh.y[i, j], t_mesh.z[i, j], 0, 0, 0]))
            #disp(t_mesh.x[i, j])
            t_new = tm @ t_mesp
            mesp_aa = fsr.TMtoTAA(t_new)
            t_mesh.x[i, j] = mesp_aa[0] * scale
            t_mesh.y[i, j] = mesp_aa[1] * scale
            t_mesh.z[i, j] = mesp_aa[2] * scale


    X = t_mesh.x
    Y = t_mesh.y
    Z = t_mesh.z

    light = LightSource(90, 45)
    illuminated_surface = light.shade(Z, cmap=cm.coolwarm)

    ax.plot_surface(t_mesh.x, t_mesh.y, t_mesh.z, rstride=1, cstride=1, linewidth=0, antialiased=False,
                    facecolors=illuminated_surface)
    #ax.add_collection3d(mplot3d.art3d.Poly3DCollection(t_mesh.vectors))
    #ax.auto_scale_xyz(scale, scale, scale)

def getSTLProps(fname):
    """Short summary.

    Args:
        fname (type): Description of parameter `fname`.

    Returns:
        type: Description of returned object.

    """
    #Return center of Mass, inertia, etc
    new_mesh = mesh.Mesh.from_file(fname)

    return new_mesh.get_mass_properties()

def QuadPlot(p1, p2, dim, ax, c = 'b'):
    """Short summary.

    Args:
        p1 (type): Description of parameter `p1`.
        p2 (type): Description of parameter `p2`.
        dim (type): Description of parameter `dim`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.

    Returns:
        type: Description of returned object.

    """
    bl = p1.spawnNew([0, -dim[0]/2, -dim[1]/2, 0, 0, 0])
    br = p1.spawnNew([0, -dim[0]/2, dim[1]/2, 0,  0, 0])
    tl = p1.spawnNew([0, dim[0]/2, -dim[1]/2, 0,  0, 0])
    tr = p1.spawnNew([0, dim[0]/2, dim[1]/2, 0,  0, 0])
    p1a = p1
    p2a = p2
    p1bl = p1 @ bl
    p2bl = p2 @ bl
    p1br = p1 @ br
    p2br = p2 @ br
    p1tl = p1 @ tl
    p2tl = p2 @ tl
    p1tr = p1 @ tr
    p2tr = p2 @ tr
    #Core
    ax.plot3D((p1bl[0], p2bl[0]),(p1bl[1], p2bl[1]),(p1bl[2], p2bl[2]), c)
    ax.plot3D((p1br[0], p2br[0]),(p1br[1], p2br[1]),(p1br[2], p2br[2]), c)
    ax.plot3D((p1tl[0], p2tl[0]),(p1tl[1], p2tl[1]),(p1tl[2], p2tl[2]), c)
    ax.plot3D((p1tr[0], p2tr[0]),(p1tr[1], p2tr[1]),(p1tr[2], p2tr[2]), c)

    #End
    ax.plot3D((p2tl[0], p2bl[0]),(p2tl[1], p2bl[1]),(p2tl[2], p2bl[2]), c)
    ax.plot3D((p2tr[0], p2br[0]),(p2tr[1], p2br[1]),(p2tr[2], p2br[2]), c)
    ax.plot3D((p2bl[0], p2br[0]),(p2bl[1], p2br[1]),(p2bl[2], p2br[2]), c)
    ax.plot3D((p2tl[0], p2tr[0]),(p2tl[1], p2tr[1]),(p2tl[2], p2tr[2]), c)

    #ax.plot3D((p1tl[0], p1bl[0]),(p1tl[1], p1bl[1]),(p1tl[2], p1bl[2]), c)
    #ax.plot3D((p1tr[0], p1br[0]),(p1tr[1], p1br[1]),(p1tr[2], p1br[2]), c)
    #ax.plot3D((p1bl[0], p1br[0]),(p1bl[1], p1br[1]),(p1bl[2], p1br[2]), c)
    #ax.plot3D((p1tl[0], p1tr[0]),(p1tl[1], p1tr[1]),(p1tl[2], p1tr[2]), c)


def DrawArm(arm, ax, jrad = .1, jdia = .3, lens = 1, c = 'grey', forces = np.zeros((1))):
    """Short summary.

    Args:
        arm (type): Description of parameter `arm`.
        ax (type): Description of parameter `ax`.
        jrad (type): Description of parameter `jrad`.
        jdia (type): Description of parameter `jdia`.
        lens (type): Description of parameter `lens`.
        c (type): Description of parameter `c`.
        forces (type): Description of parameter `forces`.

    Returns:
        DrawArm(arm, ax, jrad = .1, jdia = .3, lens = 1, c = 'grey', forces =: Description of returned object.

    """
    startind = 0
    while (sum(arm.screw_list[3:6, startind]) == 1):
        startind = startind + 1
    poses = arm.getJointTransforms()
    p = np.zeros((3, len(poses[startind:])))
    for i in range(startind, len(poses[startind:])):
        if poses[i] == None:
            continue
        p[0, i] = (poses[i].TAA[0])
        p[1, i] = (poses[i].TAA[1])
        p[2, i] = (poses[i].TAA[2])
    ax.scatter3D(p[0,:], p[1,:], p[2,:])
    ax.plot3D(p[0,:], p[1,:], p[2,:])
    Dims = np.copy(arm.link_dimensions).T
    dofs = arm.screw_list.shape[1]
    yrot = poses[0].spawnNew([0, 0, 0, 0, np.pi/2, 0])
    xrot = poses[0].spawnNew([0, 0, 0, np.pi/2, 0, 0])
    zrot = poses[0].spawnNew([0, 0, 0, 0, 0, np.pi])
    for i in range(startind, dofs):
        zed = poses[i]
        DrawAxes(zed, lens, ax)

        try:
            #Tp = fsr.tmInterpMidpoint(poses[i], poses[i+1])
            #T = fsr.adjustRotationToMidpoint(Tp ,poses[i], poses[i+1], mode = 1)
            #disp(T)

            #DrawRectangle(T, Dims[i+1, 0:3], ax, c = c)
            QuadPlot(poses[i], poses[i+1], Dims[i+1, 0:3], ax, c = c)
            if len(forces) != 1:
                label = '%.1fNm' % (forces[i])
                ax.text(poses[i][0], poses[i][1], poses[i][2], label)
            if (arm.joint_axes[0, i] == 1):
                if len(forces) != 1:
                    DrawTube(zed @ yrot, jrad, forces[i]/300, ax)
                else:
                    DrawTube(zed @ yrot, jrad, jdia, ax)
            elif (arm.joint_axes[1, i] == 1):
                if len(forces) != 1:
                    DrawTube(zed @ xrot, jrad, forces[i]/300, ax)
                else:
                    DrawTube(zed @ xrot, jrad, jdia, ax)
            else:
                if len(forces) != 1:
                    DrawTube(zed @ zrot, jrad, forces[i]/300, ax)
                else:
                    DrawTube(zed @ zrot, jrad, jdia, ax)
        except:
            pass
    zed = poses[0].gTAA()
    if startind ==0:
        DrawRectangle(arm.base_pos_global @
            fsr.TAAtoTM(np.array([0, 0, Dims[len(Dims)-1, 2]/2, 0, 0, 0])),
            Dims[len(Dims)-1, 0:3], ax, c = c)
    for i in range(len(arm.cameras)):
        DrawCamera(arm.cameras[i][0], 1, ax)

def DrawLine(tf1, tf2, ax, col = 'blue'):
    """Short summary.

    Args:
        tf1 (type): Description of parameter `tf1`.
        tf2 (type): Description of parameter `tf2`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.

    Returns:
        type: Description of returned object.

    """
    ax.plot3D([tf1[0], tf2[0]], [tf1[1], tf2[1]], [tf1[2], tf2[2]], col)

def DrawMobilePlatform(pl, ax, col = 'blue'):
    """Short summary.

    Args:
        pl (type): Description of parameter `pl`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.

    Returns:
        type: Description of returned object.

    """
    DrawTube(pl.loc @ pl.fl, pl.wrad, .3, ax)
    DrawTube(pl.loc @ pl.fr, pl.wrad, .3, ax)
    DrawTube(pl.loc @ pl.bl, pl.wrad, .3, ax)
    DrawTube(pl.loc @ pl.br, pl.wrad, .3, ax)
    DrawRectangle(pl.loc, pl.dims, ax, col)

def DrawSP(sp, ax, col = 'green', forces = 1):
    """Short summary.

    Args:
        sp (type): Description of parameter `sp`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.
        forces (type): Description of parameter `forces`.

    Returns:
        type: Description of returned object.

    """
    for i in range(6):

        ax.plot3D([sp.getBottomJoints()[0, i], sp.getBottomJoints()[0,(i+1)%6]],
            [sp.getBottomJoints()[1, i], sp.getBottomJoints()[1,(i+1)%6]],
            [sp.getBottomJoints()[2, i], sp.getBottomJoints()[2,(i+1)%6]], 'blue')
        ax.plot3D([sp.getTopJoints()[0, i], sp.getTopJoints()[0,(i+1)%6]],
            [sp.getTopJoints()[1, i], sp.getTopJoints()[1,(i+1)%6]],
            [sp.getTopJoints()[2, i], sp.getTopJoints()[2,(i+1)%6]], 'blue')
        if i == 0:
            ax.plot3D([sp.getBottomJoints()[0, i], sp.getTopJoints()[0, i]],
                [sp.getBottomJoints()[1, i], sp.getTopJoints()[1, i]],
                [sp.getBottomJoints()[2, i], sp.getTopJoints()[2, i]], 'darkred')
        elif i == 1:
            ax.plot3D([sp.getBottomJoints()[0, i], sp.getTopJoints()[0, i]],
                [sp.getBottomJoints()[1, i], sp.getTopJoints()[1, i]],
                [sp.getBottomJoints()[2, i], sp.getTopJoints()[2, i]], 'salmon')
        else:
            ax.plot3D([sp.getBottomJoints()[0, i], sp.getTopJoints()[0, i]],
                [sp.getBottomJoints()[1, i], sp.getTopJoints()[1, i]],
                [sp.getBottomJoints()[2, i], sp.getTopJoints()[2, i]], col)
        if(sp.bottom_plate_thickness != 0):
            aa = sp.nominal_plate_transform.spawnNew([
                sp.getBottomJoints()[0, i],
                sp.getBottomJoints()[1, i],
                sp.getBottomJoints()[2, i],
                sp.getBottomT()[3],
                sp.getBottomT()[4],
                sp.getBottomT()[5]]) @ (-1 * sp.nominal_plate_transform)
            ab = sp.nominal_plate_transform.spawnNew([
                sp.getBottomJoints()[0,(i+1)%6],
                sp.getBottomJoints()[1,(i+1)%6],
                sp.getBottomJoints()[2,(i+1)%6],
                sp.getBottomT()[3],
                sp.getBottomT()[4],
                sp.getBottomT()[5]]) @ (-1 * sp.nominal_plate_transform)
            ba = sp.nominal_plate_transform.spawnNew([
                sp.getTopJoints()[0, i],
                sp.getTopJoints()[1, i],
                sp.getTopJoints()[2, i],
                sp.getTopT()[3],
                sp.getTopT()[4],
                sp.getTopT()[5]]) @ (sp.nominal_plate_transform)
            bb = sp.nominal_plate_transform.spawnNew([
                sp.getTopJoints()[0,(i+1)%6],
                sp.getTopJoints()[1,(i+1)%6],
                sp.getTopJoints()[2,(i+1)%6],
                sp.getTopT()[3],
                sp.getTopT()[4],
                sp.getTopT()[5]]) @ (sp.nominal_plate_transform)
            ax.plot3D([aa[0], ab[0]],[aa[1], ab[1]],[aa[2], ab[2]], 'blue')
            ax.plot3D([ba[0], bb[0]],[ba[1], bb[1]],[ba[2], bb[2]], 'blue')
            ax.plot3D([sp.getBottomJoints()[0, i], aa[0]],
                [sp.getBottomJoints()[1, i], aa[1]],
                [sp.getBottomJoints()[2, i], aa[2]], 'blue')
            ax.plot3D([sp.getTopJoints()[0, i], ba[0]],
                [sp.getTopJoints()[1, i], ba[1]],
                [sp.getTopJoints()[2, i], ba[2]], 'blue')

    if forces == 1 and sp.getLegForces().size > 1:
        for i in range(6):
            label = '%.1fN' % (sp.getLegForces()[i])
            if i % 2 == 0:
                pos = sp.getActuatorLoc(i, 'b')
            else:
                pos = sp.getActuatorLoc(i, 't')
            ax.text(pos[0], pos[1], pos[2], label)

def DrawInterPlate(sp1, sp2, ax, col):
    """Short summary.

    Args:
        sp1 (type): Description of parameter `sp1`.
        sp2 (type): Description of parameter `sp2`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.

    Returns:
        type: Description of returned object.

    """

    for i in range(6):
        aa = sp1.nominal_plate_transform.spawnNew([
            sp1.getTopJoints()[0, i],
            sp1.getTopJoints()[1, i],
            sp1.getTopJoints()[2, i],
            sp1.getTopT()[3],
            sp1.getTopT()[4],
            sp1.getTopT()[5]]) @ (sp1.nominal_plate_transform)
        ab = sp1.nominal_plate_transform.spawnNew([
            sp1.getTopJoints()[0,(i+1)%6],
            sp1.getTopJoints()[1,(i+1)%6],
            sp1.getTopJoints()[2,(i+1)%6],
            sp1.getTopT()[3],
            sp1.getTopT()[4],
            sp1.getTopT()[5]]) @ (sp1.nominal_plate_transform)
        ba = sp2.nominal_plate_transform.spawnNew([
            sp2.getBottomJoints()[0, i],
            sp2.getBottomJoints()[1, i],
            sp2.getBottomJoints()[2, i],
            sp2.getBottomT()[3],
            sp2.getBottomT()[4],
            sp2.getBottomT()[5]]) @ (-1 * sp2.nominal_plate_transform)
        bb = sp2.nominal_plate_transform.spawnNew([
            sp2.getBottomJoints()[0,(i+1)%6],
            sp2.getBottomJoints()[1,(i+1)%6],
            sp2.getBottomJoints()[2,(i+1)%6],
            sp2.getBottomT()[3],
            sp2.getBottomT()[4],
            sp2.getBottomT()[5]]) @ (-1 * sp2.nominal_plate_transform)
        #ax.plot3D([aa[0], ab[0]],[aa[1], ab[1]],[aa[2], ab[2]], 'g')
        #ax.plot3D([ba[0], bb[0]],[ba[1], bb[1]],[ba[2], bb[2]], 'g')
        ax.plot3D(
            [sp2.getBottomJoints()[0, i], aa[0]],
            [sp2.getBottomJoints()[1, i], aa[1]],
            [sp2.getBottomJoints()[2, i], aa[2]], 'g')
        ax.plot3D(
            [sp1.getTopJoints()[0, i], ba[0]],
            [sp1.getTopJoints()[1, i], ba[1]],
            [sp1.getTopJoints()[2, i], ba[2]], 'g')

def DrawAssembler(spl, ax, col = 'green', forces = 1):
    """Short summary.

    Args:
        spl (type): Description of parameter `spl`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.
        forces (type): Description of parameter `forces`.

    Returns:
        type: Description of returned object.

    """
    for i in range(spl.numsp):
        DrawSP(spl.splist[i], ax , col, forces)
        if i + 1 < spl.numsp:
            DrawInterPlate(spl.splist[i], spl.splist[i+1], ax, col)

def DrawCamera(cam, size, ax):
    """Short summary.

    Args:
        cam (type): Description of parameter `cam`.
        size (type): Description of parameter `size`.
        ax (type): Description of parameter `ax`.

    Returns:
        type: Description of returned object.

    """
    DrawAxes(cam.CamT, size/2, ax)
    ScreenLoc = cam.CamT @ fsr.TAAtoTM(np.array([0, 0, size, 0, 0, 0]))
    imgT = cam.getFrameSize(size)
    print(imgT)
    Scr = np.zeros((4, 3))
    t = ScreenLoc @ fsr.TAAtoTM(np.array([-imgT[0], imgT[1], 0, 0, 0, 0]))
    Scr[0, 0:3] = t[0:3].flatten()
    t = ScreenLoc @ fsr.TAAtoTM(np.array([imgT[0], imgT[1], 0, 0, 0, 0]))
    Scr[1, 0:3] = t[0:3].flatten()
    t = ScreenLoc @ fsr.TAAtoTM(np.array([-imgT[0], -imgT[1], 0, 0, 0, 0]))
    Scr[3, 0:3] = t[0:3].flatten()
    t = ScreenLoc @ fsr.TAAtoTM(np.array([imgT[0], -imgT[1], 0, 0, 0, 0]))
    Scr[2, 0:3] = t[0:3].flatten()
    for i in range(4):
        ax.plot3D((cam.CamT[0],Scr[i, 0]),
            (cam.CamT[1],Scr[i, 1]),
            (cam.CamT[2],Scr[i, 2]), 'green')
    ax.plot3D(np.hstack((Scr[0:4, 0], Scr[0, 0])),
        np.hstack((Scr[0:4, 1], Scr[0, 1])),
        np.hstack((Scr[0:4, 2], Scr[0, 2])), 'red')

def DrawAxes(zed, lv, ax, makelegend = None, zdir = None):
    """Short summary.

    Args:
        zed (type): Description of parameter `zed`.
        lv (type): Description of parameter `lv`.
        ax (type): Description of parameter `ax`.
        makelegend (type): Description of parameter `makelegend`.
        zdir (type): Description of parameter `zdir`.

    Returns:
        type: Description of returned object.

    """
    zx, zy, zz = zed.tripleUnit(lv)
    poses = zed.gTAA().flatten()
    if makelegend is not None:
        if zdir is not None:
            zed = zed @ zdir
        ax.text(zed[0], zed[1], zed[2], makelegend)
    ax.plot3D([poses[0], zx[0]], [poses[1], zx[1]], [poses[2], zx[2]], 'red')
    ax.plot3D([poses[0], zy[0]], [poses[1], zy[1]], [poses[2], zy[2]], 'blue')
    ax.plot3D([poses[0], zz[0]], [poses[1], zz[1]], [poses[2], zz[2]], 'green')

def DrawTrussElement(T, L, R, ax, c='blue', c2 = 'blue', hf = False, delt = .5, RB = .1):
    """Short summary.

    Args:
        T (type): Description of parameter `T`.
        L (type): Description of parameter `L`.
        R (type): Description of parameter `R`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.
        c2 (type): Description of parameter `c2`.
        hf (type): Description of parameter `hf`.
        delt (type): Description of parameter `delt`.
        RB (type): Description of parameter `RB`.

    Returns:
        type: Description of returned object.

    """
    if hf == True:
        R1 = T @ T.spawnNew([R, 0, 0, 0, 0, 0])
        R2 = T @ T.spawnNew([0, 0, 0, 0, 0, 2*np.pi/3]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
        R3 = T @ T.spawnNew([0, 0, 0, 0, 0, 4*np.pi/3]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
        R1 = R1 @ T.spawnNew([0, 0, -L/2, 0, 0, 0])
        R2 = R2 @ T.spawnNew([0, 0, -L/2, 0, 0, 0])
        R3 = R3 @ T.spawnNew([0, 0, -L/2, 0, 0, 0])
        for i in range(int(L/delt)):
            R1A = R1 @ T.spawnNew([0, 0, delt, 0, 0, 0])
            R2A = R2 @ T.spawnNew([0, 0, delt, 0, 0, 0])
            R3A = R3 @ T.spawnNew([0, 0, delt, 0, 0, 0])
            if cycle ==1:
                DrawTube(fsr.adjustRotationToMidpoint(
                    fsr.tmInterpMidpoint(R1, R2A), R1, R2A, mode=1),
                    fsr.distance(R1, R2A)-RB, RB/3, ax, c2, res = 3)
                DrawTube(fsr.adjustRotationToMidpoint(
                    fsr.tmInterpMidpoint(R2, R3A), R2, R3A, mode=1),
                    fsr.distance(R2, R3A)-RB, RB/3, ax, c2, res = 3)
                DrawTube(fsr.adjustRotationToMidpoint(
                    fsr.tmInterpMidpoint(R3, R1A), R3, R1A, mode=1),
                    fsr.distance(R3, R1A)-RB, RB/3, ax, c2, res = 3)
                R1 = R1A
                R2 = R2A
                R3 = R3A
            else:
                DrawTube(fsr.adjustRotationToMidpoint(
                    fsr.tmInterpMidpoint(R1, R3A), R1, R3A, mode=1),
                    fsr.distance(R1, R3A)-RB, RB/3, ax, c2, res = 3)
                DrawTube(fsr.adjustRotationToMidpoint(
                    fsr.tmInterpMidpoint(R2, R1A), R2, R1A, mode=1),
                    fsr.distance(R2, R1A)-RB, RB/3, ax, c2, res = 3)
                DrawTube(fsr.adjustRotationToMidpoint(
                    fsr.tmInterpMidpoint(R3, R2A), R3, R2A, mode=1),
                    fsr.distance(R3, R2A)-RB, RB/3, ax, c2, res = 3)
                R1 = R1A
                R2 = R2A
                R3 = R3A
            cycle*=-1
        DrawTube(fsr.adjustRotationToMidpoint(fsr.tmInterpMidpoint(R1, R2), R1, R2, mode=1),
            fsr.distance(R1, R2)-RB, RB/3, ax, 'r', res = 3)
        DrawTube(fsr.adjustRotationToMidpoint(fsr.tmInterpMidpoint(R2, R3), R2, R3, mode=1),
            fsr.distance(R2, R3)-RB, RB/3, ax, 'r', res = 3)
        DrawTube(fsr.adjustRotationToMidpoint(fsr.tmInterpMidpoint(R3, R1), R3, R1, mode=1),
            fsr.distance(R3, R1)-RB, RB/3, ax, 'r', res = 3)
    R1 = T @ T.spawnNew([R, 0, 0, 0, 0, 0])
    R2 = T @ T.spawnNew([0, 0, 0, 0, 0, 2*np.pi/3]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
    R3 = T @ T.spawnNew([0, 0, 0, 0, 0, 4*np.pi/3]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
    DrawTube(R1, L, RB, ax, c, res = 3)
    DrawTube(R2, L, RB, ax, c, res = 3)
    DrawTube(R3, L, RB, ax, c, res = 3)

def DrawQTrussElement(T, L, R, ax, c='blue', c2 = 'blue', hf = False, delt = .5, RB = .1):
    """Short summary.

    Args:
        T (type): Description of parameter `T`.
        L (type): Description of parameter `L`.
        R (type): Description of parameter `R`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.
        c2 (type): Description of parameter `c2`.
        hf (type): Description of parameter `hf`.
        delt (type): Description of parameter `delt`.
        RB (type): Description of parameter `RB`.

    Returns:
        type: Description of returned object.

    """
    R1 = T @ T.spawnNew([0, 0, 0, 0, 0, np.pi/4]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
    R2 = T @ T.spawnNew([0, 0, 0, 0, 0, np.pi/2+np.pi/4]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
    R3 = T @ T.spawnNew([0, 0, 0, 0, 0, np.pi+np.pi/4]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
    R4 = T @ T.spawnNew([0, 0, 0, 0, 0, -np.pi/4]) @ T.spawnNew([R, 0, 0, 0, 0, 0])
    DrawTube(R1, L, RB, ax, c, res = 3)
    DrawTube(R2, L, RB, ax, c, res = 3)
    DrawTube(R3, L, RB, ax, c, res = 3)
    DrawTube(R4, L, RB, ax, c, res = 3)

def DrawRectangle(T, dims, ax, c='grey', a = 0.1):
    """Short summary.

    Args:
        T (type): Description of parameter `T`.
        dims (type): Description of parameter `dims`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.
        a (type): Description of parameter `a`.

    Returns:
        type: Description of returned object.

    """
    dx = dims[0]
    dy = dims[1]
    dz = dims[2]
    corners = .5 * np.array([
        [-dx, -dy, -dz], #BBL
        [dx, -dy, -dz], #BBR
        [-dx, dy, -dz], #BFL
        [dx, dy, -dz], #BFR
        [-dx, -dy, dz], #TBL
        [dx, -dy, dz], #TBR
        [-dx, dy, dz], #TFL
        [dx, dy, dz]]).T #TFR
    Tc = np.zeros((3, 8))
    for i in range(0, 8):
        h = T.gTM() @ np.array([corners[0, i], corners[1, i], corners[2, i], 1]).T
        Tc[0:3, i] = np.squeeze(h[0:3])
    #segs = np.array([[1, 2],[1, 3],[2, 4],[3, 4],[1, 5],[2, 6],[3, 7],[4, 8],[5, 6],[5, 7],[6, 8],[7, 8]])-1
    #disp(Tc[0,(0, 1, 4, 5)])
    #disp(Tc[0,(0, 1, 4, 5)])
    #disp(Tc[2,(0, 1, 4, 5)])
    #randomarr = np.random.rand(4)/100
    #print(randomarr)
    verts = [Tc[:,(0, 1, 3, 2)].T, Tc[:,(4, 5, 7, 6)].T, Tc[:,(0, 1, 5, 4)].T,
    Tc[:,(2, 3, 7, 6)].T, Tc[:,(0, 2, 6, 4)].T, Tc[:,(1, 3, 7, 5)].T]
    ax.add_collection3d(Poly3DCollection(verts, facecolors = c, edgecolors='b', alpha = a))
    #yy, zz = np.meshgrid(Tc[1,:], Tc[2,:])
    #ax.plot_surface(xx, yy, zz)
    #xs = np.linspace(0, 10, 100)
    #zs = np.linspace(0, 10, 100)

    #X, Z = np.meshgrid(Tc[0,:], Tc[2,:])
    #Y, Z = np.meshgrid(Tc[1,:], Tc[2,:])
    #Y = 5 - X
    #ax.plot_surface(X, Y, Z, alpha = .5, color = c)


def DrawRegPoly(T, n, r, h, ax, c='grey', rot = False):
    """Short summary.

    Args:
        T (type): Description of parameter `T`.
        n (type): Description of parameter `n`.
        r (type): Description of parameter `r`.
        h (type): Description of parameter `h`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.
        rot (type): Description of parameter `rot`.

    Returns:
        type: Description of returned object.

    """
    screw = T.gTAA().reshape((6))
    x = screw[0]
    y = screw[1]
    z = screw[2]
    xs = []
    ys = []
    zb = z-h/2
    zu = z+h/2
    xus=[]
    yus=[]
    zbs=[]
    zus=[]
    disp(r)
    disp(h)
    disp(T)
    for i in range(n):
        if rot:
            xs.append(r * np.cos(2 * (np.pi * i / n + np.pi * 1 / (2*n))))
            ys.append(r * np.sin(2 * (np.pi * i / n + np.pi * 1 / (2*n))))
        else:
            xs.append(r * np.cos(2 * np.pi * i / n))
            ys.append(r * np.sin(2 * np.pi * i / n))
    for i in range(n):
        hb = (T @ T.spawnNew(np.array([xs[i], ys[i], zb, 0, 0, 0]))).gTAA()
        ht = (T @ T.spawnNew(np.array([xs[i], ys[i], zu, 0, 0, 0]))).gTAA()
        xs[i] = hb[0].squeeze()
        ys[i] = hb[1].squeeze()
        xus.append(ht[0].squeeze())
        yus.append(ht[1].squeeze())
        zbs.append(hb[2].squeeze())
        zus.append(ht[2].squeeze())
    for i in range(n+1):
        if i == 0:
            continue
        if i == n:
            ax.plot3D((xs[i-1], xs[0]), (ys[i-1], ys[0]), (zbs[i-1], zbs[0]), c)
            ax.plot3D((xus[i-1], xus[0]), (yus[i-1], yus[0]), (zus[i-1], zus[0]), c)
        else:
            ax.plot3D((xs[i-1], xs[i]), (ys[i-1], ys[i]), (zbs[i-1], zbs[i]), c)
            ax.plot3D((xus[i-1], xus[i]), (yus[i-1], yus[i]), (zus[i-1], zus[i]), c)
        ax.plot3D((xs[i-1], xus[i-1]), (ys[i-1], yus[i-1]), (zbs[i-1], zus[i-1]), c)

def DrawCore(core, ax, c='grey', size = .5):
    """Short summary.

    Args:
        core (type): Description of parameter `core`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.
        size (type): Description of parameter `size`.

    Returns:
        type: Description of returned object.

    """
    DrawRegPoly(core.pos, 8, core.radius, core.height, ax, c, True)
    for i in range(6):
        DrawTube(core.attachmentTMs[i], .1, core.height/2, ax, c = 'purple')
        DrawAxes(core.attachmentTMs[i], .2, ax)
    for i in range(len(core.cameras)):
        DrawCamera(core.cameras[i], size, ax)
    for i in range(6):
        if core.attached[i] != 0:
            core.attached[i].Draw(ax)

def MakeVideo(dir = "VideoTemp"):
    """Short summary.

    Args:
        dir (type): Description of parameter `dir`.

    Returns:
        type: Description of returned object.

    """
    os.chdir(dir)
    os.system("ffmpeg -r 30 -i img%04d.png -vcodec mpeg4 -qscale 0 -y movie.mp4")
    #for file_name in glob.glob("*.png"):
    #    os.remove(file_name)

def Animate(obj, ax, plt, k, i, framelimiter, folder = 'VideoTemp', limits=[[-7, 7],[-7, 7],[0, 8]]):
    """Short summary.

    Args:
        obj (type): Description of parameter `obj`.
        ax (type): Description of parameter `ax`.
        plt (type): Description of parameter `plt`.
        k (type): Description of parameter `k`.
        i (type): Description of parameter `i`.
        framelimiter (type): Description of parameter `framelimiter`.
        folder (type): Description of parameter `folder`.
        limits (type): Description of parameter `limits`.

    Returns:
        type: Description of returned object.

    """
    if i % framelimiter == 0:
        ax = plt.axes(projection = '3d')
        ax.set_xlim3d(limits[0][0], limits[0][1])
        ax.set_ylim3d(limits[1][0], limits[2][1])
        ax.set_zlim3d(limits[2][0], limits[2][1])
        obj.Draw(ax)
        plt.show()
        plt.savefig(folder + '/file%05d' % k)
        ax.clear()
        k = k + 1
    return k, ax, plt

def AnimateCoreIndices(thetas, indices, core, ax, plt, k, framelimiter, title = "", folder = 'VideoTemp', limits=[[-7, 7],[-7, 7],[0, 8]]):
    """Short summary.

    Args:
        thetas (type): Description of parameter `thetas`.
        indices (type): Description of parameter `indices`.
        core (type): Description of parameter `core`.
        ax (type): Description of parameter `ax`.
        plt (type): Description of parameter `plt`.
        k (type): Description of parameter `k`.
        framelimiter (type): Description of parameter `framelimiter`.
        title (type): Description of parameter `title`.
        folder (type): Description of parameter `folder`.
        limits (type): Description of parameter `limits`.

    Returns:
        type: Description of returned object.

    """
    maxes = []
    for i in range(len(thetas)):
        maxes.append(len(thetas[i]))
    lim = max(maxes)
    for i in range(lim):
        progressBar(i, lim, title)
        c = 0
        for j in indices:
            try:
                core.attached[j].FK(thetas[c][i])
            except:
                print("fail")
            c = c + 1
            k, ax, plt = Animate(core, ax, plt, k, i, 12)

    return k, ax, plt

def DrawRRT(listPoints, ax):
    """Short summary.

    Args:
        listPoints (type): Description of parameter `listPoints`.
        ax (type): Description of parameter `ax`.

    Returns:
        type: Description of returned object.

    """
    for i in range(len(listPoints)):
        temp = listPoints[i].object.getPosition()
        ax.scatter3D(temp[0], temp[1], temp[2], s = 3)
    for i in range(len(listPoints)):
        temp1 = listPoints[i].object.getPosition()
        if listPoints[i].object.getParent() == None:
            DrawRectangle(temp1, [.05, .05, .05], ax, 'b')
            continue
        temp2 = listPoints[i].object.getParent().getPosition()
        c = 'green'
        if  listPoints[i].object.type == 1 and  listPoints[i].object.getParent().type == 1:
            c = 'm'
            continue
        elif listPoints[i].object.type == 0 and listPoints[i].object.getParent().type == 0:
            c = 'blue'
        ax.plot3D([temp1[0], temp2[0]], [temp1[1], temp2[1]], [temp1[2], temp2[2]], c)

def DrawRRTPath(listPoints, ax, col = 'red'):
    """Short summary.

    Args:
        listPoints (type): Description of parameter `listPoints`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.

    Returns:
        type: Description of returned object.

    """
    DrawRectangle(listPoints[0], [.1, .1, .1], ax, 'b')
    DrawRectangle(listPoints[len(listPoints)-1], [.1, .1, .1], ax, 'r', )
    for i in range(len(listPoints) - 1):
        temp1 = listPoints[i]
        temp2 = listPoints[i+1]
        ax.plot3D([temp1[0], temp2[0]], [temp1[1], temp2[1]], [temp1[2], temp2[2]], col)

def DrawObstructions(listObs, ax, col = 'red', a = .1):
    """Short summary.

    Args:
        listObs (type): Description of parameter `listObs`.
        ax (type): Description of parameter `ax`.
        col (type): Description of parameter `col`.
        a (type): Description of parameter `a`.

    Returns:
        type: Description of returned object.

    """
    for obs in listObs:
        center = obs[1].spawnNew([(obs[1][0] + obs[0][0])/2, (obs[1][1] + obs[0][1])/2, (obs[1][2] + obs[0][2])/2, 0, 0, 0])
        dims = [(obs[1][0] - obs[0][0]), (obs[1][1] - obs[0][1]), (obs[1][2] - obs[0][2])]
        DrawRectangle(center, dims, ax, col, a)

def drawMesh(mesh, ax):
    ax.plot_trisurf(mesh.vertices[:, 0], mesh.vertices[:, 1],
            triangles=mesh.faces, Z=mesh.vertices[:, 2])

def DrawWrench(tr, weight, dir, ax):
    """Short summary.

    Args:
        tr (type): Description of parameter `tr`.
        weight (type): Description of parameter `weight`.
        dir (type): Description of parameter `dir`.
        ax (type): Description of parameter `ax`.

    Returns:
        type: Description of returned object.

    """
    trb = tr.spawnNew([tr[0], tr[1], tr[2], 0, 0, 0])
    other = tr.spawnNew([dir[0], dir[1], dir[2], 0, 0, 0])
    np = trb @ (.4*other)
    a = trb @ (.3*other)
    DrawRectangle(trb, [.1, .1, .1], ax)
    a1 = a @ tr.spawnNew([.05, 0, 0, 0, 0, 0])
    a2 = a @ tr.spawnNew([-.05, 0, 0, 0, 0, 0])
    a3 = a @ tr.spawnNew([0, -.05,  0, 0, 0, 0])
    a4 = a @ tr.spawnNew([0, .05, 0, 0, 0, 0])
    label = '%.1fN' % weight
    ax.text(tr[0], tr[1], tr[2], label)

    ax.plot3D([tr[0], np[0]], [tr[1], np[1]], [tr[2], np[2]], 'b')
    ax.plot3D([np[0], a1[0]],[np[1], a1[1]],[np[2], a1[2]], 'r')
    ax.plot3D([np[0], a2[0]],[np[1], a2[1]],[np[2], a2[2]] ,'r')
    ax.plot3D([np[0], a3[0]],[np[1], a3[1]],[np[2], a3[2]] ,'r')
    ax.plot3D([np[0], a4[0]],[np[1], a4[1]],[np[2], a4[2]] ,'r')

def DrawTube(T, height, r, ax, c = 'blue', res = 12):
    """Short summary.

    Args:
        T (type): Description of parameter `T`.
        height (type): Description of parameter `height`.
        r (type): Description of parameter `r`.
        ax (type): Description of parameter `ax`.
        c (type): Description of parameter `c`.
        res (type): Description of parameter `res`.

    Returns:
        type: Description of returned object.

    """
    # find points along x and y axes
    points  = np.linspace(0, 2*np.pi, res+1)
    x = np.cos(points)*r
    y = np.sin(points)*r

    # find points along z axis
    rpoints = np.atleast_2d(np.linspace(0, 1, 1))
    z = np.ones((res+1))*height/2
    tres = np.zeros((6, len(x)))
    tres2 = np.zeros((6, len(x)))
    for i in range(len(x)):
        #disp(i)
        tres[0:6, i] = (T @ T.spawnNew([x[i], y[i], z[i], 0, 0, 0])).TAA.flatten()
        tres2[0:6, i] = (T @ T.spawnNew([x[i], y[i], -z[i], 0, 0, 0])).TAA.flatten()
        bx = np.array([tres[0, i], tres2[0, i]], dtype=object)
        by = np.array([tres[1, i], tres2[1, i]], dtype=object)
        bz = np.array([tres[2, i], tres2[2, i]], dtype=object)
        ax.plot3D(bx, by, bz, c)
    ax.plot3D(tres[0,:], tres[1,:], tres[2,:], c)
    ax.plot3D(tres2[0,:], tres2[1,:], tres2[2,:], c)
