#!/usr/bin/python3

import os,sys,math
import numpy as np
import matplotlib.pyplot as plt

cos = np.cos
sin = np.sin
pi = np.pi
sqrt = np.sqrt
exp = np.exp
linspace = np.linspace

## Gear Profile Generation Scripts
## July 2022
## Copyright Aaron M. Schinder
## Released under the MIT/BSD License
## Do whatever the heck you want with this code.


###################################
## Newton's Method Scalar Solver ##
###################################

def mergedefaults(kwargs,defs):

    merged = dict()

    for k in defs:
        if(k in kwargs):
            merged[k] = kwargs[k]
        else:
            merged[k] = defs[k]

    return merged

def newtonsolve_defaults():

    defs = dict();

    defs["maxiter"] = 1000
    defs["dx"] = 1E-5
    defs["tol"] = 1E-5
    defs["alpha"] = 0.5
    defs["randstep"] = 0.5
    defs["use_maxstep"] = False
    defs["maxstep"] = 1.0

    return defs

#Scalar Newton Solver
#Takes a function of the form y = fptr(x,fpars) and returns a solution x where fptr(x,fpars)=0
#Use: newtonsolve(fptr,x0,fpars,**kwargs)
#Optional Arguments
#
#
def newtonsolve(fptr,x0,fpars,**kwargs):

    x0 = np.float64(x0)

    flag = -1

    defs = newtonsolve_defaults()
    defs = mergedefaults(kwargs,defs)
    maxiter = np.int32(defs["maxiter"])
    dx = np.float64(defs["dx"])
    tol = np.float64(defs["tol"])
    alpha = np.float64(defs["alpha"])
    randstep = np.float64(defs["randstep"])
    use_maxstep = defs["use_maxstep"]
    maxstep = defs["maxstep"]

    I = 0
    x = x0
    while(I<maxiter):

        f0 = np.float64(fptr(x,fpars))
        fp = np.float64(fptr(x+dx,fpars))
        fm = np.float64(fptr(x-dx,fpars))
        dfdx = (fp-fm)/(2.0*dx)

        if(np.abs(f0)<tol):
            flag = 1
            break

        if((not np.isnan(dfdx)) and (not np.isinf(dfdx))):
            deltax = -f0/dfdx*alpha
            if(use_maxstep):
                if(np.abs(deltax)>maxstep):
                    deltax = deltax/(np.abs(deltax))*maxstep
        else:
            deltax = (2.0*np.random.randint(0,2)-1.0)*randstep
        
        x = x + deltax

        I = I + 1

    return [x,flag]

def test_newtonsolve_fn(x,fpars):

    y = np.sin(x*x)+fpars

    return y

def test_newtonsolve():

    fpars = 0.75
    fpnt = test_newtonsolve_fn

    for I in range(0,10):
        x0 = np.random.rand()*10.0
        [x,flag] = newtonsolve(fpnt,x0,fpars)
        y = fpnt(x,fpars)
        print("I:{}: x:{:1.3f} y:{:1.4f} flg:{}".format(I,x,y,flag))
    

    return

####################
## Basic Geometry ##
####################

#Vector norm
def vnorm(v):
    n = np.sqrt(np.sum(v*v))
    return n

#Normalize vector
def vnormalize(v):
    n = vnorm(v)
    if(n>0.0):
        v = v/n
    else:
        v = np.zeros(v.shape)
    return v

#2d Vector Hodge Dual
def vdual2d(v):
    v2 = np.zeros((2),dtype=np.float64)
    v2[0] = -v[1]
    v2[1] = v[0]
    return v2

#2d vector cross product
def vcross2d(v1,v2):
    c = v1[0]*v2[1] - v1[1]*v2[0]
    return c

def geom_arg(xy):
    return np.arctan2(xy[1],xy[0])

def involute(phi):
    
    x = cos(phi)+phi*sin(phi)
    y = sin(phi)-phi*cos(phi)
    ri = np.sqrt(x*x+y*y)
    theta = np.arctan2(y,x)

    return [x,y,ri,theta]

def involute_r_fn(phi,fpars):

    [x,y,ri,theta] = involute(phi)
    
    deltar = ri*fpars[0] - fpars[1]

    return deltar

def involute_r(
                r: np.float64,
                rbase: np.float64 = 1.0
                ):

    r = np.float64(r)
    rbase = np.float64(rbase)

    if(r<=rbase):
        theta = np.float64(0.0)
        phi = np.float64(0.0)
        ri = r
    else:
        fpnt = involute_r_fn
        fpars = [rbase,r]
        phi0 = pi/4.0
        [phi,flag] = newtonsolve(fpnt,phi0,fpars,dx=1E-4*rbase,tol=1E-4*rbase,alpha=0.5,maxiter=1000)
        [x,y,ri,theta] = involute(phi)
        x = x*rbase; y=y*rbase; ri=ri*rbase

    return [phi,theta,ri]

def involute_theta_fn(phi,fpars):

    [x,y,ri,theta] = involute(phi)
    
    deltatheta = theta - fpars

    return deltatheta

def involute_theta(
                theta: np.float64,
                rbase: np.float64 = 1.0
                ):

    theta = np.float64(theta)
    rbase = np.float64(rbase)

    fpnt = involute_theta_fn
    fpars = theta
    phi0 = pi/4.0
    [phi,flag] = newtonsolve(fpnt,phi0,fpars,dx=1E-4,tol=1E-4,alpha=0.5,maxiter=1000)
    [x,y,ri,thetai] = involute(phi)
    x = x*rbase; y=y*rbase; ri=ri*rbase

    return [phi,thetai,ri]


def test_involute_r():

    phi = linspace(0,pi/2,500)
    rbase = 1.75
    r = 3.0

    theta = linspace(0,2*pi,500)
    xc0 = cos(theta)*r; yc0 = sin(theta)*r;
    xc1 = cos(theta)*rbase; yc1 = sin(theta)*rbase;

    [xi,yi,ri,theta] = involute(phi)
    xi = xi*rbase; yi=yi*rbase; ri=ri*rbase;

    [phi,theta0,ri0] = involute_r(r,rbase)

    [phi2,theta2,ri2] = involute_theta(pi/4,rbase)

    plt.figure(1)
    plt.plot(xc0,yc0,'r')
    plt.plot(xc1,yc1,'k')
    plt.plot(xi,yi,'b')
    plt.plot(cos(theta0)*ri0,sin(theta0)*ri0,'ro')
    plt.plot(cos(theta2)*ri2,sin(theta2)*ri2,'ro')
    plt.show()
    

    return

def geom_involute_left(rbase,xycenter,theta0,phi1,phi2,N):

    rbase = np.float64(rbase)
    xycenter = np.float64(xycenter)
    theta0 = np.float64(theta0)
    phi1 = np.float64(phi1)
    phi2 = np.float64(phi2)
    N = np.int32(N)
    
    xy = np.zeros((2,N),dtype=np.float64)
    phi = np.linspace(phi1,phi2,N)
    xy[0,:] = cos(phi-theta0) + (phi)*sin(phi-theta0)
    xy[1,:] = sin(phi-theta0) - (phi)*cos(phi-theta0)
    xy[0,:] = rbase*xy[0,:] + xycenter[0]
    xy[1,:] = rbase*xy[1,:] + xycenter[1]

    return xy

def geom_involute_right(rbase, xycenter, theta0, phi1, phi2, N):

    rbase = np.float64(rbase)
    xycenter = np.float64(xycenter)
    theta0 = np.float64(theta0)
    phi1 = np.float64(phi1)
    phi2 = np.float64(phi2)
    N = np.int32(N)

    xy = np.zeros((2,N),dtype=np.float64)
    phi = np.linspace(-phi1,-phi2,N)
    xy[0,:] = cos(phi-theta0) + (phi)*sin(phi-theta0)
    xy[1,:] = sin(phi-theta0) - (phi)*cos(phi-theta0)
    xy[0,:] = rbase*xy[0,:] + xycenter[0]
    xy[1,:] = rbase*xy[1,:] + xycenter[1]
    
    return xy

def delta_involute_left(phi,theta0,rbase):

    dxy = np.zeros(2,dtype=np.float64)
    dxy[0] = cos(phi-theta0)*(phi)*rbase
    dxy[1] = sin(phi-theta0)*(phi)*rbase
    return dxy

def delta_involute_right(phi,theta0,rbase):

    dxy = np.zeros(2,dtype=np.float64)
    dxy[0] = cos(phi-theta0)*(phi)*rbase
    dxy[1] = -sin(phi-theta0)*(phi)*rbase

    return dxy

def geom_circulararc(r,xycenter,theta0,theta1,N):

    r = np.float64(r)
    xycenter = np.float64(xycenter)
    theta0 = np.float64(theta0)
    theta1 = np.float64(theta1)
    N = np.int32(N)

    th = np.linspace(theta0,theta1,N)
    xy = np.zeros((2,N),dtype=np.float64)

    for I in range(0,N):
        xy[0,I] = xycenter[0] + r*cos(th[I])
        xy[1,I] = xycenter[1] + r*sin(th[I])

    return xy

def geom_circle(r,N):

    r = np.float64(r)
    N = np.int32(N)

    th = np.linspace(0,2.0*pi,N-1)
    xy = np.zeros((2,N),dtype=np.float64)

    for I in range(0,N-1):
        xy[0,I] = r*cos(th[I])
        xy[1,I] = r*sin(th[I])

        xy[0,N-1] = xy[0,0]
        xy[1,N-1] = xy[1,0]
        

    return xy

def geom_cubicspline(xy0,dxy0,xy1,dxy1,N,r0=1.5,r1=1.5):

    xy0 = np.float64(xy0)
    dxy0 = np.float64(dxy0)
    xy1 = np.float64(xy1)
    dxy1 = np.float64(dxy1)

    L = xy1-xy0 #two lengthscales

    dxy0s = vnormalize(dxy0)*r0
    dxy1s = vnormalize(dxy1)*r1
    
    if(L[0]<0): 
        dxy0s[0] = -dxy0s[0]
        dxy1s[0] = -dxy1s[0]
    if(L[1]<0): 
        dxy0s[1] = -dxy0s[1]
        dxy1s[1] = -dxy1s[1]

    xy = np.zeros((2,N))
    xys = np.zeros((2,N))

    ax = np.zeros((4,1))
    ay = np.zeros((4,1))
    bx = np.float64([[0],[1],[dxy0s[0]],[dxy1s[0]]])
    by = np.float64([[0],[1],[dxy0s[1]],[dxy1s[1]]])
    
    A = np.array([[1,0,0,0],[1,1,1,1],[0,1,0,0],[0,1,2,3]],dtype=np.float64)
    Ai = np.linalg.inv(A)

    ax = np.matmul(Ai,bx)
    ay = np.matmul(Ai,by)
    
    t = np.linspace(0,1,N)
    o = np.ones(t.shape,dtype=np.float64)

    xys[0,:] = ax[0]*o+ax[1]*t+ax[2]*(t**2)+ax[3]*(t**3)
    xys[1,:] = ay[0]*o+ay[1]*t+ay[2]*(t**2)+ay[3]*(t**3)

    xy[0,:] = xys[0,:]*L[0]+xy0[0]
    xy[1,:] = xys[1,:]*L[1]+xy0[1]
    

    return xy

def test_geom_cubic_spline():

    xy = geom_cubicspline([0,0],[0,55],[1,1],[1,0],100)

    plt.plot(xy[0,:],xy[1,:],'k')
    plt.show()

    return

# def geom_twoline_arc(p1,l1,p2,l2,r,N):

#     ##future work ....

#     return xy

#Oriented two-point arc. Circle joins p1 to p2 with radius r, offset dual to p1p2
def geom_twopoint_arc(p1,p2,r,N):

    p0 = 0.5*(p1+p2)
    l = (p2-p1)
    l = vnormalize(l)
    n = vdual(l)
    b = vnorm(p2-p1)/2.0

    xy = None

    if(b<=r):
        a = sqrt(r*r-b*b)
        pc = p0 + a*n
        th1 = geom_arg(p1-pc)
        th2 = geom_arg(p2-pc)
        if(th2<th1):
            th2 = th2 + 2*pi
        xy = geom_circulararc(r,pc,th1,th2,N)

    return xy


def test_involute():

    th = np.linspace(-pi/4,pi,1000)
    [x,y,ri,theta] = involute(th)
    x2 = cos(th)
    y2 = sin(th)

    plt.plot(x,y,'k')
    plt.plot(x2,y2,'r--')
    plt.axis('equal')
    plt.show()

    return

#########################
## Geometry Operations ##
#########################

def geom_pt_equal(xy0,xy1, dx=0.0005):

    bpe = False

    if(np.abs(xy0[0]-xy1[0])<dx and np.abs(xy0[1]-xy1[1])<dx):
        bpe = True

    return bpe

def geom_translate(xy,xy_trans):

    xy = np.float64(xy)
    xy_trans = np.float64(xy_trans)

    xy2 = np.zeros(xy.shape,dtype=np.float64)
    xy2[0,:] = xy[0,:] + xy_trans[0]
    xy2[1,:] = xy[1,:] + xy_trans[1]

    return xy2

def geom_rotate(xy,theta):

    xy = np.float64(xy)
    theta = np.float64(theta)

    xy2 = np.zeros(xy.shape,dtype=np.float64)

    xy2[0,:] = xy[0,:]*cos(theta) - xy[1,:]*sin(theta)
    xy2[1,:] = xy[0,:]*sin(theta) + xy[1,:]*cos(theta)

    return xy2


#Joins two curves at one of the curves' edges
#Finds which end of each curve is equal to which end of the other
def geom_join_curve(xy1,xy2):


    N1 = xy1.shape[1]
    N2 = xy2.shape[1]

    xyout = None

    p11 = np.squeeze(xy1[:,0])
    p12 = np.squeeze(xy1[:,N1-1])
    p21 = np.squeeze(xy2[:,0])
    p22 = np.squeeze(xy2[:,N2-1])

    if(geom_pt_equal(p11,p21)):
        xyout = np.zeros((2,N1+N2-1),dtype=np.float64)
        xy2r = geom_curve_reverse(xy2)
        xyout[:,0:N2-1] = xy2r[:,1:N2]
        xyout[:,N2-1:N2+N1-1] = xy1[:,:]
    if(geom_pt_equal(p12,p21)):
        xyout = np.zeros((2,N1+N2-1),dtype=np.float64)
        xyout[:,0:N1-1] = xy1[:,0:N1-1]
        xyout[:,N1-1:N1+N2-1] = xy2[:,:]
    if(geom_pt_equal(p11,p22)):
        xyout = np.zeros((2,N1+N2-1),dtype=np.float64)
        xyout[:,0:N2-1:1] = xy2[:,0:N2-1:1]
        xyout[:,N2-1:N2+N1-1:1] = xy1[:,0:N1]
    if(geom_pt_equal(p12,p22)):
        xyout = np.zeros((2,N1+N2-1),dtype=np.float64)
        xy2r = geom_curve_reverse(xy2)
        xyout[:,0:N1-1:1] = xy1[:,0:N1-1:1]
        xyout[:,N1-1:N1+N2-1] = xy2r[:,:]
    
    return xyout

def geom_curve_reverse(xy):

    xy2 = np.zeros(xy.shape)
    xy2[:,:] = xy[:,::-1]
    return xy2

def test_geom_join_curve():

    xy0 = geom_circulararc(1,[0,0],-pi/2,pi/2,10);
    xy1 = geom_circulararc(1,[0,2],pi/2,3*pi/2,11);

    print(np.transpose(xy0))
    print(np.transpose(xy1))

    #xy0 = geom_curve_reverse(xy0)
    xy1 = geom_curve_reverse(xy1)

    xyc = geom_join_curve(xy0,xy1)

    plt.plot(xyc[0,:],xyc[1,:],'k')
    plt.show()


    return;


def geom_lineseg(xy1,xy2):

    xy1 = np.float64(xy1)
    xy2 = np.float64(xy2)

    xy = np.zeros((2,2),dtype=np.float64)
    xy[0,0] = xy1[0]; xy[1,0] = xy1[1]
    xy[0,1] = xy2[0]; xy[1,1] = xy2[1]

    return xy


def geom_closecontour(xy):

    pt1 = np.squeeze(xy[:,0])
    pt2 = np.squeeze(xy[:,xy.shape[1]-1])
    if(not geom_pt_equal(pt1,pt2)):
        N = xy.shape[1]+1
        xy2 = np.zeros((2,N),dtype=np.float64)
        xy2[:,0:N-1] = xy[:,:]
        xy2[:,N-1] = xy[:,0]
    else:
        xy2 = np.copy(xy)

    return xy2

###################
## Gear Routines ##
###################

#Ref Machinery's Handbook,28, pp 2043.
#This references ANSI B6.1-1968 R1974
def spurgear_ANSI_proportions(**kwargs):

    P = 48
    N = 18
    phi = 20

    add_clearance = 0.000
    add_outerclearance = 0.000
    add_sideclearance = 0.000

    if("pressure_angle" in kwargs):
        phi = np.float64(kwargs["pressure_angle"])
    if("phi" in kwargs):
        phi = np.float64(kwargs["phi"])

    if("num_teeth" in kwargs):
        N = np.float64(np.int32(kwargs["num_teeth"]))
    if("N" in kwargs):
        N = np.float64(np.int32(kwargs["N"]))

    if("diametral_pitch" in kwargs):
        P = np.float64(kwargs["diametral_pitch"])
        Dp = N/P
    if("P" in kwargs):
        P = np.float64(kwargs["P"])
        Dp = N/P

    if("pitch_diameter" in kwargs):
        Dp = np.float64(kwargs["pitch_diameter"])
        P = N/Dp
    if("Dp" in kwargs):
        Dp = np.float64(kwargs["Dp"])
        P = N/Dp

    
    a = 1.000/P         #addendum
    b = 1.200/P + 0.002 #dedendum
    c = 0.200/P + 0.002 #clearance
    f = 0.300/P         #fillet radius

    if("addendum" in kwargs):
        a = np.float64(kwargs["addendum"])
    if("dedendum" in kwargs):
        b = np.float64(kwargs["dedendum"])
    if("clearance" in kwargs):
        c = np.float64(kwargs["clearance"])
    if("fillet_radius" in kwargs):
        f = np.float64(kwargs["fillet_radius"])
    if("add_clearance" in kwargs):
        add_clearance = np.float64(kwargs["add_clearance"])
    if("add_outerclearance" in kwargs):
        add_outerclearance = np.float64(kwargs["add_outerclearance"])
    if("add_sideclearance" in kwargs):
        add_sideclearance = np.float64(kwargs["add_sideclearance"])

    Db = Dp*cos(pi/180.0*phi)
    Do = Dp + 2*a - 2*add_outerclearance
    Di = Dp - 2*b - 2*c - 2*add_clearance

    t = pi/2*Dp/N - 2*add_sideclearance

    Cp = pi*Dp/N

    gearpars = dict()

    gearpars["pressure_angle"] = phi
    gearpars["num_teeth"] = N
    gearpars["diametral_pitch"] = P
    gearpars["pitch_diameter"] = Dp
    gearpars["base_diameter"] = Db
    gearpars["outer_diameter"] = Do
    gearpars["inner_diameter"] = Di
    gearpars["circular_pitch"] = Cp
    gearpars["addendum"] = a
    gearpars["dedendum"] = b
    gearpars["clearance"] = c
    gearpars["fillet_radius"] = f
    gearpars["tooth_thickness"] = t
    gearpars["add_clearance"] = add_clearance
    gearpars["add_outerclearance"] = add_outerclearance
    gearpars["add_sideclearance"] = add_sideclearance
    
    return gearpars

#Must include:
# Npts - number of points to use per curved arc
# diametral_pitch, P, pitch_diameter, Dp
# phi, pressure_angle
# num_teeth, N
def spurgear_profile(**kwargs):
    
    gearpars = spurgear_ANSI_proportions(**kwargs)

    Npts = 10
    if("Npts" in kwargs):
        Npts = kwargs["Npts"] #number of points to use per curved arc

    phi = gearpars["pressure_angle"]
    N = gearpars["num_teeth"]
    P = gearpars["diametral_pitch"]
    Dp = gearpars["pitch_diameter"]
    Db = gearpars["base_diameter"]
    Do = gearpars["outer_diameter"]
    Di = gearpars["inner_diameter"]
    Cp = gearpars["circular_pitch"]
    a = gearpars["addendum"]
    b = gearpars["dedendum"]
    c = gearpars["clearance"]
    f = gearpars["fillet_radius"]
    add_clearance = gearpars["add_clearance"]
    add_outerclearance = gearpars["add_outerclearance"]
    add_sideclearance = gearpars["add_sideclearance"]

    Rp = Dp/2.0
    Ro = Do/2.0
    Rb = Db/2.0
    Ri = Di/2.0

    #calculate involute offset angle at pitch radius
    thw = 2*pi/N    #angular width of tooth profile
    [phid,thetad,rd] = involute_r(Rp,Rb) #theta offset for involtue placement

    circp = geom_circle(Rp,360)
    circi = geom_circle(Ri,360)
    circo = geom_circle(Ro,360)
    circb = geom_circle(Rb,360)

    if(Rb<Ri+f):
        #Scenario1: base circle is inside inner circle
        [phid3,thetad3,rd3] = involute_r(Ri+f,Rb)
        [phid4,thetad4,rd4] = involute_r(Ro,Rb)
        
        theta1 = -thw/2.0
        thetaoff1 = -thw/4.0-thetad+add_sideclearance/Rp
        theta3 = thetaoff1+thetad3
        theta2 = theta3 - f/Ri
        theta4 = thetaoff1 + thetad4

        xy2 = Ri*np.float64([cos(theta2),sin(theta2)])
        xy3 = rd3*np.float64([cos(theta3),sin(theta3)])
        dxy2 = np.float64([-sin(theta2),cos(theta2)])
        dxy3 = delta_involute_left(phid3,thetaoff1-thetad3,Rb)

        cxy1 = geom_circulararc(Ri,[0,0],theta1,theta2,Npts)
        cxy2 = geom_cubicspline(xy2,dxy2,xy3,dxy3,Npts)
        cxy3 = geom_involute_left(Rb,[0,0],-thetaoff1,phid3,phid4,Npts)
        cxy4 = geom_circulararc(Ro,[0,0],theta4,-theta4,Npts)
        cxy5 = np.copy(cxy3); cxy5[1,:] = -cxy3[1,:] #why make things difficult?
        cxy6 = np.copy(cxy2); cxy6[1,:] = -cxy2[1,:] #why make things difficult?
        cxy7 = np.copy(cxy1); cxy7[1,:] = -cxy1[1,:] #why make things difficult?

        prof = geom_join_curve(cxy1,cxy2)
        prof = geom_join_curve(prof,cxy3)
        prof = geom_join_curve(prof,cxy4)
        prof = geom_join_curve(prof,cxy5)
        prof = geom_join_curve(prof,cxy6)
        prof = geom_join_curve(prof,cxy7)
        

        #debug profile
        # plt.plot(circp[0,:],circp[1,:],'r--')
        # plt.plot(circi[0,:],circi[1,:],'k--')
        # plt.plot(circb[0,:],circb[1,:],'b--')
        # plt.plot(circo[0,:],circo[1,:],'k--')

        # plt.plot(cxy1[0,:],cxy1[1,:],'r')
        # plt.plot(cxy2[0,:],cxy2[1,:],'r')
        # plt.plot(cxy3[0,:],cxy3[1,:],'r')
        # plt.plot(cxy4[0,:],cxy4[1,:],'r')
        # plt.plot(cxy5[0,:],cxy5[1,:],'r')
        # plt.plot(cxy6[0,:],cxy6[1,:],'r')
        # plt.plot(cxy7[0,:],cxy7[1,:],'r')

        # plt.plot(prof[0,:],prof[1,:],'g--')
        # plt.plot(xy2[0],xy2[1],'rd')
        # plt.plot(xy3[0],xy3[1],'rd')

        # plt.axis("equal")
        # plt.show()

    elif(Rb>Ri+f):
        #Scenario2 base circle is outside of inner circle


        
        
        theta1 = -thw/2.0
        thetaoff1 = -thw/4.0-thetad+add_sideclearance/Rp

        [phid5,thetad5,rd5] = involute_r(Ro,Rb)

        phid4 = 0; theta4 = thetaoff1; r4 = Rb
        theta3 = theta4; r3 = Ri+f
        theta2 = theta3 - f/Ri; r2 = Ri
        theta5 = thetaoff1+thetad5

        #theta3 = thetaoff1+thetad3
        #theta2 = theta3 - f/Ri
        #theta4 = thetaoff1 + thetad4

        xy2 = r2*np.float64([cos(theta2),sin(theta2)])
        xy3 = r3*np.float64([cos(theta3),sin(theta3)])
        dxy2 = np.float64([-sin(theta2),cos(theta2)])
        dxy3 = np.float64([cos(theta3),sin(theta3)])

        xy4 = r4*np.float64([cos(theta4),sin(theta4)])

        cxy1 = geom_circulararc(Ri,[0,0],theta1,theta2,Npts)
        cxy2 = geom_cubicspline(xy2,dxy2,xy3,dxy3,Npts)
        cxy3 = np.float64([[xy3[0],xy4[0]],[xy3[1],xy4[1]]])
        cxy4 = geom_involute_left(Rb,[0,0],-thetaoff1,0,phid5,Npts)
        cxy5 = geom_circulararc(Ro,[0,0],theta5,-theta5,Npts)
        ### reflect
        cxy6 = np.copy(cxy4); cxy6[1,:] = -cxy4[1,:]
        cxy7 = np.copy(cxy3); cxy7[1,:] = -cxy3[1,:]
        cxy8 = np.copy(cxy2); cxy8[1,:] = -cxy2[1,:]
        cxy9 = np.copy(cxy1); cxy9[1,:] = -cxy1[1,:]

        prof = geom_join_curve(cxy1,cxy2)
        prof = geom_join_curve(prof,cxy3)
        prof = geom_join_curve(prof,cxy4)
        prof = geom_join_curve(prof,cxy5)
        prof = geom_join_curve(prof,cxy6)
        prof = geom_join_curve(prof,cxy7)
        prof = geom_join_curve(prof,cxy8)
        prof = geom_join_curve(prof,cxy9)

        #debug profile
        # plt.plot(circp[0,:],circp[1,:],'r--')
        # plt.plot(circi[0,:],circi[1,:],'k--')
        # plt.plot(circb[0,:],circb[1,:],'b--')
        # plt.plot(circo[0,:],circo[1,:],'k--')

        # plt.plot(cxy1[0,:],cxy1[1,:],'r')
        # plt.plot(cxy2[0,:],cxy2[1,:],'r')
        # plt.plot(cxy3[0,:],cxy3[1,:],'r')
        # plt.plot(cxy4[0,:],cxy4[1,:],'r')
        # plt.plot(cxy5[0,:],cxy5[1,:],'r')
        # plt.plot(cxy6[0,:],cxy6[1,:],'r')
        # plt.plot(cxy7[0,:],cxy7[1,:],'r')
        # plt.plot(cxy8[0,:],cxy8[1,:],'r')
        # plt.plot(cxy9[0,:],cxy9[1,:],'r')

        # plt.plot(prof[0,:],prof[1,:],'g--')
        # plt.plot(xy2[0],xy2[1],'rd')
        # plt.plot(xy3[0],xy3[1],'rd')

        # plt.axis("equal")
        # plt.show()
        

        pass

    gearprof = np.copy(prof)
    for I in range(0,int(N)-1):
        prof = geom_rotate(prof,thw)
        #plt.plot(prof[0,:],prof[1,:])
        gearprof = geom_join_curve(gearprof,prof)
    
    # plt.plot(gearprof[0,:],gearprof[1,:],'k')
    # plt.axis("equal")
    # plt.show()


    return [gearprof, gearpars]

def test_genpitchseries(**kwargs):
    diametral_pitch = kwargs["diametral_pitch"]
    N = kwargs["N"]

    for I in range(0,len(diametral_pitch)):
        for J in range(0,len(N)):
            dd = diametral_pitch[I]
            NN = N[J]

            [gearprof1, gearpars1] = spurgear_profile(Npts = 30, diametral_pitch=dd, N=NN, phi=20,add_sideclearance=0.003,add_outerclearance=0.003)
            gname = "gear_{:d}_{:d}".format(dd,NN)
            fname = "{}_v1.scad".format(gname)
            save_closedcontour_scad(gearprof1,fname,gname)


def test_spurgear_profile():

    [gearprof1, gearpars1] = spurgear_profile(Npts = 10, diametral_pitch=36, N=72, phi=20,add_sideclearance=0.003,add_outerclearance=0.003)
    [gearprof2, gearpars2] = spurgear_profile(Npts = 10, diametral_pitch=36, N=54, phi=20,add_sideclearance=0.003,add_outerclearance=0.003)

    print("Gearpars1:")
    print(gearpars1)
    print("Gearpars2:")
    print(gearpars2)

    off = 0.5*(gearpars1["pitch_diameter"]+gearpars2["pitch_diameter"])
    thw = 2*pi/gearpars2["num_teeth"]
    dth = -0.285
    GR = gearpars1["num_teeth"]/gearpars2["num_teeth"]

    gearprof2 = geom_rotate(gearprof2,thw/2.0+dth)
    gearprof1 = geom_rotate(gearprof1,-dth/GR)

    save_closedcontour_scad(gearprof1,"gear_36_72_v1.scad","gear_36_72")
    save_closedcontour_scad(gearprof2,"gear_36_54_v1.scad","gear_36_54")

    plt.figure(1)
    plt.plot(gearprof1[0,:],gearprof1[1,:],'b',linewidth=1)
    plt.plot(gearprof1[0,0],gearprof1[1,0],'bo')
    plt.plot(gearprof2[0,:]+off,gearprof2[1,:],'r',linewidth=1)
    plt.axis("equal")
    plt.show()
    
## racks

#Ref Machinery's Handbook,28, pp 2043.
#This references ANSI B6.1-1968 R1974
def linearrack_ANSI_proportions(**kwargs):

    P = 48
    N = 18
    phi = 20

    add_clearance = 0.000
    add_outerclearance = 0.000
    add_sideclearance = 0.000

    if("pressure_angle" in kwargs):
        phi = np.float64(kwargs["pressure_angle"])
    if("phi" in kwargs):
        phi = np.float64(kwargs["phi"])

    if("num_teeth" in kwargs):
        N = np.float64(np.int32(kwargs["num_teeth"]))
    if("N" in kwargs):
        N = np.float64(np.int32(kwargs["N"]))

    if("diametral_pitch" in kwargs):
        P = np.float64(kwargs["diametral_pitch"])
        Cp = pi/P
    if("P" in kwargs):
        P = np.float64(kwargs["P"])
        Cp = pi/P

    if("circular_pitch" in kwargs):
        Cp = np.float64(kwargs["circular_pitch"])
        P = pi/Cp
    if("Cp" in kwargs):
        Cp = np.float64(kwargs["Cp"])
        P = pi/Cp

    
    a = 1.000/P         #addendum
    b = 1.200/P + 0.002 #dedendum
    c = 0.200/P + 0.002 #clearance
    f = 0.300/P         #fillet radius

    if("addendum" in kwargs):
        a = np.float64(kwargs["addendum"])
    if("dedendum" in kwargs):
        b = np.float64(kwargs["dedendum"])
    if("clearance" in kwargs):
        c = np.float64(kwargs["clearance"])
    if("fillet_radius" in kwargs):
        f = np.float64(kwargs["fillet_radius"])
    if("add_clearance" in kwargs):
        add_clearance = np.float64(kwargs["add_clearance"])
    if("add_outerclearance" in kwargs):
        add_outerclearance = np.float64(kwargs["add_outerclearance"])
    if("add_sideclearance" in kwargs):
        add_sideclearance = np.float64(kwargs["add_sideclearance"])


    Ho = a - add_outerclearance
    Hi = -b - c - add_clearance
    t = Cp/2 - 2*add_sideclearance

    Hb = 1.50*Hi
    L = N*Cp

    if("bottom_height" in kwargs):
        Hb = np.float64(kwargs["bottom_height"])
    if("Hb" in kwargs):
        Hb = np.float64(kwargs["Hb"])

    rackpars = dict()

    rackpars["pressure_angle"] = phi
    rackpars["num_teeth"] = N
    rackpars["diametral_pitch"] = P
    rackpars["circular_pitch"] = Cp

    rackpars["outer_height"] = Ho
    rackpars["pitch_height"] = 0.0
    rackpars["inner_height"] = Hi
    rackpars["bottom_height"] = Hb 
    rackpars["length"] = L
    
    rackpars["addendum"] = a
    rackpars["dedendum"] = b
    rackpars["clearance"] = c
    rackpars["fillet_radius"] = f
    rackpars["tooth_thickness"] = t
    rackpars["add_clearance"] = add_clearance
    rackpars["add_outerclearance"] = add_outerclearance
    rackpars["add_sideclearance"] = add_sideclearance
    
    return rackpars

def linearrack_profile(**kwargs):

    rackpars = linearrack_ANSI_proportions(**kwargs)
    rackprofile = None

    Npts = 10
    if("Npts" in kwargs):
        Npts = kwargs["Npts"]

    phi = rackpars["pressure_angle"] 
    N = rackpars["num_teeth"]
    P = rackpars["diametral_pitch"]
    Cp = rackpars["circular_pitch"]

    Ho = rackpars["outer_height"]
    Hp = rackpars["pitch_height"]
    Hi = rackpars["inner_height"]
    Hb = rackpars["bottom_height"]
    L = rackpars["length"]
    
    a = rackpars["addendum"]
    b = rackpars["dedendum"]
    c = rackpars["clearance"]
    f = rackpars["fillet_radius"]
    t = rackpars["tooth_thickness"]
    add_clearance = rackpars["add_clearance"]
    add_outerclearance = rackpars["add_outerclearance"]
    add_sideclearance = rackpars["add_sideclearance"]

    x1 = 0
    x2 = Cp/4.0 - add_sideclearance - sin(pi/180*phi)*a
    x3 = Cp/4.0 - add_sideclearance + sin(pi/180*phi)*b -sin(pi/180*phi)*f
    x4 = Cp/4.0 - add_sideclearance + sin(pi/180*phi)*b + f
    x5 = Cp - x4
    x6 = Cp - x3
    x7 = Cp - x2
    x8 = Cp

    y1 = Ho
    y2 = Ho
    y3 = Hi + cos(pi/180*phi)*f
    y4 = Hi
    y5 = y4
    y6 = y3
    y7 = y2
    y8 = y1

    d3 = np.float64([x3-x2,y3-y2])
    d6 = np.float64([x7-x6,y7-y6])

    cxy1 = geom_lineseg([x1,y1],[x2,y2])
    cxy2 = geom_lineseg([x2,y2],[x3,y3])
    cxy3 = geom_cubicspline(np.float64([x3,y3]),d3,np.float64([x4,y4]),np.float64([1,0]),Npts)
    cxy4 = geom_lineseg([x4,y4],[x5,y5])
    cxy5 = geom_cubicspline([x5,y5],[1,0],[x6,y6],d6,Npts)
    cxy6 = geom_lineseg([x6,y6],[x7,y7])
    cxy7 = geom_lineseg([x7,y7],[x8,y8])
    
    prof = np.copy(cxy1)
    prof = geom_join_curve(prof,cxy2)
    prof = geom_join_curve(prof,cxy3)
    prof = geom_join_curve(prof,cxy4)
    prof = geom_join_curve(prof,cxy5)
    prof = geom_join_curve(prof,cxy6)
    prof = geom_join_curve(prof,cxy7)

    rackprofile = np.copy(prof)
    for I in range(0,int(N)-1):
        prof = geom_translate(prof,[Cp,0])
        rackprofile = geom_join_curve(rackprofile,prof)

    bxy0 = geom_lineseg([0,Ho],[0,Hb])
    bxy1 = geom_lineseg([L,Ho],[L,Hb])
    bxy2 = geom_lineseg([0,Hb],[L,Hb])
    
    rackprofile = geom_join_curve(rackprofile,bxy0)
    rackprofile = geom_join_curve(rackprofile,bxy1)
    rackprofile = geom_join_curve(rackprofile,bxy2)

    rackprofile = geom_closecontour(rackprofile)

    return [rackprofile,kwargs]

def test_linearrack_profile():

    [rackprofile, rackpars] = linearrack_profile(diametral_pitch=36,N=10,phi=20,Hb=-0.25)
    print(rackpars)
    save_closedcontour_scad(rackprofile,"rack36.scad","rack36")

    return

def save_closedcontour_scad(contour, fname, modulename="mycontour"):

    try:
        fp = open(fname,"w+")
    except:
        printf("{} could not be opened for writing.\n".format(fname))
        return

    contour = np.float64(contour)

    fp.writelines("// openscad\n\n")
    fp.writelines("module {}()\n".format(modulename))
    fp.writelines("{\n")

    N = contour.shape[1]

    fp.writelines("pts = [")
    for I in range(0,N):
        if(I<N):
            fp.writelines("\t[{:1.6g},{:1.6g}],\n".format(contour[0,I],contour[1,I]))
        else:
            fp.writelines("\t[{:1.6g},{:1.6g}]".format(contour[0,I],contour[1,I]))
    fp.writelines("];\n\n")
    fp.writelines("polygon(points=pts,convexity=200);\n")
    fp.writelines("}\n\n")

    fp.writelines("linear_extrude(height=0.1,convexity=200)\n")
    fp.writelines("{}();\n\n".format(modulename))

    fp.close()

    return

## internal gears

###############
## Run Tests ##
###############

if(__name__=="__main__"):

    #test_involute()
    #test_newtonsolve()
    #test_involute_r()
    #test_geom_join_curve()
    #test_geom_cubic_spline()
    #test_spurgear_profile()

    test_genpitchseries(diametral_pitch=[20],N=[20,30,40,60])
    #test_linearrack_profile()

    pass