# 1.去掉重叠部分
# 2.将数据和标准数据对比，旋转角度和平移使之初步重合

import pandas as pd
import numpy as np
import scipy.linalg as linalg 
import scipy.spatial.distance as dis
import matplotlib.pyplot as plt
import math
import cv2


# 计算距离矩阵
def EuclideanDistances(A, B):
    BT = B.transpose()
    vecProd = A * BT
    SqA =  A.getA()**2
    sumSqA = np.matrix(np.sum(SqA, axis=1))
    sumSqAEx = np.tile(sumSqA.transpose(), (1, vecProd.shape[1]))    
    SqB = B.getA()**2
    sumSqB = np.sum(SqB, axis=1)
    sumSqBEx = np.tile(sumSqB, (vecProd.shape[0], 1))    
    SqED = sumSqBEx + sumSqAEx - 2*vecProd   
    ED = (SqED.getA())**0.5
    return np.matrix(ED)


standard = pd.read_csv("D:\\standard.csv")
standardArray = np.array(standard).transpose()
data = pd.read_csv("D:\\Level1.csv")

# **************************去掉重叠部分*******************************
A = np.matrix(np.array(data[(data["0"].index % 2 == 0) & (data["0"] > 4000) & (data["0"] <61000)])[:,1:4:2])
B = np.matrix(np.array(data[(data["0"].index % 2 == 1) & (data["0"] > 4000) & (data["0"] <61000)])[:,1:4:2])
C = np.matrix(np.array(data[(data["0"] <= 4000) | (data["0"] >=61000)])[:,1:4:2])

# 根据距离计算
#distanceMatrix = EuclideanDistances(A , B)
#print(distanceMatrix)
#print(distanceMatrix.shape)
#overlapIndex = np.argwhere(distanceMatrix.diagonal()<5)

# 根据Y方向偏差计算
if(A.shape[0] > B.shape[0]):
    A = np.delete(A,A.shape[0]-1,0)
else: 
    if(A.shape[0] < B.shape[0]):
         B = np.delete(B,B.shape[0]-1,0)
yoffset = (A - B)[:,1]
overlapIndex =np.argwhere(np.abs(yoffset)>2.5)[:,0]
A = np.delete(A,overlapIndex,0)
D = np.r_[A,B,C]

#plt.scatter(D[:,0].getA(),D[:,1].getA(),color='r')
#plt.grid()
#plt.show()
# *********************************************************************


#dataArray = np.array(data).transpose()[1:4:2,:]/1000
dataArray = np.array(D).transpose()/1000
mirroring = np.array([[1,0],[0,-1]])
dataMirroring = mirroring.dot(dataArray)

# 只取第一个数据x方向
mean_x = dataMirroring.mean(axis = 1)[0]
mean_x_s = standardArray.mean(axis = 1)[0]
dataFrameMirroring  = pd.DataFrame(dataMirroring.transpose())
dataLine = np.array(dataFrameMirroring[(dataFrameMirroring[0] > 20) & (dataFrameMirroring[0] < 50)]).transpose()
dataLine_s = np.array(standard[(standard['0'] > 260) & (standard['0'] < 290)]).transpose()

# 只取y方向
mean_y = dataLine.mean(axis =1)[1]
mean_y_s = dataLine_s.mean(axis = 1)[1]

p = np.poly1d(np.polyfit(dataLine[0,:],dataLine[1,:],1))
yval = p(dataLine[0,:])
p_s = np.poly1d(np.polyfit(dataLine_s[0,:],dataLine_s[1,:],1))

phi = math.atan(p[1])*180/math.pi
rotateMatrix = cv2.getRotationMatrix2D((mean_x,mean_y),phi,1)
dataTrans1 = rotateMatrix.dot(np.r_[dataMirroring,np.ones(dataMirroring.shape[1]).reshape(1,dataMirroring.shape[1])])
transMatrix = np.array([[1,0,mean_x_s - mean_x],[0,1,mean_y_s - mean_y]])
dataTrans2 = transMatrix.dot(np.r_[dataTrans1,np.ones(dataTrans1.shape[1]).reshape(1,dataTrans1.shape[1])])

dataFrameTrans2 = pd.DataFrame(dataTrans2.transpose())
dataFrameTrans2[dataFrameTrans2[1]>443.71].to_csv("D:\\Level1Trans.csv")

plt.figure()

plt.scatter(dataTrans2[0,:] , dataTrans2[1,:])
plt.plot(standardArray[0,:] , standardArray[1,:],color='r')

plt.show()