import numpy as np
import math

# a represents the initial state
a=[[-0.0035644599486394936, 0.006408336275239223, 0.02831667599145281], [-0.006929220093817958, 0.006603664106341208, 0.029805288103772205], [-0.0038226953833251187, 0.004088709355027855, 0.031169833753432685], [-0.00708954832787582, 0.004118515643824083, 0.032197491279898134], [0.0009933312309624146, 0.0016174916608240654, 0.02683165711156252], [-0.011326444989543803, 0.0015606876889666121, 0.02839028326857475], [-0.0010440261477593913, 0.0013701428655820394, 0.029578174674310176], [-0.003843198425546198, 0.0012745166474474945, 0.03176714293092479], [-0.006979269920409792, 0.0012801652425599454, 0.03252014299832164], [-0.009578825777621626, 0.0012921324232218468, 0.031244967870895132], [-0.003690902996697742, -0.0016984469610059388, 0.03108411756017471], [-0.0009477573039302866, -0.0016202110953250405, 0.029611707127701094], [-0.0067361118752011185, -0.0017858717031500756, 0.03161617058238712], [-0.009147505672219718, -0.0016945971269332733, 0.02962841163473264], [-0.01107839148576767, -0.001508393943643895, 0.027573454373876056], [0.0010713504630769093, -0.0014374190015882936, 0.02607192272539872], [-0.0034244655300324704, -0.004308567694604256, 0.029398062013654674], [-0.006618899784838727, -0.004506246378557009, 0.030392028333751032], [-0.0031288460090818605, -0.006712262535460544, 0.02772318171134483], [-0.006305751214061047, -0.006724498506584483, 0.027820448253347306]]


# b represents the current state
b=[[-0.00670995285871181, 0.0065719522004526135, 0.029141298469820498], [-0.0035911371687466827, 0.006475008346232531, 0.028858223084630256], [-0.007206770358545957, 0.0042283439580613775, 0.031164358370477575], [-0.004074031169820162, 0.003995847204651612, 0.030470575173226783], [0.0005040522308670151, 0.0020820370691154674, 0.026555915729995154], [-0.012133506742269538, 0.0019319299028886221, 0.028204138705818924], [-0.01010897766762888, 0.0016400735543064858, 0.030554014575064903], [-0.0018253387570953494, 0.0016443775095757612, 0.03074144653726478], [-0.007255878106793772, 0.0014152385771374863, 0.030741695380820708], [-0.004414595496530646, 0.001345535527439485, 0.031335361066391974], [0.00025446495098404665, -0.0006987928047230355, 0.025754456166508708], [-0.012130903013170763, -0.0011674545286262947, 0.027542749122995154], [-0.0019960657645732302, -0.001284778932942001, 0.029286031145925373], [-0.010140416663718477, -0.0014052814268960356, 0.029945493376675285], [-0.0073882354810257695, -0.0015659706374288739, 0.030888158373551744], [-0.004563964111686898, -0.0016224428315243485, 0.03162800168292884], [-0.0075973890912084474, -0.004241123925148353, 0.030204204070768733], [-0.004415506582928643, -0.004292670039846637, 0.029787804257584222], [-0.007450928767643901, -0.006178679843764624, 0.0271947424188492], [-0.004042398872085348, -0.0060895449252407665, 0.02578435405327505]]


D=[]
for i in range(20):
 d=np.linalg.norm(np.array(a[i])-np.array(b[i]))
 D.append(d)
# print(D) #the displacement matrix
Dis=D
print("the displacement of each maker:",Dis)

for j in range(20):
  if D[j]<0.005:
    D[j]=0
#print(D)  #filter out the negligible points,the new D =K*D

W=[]
for q in range(20):
  w=D[q]/sum(D)
  W.append(w)
print("the weight values:",W)   # the W matrix


C=[]
P=[]
F=[]
for m in range(20):
  for l in range(3):
    c=a[m][l]*W[m]
    C.append(c)
  P=C
  F.append(P)
  C=[]
#print(F)   #the convolution calculation 

X=[]
Y=[]
Z=[]
for i in range(20):
  x=F[i][0]
  y=F[i][1]
  z=F[i][2]
  X.append(x)
  Y.append(y)
  Z.append(z)
print("the final estimation of contact point:")
print(sum(X),sum(Y),sum(Z)) #the final values of (xc,yc,zc)


E=[]
g=[[6.93,0,24],[4,0,26.93],[0,0,28],[-4,0,26.93],[-6.93,0,24]]   #ground truth
est=[[3.32,0.74,28.33],[2.23,0.12,29.12],[0.502,0.051,30.02],[-1.56,0.89,29.27],[-4.43,0.73,28.72]] #estimated value
for i in range(5):
   e=np.linalg.norm(np.array(g[i])-np.array(est[i]))
   E.append(e)
print("the error compared with the ground truth")
print(E) #the displacement matrix