for i in range(0,numsteps):   
    intersects = circcirc(crankpoints[i,:],linke,fp2,linkd)
    joints4[i,:] = intersects[0,:]
    joints3[i,:] = intersects[1,:]

plot(joints3[:,0],joints3[:,1],color = 'g',linewidth=0.5)
plot(joints4[:,0],joints4[:,1],color = 'g',linewidth=0.5)


#Now we can get joints5. The rigid triangles with linkd
#create a bent link for which the coupler is joint5.
#In fact, because fp2 is fixed, it traces an arc like
#joint3, but phase-shifted by (pi-2.97)
joints5 = np.zeros((numsteps,2),float)
for i in range(0,numsteps): 
    joints5[i,:] = coupler(joints3[i,:],fp2,linkd,gammad)
    
plot(joints5[:,0],joints5[:,1],color = 'g',linewidth=0.5)


#Finally we get the foot location, again using circcirc().
#Going from joint5 to joint4, we want the intersection on RHS
foots = np.zeros((numsteps,2),float)
for i in range(0,numsteps):   
    intersections = circcirc(joints5[i,:],linkf,joints4[i,:],linkf)
    foots[i,:] = intersections[1,:]

# Plot foot locations, with big dot at start
# Note that it's quite fast at top of step.
plot(foots[:,0],foots[:,1],'.')
plot(foots[0,0],foots[0,1],'o')
# as input crank goes from thetastart to thetaend
joints23 = arcpoints(joint12,l2,thetastart,thetaend,numsteps)
# Plot them, with dot at start and square at end:
plot(joints23[:,0],joints23[:,1],color = 'g',linewidth=0.5)
plot(joints23[0,0],joints23[0,1],'o')
plot(joints23[numsteps-1,0],joints23[numsteps-1,1],'s')


# Compute all the locations of joint34 and joint36, treating
# joint36 as a coupler point
joints34 = np.zeros((numsteps,2),float)
joints36 = np.zeros((numsteps,2),float)
for i in range(0,numsteps):   
    intersections = circcirc(joints23[i,:],l3,joint14,l4)
    joints34[i,:] = intersections[assembly,:]
    joints36[i,:] = coupler(joints23[i,:],joints34[i,:],c3,gamma3)


# If desired, plot joint34 locations, with dot at start and square at end:
#plot(joints34[:,0],joints34[:,1],color = 'g',linewidth=0.5)
#plot(joints34[0,0],joints34[0,1],'o')
#plot(joints34[numsteps-1,0],joints34[numsteps-1,1],'s')   
    
# Plot joint36 locations, with dot at start and square at end:
plot(joints36[:,0],joints36[:,1],color = 'r',linewidth=0.5)
plot(joints36[0,0],joints36[0,1],'o')
plot(joints36[numsteps-1,0],joints36[numsteps-1,1],'s')   


#### Repeat the process above for the output of the mechanism
###  starting with locations of joint36 which we now know.
else:
    print('Hmmm, neither solution matches the input point...')

# Use Arcpoints() to get the positions of joint23,
# i.e., of the joint that connects links 2 and 3, where
# link2 is the input crank.
joints23 = arcpoints(initjoints[0,:],l2,thetastart,thetaend,numsteps)

# For each joint23 location, find the corresponding
# joint34 and coupler locations
joints34 = np.zeros((numsteps,2),float)
couplerpts = np.zeros((numsteps,2),float)
for i in range(0,numsteps):   
    intersects = circcirc(joints23[i,:],l3,initjoints[3,:],l4)
    joints34[i,:] = intersects[assembly,:]
    couplerpts[i,:] = coupler(joints34[i,:],joints23[i,:],lc,np.pi+gammac)


#Plot the various points - may want to modify this depending
# on what effects we are looking for.
# Currently plots a fat dot at start of angle range and a
# square at end of angle range.
# numpy.plot() works conveniently for lists of 'x' and 'y' values.
plot(joints23[:,0],joints23[:,1],color = 'g',linewidth=0.5)
plot(joints23[0,0],joints23[0,1],'o')
plot(joints23[numsteps-1,0],joints23[numsteps-1,1],'s')
plot(joints34[:,0],joints34[:,1],color = 'r',linewidth=0.5)
plot(couplerpts[:,0],couplerpts[:,1],color = 'k',linewidth=0.5)
plot(couplerpts[:,0],couplerpts[:,1],'*')
plot(couplerpts[0,0],couplerpts[0,1],'o')
plot(couplerpts[numsteps-1,0],couplerpts[numsteps-1,1],'s')