# plot3dp.py3
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from math import *  # sin, cos, pi
from numpy import array
from numpy import linalg

# from test_rotate.py3
def vecmul(a, p): # return pp
  pp = [0.0, 0.0, 0.0]
  for i in range(3):
    for j in range(3):
      pp[i]= pp[i] + a[i][j] * p[j];

    # end j
      
  # end i
  return pp
# end vecmul

print("plot3dp.py3 running")
ax = pi/4.0
print("ax=",ax)
ay = pi/4.0
print("ay=",ay)
az = pi/4.0
print("az=",az)
# The rotation matrix by angle ax about the X axis is
xm = array([[1.0, 0.0, 0.0],[0.0, cos(ax), -sin(ax)],[0.0, sin(ax), cos(ax)]])
print("xm=",xm)
# p2 = vecmul(xm,p1)
# print("p2=",p2)
# The rotation matrix by angle ay about the Y axis is
ym = array([[cos(ay), 0.0, sin(ay)],[0.0, 1.0, 0.0],[-sin(ay), 0.0, cos(ay)]])
print("ym=",ym)
# The rotation matrix by angle az about the Z axis is
zm = array([[cos(az), -sin(az), 0.0],[sin(az), cos(az), 0.0],[0.0, 0.0, 1.0]])
print("zm=",zm)
zup = [8.0]    # z size of box 16,16,16
xup = [8.0]    # x              8, 8, 8
yup = [8.0]    # y  will have rotation about center 0,0,0
zdn = [-8.0]   # z size of box 16,16,16
xdn = [-8.0]   # x             -8,-8,-8
ydn = [-8.0]   # y
xbc = [-8,4]   # x body center
ybc = [0,0]    # y
zbc = [0,0]    #
zbh = [-1,1]   # z body height
xbh = [0,0]    # x
ybh = [0,0]    # y
ywc = [-7,7]   # y wing center
xwc = [0,0]    # x
zwc = [0,0]    # z
ztc = [0,2]    # z tail center
xtc = [-7,-7]  # x
ytc = [0,0]    # y
bc0 = array([xbc[0], ybc[0], zbc[0]]) # first
print("bc0=",bc0)
bc1 = array([xbc[1], ybc[1], zbc[1]])
print("bc1=",bc1)
print(" ")
bh0 = array([xbh[0], ybh[0], zbh[0]])
print("bh0=",bh0)
bh1 = array([xbh[1], ybh[1], zbh[1]])
print("bh1=",bh1)
print(" ")
wc0 = array([xwc[0],ywc[0], zwc[0]])
print("wc0=",wc0)
wc1 = array([xwc[1],ywc[1], zwc[1]])
print("wc1=",wc1)
print(" ")
tc0 = array([xtc[0],ytc[0],ztc[0]])
print("tc0=",tc0)
tc1 = array([xtc[1],ytc[1],ztc[1]])
print("tc1=",tc1)
print(" ")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(xup,yup,zup,color="white")
ax.plot(xdn,ydn,zdn,color="white")
ax.plot(xbc,ybc,zbc,color="green",label="x body")
ax.plot(xbh,ybh,zbh,color="green")
ax.plot(xwc,ywc,zwc,color="red",label="y wing")
ax.plot(xtc,ytc,ztc,color="blue",label="z tail")
plt.title("plot3dp.py3 click X 8 for rool, 8 for pitch, 8 for yaw")
plt.legend()
plt.show() # need to click X
# xang, yang, zang  angle to rotate, one at a time, may repeat
xang = pi/2.0
yang = pi/2.0
zang = pi/2.0
for ix in range(8):
  # rotate about x roll   y,z changes
  print("rotate around x axis")
  bc2 = vecmul(xm,bc0)
  bc0 = bc2 # for next iteration
  print("bc2=",bc2)
  bc3 = vecmul(xm,bc1)
  bc1 = bc3
  print("bc3=",bc3)
  print(" ")
  bh2 = vecmul(xm,bh0)
  bh0 = bh2
  print("bh2=",bh2)
  bh3 = vecmul(xm,bh1)
  bh1 = bh3
  print("bh3=",bh3)
  print(" ")
  wc2 = vecmul(xm,wc0)
  wc0 = wc2
  print("wc2=",wc2)
  wc3 = vecmul(xm,wc1)
  wc1 = wc3
  print("wc3=",wc3)
  print(" ")
  tc2 = vecmul(xm,tc0)
  tc0 = tc2
  print("tc2=",tc2)
  tc3 = vecmul(xm,tc1)
  tc1 = tc3
  print("tc3=",tc3)
  print(" ")
  xbc[0] = bc2[0]
  ybc[0] = bc2[1]
  zbc[0] = bc2[2]
  xbc[1] = bc3[0]
  ybc[1] = bc3[1]
  zbc[1] = bc3[2]
  xbh[0] = bh2[0]
  ybh[0] = bh2[1]
  zbh[0] = bh2[2]
  xbh[1] = bh3[0]
  ybh[1] = bh3[1]
  zbh[1] = bh3[2]
  xwc[0] = wc2[0]
  ywc[0] = wc2[1]
  zwc[0] = wc2[2]
  xwc[1] = wc3[0]
  ywc[1] = wc3[1]
  zwc[1] = wc3[2]
  xtc[0] = tc2[0]
  ytc[0] = tc2[1]
  ztc[0] = tc2[2]
  xtc[1] = tc3[0]
  ytc[1] = tc3[1]
  ztc[1] = tc3[2]
  fig = plt.figure()
  ax = fig.add_subplot(111, projection='3d')
  ax.plot(xup,yup,zup,color="white")
  ax.plot(xdn,ydn,zdn,color="white")
  ax.plot(xbc,ybc,zbc,color="green",label="x body")
  ax.plot(xbh,ybh,zbh,color="green")
  ax.plot(xwc,ywc,zwc,color="red",label="y wing")
  ax.plot(xtc,ytc,ztc,color="blue",label="z tail")
  plt.title("plot3dp.py3 click X 8 for rool, 8 for pitch, 8 for yaw")
  plt.legend()
  plt.show() # need to click X

# end ix

# rotate about y pitch  x,z changes
print("rotate around y pitch")
for iy in range(8):
  # rotate about y pitch   x,z changes
  print("rotate around y axis")
  bc2 = vecmul(ym,bc0)
  bc0 = bc2 # for next iteration
  print("bc2=",bc2)
  bc3 = vecmul(ym,bc1)
  bc1 = bc3
  print("bc3=",bc3)
  print(" ")
  bh2 = vecmul(ym,bh0)
  bh0 = bh2
  print("bh2=",bh2)
  bh3 = vecmul(ym,bh1)
  bh1 = bh3
  print("bh3=",bh3)
  print(" ")
  wc2 = vecmul(ym,wc0)
  wc0 = wc2
  print("wc2=",wc2)
  wc3 = vecmul(ym,wc1)
  wc1 = wc3
  print("wc3=",wc3)
  print(" ")
  tc2 = vecmul(ym,tc0)
  tc0 = tc2
  print("tc2=",tc2)
  tc3 = vecmul(ym,tc1)
  tc1 = tc3
  print("tc3=",tc3)
  print(" ")
  xbc[0] = bc2[0]
  ybc[0] = bc2[1]
  zbc[0] = bc2[2]
  xbc[1] = bc3[0]
  ybc[1] = bc3[1]
  zbc[1] = bc3[2]
  xbh[0] = bh2[0]
  ybh[0] = bh2[1]
  zbh[0] = bh2[2]
  xbh[1] = bh3[0]
  ybh[1] = bh3[1]
  zbh[1] = bh3[2]
  xwc[0] = wc2[0]
  ywc[0] = wc2[1]
  zwc[0] = wc2[2]
  xwc[1] = wc3[0]
  ywc[1] = wc3[1]
  zwc[1] = wc3[2]
  xtc[0] = tc2[0]
  ytc[0] = tc2[1]
  ztc[0] = tc2[2]
  xtc[1] = tc3[0]
  ytc[1] = tc3[1]
  ztc[1] = tc3[2]
  fig = plt.figure()
  ax = fig.add_subplot(111, projection='3d')
  ax.plot(xup,yup,zup,color="white")
  ax.plot(xdn,ydn,zdn,color="white")
  ax.plot(xbc,ybc,zbc,color="green",label="x body")
  ax.plot(xbh,ybh,zbh,color="green")
  ax.plot(xwc,ywc,zwc,color="red",label="y wing")
  ax.plot(xtc,ytc,ztc,color="blue",label="z tail")
  plt.title("plot3dp.py3 click X 8 for rool, 8 for pitch, 8 for yaw")
  plt.legend()
  plt.show() # need to click X

# end iy

# rotate about z yaw    x,y changes
print("rotate about z yaw")
for iz in range(8):
  # rotate about z yaw   x,y changes
  print("rotate around x axis")
  bc2 = vecmul(zm,bc0)
  bc0 = bc2 # for next iteration
  print("bc2=",bc2)
  bc3 = vecmul(zm,bc1)
  bc1 = bc3
  print("bc3=",bc3)
  print(" ")
  bh2 = vecmul(zm,bh0)
  bh0 = bh2
  print("bh2=",bh2)
  bh3 = vecmul(zm,bh1)
  bh1 = bh3
  print("bh3=",bh3)
  print(" ")
  wc2 = vecmul(zm,wc0)
  wc0 = wc2
  print("wc2=",wc2)
  wc3 = vecmul(zm,wc1)
  wc1 = wc3
  print("wc3=",wc3)
  print(" ")
  tc2 = vecmul(zm,tc0)
  tc0 = tc2
  print("tc2=",tc2)
  tc3 = vecmul(zm,tc1)
  tc1 = tc3
  print("tc3=",tc3)
  print(" ")
  xbc[0] = bc2[0]
  ybc[0] = bc2[1]
  zbc[0] = bc2[2]
  xbc[1] = bc3[0]
  ybc[1] = bc3[1]
  zbc[1] = bc3[2]
  xbh[0] = bh2[0]
  ybh[0] = bh2[1]
  zbh[0] = bh2[2]
  xbh[1] = bh3[0]
  ybh[1] = bh3[1]
  zbh[1] = bh3[2]
  xwc[0] = wc2[0]
  ywc[0] = wc2[1]
  zwc[0] = wc2[2]
  xwc[1] = wc3[0]
  ywc[1] = wc3[1]
  zwc[1] = wc3[2]
  xtc[0] = tc2[0]
  ytc[0] = tc2[1]
  ztc[0] = tc2[2]
  xtc[1] = tc3[0]
  ytc[1] = tc3[1]
  ztc[1] = tc3[2]
  fig = plt.figure()
  ax = fig.add_subplot(111, projection='3d')
  ax.plot(xup,yup,zup,color="white")
  ax.plot(xdn,ydn,zdn,color="white")
  ax.plot(xbc,ybc,zbc,color="green",label="x body")
  ax.plot(xbh,ybh,zbh,color="green")
  ax.plot(xwc,ywc,zwc,color="red",label="y wing")
  ax.plot(xtc,ytc,ztc,color="blue",label="z tail")
  plt.title("plot3dp.py3")
  plt.legend()
  plt.show() # need to click X

# end iz

print("plot3dp.py3 finished")