import pandas as pd
import numpy as np
import cv2



def Joint():

    matrix12 = np.matrix([[    0.999994, -2.30179e-005,  -0.00338715,  22.9331],
                        [2.30258e-005,           1.0, 2.28675e-006,  47.9336],
                        [  0.00338715, -2.36473e-006,     0.999994, -30.3661],
                        [         0.0,           0.0,          0.0,      1.0]])
    matrix13 = np.matrix([[    0.999988, -1.16345e-005,  -0.00490587,  48.7739],
                        [1.17334e-005,           1.0, 2.01129e-005,  91.5852],
                        [  0.00490587, -2.01702e-005,     0.999988, -71.2706],
                        [         0.0,           0.0,          0.0,      1.0]])
    matrix14 = np.matrix([[    0.999985, -2.39867e-005,  -0.00554939,  74.1329],
                        [2.41246e-005,           1.0, 2.47831e-005,  142.206],
                        [  0.00554939, -2.49166e-005,     0.999985, -113.294],
                        [         0.0,           0.0,          0.0,      1.0]])
    matrix15 = np.matrix([[    0.999981, -4.08954e-005,	 -0.00621533,  98.6007],
                        [4.11151e-005,       	   1.0,	3.52216e-005,  192.034],
                        [  0.00621533, -3.54764e-005, 	0.999981, -163.256],
                        [         0.0,       	   0.0,	         0.0,	   1.0]])
    matrix16 = np.matrix([[    0.999975, -6.00843e-005,	 -0.00709497,  129.069],
                        [6.03902e-005,           1.0,	4.28944e-005,  236.579],
                        [  0.00709497, -4.33218e-005,	    0.999975, -226.242],
                        [         0.0, 	       0.0,	         0.0,	   1.0]])
    matrix17 = np.matrix([[    0.999971, -7.82354e-005,	 -0.00755777,  154.855],
                        [7.86353e-005,           1.0,	5.26223e-005,  282.774],
                        [  0.00755777, -5.32151e-005,	    0.999971, -279.706],
                        [         0.0, 	       0.0,	         0.0,	   1.0]])
    matrix18 = np.matrix([[    0.999965,  -0.000100226,	 -0.00832648,  184.996],
                        [ 0.000100805,	       1.0,	6.90642e-005,  323.342],
                        [  0.00832648, -6.99012e-005,	    0.999965, -354.304],
                        [         0.0,	       0.0,	         0.0,	   1.0]])
    matrix22 = np.matrix([[    0.872123,	  0.000773,	    0.489285,  4942.74],
                        [-0.000745115,	       1.0,	-0.000251729,  -2.8314],
                        [   -0.489285,  -0.000145035,	    0.872124, -598.535],
                        [         0.0,	       0.0,	         0.0,	   1.0]])
    matrix32 = np.matrix([[   -0.869019,   -0.00114372,	   -0.494777,  64430.3],
                        [-0.000670692,	 -0.999994,	  0.00348957, 186874.4],
                        [   -0.494778,	0.00336434,	    0.869013, -325.194],
                        [         0.0,	       0.0,	         0.0,	   1.0]])

    data12 = pd.read_csv("RawData\\12.csv");
    print("data12 load completed!")
    data13 = pd.read_csv("RawData\\13.csv");
    print("data13 load completed!")
    data14 = pd.read_csv("RawData\\14.csv");
    print("data14 load completed!")
    data15 = pd.read_csv("RawData\\15.csv");
    print("data15 load completed!")
    data16 = pd.read_csv("RawData\\16.csv");
    print("data16 load completed!")
    data17 = pd.read_csv("RawData\\17.csv");
    print("data17 load completed!")
    data18 = pd.read_csv("RawData\\18.csv");
    print("data18 load completed!")
    data22 = pd.read_csv("RawData\\22.csv");
    print("data22 load completed!")
    data32 = pd.read_csv("RawData\\32.csv");
    print("data32 load completed!")

    dataArray12 = np.c_[np.array(data12),np.ones(np.array(data12).shape[0])] 
    dataArray13 = np.c_[np.array(data13),np.ones(np.array(data13).shape[0])] 
    dataArray14 = np.c_[np.array(data14),np.ones(np.array(data14).shape[0])] 
    dataArray15 = np.c_[np.array(data15),np.ones(np.array(data15).shape[0])] 
    dataArray16 = np.c_[np.array(data16),np.ones(np.array(data16).shape[0])] 
    dataArray17 = np.c_[np.array(data17),np.ones(np.array(data17).shape[0])] 
    dataArray18 = np.c_[np.array(data18),np.ones(np.array(data18).shape[0])] 
    dataArray22 = np.c_[np.array(data22),np.ones(np.array(data22).shape[0])] 
    dataArray32 = np.c_[np.array(data32),np.ones(np.array(data32).shape[0])] 



    dataTrans12 = matrix12.dot(dataArray12.transpose())
    dataTrans13 = matrix13.dot(dataArray13.transpose())
    dataTrans14 = matrix14.dot(dataArray14.transpose())
    dataTrans15 = matrix15.dot(dataArray15.transpose())
    dataTrans16 = matrix16.dot(dataArray16.transpose())
    dataTrans17 = matrix17.dot(dataArray17.transpose())
    dataTrans18 = matrix18.dot(dataArray18.transpose())
    dataTrans22 = matrix22.dot(dataArray22.transpose())
    dataTrans32 = matrix32.dot(dataArray32.transpose())

    dataTrans = np.r_[dataTrans12.transpose(),
                        dataTrans13.transpose(),
                        dataTrans14.transpose(),
                        dataTrans15.transpose(),
                        dataTrans16.transpose(),
                        dataTrans17.transpose(),
                        dataTrans18.transpose(),
                        dataTrans22.transpose(),
                        dataTrans32.transpose()]

    return dataTrans