text:
####################################################################################################
# 2020-2020, David Rowenhorst/The US Naval Research Laboratory, Washington DC
# Pursuant to title 17 section 105 of the United States Code, works of US Governement employees
# are not not subject to copyright protection.
#
# Copyright (c) 2013-2014, Marc De Graef/Carnegie Mellon University
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification, are
# permitted provided that the following conditions are met:
#
#     - Redistributions of source code must retain the above copyright notice, this list
#        of conditions and the following disclaimer.
#     - Redistributions in binary form must reproduce the above copyright notice, this
#        list of conditions and the following disclaimer in the documentation and/or
#        other materials provided with the distribution.
#     - Neither the names of Marc De Graef, Carnegie Mellon University nor the names
#        of its contributors may be used to endorse or promote products derived from
#        this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
# USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
####################################################################################################

#Author David Rowenhorst/The US Naval Research Laboratory, Washington DC
# A translation of the original f.90 code produced by Marc De Graef
#
# This implementation is dependant on the Numba Python package, which gives a significant increase in
# speed (other than the first time called due to compilation time). The numba implmentation is very non-Numpy-like in that
# Loops are not discouraged.
# No guarantee - but this library is typically numerically accurate to around 5e-6 degrees or better.
# The rotations.py is typically more accurate at the cost of being about 2-10x slower.

# All inputs can be numpy array-like, and either arrays of shape (n,m) OR (m)  where m is the length of a single
# representation (i.e. for Euler m = 3, for quaternions m = 4); in the case of orientation matrices (om)
# the inputs/outputs should be (n,3,3) OR (3,3).
#
# If the inputs are not in C-order or not aligned, a copy of the input will be made.
#
#

import numpy as np
import numba

P = 1
eps = 1.0e-12 # used for "closeto" approximations in Numba code
PI = np.pi
PI2 = 2.0 * np.pi
nbcache = False # switch to false for debugging.
nbParallel = True # decide if parallelism is desired.
# Once the JIT compiler runs over the code, changing this will not change the parallelism.
# The package will need to be reimported
nbFastmath = True

# this will check the type of the input and its size.
# it will also reshape the data in case it is a single dimension
# returns a 2D view of the data, the length of one entry, the number of entries, and datatype.
@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache)
def prepIn(a):
  shape = a.shape
  intype = a.dtype
  m = shape[-1]
  n = numba.int64(a.size / m)
  aout = a.reshape(n,m)
  return (aout,m,n,intype)

def eu2om(eu,p=P):
  '''Bunge Euler angles to orientation matrix'''
  om = eu2omL(np.require(eu,requirements=['C','A']),p=p)
  om = np.squeeze(om)
  return om

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def eu2omL(euIn,p=P):
  eu,m,n,intype = prepIn(euIn)
  om = np.zeros((n,3,3),dtype=intype)
  for i in numba.prange(n):
    c1 = np.cos(eu[i,0])
    c2 = np.cos(eu[i,1])
    c3 = np.cos(eu[i,2])
    s1 = np.sin(eu[i,0])
    s2 = np.sin(eu[i,1])
    s3 = np.sin(eu[i,2])
    om[i,0,0] = c1 * c3 - s1 * s3 * c2
    om[i,0,1] = s1 * c3 + c1 * s3 * c2
    om[i,0,2] = s3 * s2
    om[i,1,0] = -c1 * s3 - s1 * c3 * c2
    om[i,1,1] = -s1 * s3 + c1 * c3 * c2
    om[i,1,2] = c3 * s2
    om[i,2,0] = s1 * s2
    om[i,2,1] = -c1 * s2
    om[i,2,2] = c2
  return om

def eu2ax(eu,p=P):
  '''Bunge Euler angles to axis-angle (radians)  '''
  ax = eu2axL(np.require(eu,requirements=['C','A']),p=p)
  ax = np.squeeze(ax)
  return ax

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def eu2axL(euIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  eu,m,n,intype = prepIn(euIn)
  ax = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    t = np.tan(eu[i,1] * 0.5)
    sig = 0.50 * (eu[i,0] + eu[i,2])
    ssig = np.sin(sig)
    delt = 0.50 * (eu[i,0] - eu[i,2])
    tau = np.sqrt(t * t + ssig * ssig)

    if (np.abs(sig - PI2) < eps):
      alpha = PI
    else:
      alpha = 2.0 * np.arctan(tau / np.cos(sig))

    if np.abs(alpha) < eps:
      ax[i,0] = 0.0
      ax[i,1] = 0.0
      ax[i,2] = -1.0 * pf
      ax[i,3] = 0.0
    else:
      ax[i,0] = -1.0 * pf * t * np.cos(delt) / tau
      ax[i,1] = -1.0 * pf * t * np.sin(delt) / tau
      ax[i,2] = -1.0 * ssig / tau
      ax[i,3] = alpha

    if alpha < 0.0:
      for j in range(4):
        ax[i,j] *= -1.0
  return ax

def eu2qu(eu,p=P):
  '''Bunge Euler angles to quaternions (scalar, axis)  '''
  qu = eu2quL(np.require(eu,requirements=['C','A']),p=p)
  qu = np.squeeze(qu)
  return qu

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def eu2quL(euIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  eu,m,n,intype = prepIn(euIn)
  qu = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    phi1 = eu[i,0] * 0.5
    phi = eu[i,1] * 0.5
    phi2 = eu[i,2] * 0.5

    dphi1phi2 = phi1 - phi2
    sphi1phi2 = phi1 + phi2
    cphi = np.cos(phi)
    sphi = -1.0 * np.sin(phi)
    qu[i,0] = cphi * np.cos(sphi1phi2) + 0.0
    sign = 1.0 if (qu[i,0] >= 0.0) else -1.0
    qu[i,0] *= sign
    qu[i,1] = pf * sign * sphi * np.cos(dphi1phi2) + 0.0
    qu[i,2] = pf * sign * sphi * np.sin(dphi1phi2) + 0.0
    qu[i,3] = -1.0 * pf * sign * cphi * np.sin(sphi1phi2) + 0.0
  qu = quatnormL(qu)
  return qu

def om2eu(om,p=P):
  '''orientation matrix to Bunge Euler angles'''
  if om.ndim == 3:
    omIn = np.require(om,requirements=['C','A'])
  elif om.ndim == 2:
    omIn = np.require(om,requirements=['C','A']).reshape(1,3,3)
  else:
    return -1
  eu = om2euL(omIn,p=p)
  eu = np.squeeze(eu)
  return eu

@numba.jit(nopython=True,fastmath=nbFastmath, cache=nbcache, parallel=nbParallel)
def om2euL(om, p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  intype = om.dtype
  n = np.int64(om.size / 9)
  eu = np.zeros((n, 3), dtype=intype)
  for i in numba.prange(n):
    # [0,0], [0,1], [0, 2], [1,0], [1,1], [1,2], [2,0], [2,1], [2,2]
    #    0      1      2      3      4      5      6      7      8
    if (1.0 - np.abs(om[i, 2, 2])) > eps:
      eu[i,1] = np.arccos(om[i,2,2])
      zeta = 1.0 / (np.sqrt(1.0 - om[i,2,2] * om[i,2,2]))
      eu[i,0] = np.arctan2(om[i,2,0] * zeta,-1.0 * om[i,2,1] * zeta)
      eu[i,2] = np.arctan2(om[i,0,2] * zeta,om[i,1,2] * zeta)
    else:
      if ((np.abs(om[i,2,2] - 1.0)) < eps):
        eu[i,0] = np.arctan2(om[i,0,1],om[i,0,0])
        eu[i,1] = 0.0
        eu[i,2] = 0.0
      else:
        eu[i,0] = -np.arctan2(om[i,0,1],om[i,0,0])
        eu[i,1] = PI
        eu[i,2] = 0.0
    eu[i,0] += 100.0 * PI
    eu[i,0] = np.fmod(eu[i,0],PI2)
    eu[i,2] += 100.0 * PI
    eu[i,2] = np.fmod(eu[i,2],PI2)
  return eu

def ax2om(ax,p=P):
  '''axis-angle (radians) to orientation matrix  '''
  om = ax2omL(np.require(ax,requirements=['C','A']),p=p)
  om = np.squeeze(om)
  return om

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ax2omL(axIn,p=P):
  ax,m,n,intype = prepIn(axIn)
  om = np.zeros((n,3,3),dtype=intype)
  axn = axnormL(ax)
  # [0,0], [0,1], [0, 2], [1,0], [1,1], [1,2], [2,0], [2,1], [2,2]
  #    0      1      2      3      4      5      6      7      8
  for i in numba.prange(n):
    c = np.cos(axn[i,3])
    s = np.sin(axn[i,3])
    omc = 1.0 - c
    for j in range(3):
      om[i,j,j] = axn[i,j] * axn[i,j] * omc + c
    q = omc * axn[i,0] * axn[i,1]
    om[i,1,0] = q + s * axn[i,2]
    om[i,0,1] = q - s * axn[i,2]

    q = omc * axn[i,1] * axn[i,2]
    om[i,2,1] = q + s * axn[i,0]
    om[i,1,2] = q - s * axn[i,0]

    q = omc * axn[i,2] * axn[i,0]
    om[i,0,2] = q + s * axn[i,1]
    om[i,2,0] = q - s * axn[i,1]

    if (p < 0):
      om[i,:,:] = np.transpose(om[i,:,:])

  return om

def qu2eu(qu,p=P):
  '''quaternions (scalar, axis) to Bunge Euler angles  '''
  eu = qu2euL(np.require(qu,requirements=['C','A']),p=p)
  eu = np.squeeze(eu)
  return eu

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def qu2euL(quIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  qu,m,n,intype = prepIn(quIn)
  eu = np.zeros((n,3),dtype=intype)
  for i in numba.prange(n):
    q0 = pf * qu[i,0]
    q03 = q0 * q0 + qu[i,3] * qu[i,3]
    q12 = qu[i,1] * qu[i,1] + qu[i,2] * qu[i,2]
    chi = np.sqrt(q03 * q12)

    if (chi == 0.0):
      if (q12 == 0.0):
        eu[i,2] = 0.0
        eu[i,1] = 0.0
        eu[i,0] = np.arctan2(-2.0 * q0 * qu[i,3],q0 * q0 - qu[i,3] * qu[i,3])
      else:
        eu[i,2] = 0.0
        eu[i,1] = PI
        eu[i,0] = np.arctan2(2.0 * qu[i,1] * qu[i,2],qu[i,1] * qu[i,1] - qu[i,2] * qu[i,2])
    else:
      eu[i,1] = np.arctan2(2.0 * chi,q03 - q12)

      diff = np.arctan2(-qu[i,2],-qu[i,1])
      sum = np.arctan2(-qu[i,3],q0)
      eu[i,0] = diff + sum
      eu[i,2] = sum - diff

    eu[i,0] += PI2
    eu[i,0] = np.fmod(eu[i,0],PI2)
    eu[i,2] += PI2
    eu[i,2] = np.fmod(eu[i,2],PI2)

  return eu

def ax2ho(ax,p=P):
  '''axis-angle (radians) to homochoric vector  '''
  ho = ax2hoL(np.require(ax,requirements=['C','A']))
  ho = np.squeeze(ho)
  return ho

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ax2hoL(axIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  # intype = ax.dtype
  # n = np.int64(ax.size / 4)
  ax,m,n,intype = prepIn(axIn)
  ho = np.zeros((n,3),dtype=intype)
  axn = axnormL(ax)
  #axn = ax
  for i in numba.prange(n):
    f = 0.75 * (axn[i,3] - np.sin(axn[i,3]))
    f = f ** (1.0 / 3.0)
    for j in range(3):
      ho[i,j] = f * axn[i,j]
  return ho

def ho2ax(ho,p=P):
  '''homochoric vector to axis-angle (radians)   '''
  ax = ho2axL(np.require(ho,requirements=['C','A']))
  ax = np.squeeze(ax)
  return ax

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ho2axL(hoIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  ho,m,n,intype = prepIn(hoIn)
  ax = np.zeros((n,4),dtype=intype)

  tfit = np.array([1.0000000000018852,-0.5000000002194847,-0.024999992127593126,
                   -0.003928701544781374,-0.0008152701535450438,-0.0002009500426119712,
                   -0.00002397986776071756,-0.00008202868926605841,0.00012448715042090092,
                   -0.0001749114214822577,0.0001703481934140054,-0.00012062065004116828,
                   0.000059719705868660826,-0.00001980756723965647,0.000003953714684212874,
                   -0.00000036555001439719544],dtype=np.float64)

  for i in numba.prange(n):
    hmag = np.float64(0.0)
    for j in range(3):
      hmag += ho[i,j] * ho[i,j]
    if hmag < eps:
      ax[i,0] = 0.0
      ax[i,1] = 0.0
      ax[i,2] = -1.0 * pf
      ax[i,3] = 0.0
    else:
      hm = hmag
      sqrthm = np.sqrt(hm)
      hn = np.zeros(3,dtype=intype)
      for j in range(3):
        hn[j] = ho[i,j] / sqrthm
      # hn = ho[i,:]/sqrthm
      s = tfit[0] + tfit[1] * hmag
      for j in range(2,16):
        hm *= hmag
        s += tfit[j] * hm
      s = 1.0 if (s > 1.0) else s
      s = -1.0 if (s < -1.0) else s
      s = 2.0 * np.arccos(s)

      for j in range(3):
        ax[i,j] = hn[j]
      if np.abs(s - PI) < eps:
        ax[i,3] = PI
      else:
        ax[i,3] = s

  return ax

def ro2ax(ro,p=P):
  '''Rodrigues-Frank vector to axis-angle (radians)  '''
  ax = ro2axL(np.require(ro,requirements=['C','A']),p=p)
  ax = np.squeeze(ax)
  return ax

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ro2axL(roIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  ro,m,n,intype = prepIn(roIn)
  ax = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    ta = ro[i,3]
    if ta < eps:
      ax[i,0] = 0.0
      ax[i,1] = 0.0
      ax[i,2] = -1 * pf
      ax[i,3] = 0.0
    else:
      if ta == np.inf:
        ax[i,0] = ro[i,0]
        ax[i,1] = ro[i,1]
        ax[i,2] = ro[i,2]
        ax[i,3] = PI
      else:
        ax[i,3] = 2.0 * np.arctan(ta)
        ta = 0.0
        for j in range(3):
          ta += ro[i,j] * ro[i,j]
        ta = 1.0 / np.sqrt(ta)
        for j in range(3):
          ax[i,j] = ro[i,j] * ta
  return ax

def ax2ro(ax,p=P):
  '''axis-angle (radians) to Rodrigues-Frank vector  '''
  ro = ax2roL(np.require(ax,requirements=['C','A']),p=p)
  ro = np.squeeze(ro)
  return ro

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ax2roL(axIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  ax,m,n,intype = prepIn(axIn)
  ro = np.zeros((n,4),dtype=intype)
  axn = axnormL(ax)
  for i in numba.prange(n):
    if axn[i,3] == 0:
      ro[i,0] = 0.0
      ro[i,1] = 0.0
      ro[i,2] = -1.0 * pf
      ro[i,3] = 0.0
    else:
      for j in range(3):
        ro[i,j] = axn[i,j]
      if (PI - ax[i,3]) < eps:
        ro[i,3] = np.inf
      else:
        ro[i,3] = np.tan(axn[i,3] * 0.5)
  return ro

def ax2qu(ax,p=P):
  '''axis-angle (radians) to quaternion (scalar - vector)  '''
  qu = ax2quL(np.require(ax,requirements=['C','A']),p=p)
  qu = np.squeeze(qu)
  return qu

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ax2quL(axIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  ax,m,n,intype = prepIn(axIn)
  qu = np.zeros((n,4),dtype=intype)
  axn = axnormL(ax)
  #axn = ax
  for i in numba.prange(n):
    if axn[i,3] < eps:
      qu[i,0] = 1.0
      qu[i,1] = 0.0
      qu[i,2] = 0.0
      qu[i,3] = 0.0
    else:
      qu[i,0] = np.cos(ax[i,3] * 0.5)
      mag = 0.0
      for j in range(3):
        mag += ax[i,j] * ax[i,j]
      s = np.sin(ax[i,3] * 0.5) / mag
      for j in range(3):
        qu[i,j + 1] = ax[i,j] * s
  qu = quatnormL(qu)
  return qu

def ro2ho(ro,p=P):
  '''Rodrigues-Frank  to homochoric  '''
  ho = ro2hoL(np.require(ro,requirements=['C','A']),p=p)
  ho = np.squeeze(ho)
  return ho

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def ro2hoL(roIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  ro,m,n,intype = prepIn(roIn)
  ho = np.zeros((n,3),dtype=intype)
  for i in numba.prange(n):
    mag = ro[i,3]
    if mag < eps:
      ho[i,0] = 0.0
      ho[i,1] = 0.0
      ho[i,2] = 0.0
    else:
      if mag > 1e9:
        f = 0.75 * PI
      else:
        t = 2.0 * np.arctan(ro[i,3])
        f = 0.75 * (t - np.sin(t))
      f = f ** (1.0 / 3.0)
      for j in range(3):
        ho[i,j] = ro[i,j] * f

  return ho

def qu2om(qu,p=P):
  '''quaternion (scalar-vector) to orientation matrix  '''
  om = qu2omL(np.require(qu,requirements=['C','A']),p=p)
  om = np.squeeze(om)
  return om

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def qu2omL(quIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  qu,m,n,intype = prepIn(quIn)
  om = np.zeros((n,3,3),dtype=intype)
  # [0,0], [0,1], [0, 2], [1,0], [1,1], [1,2], [2,0], [2,1], [2,2]
  #    0      1      2      3      4      5      6      7      8
  qu = quatnormL(qu)

  for i in numba.prange(n):
    q0 = qu[i,0] * pf
    qq = q0 * q0 - (qu[i,1] ** 2 + qu[i,2] ** 2 + qu[i,3] ** 2)
    om[i,0,0] = qq + 2.0 * qu[i,1] * qu[i,1]
    om[i,0,1] = 2.0 * (qu[i,1] * qu[i,2] - qu[i,0] * qu[i,3])
    om[i,0,2] = 2.0 * (qu[i,1] * qu[i,3] + qu[i,0] * qu[i,2])
    om[i,1,0] = 2.0 * (qu[i,2] * qu[i,1] + qu[i,0] * qu[i,3])
    om[i,1,1] = qq + 2.0 * qu[i,2] * qu[i,2]
    om[i,1,2] = 2.0 * (qu[i,2] * qu[i,3] - qu[i,0] * qu[i,1])
    om[i,2,0] = 2.0 * (qu[i,3] * qu[i,1] - qu[i,0] * qu[i,2])
    om[i,2,1] = 2.0 * (qu[i,3] * qu[i,2] + qu[i,0] * qu[i,1])
    om[i,2,2] = qq + 2.0 * qu[i,3] * qu[i,3]

    tr = om[i,0,0] + om[i,1,1] + om[i,2,2]
    t = 0.50 * (tr - 1.0)
    if np.abs(t) > 1.0:  # there has been enough rounding to justify a re normalization of the matrix
      #using a Gram-Schmidt normalization
      u1 = om[i,0,:]
      u1 /= np.linalg.norm(u1)
      om[i,0,:] = u1
      u2 = om[i,1,:]
      u2 = u2-np.sum(u2*u1)*u1
      u2 /= np.linalg.norm(u2)
      om[i,1,:] = u2
      u3 = om[i,2,:]
      u3 - np.sum(u3*u1)*u1 - np.sum(u3*u2)*u2
      u3 /= np.linalg.norm(u3)
      om[i,2,:] = u3

  return om

def om2ax_old(om,p=P):
  '''orientation matrix to axis-angle (radians) -- depreciated now use qu2ax(om2qu()) '''
  if om.ndim == 3:
    omIn = np.require(om,requirements=['C','A'])
  elif om.ndim == 2:
    omIn = np.require(om,requirements=['C','A']).reshape(1,3,3)
  else:
    return -1
  ax = om2axL(omIn,p=p)
  ax = np.squeeze(ax)
  return ax

@numba.jit(nopython=True,fastmath=nbFastmath, cache=nbcache, parallel=nbParallel)
def om2axL(om, p=P):# depreciated now use qu2ax(om2qu()) -- kept for historical reasons.
  pf = numba.float32(p > 0) * 2.0 - 1.0
  intype = om.dtype
  n = np.int64(om.size / 9)
  ax = np.zeros((n, 4), dtype=intype)
  #  help for translating out of C version
  # [0,0], [0,1], [0, 2], [1,0], [1,1], [1,2], [2,0], [2,1], [2,2]
  #    0      1      2      3      4      5      6      7      8
  for i in numba.prange(n):

    tr = om[i,0,0] + om[i,1,1] + om[i,2,2]
    t = 0.50 * (tr - 1.0)
    t = 1.0 if (t > 1.0) else t
    t = -1.0 if (t < -1.0) else t
    ax[i,3] = np.arccos(t)
    #if ((1.0 - np.abs(t)) > eps):
    mag = numba.float64(0.0)
    ax[i,2] = pf * (om[i,1,0] - om[i,0,1])
    mag += ax[i,2]*ax[i,2]
    ax[i,1] = pf * (om[i,0,2] - om[i,2,0])
    mag += ax[i,1] * ax[i,1]
    ax[i,0] = pf * (om[i,2,1] - om[i,1,2])
    mag += ax[i,0] * ax[i,0]
    mag = np.sqrt(mag)
    if mag > eps:
      for j in range(3):
        ax[i,j] *= 1.0/mag
    else:
      if t > 0.0:
        ax[i, 0] = 0.0
        ax[i, 1] = 0.0
        ax[i, 2] = -1.0*pf
      else:
        d = np.zeros(3,dtype=intype)
        for j in range(3):
          d[j] = np.sqrt(0.5*(om[i,j,j]+1.0))
        dargsrt = np.argsort(d)
        d[dargsrt[1]] = (om[i, dargsrt[2], dargsrt[1]] + om[i, dargsrt[1], dargsrt[2] ]) / (4.0 * d[dargsrt[2]])
        d[dargsrt[0]] = (om[i, dargsrt[2], dargsrt[0]] + om[i, dargsrt[0], dargsrt[2]]) / (4.0 * d[dargsrt[2]])

        for j in range(3):
          ax[i,j] = pf*d[j]

  ax = axnormL(ax)
  return ax

@numba.jit(nopython=True,fastmath=False, cache=nbcache, parallel=nbParallel)
def om2axL_2(om, p=P): # kept for historical reasons
  # ** not sure it gets the axis correct when angle = pi **
  pf = numba.float32(p > 0) * 2.0 - 1.0
  intype = om.dtype
  n = np.int64(om.size / 9)
  ax = np.zeros((n, 4), dtype=intype)
  #  help for translating out of C version
  # [0,0], [0,1], [0, 2], [1,0], [1,1], [1,2], [2,0], [2,1], [2,2]
  #    0      1      2      3      4      5      6      7      8
  for i in numba.prange(n):

    tr = om[i,0,0] + om[i,1,1] + om[i,2,2]
    t = 0.50 * (tr - 1.0)
    t = 1.0 if (t > 1.0) else t
    t = -1.0 if (t < -1.0) else t
    ax[i,3] = np.arccos(t)
    if (ax[i,3] < 1.0e-7):
      ax[i,0] = 0.0
      ax[i,1] = 0.0
      ax[i,2] = -1.0 * pf
    else:
      omcomplex = np.zeros((3,3), dtype=numba.complex128)
      omcomplex[:,:] = om[i,:,:]
      eigval, eigvect = np.linalg.eig(omcomplex)
      whmin = np.argmin(np.absolute(eigval-1.0))
      vect = np.real(eigvect[:,whmin])
      d = (om[i,2,1] - om[i,1,2])
      if d != 0.0:
        vect[0] = np.abs(vect[0])*np.sign(d)

      d = (om[i,0,2] - om[i,2,0])
      if d != 0.0:
        vect[1] = np.abs(vect[1]) * np.sign(d)

      d = (om[i,1,0] - om[i,0,1])
      if d != 0.0:
        vect[2] = np.abs(vect[2]) * np.sign(d)

      ax[i,0] = pf*vect[0]
      ax[i,1] = pf*vect[1]
      ax[i,2] = pf*vect[2]
  ax = axnormL(ax)
  return ax

def om2qu(om,p=P):
  '''orientation matrix to quaternion (scalar-vector)  '''
  if om.ndim == 3:
    omIn = np.require(om,requirements=['C','A'])
  elif om.ndim == 2:
    omIn = np.require(om,requirements=['C','A']).reshape(1,3,3)
  else:
    return -1
  qu = om2quL(omIn,p=p)
  qu = np.squeeze(qu)
  return qu

@numba.jit(nopython=True,fastmath=nbFastmath, cache=nbcache, parallel=nbParallel)
def om2quL(om, p=P):
  """
      Thanks to Martin-Diehl using the same formulation as his rotations.py package.
      This fixes the ambiguity of the axis direction when the angle = pi.
      This also means that om2ax is now qu2ax(om2qu())
      This formulation is from  www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion.
      The original formulation had issues.
  """
  pf = numba.float32(p > 0) * 2.0 - 1.0
  intype = om.dtype
  n = np.int64(om.size / 9)
  qu = np.zeros((n, 4), dtype=intype)

  for i in numba.prange(n):
    s = 0.0
    tr = om[i,0,0] + om[i,1,1] + om[i,2,2]
    if tr > 0:
      s = 0.5 / np.sqrt(tr + 1.0)
      qu[i,0] = 0.25/s
      qu[i,1] = s * (om[i,2,1] - om[i,1,2])
      qu[i,2] = s * (om[i,0,2] - om[i,2,0])
      qu[i,3] = s * (om[i,1,0] - om[i,0,1])
    else:
      if (om[i,0,0] > om[i,1,1]) and (om[i,0,0] > om[i,2,2]):
        s = 2.0 * np.sqrt( 1.0 + om[i,0,0] - om[i,1,1] - om[i,2,2])
        qu[i,0] = (om[i,2,1] - om[i,1,2]) / s
        qu[i,1] = 0.25 * s
        qu[i,2] = (om[i,0,1] + om[i,1,0]) / s
        qu[i,3] = (om[i,0,2] + om[i,2,0]) / s
      elif om[i,1,1] > om[i,2,2]:
        s = 2.0 * np.sqrt(1.0 - om[i,0,0] + om[i,1,1] - om[i,2,2])
        qu[i,0] = (om[i,0,2] - om[i,2,0]) / s
        qu[i,1] = (om[i,0,1] + om[i,1,0]) / s
        qu[i,2] = 0.25 * s
        qu[i,3] = (om[i,1,2] + om[i,2,1]) / s
      else:
        s = 2.0 * np.sqrt(1.0 - om[i,0,0] - om[i,1,1] + om[i,2,2])
        qu[i,0] = (om[i,1,0] - om[i,0,1]) / s
        qu[i,1] = (om[i,0,2] + om[i,2,0]) / s
        qu[i,2] = (om[i,1,2] + om[i,2,1]) / s
        qu[i,3] = 0.25 * s
  qu = quatnormL(qu)
  return qu

def qu2ax(qu,p=P):
  '''quaternion (scalar-vector) to axis-angle (radians)  '''
  ax = qu2axL(np.require(qu,requirements=['C','A']),p=p)
  ax = np.squeeze(ax)
  return ax

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def qu2axL(quIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  qu,m,n,intype = prepIn(quIn)
  qu = quatnormL(qu)
  ax = np.zeros((n,4),dtype=intype)

  for i in numba.prange(n):

    if qu[i,0] > (1.0-eps):
      ax[i,0] = 0.0
      ax[i,1] = 0.0
      ax[i,2] = -pf
      ax[i,3] = 0.0
    else:
      omega = 2.0 * np.arccos(qu[i,0])
      mag = 0.0
      for j in range(3):
        mag += qu[i,j + 1] ** 2
      mag = 1.0 / (np.sqrt(mag)+1e-35)
      for j in range(3):
        ax[i,j] = qu[i,j + 1] * mag
      ax[i,3] = omega

  return ax

def qu2ro(qu,p=P):
  '''quaternion (scalar-vector) to Rodrigues-Frank vector  '''
  ro = qu2roL(np.require(qu,requirements=['C','A']),p=p)
  ro = np.squeeze(ro)
  return ro

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def qu2roL(quIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  qu,m,n,intype = prepIn(quIn)
  qu = quatnormL(qu)
  ro = np.zeros((n,4),dtype=intype)

  for i in numba.prange(n):

    if qu[i,0] < eps:
      ro[i,3] = np.inf
      for j in range(3):
        ro[i,j] = qu[i,j + 1]
    else:
      mag = 0.0
      for j in range(3):
        mag += qu[i,j + 1] ** 2
      if mag < eps:
        ro[0] = 0.0
        ro[1] = 0.0
        ro[2] = -pf
        ro[3] = 0.0
      else:
        mag = 1.0 / np.sqrt(mag)
        q0 = qu[i,0]
        q0 = 1.0-eps if q0 > 1.0 else q0
        q0 = -1.0+eps if q0 < -1.0 else q0
        ro[i,3] = np.tan(np.arccos(q0))
        for j in range(3):
          ro[i,j] = qu[i,j + 1] * mag
  return ro

def qu2ho(qu,p=P):
  '''quaternion (scalar-vector) to homochoric  '''
  ho = qu2hoL(np.require(qu,requirements=['C','A']),p=p)
  ho = np.squeeze(ho)
  return ho

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def qu2hoL(quIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  qu,m,n,intype = prepIn(quIn)
  qu = quatnormL(qu)
  ho = np.zeros((n,3),dtype=intype)

  for i in numba.prange(n):

    if qu[i,0] > (1.0-eps):
      ho[i,0] = 0.0
      ho[i,1] = 0.0
      ho[i,2] = 0.0
    else:
      omega = 2.0 * np.arccos(qu[i,0])
      mag = 0.0
      for j in range(3):
        mag += qu[i,j + 1] ** 2
      f = 0.75 * (omega - np.sin(omega))
      f = f ** (1.0 / 3.0)
      mag = f / np.sqrt(mag)
      for j in range(3):
        ho[i,j] = qu[i,j + 1] * mag
  return ho

def ho2cu(ho,p=P):
  ''' homochoric to cubochoric  '''
  cu = ho2cuL(np.require(ho,requirements=['C','A']),p=p)
  cu = np.squeeze(cu)
  return cu

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache, parallel=nbParallel)
def ho2cuL(hoIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  ho,m,n,intype = prepIn(hoIn)
  cu = np.zeros((n,3),dtype=intype)
  LPR1 = np.float(1.33067003949147)  # (3pi/4)**(1/3)
  LPpref = np.float(1.38197659788534)  # sqrt(6/pi)
  LPbeta = np.float(0.962874509979126)  # pi**(5/6)/6**(1/6)/2
  LPr2 = np.float(1.4142135623731)  # sqrt(2)
  LPpi12 = np.float(0.261799387799149)  # pi/12
  LPsc = np.float(0.897772786961286)  # a/ap == (pi**(5/6)/6**(1/6)) / pi**(2/3)
  eps_loc = 1e-7

  for i in numba.prange(n):
    #cu[i,:] = lambert3DBallToCube(ho[i,:])
    # I have inlined all the function of lambert3DCubeToBall
    # numba really dislikes nested function calls.  This was about a 10x speedup.
    xyz = np.zeros(3,dtype=np.float64)
    xyzcu = np.zeros(3,dtype=np.float64)
    xyz3 = np.zeros(3,dtype=np.float64)
    xyz2 = np.zeros(3,dtype=np.float64)
    xyz1 = np.zeros(3,dtype=np.float64)
    rs = 0.0
    for j in range(3):
      xyz[j] = ho[i,j]
      rs += xyz[j] * xyz[j]
    rs = np.sqrt(np.float64(max(rs,0.0)))

    # if rs > (LPR1 + eps_loc):
    #  return xyzcu-np.float64(5.0) # out of range
    py = 10
    if (np.abs(xyz[0]) <= xyz[2]) and (np.abs(xyz[1]) <= xyz[2]):
      py = 1
    if (np.abs(xyz[0]) <= -xyz[2]) and (np.abs(xyz[1]) <= -xyz[2]):
      py = 2
    if (np.abs(xyz[2]) <= xyz[0]) and (np.abs(xyz[1]) <= xyz[0]):
      py = 3
    if (np.abs(xyz[2]) <= -xyz[0]) and (np.abs(xyz[1]) <= -xyz[0]):
      py = 4
    if (np.abs(xyz[0]) <= xyz[1]) and (np.abs(xyz[2]) <= xyz[1]):
      py = 5
    if (np.abs(xyz[0]) <= -xyz[1]) and (np.abs(xyz[2]) <= -xyz[1]):
      py = 6

    if (py == 1) or (py == 2):
      xyz3[0] = xyz[0]
      xyz3[1] = xyz[1]
      xyz3[2] = xyz[2]
    if (py == 3) or (py == 4):
      xyz3[0] = xyz[1]
      xyz3[1] = xyz[2]
      xyz3[2] = xyz[0]
    if (py == 5) or (py == 6):
      xyz3[0] = xyz[2]
      xyz3[1] = xyz[0]
      xyz3[2] = xyz[1]

    q = np.sqrt(np.float64(max(2.0 * rs / (rs + np.abs(xyz3[2]) + eps),0.0)))
    xyz2[0] = xyz3[0] * q
    xyz2[1] = xyz3[1] * q
    sign = 1.0 if (xyz3[2] >= 0.0) else -1.0
    xyz2[2] = sign * rs / LPpref

    qxy = 0.0
    for j in range(2):
      qxy += xyz2[j] * xyz2[j]

    if qxy == 0.0:
      xyz1[0] = 0.0
      xyz1[1] = 0.0
    else:
      xyswitch = np.abs(xyz2[1]) <= np.abs(xyz2[0])
      if xyswitch:
        x = xyz2[0]
        y = xyz2[1]
      else:
        x = xyz2[1]
        y = xyz2[0]
      q2xy = qxy + x * x
      sq2xy = np.sqrt(q2xy)

      q = q2xy * qxy / (q2xy - np.abs(x) * sq2xy)
      q = np.sqrt(np.float64(max(q,0.0)))
      q *= LPbeta / LPr2 / LPR1

      tt = (y * y + np.abs(x) * sq2xy) / LPr2 / qxy
      if tt > 1.0:
        tt = 1.0
      if tt < -1.0:
        tt = -1.0

      ac = np.arccos(tt)
      sx = 1.0 if (x >= 0.0) else -1.0
      sy = 1.0 if (y >= 0.0) else -1.0

      T1inv = q * sx
      T2inv = q * sy * ac / LPpi12

      if xyswitch:
        xyz1[0] = T1inv
        xyz1[1] = T2inv
      else:
        xyz1[1] = T1inv
        xyz1[0] = T2inv

    xyz1[2] = xyz2[2]
    for j in range(3):
      xyz1[j] *= 1.0 / LPsc

    if (py == 1) or (py == 2):
      xyzcu[0] = xyz1[0]
      xyzcu[1] = xyz1[1]
      xyzcu[2] = xyz1[2]
    if (py == 3) or (py == 4):
      xyzcu[0] = xyz1[2]
      xyzcu[1] = xyz1[0]
      xyzcu[2] = xyz1[1]
    if (py == 5) or (py == 6):
      xyzcu[0] = xyz1[1]
      xyzcu[1] = xyz1[2]
      xyzcu[2] = xyz1[0]
    for j in range(3):
      cu[i, j] = xyzcu[j]
  return cu

def cu2ho(cu,p=P):
  ''' cubochoric to homochoric  '''
  ho = cu2hoL(np.require(cu,requirements=['C','A']),p=p)
  ho = np.squeeze(ho)
  return ho

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache, parallel=nbParallel)
def cu2hoL(cuIn,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  cu,m,n,intype = prepIn(cuIn)
  ho = np.zeros((n,3),dtype=intype)
  LPap = np.float64(2.14502939711103)  # pi**(2/3)
  LPa = np.float64(1.92574901995825)  # pi**(5/6)/6**(1/6)
  LPsc = np.float64(0.897772786961286)  # a/ap == (pi**(5/6)/6**(1/6)) / pi**(2/3)
  LPpref = np.float64(1.38197659788534)  # sqrt(6/pi)
  LPpi12 = np.float64(0.261799387799149)  # pi/12
  LPr2 = np.float64(1.4142135623731)  # sqrt(2)
  LPprek = np.float64(1.6434564029725)  # R1 2**(1/4)/beta == (3pi/4)^(1/3) * 2**(1/4) / (pi**(5/6)/6**(1/6)/2)
  LPsPi = np.float64(1.77245385090552)  # sqrt(pi)
  LPr24 = np.float64(4.89897948556636)  # sqrt(24)
  eps_loc = 2e-6

  # I have inlined all the function of lambert3DCubeToBall
  # numba really dislikes nested function calls.  This was about a 10x speedup.
  # for i in numba.prange(n):
  #   ho[i, :] = lambert3DCubeToBall(cu[i,:])

  for i in numba.prange(n):
    xyz = np.zeros(3,dtype=np.float64)
    xyzba = np.zeros(3,dtype=np.float64)
    sXYZ = np.zeros(3,dtype=np.float64)
    #xyz[:] = cu[i,:]
    xyz[0] = cu[i,0]
    xyz[1] = cu[i,1]
    xyz[2] = cu[i,2]
    mx = np.abs(xyz).max()

    # if mx > (LPap/2.0 + eps_loc):
    #  return np.float64([-5,-5, -5]) #xyzba out of range

    py = 10
    if (np.abs(xyz[0]) <= xyz[2]) and (np.abs(xyz[1]) <= xyz[2]):
      py = 1
    if (np.abs(xyz[0]) <= -xyz[2]) and (np.abs(xyz[1]) <= -xyz[2]):
      py = 2
    if (np.abs(xyz[2]) <= xyz[0]) and (np.abs(xyz[1]) <= xyz[0]):
      py = 3
    if (np.abs(xyz[2]) <= -xyz[0]) and (np.abs(xyz[1]) <= -xyz[0]):
      py = 4
    if (np.abs(xyz[0]) <= xyz[1]) and (np.abs(xyz[2]) <= xyz[1]):
      py = 5
    if (np.abs(xyz[0]) <= -xyz[1]) and (np.abs(xyz[2]) <= -xyz[1]):
      py = 6

    if (py == 1) or (py == 2):
      sXYZ[0] = xyz[0]
      sXYZ[1] = xyz[1]
      sXYZ[2] = xyz[2]
    if (py == 3) or (py == 4):
      sXYZ[0] = xyz[1]
      sXYZ[1] = xyz[2]
      sXYZ[2] = xyz[0]
    if (py == 5) or (py == 6):
      sXYZ[0] = xyz[2]
      sXYZ[1] = xyz[0]
      sXYZ[2] = xyz[1]

    for j in range(3):
      sXYZ[j] = sXYZ[j] * LPsc

    if mx < eps:
      sXYZ[:] = 0.0
    else:
      absX = np.abs(sXYZ[0])
      absY = np.abs(sXYZ[1])

      if (absX < eps) and (absY < eps):
        sXYZ[0] = 0.0
        sXYZ[1] = 0.0
        sXYZ[2] = LPpref * sXYZ[2]
      else:
        z = np.float64(sXYZ[2])
        xyswitch = np.abs(sXYZ[1]) <= np.abs(sXYZ[0])
        if xyswitch:
          x = np.float64(sXYZ[0])
          y = np.float64(sXYZ[1])
        else:
          x = np.float64(sXYZ[1])
          y = np.float64(sXYZ[0])

        q = LPpi12 * y / x
        c = np.float64(np.cos(q))
        s = np.float64(np.sin(q))

        q = LPprek * x / np.sqrt(np.float64(max(LPr2 - c,0.0)))
        T1p = (LPr2 * c - 1.0) * q
        T2p = LPr2 * s * q
        if xyswitch:
          T1 = np.float64(T1p)
          T2 = np.float64(T2p)
        else:
          T1 = np.float64(T2p)
          T2 = np.float64(T1p)

        c = T1 * T1 + T2 * T2
        s = np.pi * c / (24.0 * z * z)

        c = LPsPi * c / LPr24 / z
        q = np.sqrt(np.float64(max([1.0 - s,0.0])))

        sXYZ[0] = T1 * q
        sXYZ[1] = T2 * q
        sXYZ[2] = LPpref * z - c

    if (py == 1) or (py == 2):
      xyzba[0] = sXYZ[0]
      xyzba[1] = sXYZ[1]
      xyzba[2] = sXYZ[2]
    if (py == 3) or (py == 4):
      xyzba[0] = sXYZ[2]
      xyzba[1] = sXYZ[0]
      xyzba[2] = sXYZ[1]
    if (py == 5) or (py == 6):
      xyzba[0] = sXYZ[1]
      xyzba[1] = sXYZ[2]
      xyzba[2] = sXYZ[0]

    for j in range(3):
      ho[i,j] = xyzba[j]

  return ho

# cross dependant codes start here
def eu2ro(eu,p=P):
  '''Bunge Euler angles to Rodrigues-Frank vector  '''
  ro = ax2ro(eu2ax(eu, p=p), p=p)
  return ro

def ro2om(ro,p=P):
  '''Rodrigues-Frank vector to orientation matrix   '''
  om = ax2om(ro2ax(ro, p=p), p=p)
  return om

def ro2eu(ro,p=P):
  '''Rodrigues-Frank vector to Bunge Euler angles  '''
  eu = om2eu(ro2om(ro, p=p), p=p)
  return eu

def eu2ho(eu,p=P):
  '''Bunge Euler angles to homochoric  '''
  ho = ax2ho(eu2ax(eu, p=p), p=p)
  return ho

def om2qu_old(om,p=P):
  '''orientation matrix to quaternion (scalar-vector)  '''
  qu = ax2qu(om2ax(om, p=p), p=p)
  return qu

def om2ax(om,p=P):
  '''orientation matrix to quaternion (scalar-vector)  '''
  ax = qu2ax(om2qu(om, p=p), p=p)
  return ax

def om2ro(om,p=P):
  '''orientation matrix to Rodrigues-Frank vector  '''
  ro = eu2ro(om2eu(om, p=p), p=p)
  return ro

def om2ho(om,p=P):
  '''orientation matrix to homochoric  '''
  ho = ax2ho(om2ax(om, p=p), p=p)
  return ho

def ax2eu(ax,p=P):
  '''axis-angle to Bunge Euler angle  '''
  eu = om2eu(ax2om(ax, p=p), p=p)
  return eu

def ro2qu(ro,p=P):
  '''Rodrigues-Frank vector to quaternion (scalar-vector)  '''
  qu = ax2qu(ro2ax(ro, p=p), p=p)
  return qu

def ho2eu(ho,p=P):
  '''homochoric to Bunge Euler angles  '''
  eu = ax2eu(ho2ax(ho, p=p), p=p)
  return eu

def ho2om(ho,p=P):
  '''homochoric to orientation matrix  '''
  om = ax2om(ho2ax(ho, p=p), p=p)
  return om

def ho2ro(ho,p=P):
  '''homochoric to Rodrigues-Frank vector  '''
  ro = ax2ro(ho2ax(ho, p=p), p=p)
  return ro

def ho2qu(ho,p=P):
  '''homochoric to quaternion (scalar-vector)  '''
  qu = ax2qu(ho2ax(ho, p=p), p=p)
  return qu

def eu2cu(eu,p=P):
  '''Bunge Euler angles in cubochoric  '''
  cu = ho2cu(eu2ho(eu, p=p), p=p)
  return cu

def om2cu(om,p=P):
  '''orientation matrix to cubochoric  '''
  cu = ho2cu(om2ho(om, p=p), p=p)
  return cu

def ax2cu(ax,p=P):
  '''axis-angle to cubochoric  '''
  cu = ho2cu(ax2ho(ax, p=p), p=p)
  return cu

def ro2cu(ro,p=P):
  '''Rodrigues-Frank vector to cubochoric  '''
  cu = ho2cu(ro2ho(ro, p=p), p=p)
  return cu

def qu2cu(qu,p=P):
  '''quaternion (scalar-vector) to cubochoric  '''
  cu = ho2cu(qu2ho(qu, p=p), p=p)
  return cu

def cu2eu(cu,p=P):
  '''cubochoric to Bunge Euler angles  '''
  eu = ho2eu(cu2ho(cu, p=p), p=p)
  return eu

def cu2om(cu,p=P):
  '''cubochoric to orientation matrix  '''
  om = ho2om(cu2ho(cu, p=p), p=p)
  return om

def cu2ax(cu,p=P):
  '''cubochoric to axis-angle  '''
  ax = ho2ax(cu2ho(cu, p=p), p=p)
  return ax

def cu2ro(cu,p=P):
  '''cubochoric to Rodrigues-Frank vector  '''
  ro = ho2ro(cu2ho(cu, p=p), p=p)
  return ro

def cu2qu(cu,p=P):
  '''cubochoric to quaternion (scalar-vector)  '''
  qu = ho2qu(cu2ho(cu, p=p), p=p)
  return qu

# Lambert Ball <--> Cube codes

@numba.jit(['int32(f8[:])','int32(f4[:])'], nopython=True,fastmath=nbFastmath,cache=nbcache)
def lambertGetPyramid(xyz):
  out = numba.int32(10)
  if (np.abs(xyz[0]) <= xyz[2]) and (np.abs(xyz[1]) <= xyz[2]):
    out = 1
  if (np.abs(xyz[0]) <= -xyz[2] ) and (np.abs(xyz[1]) <= -xyz[2]):
    out = 2
  if (np.abs(xyz[2]) <= xyz[0]) and (np.abs(xyz[1]) <= xyz[0]):
    out = 3
  if (np.abs(xyz[2]) <= -xyz[0]) and (np.abs(xyz[1]) <= -xyz[0]):
    out = 4
  if (np.abs(xyz[0]) <= xyz[1]) and (np.abs(xyz[2]) <= xyz[1]):
    out = 5
  if (np.abs(xyz[0]) <= -xyz[1]) and (np.abs(xyz[2]) <= -xyz[1]):
    out = 6
  return out

@numba.jit(['f8[:](f8[:])','f8[:](f4[:])'], nopython=True,fastmath=nbFastmath,cache=nbcache)
def lambert3DCubeToBall(xyz):
  LPap = np.float64(2.14502939711103) # pi**(2/3)
  LPa =   np.float64(1.92574901995825) # pi**(5/6)/6**(1/6)
  LPsc =  np.float64(0.897772786961286) # a/ap == (pi**(5/6)/6**(1/6)) / pi**(2/3)
  LPpref =  np.float64(1.38197659788534) # sqrt(6/pi)
  LPpi12 =  np.float64(0.261799387799149) # pi/12
  LPr2 =  np.float64(1.4142135623731) # sqrt(2)
  LPprek =  np.float64(1.6434564029725) # R1 2**(1/4)/beta == (3pi/4)^(1/3) * 2**(1/4) / (pi**(5/6)/6**(1/6)/2)
  LPsPi =  np.float64(1.77245385090552) # sqrt(pi)
  LPr24 =  np.float64(4.89897948556636) # sqrt(24)
  eps_loc = 2e-6

  xyzba = np.zeros(3, dtype = np.float64)
  sXYZ = np.zeros(3, dtype = np.float64)
  mx = np.abs(xyz).max()

  #if mx > (LPap/2.0 + eps_loc):
  #  return np.float64([-5,-5, -5]) #xyzba out of range

  p = lambertGetPyramid(xyz)

  if (p == 1) or (p == 2):
    sXYZ[0] = xyz[0]
    sXYZ[1] = xyz[1]
    sXYZ[2] = xyz[2]
  if (p == 3) or (p == 4):
    sXYZ[0] = xyz[1]
    sXYZ[1] = xyz[2]
    sXYZ[2] = xyz[0]
  if (p == 5) or (p == 6):
    sXYZ[0] = xyz[2]
    sXYZ[1] = xyz[0]
    sXYZ[2] = xyz[1]

  for i in range(3):
    sXYZ[i] = sXYZ[i] * LPsc

  if mx < eps:
    sXYZ[:] = 0.0
  else:
    absX = np.abs(sXYZ[0])
    absY = np.abs(sXYZ[1])

    if (absX < eps) and (absY < eps):
      sXYZ[0] = 0.0
      sXYZ[1] = 0.0
      sXYZ[2] = LPpref *sXYZ[2]
    else:
      z = np.float64(sXYZ[2])
      xyswitch = np.abs(sXYZ[1]) <= np.abs(sXYZ[0])
      if xyswitch:
        x = np.float64(sXYZ[0])
        y = np.float64(sXYZ[1])
      else:
        x = np.float64(sXYZ[1])
        y = np.float64(sXYZ[0])

      q = LPpi12 * y / x
      c = np.float64(np.cos(q))
      s = np.float64(np.sin(q))

      q = LPprek * x/np.sqrt(np.float64(max(LPr2 - c, 0.0)))
      T1p = (LPr2 * c - 1.0) * q
      T2p = LPr2 *  s * q
      if xyswitch:
        T1 = np.float64(T1p)
        T2 = np.float64(T2p)
      else:
        T1 = np.float64(T2p)
        T2 = np.float64(T1p)

      c = T1*T1 + T2*T2
      s = np.pi * c / (24.0 * z *z)

      c = LPsPi * c / LPr24 / z
      q = np.sqrt(np.float64(max([1.0-s, 0.0])))

      sXYZ[0] = T1 * q
      sXYZ[1] = T2 * q
      sXYZ[2] = LPpref * z -c

  if (p == 1) or (p == 2):
    xyzba[0] = sXYZ[0]
    xyzba[1] = sXYZ[1]
    xyzba[2] = sXYZ[2]
  if (p == 3) or (p == 4):
    xyzba[0] = sXYZ[2]
    xyzba[1] = sXYZ[0]
    xyzba[2] = sXYZ[1]
  if (p == 5) or (p == 6):
    xyzba[0] = sXYZ[1]
    xyzba[1] = sXYZ[2]
    xyzba[2] = sXYZ[0]
  return xyzba

@numba.jit(['f8[:](f8[:])','f8[:](f4[:])'], nopython=True,fastmath=nbFastmath, cache=nbcache)
def lambert3DBallToCube(xyz):
  LPR1 = np.float(1.33067003949147) # (3pi/4)**(1/3)
  LPpref = np.float(1.38197659788534) # sqrt(6/pi)
  LPbeta = np.float(0.962874509979126) # pi**(5/6)/6**(1/6)/2
  LPr2 = np.float(1.4142135623731) # sqrt(2)
  LPpi12 = np.float(0.261799387799149) # pi/12
  LPsc = np.float(0.897772786961286) # a/ap == (pi**(5/6)/6**(1/6)) / pi**(2/3)
  eps_loc = 1e-7

  xyzcu = np.zeros(3, dtype = np.float64)
  xyz3 = np.zeros(3,dtype=np.float64)
  xyz2 = np.zeros(3,dtype=np.float64)
  xyz1 = np.zeros(3,dtype=np.float64)
  rs = 0.0
  for i in range(3):
    rs += xyz[i] * xyz[i]
  rs = np.sqrt(np.float64(max(rs, 0.0)))

  #if rs > (LPR1 + eps_loc):
  #  return xyzcu-np.float64(5.0) # out of range

  p = lambertGetPyramid(xyz)
  if (p == 1) or (p == 2):
    xyz3[0] = xyz[0]
    xyz3[1] = xyz[1]
    xyz3[2] = xyz[2]
  if (p == 3) or (p == 4):
    xyz3[0] = xyz[1]
    xyz3[1] = xyz[2]
    xyz3[2] = xyz[0]
  if (p == 5) or (p == 6):
    xyz3[0] = xyz[2]
    xyz3[1] = xyz[0]
    xyz3[2] = xyz[1]

  q = np.sqrt(np.float64(max(2.0 * rs/(rs+np.abs(xyz3[2])+eps), 0.0)))
  xyz2[0] = xyz3[0] * q
  xyz2[1] = xyz3[1] * q
  sign = 1.0 if (xyz3[2] >= 0.0) else -1.0
  xyz2[2] = sign * rs/LPpref

  qxy = 0.0
  for i in range(2):
    qxy += xyz2[i]*xyz2[i]

  if qxy == 0.0:
    xyz1[0] = 0.0
    xyz1[1] = 0.0
  else:
    xyswitch = np.abs(xyz2[1]) <= np.abs(xyz2[0])
    if xyswitch:
      x = xyz2[0]
      y = xyz2[1]
    else:
      x = xyz2[1]
      y = xyz2[0]
    q2xy = qxy + x * x
    sq2xy = np.sqrt(q2xy)

    q = q2xy * qxy/(q2xy - np.abs(x)*sq2xy)
    q = np.sqrt(np.float64(max(q,0.0)))
    q *= LPbeta / LPr2 / LPR1

    tt = (y * y + np.abs(x) * sq2xy) / LPr2 / qxy
    if tt > 1.0:
      tt = 1.0
    if tt < -1.0:
      tt = -1.0

    ac = np.arccos(tt)
    sx = 1.0 if (x >= 0.0) else -1.0
    sy = 1.0 if (y >= 0.0) else -1.0

    T1inv = q * sx
    T2inv = q * sy * ac/LPpi12

    if xyswitch:
      xyz1[0] = T1inv
      xyz1[1] = T2inv
    else:
      xyz1[1] = T1inv
      xyz1[0] = T2inv

  xyz1[2] = xyz2[2]
  for i in range(3):
    xyz1[i] *= 1.0/LPsc

  if (p == 1) or (p == 2):
    xyzcu[0] = xyz1[0]
    xyzcu[1] = xyz1[1]
    xyzcu[2] = xyz1[2]
  if (p == 3) or (p == 4):
    xyzcu[0] = xyz1[2]
    xyzcu[1] = xyz1[0]
    xyzcu[2] = xyz1[1]
  if (p == 5) or (p == 6):
    xyzcu[0] = xyz1[1]
    xyzcu[1] = xyz1[2]
    xyzcu[2] = xyz1[0]
  return xyzcu

# Basic quaternion functions

def quatnorm(qu):
  '''quaternion normalization -- first term >=0 , unit quaternion length'''
  qout = quatnormL(np.require(qu,requirements=['C','A']))
  qout = np.squeeze(qout)
  return qout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def quatnormL(qu,unitNorm=True):
  qin,m,n,intype = prepIn(qu)
  qout = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    sign = 1.0 if (qin[i,0] >= 0.0) else -1.0
    sum = 0.0
    for j in range(4):
      qout[i,j] = qin[i,j] * sign
      sum += qout[i,j] * qout[i,j]
    sum = 1.0/(np.sqrt(sum)+1.0e-35)
    if unitNorm == True:
      for j in range(4):
        qout[i,j] *= sum
  return qout

def quatconj(qu):
  '''Provides a quaternion conjugate. For rotations quaternions, this is the inverse quaternion'''
  qout = quatconjL(np.require(qu,requirements=['C','A']))
  qout = np.squeeze(qout)
  return qout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def quatconjL(quin,unitNorm=True):
  qu,m,n,intype = prepIn(quin)
  qout = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    qout[i,0] = qu[i,0]
    for j in range(3):
      qout[i,j+1] = -1.0*qu[i,j+1]
  return qout

def quat_multiply(q1,q2,p=P):
  '''quaternion multiplication. Can be q1(Nx4) x q2(N x4) or q1(4) x q2(Nx4)'''
  q1q2 = quat_multiplyL(np.require(q1,requirements=['C','A']),np.require(q2,requirements=['C','A']),p=p)
  q1q2 = np.squeeze(q1q2)
  return q1q2

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache)
def quat_multiplyL(q1In,q2In,p=P):
  q1,m,n1,intype = prepIn(q1In)
  q2,m,n2,intype = prepIn(q2In)
  n = max(n1,n2)
  if n1 == n2:
    q1q2 = quat_multiplyLNN(q1,q2,n,intype,p=p)
  elif n1 < n2:
    q0 = q1[0,:]
    q1q2 = quat_multiplyL1N(q0,q2,n,intype,p=p)
  elif n2 < n1:
    q0 = q2[0,:]
    q1q2 = quat_multiplyL1N(q0,q1,n,intype,p=-p)
  #q1q2 = quatnormL(q1q2,unitNorm=False)
  return q1q2

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def quat_multiplyLNN(q1,q2,n,intype,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  q1q2 = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    q1q2[i,0] = q1[i,0] * q2[i,0] - (q1[i,1] * q2[i,1] + q1[i,2] * q2[i,2] + q1[i,3] * q2[i,3])
    q1q2[i,1] = q1[i,0] * q2[i,1] + q2[i,0] * q1[i,1] + pf * (q1[i,2] * q2[i,3] - q1[i,3] * q2[i,2])
    q1q2[i,2] = q1[i,0] * q2[i,2] + q2[i,0] * q1[i,2] + pf * (q1[i,3] * q2[i,1] - q1[i,1] * q2[i,3])
    q1q2[i,3] = q1[i,0] * q2[i,3] + q2[i,0] * q1[i,3] + pf * (q1[i,1] * q2[i,2] - q1[i,2] * q2[i,1])
  return q1q2

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def quat_multiplyL1N(q1,q2,n,intype,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  q1q2 = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    q1q2[i,0] = q1[0] * q2[i,0] - (q1[1] * q2[i,1] + q1[2] * q2[i,2] + q1[3] * q2[i,3])
    q1q2[i,1] = q1[0] * q2[i,1] + q2[i,0] * q1[1] + pf * (q1[2] * q2[i,3] - q1[3] * q2[i,2])
    q1q2[i,2] = q1[0] * q2[i,2] + q2[i,0] * q1[2] + pf * (q1[3] * q2[i,1] - q1[1] * q2[i,3])
    q1q2[i,3] = q1[0] * q2[i,3] + q2[i,0] * q1[3] + pf * (q1[1] * q2[i,2] - q1[2] * q2[i,1])
  return q1q2

# @numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=True)
# def quatMultiplyLN1(q1,q2,n,intype, p=P):
#   q1q2 = np.zeros((n,4),dtype=intype)
#   for i in numba.prange(n):
#     q1q2[i,:] = quatMultiply1(q1[i,:],q2,p=p)
#   return q1q2

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache)
def quat_multiply1(q1,q2,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  q1q2 = np.zeros_like(q1)
  q1q2[0] = q1[0] * q2[0] - (q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3])
  q1q2[1] = q1[0] * q2[1] + q2[0] * q1[1] + pf * (q1[2] * q2[3] - q1[3] * q2[2])
  q1q2[2] = q1[0] * q2[2] + q2[0] * q1[2] + pf * (q1[3] * q2[1] - q1[1] * q2[3])
  q1q2[3] = q1[0] * q2[3] + q2[0] * q1[3] + pf * (q1[1] * q2[2] - q1[2] * q2[1])
  return q1q2

def quat_vector(q,v,p=P):
  '''quaternion vector rotation. Can be q1(Nx4) x v(Nx3) or q1(4) x v(Nx3) or q1(Nx4) x v(3)'''
  vout = quat_vectorL(np.require(q,requirements=['C','A']),np.require(v,requirements=['C','A']),p=p)
  vout = np.squeeze(vout)
  return vout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache)
def quat_vectorL(qIn,vIn,p=P):
  q,mq,n1,intype = prepIn(qIn)
  v,mv,n2,intype = prepIn(vIn)
  n = max(n1,n2)
  #vout = np.zeros((n,3),dtype=intype)
  if n1 == n2:
    vout = quat_vectorLNN(q,v,n,intype,p=p)
  if n1 < n2:
    q0 = q[0,:]
    vout = quat_vectorL1N(q0,v,n,intype,p=p)
  if n2 < n1:
    v0 = v[0,:]
    vout = quat_vectorLN1(q,v0,n,intype,p=p)
  return vout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=True)
def quat_vectorLNN(q,v,n,intype,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  vout = np.zeros((n,3),dtype=intype)
  vtemp = np.zeros((3),dtype=intype)
  vtemp2 = np.zeros((3),dtype=intype)
  r = np.zeros((3),dtype=intype)
  w = numba.float64(0.0)
  for i in numba.prange(n):
    w = q[i,0]
    r[0] = pf * q[i,1]
    r[1] = pf * q[i,2]
    r[2] = pf * q[i,3]

    # Wow oh wow, numba hates function calls in a loop! 100x speedup
    #vtemp = np.cross(r,v[i,:])
    #vtemp = qv_crossp(r,v[i,:])
    vtemp[0] = r[1] * v[i,2] - r[2] * v[i,1]
    vtemp[1] = r[2] * v[i,0] - r[0] * v[i,2]
    vtemp[2] = r[0] * v[i,1] - r[1] * v[i,0]

    for j in range(3):
      vtemp[j] += w * v[i,j]

    #vtemp2 = np.cross(r,vtemp)
    #vtemp2 = qv_crossp(r,vtemp)
    vtemp2[0] = r[1] * vtemp[2] - r[2] * vtemp[1]
    vtemp2[1] = r[2] * vtemp[0] - r[0] * vtemp[2]
    vtemp2[2] = r[0] * vtemp[1] - r[1] * vtemp[0]

    for j in range(3):
      vout[i,j] = 2.0 * vtemp2[j] + v[i,j]

  return vout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=True)
def quat_vectorL1N(q,v,n,intype,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  vout = np.zeros((n,3),dtype=intype)
  vtemp = np.zeros((3),dtype=intype)
  vtemp2 = np.zeros((3),dtype=intype)
  r = np.zeros((3),dtype=intype)
  w = numba.float64(0.0)
  for i in numba.prange(n):
    w = q[0]
    r[0] = pf * q[1]
    r[1] = pf * q[2]
    r[2] = pf * q[3]

    # Wow oh wow, numba hates function calls in a loop! 100x speedup
    #vtemp = np.cross(r,v[i,:])
    #vtemp = qv_crossp(r,v[i,:])
    vtemp[0] = r[1] * v[i,2] - r[2] * v[i,1]
    vtemp[1] = r[2] * v[i,0] - r[0] * v[i,2]
    vtemp[2] = r[0] * v[i,1] - r[1] * v[i,0]

    for j in range(3):
      vtemp[j] += w * v[i,j]

    #vtemp2 = np.cross(r,vtemp)
    #vtemp2 = qv_crossp(r,vtemp)
    vtemp2[0] = r[1] * vtemp[2] - r[2] * vtemp[1]
    vtemp2[1] = r[2] * vtemp[0] - r[0] * vtemp[2]
    vtemp2[2] = r[0] * vtemp[1] - r[1] * vtemp[0]

    for j in range(3):
      vout[i,j] = 2.0 * vtemp2[j] + v[i,j]

  return vout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=True)
def quat_vectorLN1(q,v,n,intype,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  vout = np.zeros((n,3),dtype=intype)
  vtemp = np.zeros((3),dtype=intype)
  vtemp2 = np.zeros((3),dtype=intype)
  r = np.zeros((3),dtype=intype)
  w = numba.float64(0.0)
  for i in numba.prange(n):
    w = q[i,0]
    r[0] = pf * q[i,1]
    r[1] = pf * q[i,2]
    r[2] = pf * q[i,3]

    # Wow oh wow, numba hates function calls in a loop! 100x speedup
    # vtemp = np.cross(r,v)
    # vtemp = qv_crossp(r,v)
    vtemp[0] = r[1] * v[2] - r[2] * v[1]
    vtemp[1] = r[2] * v[0] - r[0] * v[2]
    vtemp[2] = r[0] * v[1] - r[1] * v[0]

    for j in range(3):
      vtemp[j] += w * v[j]

    # vtemp2 = np.cross(r,vtemp)
    # vtemp2 = qv_crossp(r,vtemp)
    vtemp2[0] = r[1] * vtemp[2] - r[2] * vtemp[1]
    vtemp2[1] = r[2] * vtemp[0] - r[0] * vtemp[2]
    vtemp2[2] = r[0] * vtemp[1] - r[1] * vtemp[0]

    for j in range(3):
      vout[i,j] = 2.0 * vtemp2[j] + v[j]

  return vout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache)
def quat_vector1(q,v,p=P):
  pf = numba.float32(p > 0) * 2.0 - 1.0
  vout = np.zeros_like(v)
  vtemp = np.zeros_like(v)
  vtemp2 = np.zeros_like(v)
  r = np.zeros_like(v)
  w = q[0]
  r[0] = pf * q[1]
  r[1] = pf * q[2]
  r[2] = pf * q[3]
  #cross product r,v
  vtemp[0] = r[1] * v[2] - r[2] * v[1]
  vtemp[1] = r[2] * v[0] - r[0] * v[2]
  vtemp[2] = r[0] * v[1] - r[1] * v[0]
  for j in range(3):
    vtemp[j] += w * v[j]

  # cross product r, vtemp
  vtemp2[0] = r[1] * vtemp[2] - r[2] * vtemp[1]
  vtemp2[1] = r[2] * vtemp[0] - r[0] * vtemp[2]
  vtemp2[2] = r[0] * vtemp[1] - r[1] * vtemp[0]

  for j in range(3):
    vout[j] = 2.0 * vtemp2[j] + v[j]

  return vout

#
def axnorm(ax):
  '''axis-angle normalization -- unit vector length, corrects negative angles and angles over 2Pi'''
  axout = axnormL(np.require(ax,requirements=['C','A']))
  axout = np.squeeze(axout)
  return axout

@numba.jit(nopython=True,fastmath=nbFastmath,cache=nbcache,parallel=nbParallel)
def axnormL(ax):
  axin,m,n,intype = prepIn(ax)
  axout = np.zeros((n,4),dtype=intype)
  for i in numba.prange(n):
    sign = 1.0 if (axin[i,3] >= eps) else -1.0
    axout[i,3] = sign*axin[i,3]
    sum = 0.0
    for j in range(3):
      axout[i,j] = sign * axin[i,j]
      sum += axout[i,j]*axout[i,j]
    axout[i,3] = np.fmod(axout[i,3],PI2)
    sum = 1.0/(np.sqrt(sum)+1e-35)
    for j in range(3):
      axout[i,j] *= sum
    if axout[i,3] > (PI+1e-6):
      for j in range(3):
        axout[i,j] *= -1.0
      axout[i,3] = PI2 - axout[i,3]
  return axout

reason: import numpy,import numba


text:
from dronekit import connect, VehicleMode, Vehicle
import time
import logging
from rigidBodyState import *
import os
import Queue
import threading
import math as m
import numpy as np
from datetime import datetime, timedelta
from pymavlink import mavutil

logging.basicConfig(level=logging.WARNING)

class Control(threading.Thread):
    def __init__(self,logQueue,receiveQueue,startTime,localIP,defaultParams,vehicle):
        threading.Thread.__init__(self)
        self.isRunning=True
        self.logQueue = logQueue
        self.receiveQueue = receiveQueue
        self.RigidBodies = {}
        self.rigidBodyState = RigidBodyState()
        self.rigidBodyState.startTime = datetime.now()
        self.counter = 0
        self.stoprequest = threading.Event()
        self.startTime = startTime
        self.rigidBodyState.ID = 1 # BE SURE TO UPDATE THIS WHEN IT COMES TIME FOR MULTIAGENT TESTING!!!!!
        self.rigidBodyState.parameters = defaultParams
        self.vehicle = vehicle
        self.lastGPSContact = -1
        self.prepTakeoff()
        self.scoobyDoo = MessageState()

    def stop(self):
        self.stoprequest.set()
        print "Stop Flag Set - Control"

    def run(self):
        while(not self.stoprequest.is_set()):
        	while(not self.receiveQueue.empty()):
                	try:
                            msg = self.receiveQueue.get(False)
                            self.decodeData(msg)
                            self.receiveQueue.task_done()
                	except Queue.Empty:
                    		break #no more messages
            	# Implement a failsafe to ensure the UP is recceving data from the computer before it computes the control
            	if(not self.rigidBodyState.isGPS):
                	self.checkGPS()
            	if(not self.rigidBodyState.parameters.InitPos):
                	self.rigidBodyState.parameters.InitPos = self.setInitialPos()
                	self.getLeaderData()
#			self.setStaticFlockingLeader()
			self.rigidBodyState.leader.flocking.qgz = -self.rigidBodyState.parameters.targetAltitude
                	self.rigidBodyState.leader.qgz = -self.rigidBodyState.parameters.targetAltitude
                	print 'Setting Initial Position'
            	if(self.rigidBodyState.isGPS and True):
                	self.switch_Flight_Sequence()
            	#self.pushStatetoTxQueue()
            	self.pushStatetoLoggingQueue()
            	self.counter = self.counter + 1
            	time.sleep(self.rigidBodyState.parameters.Ts)
        self.releaseControl()
        self.stop()
        print "Control Stopped"

    def switch_Flight_Sequence(self):
	#print self.rigidBodyState.batt
        arg = self.rigidBodyState.flightSeq
        flightSequence = {0: self.idleFunction, 1: self.takeoff, 2: self.hover, 3: self.flocking, 4: self.landing}#, 5: self.prepTakeoff}
        Keyboard_Command_Handler = flightSequence.get(arg, lambda: 'Invalid Command')
        Keyboard_Command_Handler()

    def idleFunction(self):
        print "Waiting for Command"

    def landing(self):
        if( not self.checkAbort()):
            desDest = self.rigidBodyState.position.z - self.rigidBodyState.initPos.zo
            self.rigidBodyState.leader.qgz = self.rigidBodyState.initPos.zo
            self.computeLandingVelocity(desDest)

    def flocking(self):
        print "flocking"
        if(not self.checkAbort()):
	    #self.hover()
            self.computeFlockingControl()

    def takeoff(self):
        if(not self.rigidBodyState.parameters.isTakeoff):
            if(not self.rigidBodyState.position.z <= -self.rigidBodyState.parameters.targetAltitude*0.95):
                if(not self.checkAbort()):
                    desDest =  self.rigidBodyState.position.z - self.rigidBodyState.leader.qgz # Fix this to match targetAltitude!!!
                    self.computeTakeoffVelocity(desDest)
            else:
                print "Reached Target Altitude"
                self.rigidBodyState.parameters.isTakeoff = True
                self.rigidBodyState.parameters.isHovering = True
                self.getLeaderData()
		#self.setStaticFlockingLeader()
        else:
            if(self.rigidBodyState.parameters.isHovering):
                if(not self.checkAbort()):
                    self.hover()
            else:
                if(not self.checkAbort()):
                    self.landing()

    def hover(self):
        if(not self.checkAbort()):
            self.computePIDControl()
            print "Hovering"

    def prepTakeoff(self):
        self.vehicle.mode = VehicleMode('STABILIZE')
        #print 'Basic Prearm Checks'
        print 'Arming Motors'
        self.vehicle.channels.overrides = {'3':1000}
        time.sleep(2)
        self.vehicle.armed = True

    def computeTakeoffVelocity(self,desDest):
        if(abs(desDest) >= self.rigidBodyState.parameters.stoppingDistance):
            self.rigidBodyState.leader.pgz = (self.rigidBodyState.parameters.desiredSpeed*desDest)/abs(desDest)
            if( not self.checkAbort()):
                self.computePIDControl()
                print 'Taking Off'
        else:
            self.rigidBodyState.leader.pgz = (self.rigidBodyState.parameters.desiredSpeed*desDest)/self.rigidBodyState.parameters.stoppingDistance
            if(not self.checkAbort()):
                self.computePIDControl()
                print 'Approaching Target Altitude'

    def computeLandingVelocity(self,desDest):
        if(abs(desDest) >= self.rigidBodyState.parameters.stoppingDistance):
            self.rigidBodyState.leader.pgz = (self.rigidBodyState.parameters.desiredSpeed*desDest)/abs(desDest)
            #print self.rigidBodyState.leader
            if(not self.checkAbort()):
                self.computePIDControl()
                print 'Landing'
        elif(self.rigidBodyState.position.z >= (self.rigidBodyState.initPos.zo-0.05)):
            self.vehicle.channels.overrides = {'3':1000}
            self.vehicle.armed = False
            self.rigidBodyState.parameters.isTakeoff = False
	    print "Vehicle Landed"
        else:
            self.rigidBodyState.leader.pgz = (self.rigidBodyState.parameters.desiredSpeed*desDest)/self.rigidBodyState.parameters.stoppingDistance
            if(not self.checkAbort()):
                self.computePIDControl()
                print 'Approaching Landing'

#    def updateGlobalStatewithData(self):
#        #if(msg.content.ID>0):
#        ID = int(self.scoobyDoo.ID)# + self.rigidBodyState.ID
#       	if(ID==self.rigidBodyState.ID):
#            self.RigidBodies[ID] = self.rigidBodyState
#        else:
#            self.RigidBodies[ID] = self.scoobyDoo
 #           self.rigidBodyState.position = msg.content.position
 #           print self.RigidBodies
#        self.rigidBodyState.timeout.peerLastRx[ID] = datetime.now()

    def decodeData(self,msg):
        ID = int(msg.content[1])
        if(ID == self.rigidBodyState.ID):
        	self.rigidBodyState.position.x = float(msg.content[2])
        	self.rigidBodyState.position.y = float(msg.content[3])
        	self.rigidBodyState.position.z = float(msg.content[4])
        	self.rigidBodyState.velocity.vx = float(msg.content[5])
        	self.rigidBodyState.velocity.vy = float(msg.content[6])
        	self.rigidBodyState.velocity.vz = float(msg.content[7])
        	#self.rigidBodyState.attitude.roll = float(msg.content[8])
        	#self.rigidBodyState.attitude.pitch = float(msg.content[9])
		self.rigidBodyState.leader.flocking.qgx = float(msg.content[8])
		self.rigidBodyState.leader.flocking.qgy = float(msg.content[9])
        	self.rigidBodyState.attitude.yaw = float(msg.content[10])
            	self.rigidBodyState.flightSeq = int(msg.content[11])
            	self.rigidBodyState.attitude.roll = self.vehicle.attitude.roll
            	self.rigidBodyState.attitude.pitch = self.vehicle.attitude.pitch
            	self.rigidBodyState.channels = self.vehicle.channels
		self.rigidBodyState.batt = self.vehicle.battery.voltage
            	self.RigidBodies[int(ID)] = self.rigidBodyState
        else:
            	self.scoobyDoo.ID = ID
            	self.scoobyDoo.position.x = float(msg.content[2])
           	self.scoobyDoo.position.y = float(msg.content[3])
            	self.scoobyDoo.position.z = float(msg.content[4])
            	self.scoobyDoo.velocity.vx = float(msg.content[5])
            	self.scoobyDoo.velocity.vy = float(msg.content[6])
            	self.scoobyDoo.velocity.vz = float(msg.content[7])
            	self.RigidBodies[ID] = self.scoobyDoo

#    def decodeMessage(self,msg):
#        if(msg.content.ID>0):
#            ID = int(msg.content.ID)# + self.rigidBodyState.ID
#            self.RigidBodies[ID] = msg.content
 #           self.rigidBodyState.position = msg.content.position
	    #print self.RigidBodies
#            self.rigidBodyState.timeout.peerLastRx[ID] = datetime.now()
#        else:
#	    print "This should not be happening"
#            ID = int(msg.content.ID) + self.rigidBodyState.ID
#            self.RigidBodies[ID] = msg.content
#            self.rigidBodyState.timeout.peerLastRx[int(msg.content.ID)] = datetime.now()

    def getLeaderData(self):
        self.rigidBodyState.leader.qgx = self.rigidBodyState.position.x
        self.rigidBodyState.leader.qgy = self.rigidBodyState.position.y
        self.rigidBodyState.leader.qgz = self.rigidBodyState.position.z
        self.rigidBodyState.leader.pgx = 0
        self.rigidBodyState.leader.pgy = 0
        self.rigidBodyState.leader.pgz = 0

#    def setStaticFlockingLeader(self):
#	print 'Set the Flocking Leader at average of the two agent Position!'
#	self.rigidBodyState.leader.flock.qgx = (self.rigidBodyState.initPos.xo + self.scoobyDoo.position.x)/2
#	self.rigidBodyState.leader.flock.qgy = (self.rigidBodyState.initPos.yo + self.scoobyDoo.position.y)/2
#	self.rigidBodyState.leader.flock.qgz = (self.rigidBodyState.position.z + self.scoobyDoo.position.z)/2
#       self.rigidBodyState.leader.flock.pgx = 0
#      	self.rigidBodyState.leader.flock.pgy = 0
#	self.rigidBodyState.leader.flock.pgz = 0

    def setInitialPos(self):
        initPosSet = False
        if(self.rigidBodyState.position.z<-0.115):
            self.rigidBodyState.initPos.xo = self.rigidBodyState.position.x
            self.rigidBodyState.initPos.yo = self.rigidBodyState.position.y
            self.rigidBodyState.initPos.zo = self.rigidBodyState.position.z
            initPosSet = True
        print initPosSet
        return initPosSet

    def checkAbort(self):
        if(self.checkTimeouts()):
            self.rigidBodyState.abortReason = "Timeout"
            self.rigdBodyState.RCLatch = True
            self.rigidBodyState.isGPS = False
            self.releaseControl()
            return True
        #! Check the proper flight mode
        if(not self.vehicle.mode == 'STABILIZE'):
            self.releaseControl()
            return True
        return False

    def checkGPS(self):
        #! Check Timeouts
        if(self.checkTimeouts()):
        	print "No GPS - Timeout"
        	self.rigidBodyState.RCLatch = True
        	return False
        #! Check configuration
        if(not self.rigidBodyState.parameters.isComplete):
        	self.rigidBodyState.RCLatch = True
        	return False
        self.rigidBodyState.RCLatch = True # Set the Latch
        self.rigidBodyState.isGPS = True # GPS is being received
        return True

    #! Check for a GPS timeout - If no GPS, control should not be engaged
    def checkTimeouts(self):
        didTimeout = False
        if(datetime.now() - timedelta(seconds = self.lastGPSContact) < datetime.now() - timedelta(seconds = self.rigidBodyState.parameters.GPSTimeout)):
            print "GPS Timeout"
            self.rigidBodyState.timeout.GPSTimeoutTime = time.time()
            didTimeout = True
        return didTimeout

  #  def pushStatetoTxQueue(self):
  #      msg = Message()
  #      msg.type = "UAV"
  #      msg.sendTime = datetime.now()
  #      msg.content = self.rigidBodyState
#        self.transmitQueue.put(msg)
  #      return msg

    def pushStatetoLoggingQueue(self):
        msg = Message()
        msg.type = "UAV_LOG"
        msg.sendTime = time.time()
        msg.content = {}
        msg.content['thisBodyState'] = self.rigidBodyState
        msg.content['RigidBodies'] = self.RigidBodies
#        print msg.content
        self.logQueue.put(msg)

    def computePIDControl(self):
	#leaderVec = np.matrix([[self.rigidBodyState.leader.qgx],[self.rigidBodyState.leader.qgy],[self.rigidBodyState.leader.qgz]])
        #accPosError = np.matrix([[self.rigidBodyState.command.accPosXError],[self.rigidBodyState.command.accPosYError],[self.rigidBodyState.command.accPosZError]])
	#! Compute the difference vectors
        dq = np.matrix([[self.diffFunction(self.rigidBodyState.position.x,self.rigidBodyState.leader.qgx)],[self.diffFunction(self.rigidBodyState.position.y,self.rigidBodyState.leader.qgy)],[self.diffFunction(self.rigidBodyState.position.z,self.rigidBodyState.leader.qgz)]])
        dp = np.matrix([[self.diffFunction(self.rigidBodyState.velocity.vx,self.rigidBodyState.leader.pgx)],[self.diffFunction(self.rigidBodyState.velocity.vy,self.rigidBodyState.leader.pgy)],[self.diffFunction(self.rigidBodyState.velocity.vz,self.rigidBodyState.leader.pgz)]])
        #! Integrate the position error
	intPrep = self.rigidBodyState.parameters.Ts*dq
	#accPosError = self.antiWindupVec(leaderVec,-10,10,accPosError,intPrep)
	self.rigidBodyState.command.accPosXError = self.antiWindup(self.rigidBodyState.leader.qgx,-10,10,self.rigidBodyState.command.accPosXError,intPrep[0,0])
	self.rigidBodyState.command.accPosYError = self.antiWindup(self.rigidBodyState.leader.qgy,-10,10,self.rigidBodyState.command.accPosYError,intPrep[1,0])
	self.rigidBodyState.command.accPosZError = self.antiWindup(self.rigidBodyState.leader.qgz,-10,10,self.rigidBodyState.command.accPosZError,intPrep[2,0])
	accPosError = np.matrix([[self.rigidBodyState.command.accPosXError],[self.rigidBodyState.command.accPosYError],[self.rigidBodyState.command.accPosZError]])
	#print 'accPosError' + str(accPosError)
	#! Compute Desired Accelerations
        kp = np.matrix([[self.rigidBodyState.parameters.kpx, 0, 0], [0, self.rigidBodyState.parameters.kpy, 0], [0, 0, self.rigidBodyState.parameters.kpz]])
        kd = np.matrix([[self.rigidBodyState.parameters.kdx, 0, 0], [0, self.rigidBodyState.parameters.kdy, 0], [0, 0, self.rigidBodyState.parameters.kdz]])
        ki = np.matrix([[self.rigidBodyState.parameters.kix, 0, 0], [0, self.rigidBodyState.parameters.kiy, 0], [0, 0, self.rigidBodyState.parameters.kiz]])
	uk = kp*dq + kd*dp + ki*accPosError
        #! Estimate the velocity command using a low pass filter
        velGain = 0.1
        pkp = np.matrix([[self.rigidBodyState.previousState.velPrev_x], [self.rigidBodyState.previousState.velPrev_y], [self.rigidBodyState.previousState.velPrev_z]])
        ukp = np.matrix([[self.rigidBodyState.previousState.accPrev_x],[self.rigidBodyState.previousState.accPrev_y],[self.rigidBodyState.previousState.accPrev_z]])
        pk = (1-self.rigidBodyState.parameters.Ts*velGain)*pkp + self.rigidBodyState.parameters.Ts*(1-(velGain*self.rigidBodyState.parameters.Ts)/2)*ukp
        self.updateControlState(uk,pk)

    def computeFlockingControl(self):
	#! Set Position Vectors and Estimate Next State
       	qi = np.matrix([[self.rigidBodyState.position.x],[self.rigidBodyState.position.y],[self.rigidBodyState.position.z]])
       	pi = np.matrix([[self.rigidBodyState.velocity.vx],[self.rigidBodyState.velocity.vy],[self.rigidBodyState.velocity.vz]])
        AR = np.matrix([[0],[0],[0]])
	VC = np.matrix([[0],[0],[0]])
	FC = np.matrix([[0],[0],[0]])
        kGamma1 = np.matrix([[self.rigidBodyState.parameters.gamma1, 0, 0], [0, self.rigidBodyState.parameters.gamma1, 0], [0, 0, self.rigidBodyState.parameters.gamma3]])
        kGamma2 = np.matrix([[self.rigidBodyState.parameters.gamma2, 0, 0], [0, self.rigidBodyState.parameters.gamma2, 0], [0, 0, self.rigidBodyState.parameters.gamma4]])
	if (self.scoobyDoo.position.x):
		print('hello')
		qihat = qi + self.rigidBodyState.parameters.Ts*pi
        	qj = np.matrix([[self.scoobyDoo.position.x],[self.scoobyDoo.position.y],[self.scoobyDoo.position.z]])
        	pj = np.matrix([[self.scoobyDoo.velocity.vx],[self.scoobyDoo.velocity.vy],[self.scoobyDoo.velocity.vz]])
        	qjhat = qj + self.rigidBodyState.parameters.Ts*pj
        	#! Difference Functions
        	dq = qj - qi
        	dp = pj - pi
        	dqhat = qjhat - qihat
        	#! Attraction / Repulsion
        	Phi = self.rigidBodyState.parameters.alpha2/(self.rigidBodyState.parameters.alpha1 + 1) - self.rigidBodyState.parameters.alpha2/(self.rigidBodyState.parameters.alpha1 + (m.pow(self.vectorNorm(dqhat),2)/m.pow(self.rigidBodyState.parameters.desiredDistance,2)))
        	AR = np.matrix([[Phi, 0, 0], [0, Phi, 0], [0, 0, Phi]])*dq #Phi*dq
        	#! Velocity Consensus
        	VC = self.rigidBodyState.parameters.beta*dp
                #! Flock Correction to Guidance Term
	        FC = (kGamma1*dq + kGamma2*dp)/self.rigidBodyState.parameters.expectedMAVs
	else:
		print('goodbye')
	#! Guidance Term
        #kGamma1 = np.matrix([[self.rigidBodyState.parameters.gamma1, 0, 0], [0, self.rigidBodyState.parameters.gamma1, 0], [0, 0, self.rigidBodyState.parameters.gamma3]])
        #kGamma2 = np.matrix([[self.rigidBodyState.parameters.gamma2, 0, 0], [0, self.rigidBodyState.parameters.gamma2, 0], [0, 0, self.rigidBodyState.parameters.gamma4]])
        qg = np.matrix([[self.rigidBodyState.leader.flocking.qgx],[self.rigidBodyState.leader.flocking.qgy],[self.rigidBodyState.leader.flocking.qgz]])
        pg = np.matrix([[self.rigidBodyState.leader.flocking.pgx],[self.rigidBodyState.leader.flocking.pgy],[self.rigidBodyState.leader.flocking.pgz]])
        GT = kGamma1*(qg-qi) + kGamma2*(pg-pi)
        #! Compute Desired Accelerations
        uk = AR + VC + GT - FC
        self.rigidBodyState.command.AR = AR[0,0] # For debugging purposes
 	self.rigidBodyState.command.VC = VC[0,0] # For debugging purposes
	self.rigidBodyState.command.GT = GT[0,0] # For debugging purposes
	self.rigidBodyState.command.FC = FC[0,0] # For debugging purposes
	#! Estimate the velocity command using a low pass filter
        velGain = 0.1
        pkp = np.matrix([[self.rigidBodyState.previousState.velPrev_x], [self.rigidBodyState.previousState.velPrev_y], [self.rigidBodyState.previousState.velPrev_z]])
        ukp = np.matrix([[self.rigidBodyState.previousState.accPrev_x],[self.rigidBodyState.previousState.accPrev_y],[self.rigidBodyState.previousState.accPrev_z]])
        pk = (1-self.rigidBodyState.parameters.Ts*velGain)*pkp + self.rigidBodyState.parameters.Ts*(1-(velGain*self.rigidBodyState.parameters.Ts)/2)*ukp
        self.updateControlState(uk,pk)

    def updateControlState(self,uk,pk):
        #! Estimate the velocity command using a low pass filter
        #velGain = 0.1
        #pkp = np.matrix([[self.rigidBodyState.previousState.velPrev_x], [self.rigidBodyState.previousState.velPrev_y], [self.rigidBodyState.previousState.velPrev_z]])
        #ukp = np.matrix([[self.rigidBodyState.previousState.accPrev_x],[self.rigidBodyState.previousState.accPrev_y],[self.rigidBodyState.previousState.accPrev_z]])
        #pk = (1-self.rigidBodyState.parameters.Ts*velGain)*pkp + self.rigidBodyState.parameters.Ts*(1-(velGain*self.rigidBodyState.parameters.Ts)/2)*ukp
        #self.updateControlState(uk,pk)
        #! Update the command state
        self.rigidBodyState.command.ux = uk[0,0]
        self.rigidBodyState.command.uy = uk[1,0]
        self.rigidBodyState.command.uz = uk[2,0]
        self.rigidBodyState.command.vel_est_x = pk[0,0]
        self.rigidBodyState.command.vel_est_y = pk[1,0]
        self.rigidBodyState.command.vel_est_z = pk[2,0]
        #! Update the previous State
        accVelError = (self.rigidBodyState.velocity.vz - self.rigidBodyState.command.vel_est_z)*self.rigidBodyState.parameters.Ts
	self.rigidBodyState.previousState.velPrev_x = pk[0,0]
        self.rigidBodyState.previousState.velPrev_y = pk[1,0]
        self.rigidBodyState.previousState.velPrev_z = pk[2,0]
        self.rigidBodyState.previousState.accPrev_x = uk[0,0]
        self.rigidBodyState.previousState.accPrev_y = uk[1,0]
        self.rigidBodyState.previousState.accPrev_z = uk[2,0]
        #! Integrate the velocity error
        #accVelError = self.rigidBodyState.velocity.vz - self.rigidBodyState.command.vel_est_z
        self.rigidBodyState.command.accVelZError = self.antiWindup(self.rigidBodyState.command.vel_est_z,-10,10,self.rigidBodyState.command.accVelZError,accVelError)
        self.computeAttitudeThrustCommands()

    def computeAttitudeThrustCommands(self):
        self.rigidBodyState.test.throttle = self.rigidBodyState.parameters.quadMass*(self.rigidBodyState.parameters.gravity - self.rigidBodyState.command.uz + self.rigidBodyState.parameters.kw_vel*(self.rigidBodyState.velocity.vz - self.rigidBodyState.command.vel_est_z) + self.rigidBodyState.parameters.intGain*self.rigidBodyState.command.accVelZError)/(np.cos(self.rigidBodyState.attitude.roll)*np.cos(self.rigidBodyState.attitude.pitch))
        self.rigidBodyState.test.pitch = np.arctan(((self.rigidBodyState.command.ux-self.rigidBodyState.parameters.ku_vel*(self.rigidBodyState.velocity.vx - self.rigidBodyState.command.vel_est_x))*np.cos(self.rigidBodyState.attitude.yaw) + (self.rigidBodyState.command.uy-self.rigidBodyState.parameters.kv_vel*(self.rigidBodyState.velocity.vy - self.rigidBodyState.command.vel_est_y))*np.sin(self.rigidBodyState.attitude.yaw))/(-self.rigidBodyState.parameters.gravity + self.rigidBodyState.command.uz - self.rigidBodyState.parameters.kw_vel*(self.rigidBodyState.velocity.vz - self.rigidBodyState.command.vel_est_z) - self.rigidBodyState.parameters.intGain*self.rigidBodyState.command.accVelZError))
        self.rigidBodyState.test.roll = np.arctan(((self.rigidBodyState.command.ux-self.rigidBodyState.parameters.ku_vel*(self.rigidBodyState.velocity.vx - self.rigidBodyState.command.vel_est_x))*np.cos(self.rigidBodyState.test.pitch)*np.sin(self.rigidBodyState.attitude.yaw) - (self.rigidBodyState.command.uy-self.rigidBodyState.parameters.kv_vel*(self.rigidBodyState.velocity.vy - self.rigidBodyState.command.vel_est_y))*np.cos(self.rigidBodyState.test.pitch)*np.cos(self.rigidBodyState.attitude.yaw))/(-self.rigidBodyState.parameters.gravity + self.rigidBodyState.command.uz - self.rigidBodyState.parameters.kw_vel*(self.rigidBodyState.velocity.vz - self.rigidBodyState.command.vel_est_z) - self.rigidBodyState.parameters.intGain*self.rigidBodyState.command.accVelZError))
        self.rigidBodyState.test.yaw = self.rigidBodyState.attitude.yaw
        self.scaleAndSendControl()

    def scaleAndSendControl(self):
	y = self.rigidBodyState.test.throttle
	x = self.vehicle.battery.voltage
        #! Scale the compute control values to match the format used in vehicle.channel.overrides{}
        ROLL =  1500 + (500/self.rigidBodyState.parameters.rollLimit)*self.rigidBodyState.test.roll
        PITCH = 1500 + (500/self.rigidBodyState.parameters.pitchLimit)*self.rigidBodyState.test.pitch
        #! Stock 3DR SOLO Propeller Scaling ---> These need to be fixed
	#THROTTLE = 1000 + 32.254*self.rigidBodyState.test.throttle - 0.257*self.rigidBodyState.test.throttle*self.rigidBodyState.test.throttle#972 + 48.484*self.rigidBodyState.test.throttle + 1.3241*self.rigidBodyState.test.throttle*self.rigidBodyState.test.throttle#
        #THROTTLE =  1000 + 1.8573*self.rigidBodyState.test.throttle + 0.9664*self.rigidBodyState.test.throttle*self.rigidBodyState.test.throttle#1000 + 0.7427*self.rigidBodyState.test.throttle + 0.9136*self.rigidBodyState.test.throttle*self.rigidBodyState.test.throttle
	#! Master Air Screw Propeller Scaling functions
	# (with strap mount) THROTTLE = 1000*(1+(y-3.779)/(0.2238*(x*x-25.82))) # R^2 = 0.9897
	THROTTLE = 1000*(1+(y-4.869)/(0.2067*(x*x-23.71))) # R^2 = 0.97
	YAW = self.rigidBodyState.attitude.yaw
        #! Saturate to keep commands in range of input values
        self.rigidBodyState.command.Roll = self.saturate(ROLL,1000,2000)
        self.rigidBodyState.command.Pitch = self.saturate(PITCH,1000,2000)
        self.rigidBodyState.command.Throttle = self.saturate(THROTTLE,1000,2000)
        self.rigidBodyState.command.Yaw = self.saturate(YAW,1000,2000)
        self.vehicle.channels.overrides = {'1': self.rigidBodyState.command.Roll,'2': self.rigidBodyState.command.Pitch,'3': self.rigidBodyState.command.Throttle}
        #print self.rigidBodyState.batt
	print self.counter

    def antiWindup(self,value,lowLimit,highLimit,accumulator,toAdd):
        if(value>=highLimit): #Saturation and anti-windup
            if(toAdd < 0):
                accumulator = accumulator + toAdd
        elif(value<=lowLimit):
            if(toAdd > 0):
                accumulator = accumulator + toAdd
        else:
            accumulator = accumulator + toAdd
        return accumulator

    def antiWindupVec(self,value,lowLimit,highLimit,accumulator,toAdd):
	print 'toAdd' + str(toAdd)
	print 'value' + str(value)
	print 'accumulator' + str(accumulator)
	for i in range(0,len(toAdd)):
		if(value[i] >= highLimit):
			if(toAdd[i] < 0):
				accumulator[i] = accumulator[i] + toAdd[i]
		if(value[i] <= lowLimit):
			if(toAdd[i] > 0):
				accumulator[i] = accumulator[i] + toAdd[i]
		else:
			accumulator[i] = accumulator[i] + toAdd[i]
	return accumulator

    def vectorNorm(self,vec):
        eta = np.sqrt(m.pow(vec[0,0],2) + m.pow(vec[1,0],2) + m.pow(vec[2,0],2))
        return eta

    def saturate(self, value, minimum, maximum):
        out = max(value,minimum)
        out = min(out,maximum)
        return out

    def releaseControl(self):
        self.vehicle.channels.overrides = {}
        print self.vehicle.channels.overrides
        print "Channels Cleared"

    def diffFunction(self,pos1,pos2):
        delta = pos2 - pos1
        return delta

ext: py

reason: import numpy


text:
"""
This file test tensor op that should also operate on CudaNdaray.
"""
from __future__ import absolute_import, print_function, division
from nose.plugins.skip import SkipTest
from nose_parameterized import parameterized

import numpy

import theano
from theano import tensor
import theano.tensor as T
import theano.tests.unittest_tools as utt

# Skip test if cuda_ndarray is not available.
import theano.sandbox.cuda as cuda
from theano.tensor.nnet.tests import test_conv3d2d
if cuda.cuda_available is False:
    raise SkipTest('Optional package cuda disabled')

if theano.config.mode == 'FAST_COMPILE':
    mode_with_gpu = theano.compile.mode.get_mode('FAST_RUN').including('gpu')
    mode_without_gpu = theano.compile.mode.get_mode('FAST_RUN').excluding('gpu')
else:
    mode_with_gpu = theano.compile.mode.get_default_mode().including('gpu')
    mode_without_gpu = theano.compile.mode.get_default_mode().excluding('gpu')

def test_shape_i():
    x = cuda.ftensor3()
    v = cuda.CudaNdarray(numpy.zeros((3, 4, 5), dtype='float32'))
    f = theano.function([x], x.shape[1])
    topo = f.maker.fgraph.toposort()
    assert f(v) == 4
    if theano.config.mode != 'FAST_COMPILE':
        assert len(topo) == 1
        assert isinstance(topo[0].op, T.opt.Shape_i)

def test_shape():
    x = cuda.ftensor3()
    v = cuda.CudaNdarray(numpy.zeros((3, 4, 5), dtype='float32'))
    f = theano.function([x], x.shape)
    topo = f.maker.fgraph.toposort()
    assert numpy.all(f(v) == (3, 4, 5))
    if theano.config.mode != 'FAST_COMPILE':
        assert len(topo) == 4
        assert isinstance(topo[0].op, T.opt.Shape_i)
        assert isinstance(topo[1].op, T.opt.Shape_i)
        assert isinstance(topo[2].op, T.opt.Shape_i)
        assert isinstance(topo[3].op, T.opt.MakeVector)

def test_softmax_optimizations():
    from theano.tensor.nnet.nnet import softmax, crossentropy_categorical_1hot
    x = tensor.fmatrix('x')
    one_of_n = tensor.lvector('one_of_n')
    op = crossentropy_categorical_1hot

    op(x, one_of_n)

    fgraph = theano.gof.FunctionGraph(
        [x, one_of_n],
        [op(softmax(x), one_of_n)])
    assert fgraph.outputs[0].owner.op == op

    mode_with_gpu.optimizer.optimize(fgraph)

    assert str(fgraph.outputs[0].owner.op) == 'OutputGuard'
    assert fgraph.outputs[0].owner.inputs[0].owner.op == cuda.host_from_gpu
    assert fgraph.outputs[0].owner.inputs[0].owner.inputs[0].owner.op == cuda.nnet.gpu_crossentropy_softmax_argmax_1hot_with_bias

def test_may_share_memory_cuda():
    from theano.misc.may_share_memory import may_share_memory
    a = cuda.CudaNdarray(numpy.zeros((3, 4), dtype='float32'))
    b = cuda.CudaNdarray(numpy.zeros((3, 4), dtype='float32'))
    na = numpy.zeros((3, 4))
    nb = numpy.zeros((3, 4))
    va = a.view()
    vb = b.view()
    ra = a.reshape((4, 3))
    rb = b.reshape((4, 3))

    # can't test the transpose as ta._strides = is not implemented
    # manual transpose of a
    # ta = a.reshape((4,3))
    # ta._strides = (ta._strides[1],ta._strides[0])#not implemented
    # elem_size=elem_size = numpy.zeros(0,dtype=a.dtype).dtype.itemsize
    # ta.gpudata += ta.size*elem_size

    for a_, b_, rep in [(a, a, True), (b, b, True), (a, b, False),
                        (a, na, False), (b, nb, False),
                        (na, b, False), (nb, a, False),
                        (a, va, True), (b, vb, True),
                        (va, b, False), (a, vb, False),
                        (a, ra, True), (b, rb, True),
                        (ra, b, False), (a, rb, False), ]:
        assert may_share_memory(a_, b_) == rep
        assert may_share_memory(b_, a_) == rep

    # test that it raise error when needed.
    for a_, b_, rep in [(a, (0,), False), (a, 1, False), (a, None, False)]:
        assert may_share_memory(a_, b_, False) == rep
        assert may_share_memory(b_, a_, False) == rep
        try:
            may_share_memory(a_, b_)
            raise Exception("An error was expected")
        except TypeError:
            pass
        try:
            may_share_memory(b_, a_)
            raise Exception("An error was expected")
        except TypeError:
            pass

def test_deepcopy():
    a = cuda.fmatrix()
    a_v = cuda.CudaNdarray(numpy.zeros((3, 4), dtype='float32'))

    # We force the c code to check that we generate c code
    mode = theano.Mode("c", mode_with_gpu.optimizer)
    f = theano.function([a], a, mode=mode)
    theano.printing.debugprint(f)
    out = f(a_v)
    assert out is not a_v
    assert numpy.allclose(numpy.asarray(a_v), numpy.asarray(out))

    # We force the python linker as the default code should work for this op
    mode = theano.Mode("py", mode_with_gpu.optimizer)
    f = theano.function([a], a, mode=mode)
    theano.printing.debugprint(f)
    out = f(a_v)
    assert out is not a_v
    assert numpy.allclose(numpy.asarray(a_v), numpy.asarray(out))

def test_get_diagonal_subtensor_view():
    test_conv3d2d.test_get_diagonal_subtensor_view(wrap=cuda.CudaNdarray)

@parameterized.expand(('valid', 'full'), utt.custom_name_func)
def test_conv3d(border_mode):
    test_conv3d2d.check_conv3d(border_mode=border_mode,
                               mode=mode_with_gpu,
                               shared=cuda.shared_constructor)

max_stars_repo_name: MarcCote/Theano

max_issues_repo_name: MarcCote/Theano

max_forks_repo_name: MarcCote/Theano

reason: import numpy,import theano,from theano


text:
# Copyright 2020 Xilinx Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
Module for transforming ONNX L3 operators to XLayer objects

L3: Additional math and transform operators

"""

import logging
import numpy as np
import pyxir as px

from typing import Dict

from pyxir.graph.layer import xlayer_factory as xlf
from pyxir.graph.layer import XLayer
from pyxir.shapes.tools import get_numpy_broadcasted_shape
from ..onnx_2_xlayer_registry import register_onnx_2_xlayer_converter
from ..onnx_tools import NodeWrapper, get_onnx_elem_type_2_dtype

logger = logging.getLogger('pyxir')

@register_onnx_2_xlayer_converter("GRU")
def gru(node: NodeWrapper,
        params: Dict[str, np.ndarray],
        xmap: Dict[str, XLayer]):
    """ ONNX GRU to XLayer AnyOp conversion function """

    logger.info("ONNX GRU -> XLayer AnyOp")

    raise NotImplementedError("ONNX GRU operator conversion not supported")

@register_onnx_2_xlayer_converter("LSTM")
def lstm(node: NodeWrapper,
         params: Dict[str, np.ndarray],
         xmap: Dict[str, XLayer]):
    """ ONNX LSTM to XLayer AnyOp conversion function """

    logger.info("ONNX LSTM -> XLayer AnyOp")

    raise NotImplementedError("ONNX LSTM operator conversion not supported")

@register_onnx_2_xlayer_converter("RNN")
def rnn(node: NodeWrapper,
        params: Dict[str, np.ndarray],
        xmap: Dict[str, XLayer]):
    """ ONNX RNN to XLayer AnyOp conversion function """

    logger.info("ONNX RNN -> XLayer AnyOp")

    raise NotImplementedError("ONNX RNN operator conversion not supported")

@register_onnx_2_xlayer_converter("Scan")
def scan(node: NodeWrapper,
         params: Dict[str, np.ndarray],
         xmap: Dict[str, XLayer]):
    """ ONNX RNN to XLayer AnyOp conversion function """

    logger.info("ONNX Scan -> XLayer AnyOp")

    raise NotImplementedError("ONNX Scan operator conversion not supported")

max_stars_repo_name: pankajdarak-xlnx/pyxir

max_issues_repo_name: pankajdarak-xlnx/pyxir

max_forks_repo_name: pankajdarak-xlnx/pyxir


text:
import pandas as pd
import pickle
from keras.utils import to_categorical
from sklearn.model_selection import train_test_split
from keras.preprocessing.sequence import pad_sequences
import numpy as np
# from keras_contrib.utils import save_load_utils
# from tf2crf import CRF
from keras_contrib.layers import CRF
from keras.models import Model
from keras.layers import Input
from keras.layers import LSTM, Embedding, Dense, TimeDistributed, Dropout, Bidirectional
from seqeval.metrics import precision_score, recall_score, f1_score, classification_report

max_len = 75

class SentenceGetter(object):

    def __init__(self, data):
        self.n_sent = 1
        self.data = data
        self.empty = False

        def agg_func(s): return [(w, p, t) for w, p, t in zip(s["Word"].values.tolist(),
                                                              s["POS"].values.tolist(),
                                                              s["Tag"].values.tolist())]

        self.grouped = self.data.groupby("Sentence #").apply(agg_func)
        self.sentences = [s for s in self.grouped]

    def get_next(self):
        try:
            s = self.grouped["Sentence: {}".format(self.n_sent)]
            self.n_sent += 1
            return s
        except:
            return None

def prepare_data(word2idx, tag2idx, n_tags, sentences, max_len):
    X = [[word2idx[w[0]] for w in s] for s in sentences]
    X = pad_sequences(maxlen=max_len, sequences=X, padding="post", value=0)

    y = [[tag2idx[w[2]] for w in s] for s in sentences]
    y = pad_sequences(maxlen=max_len, sequences=y,
                      padding="post", value=tag2idx["O"])
    y = [to_categorical(i, num_classes=n_tags) for i in y]

    X_tr, X_te, y_tr, y_te = train_test_split(X, y, test_size=0.1)
    return X_tr, X_te, y_tr, y_te

def create_model(n_tags, max_len, n_words):
    input = Input(shape=(max_len,))
    model = Embedding(input_dim=n_words + 1, output_dim=20,
                      input_length=max_len, mask_zero=True)(input)  # 20-dim embedding
    model = Bidirectional(LSTM(units=50, return_sequences=True,
                               recurrent_dropout=0.1))(model)  # variational biLSTM
    model = TimeDistributed(Dense(50, activation="relu"))(
        model)  # a dense layer as suggested by neuralNer
    crf = CRF(n_tags)  # CRF layer
    out = crf(model)  # output

    model = Model(input, out)
    model.compile(optimizer="rmsprop", loss=crf.loss_function,
                  metrics=[crf.accuracy])
    return model

def train_model(model, X_tr, y_tr):
    history = model.fit(X_tr, np.array(y_tr), batch_size=32, epochs=5,
                        validation_split=0.1, verbose=1)
    return history

def save_model(model):
    file_name = "C:\\Users\\Divakar.Sharma\\Desktop\\Travelers\\AI-Underwriting\\src\\ner\\" \
                "models\\crf_lstm_model"
    # save the model
    model.save(file_name)
    # save_load_utils.save_all_weights(model, file_name)

def pred2label(pred, idx2tag):
    out = []
    for pred_i in pred:
        out_i = []
        for p in pred_i:
            p_i = np.argmax(p)
            out_i.append(idx2tag[p_i].replace("PAD", "O"))
            out.append(out_i)
    return out

if __name__ == "__main__":
    data = pd.read_csv("C:\\Users\\Divakar.Sharma\\Desktop\\Travelers\\AI-Underwriting\\src"
                       "\\ner\\data\\ner_dataset.csv", encoding="latin1")
    data = data.fillna(method="ffill")
    getter = SentenceGetter(data)
    sentences = getter.sentences

    words = list(set(data["Word"].values))
    words.append("ENDPAD")
    n_words = len(words)

    tags = list(set(data["Tag"].values))
    word2idx = {w: i + 1 for i, w in enumerate(words)}
    with open("words.pickle", "wb") as word:
        pickle.dump(word2idx, word)
    print(word2idx)
    tag2idx = {t: i for i, t in enumerate(tags)}
    print(tags)
    n_tags = len(tags)
    X_tr, X_te, y_tr, y_te = prepare_data(word2idx, tag2idx, n_tags, sentences, max_len)
    model = create_model(n_tags, max_len, n_words)
    model.summary()
    history = train_model(model, X_tr, y_tr)
    test_pred = model.predict(X_te, verbose=1)
    idx2tag = {i: w for w, i in tag2idx.items()}
    #
    # pred_labels = pred2label(test_pred)
    # test_labels = pred2label(y_te)

    # print("F1-score: {:.1%}".format(f1_score(test_labels, pred_labels)))
    # print(classification_report(test_labels, pred_labels))

max_stars_repo_name: amit-kolluri/AI-underwriting

max_issues_repo_name: amit-kolluri/AI-underwriting

max_forks_repo_name: amit-kolluri/AI-underwriting