import pandas as pd
import numpy as np
from pathlib import Path

p = Path('.')

csv_files = [f for f in p.glob("*.csv")]

csv_files

df_ucs, df_wcs = [pd.read_csv(f, skiprows=[0]) for f in csv_files]

def retrive_2_points(df):
    '''
    retrive the 2 points from the source DataFram.
    '''
    colunms = ['Easting', 'Northing']
    pnt_start, pnt_end = [df.loc[i, colunms] for i in range(2)]
    pnt_start, pnt_end = [df.values for df in [pnt_start, pnt_end]]
    return pnt_start, pnt_end
    

def get_perpendicular_point(df):
    '''
    get a point that is perpendicular to the given two points.
    
    - caculate vector v by subtracting pnt_end to pnt_start: v = pnt_end - pnt_start
    - rotate v 90 degrees: v90 = R90@v
    - add v90 and pnt_start to get the perpendicular point: pnt_perpendicular = v90 + pnt_start
    '''
    pnt_start, pnt_end = retrive_2_points(df)
    
    v = pnt_end - pnt_start
    R90 = np.array([[0, -1], [1, 0]])
#     return v, R90
    v90 = R90@v
    
    pnt_perpendicular = v90 + pnt_start
    return pnt_perpendicular

get_perpendicular_point(df_ucs)

def construct_matrix_from_source_df(df):
    '''
    construct a matrix from three points.
    
    - 2 of the 3 points are retrieved form the source .csv file
    - the perpendicular point is construced by function get_perpendicular_point
    '''
    pnt_start, pnt_end = retrive_2_points(df)
    pnt_perpendicular = get_perpendicular_point(df)
    A = np.array([pnt_start, pnt_end, pnt_perpendicular])
    
    suffix = np.array([[1, 1, 1]]).T
    A =  np.concatenate((A, suffix), axis=1)
    
    return A.T

construct_matrix_from_source_df(df_ucs)

A_w, A_u = [construct_matrix_from_source_df(df) for df in [df_wcs, df_ucs]]

T_w_to_u = A_u@np.linalg.inv(A_w)
T_u_to_w = A_w@np.linalg.inv(A_u)

T_u_to_w@A_u - A_w

T_w_to_u

A_w.T