import csv
import math
import sys
sys.path.insert(1, '../src') # add Python path at runtime for needed imports
from pfh import load_pc, load_sydney_obj_pc, rotate
import utils

def main():
    # Import the cloud
    
    # Uncomment to use hw4 data
    # pc_source = load_pc('/home/parallels/Documents/eecs498/final_project/data/hw4/cloud_icp_source.csv')
    # pc_target = load_pc('/home/parallels/Documents/eecs498/final_project/data/hw4/cloud_icp_target0.csv')

    # Uncomment to use sydney_obj data <-- Modify file paths
    pc_source = load_sydney_obj_pc('/home/parallels/Documents/eecs498/final_project/data/sydney-urban-objects-dataset/objects/trunk.37.8444_source.csv')
    pc_target = load_sydney_obj_pc('/home/parallels/Documents/eecs498/final_project/data/sydney-urban-objects-dataset/objects/trunk.37.8444_source.csv')
    rotate(math.pi/2, "x", [1,1,1], pc_target) # <-- Modify params to create rotated target
    
    # Write to file
    with open('trunk.37.8444_target4.csv', 'w') as f: # <-- Modify file name
        write = csv.writer(f)
        write.writerows(pc_target)

    # View source and target
    utils.view_pc_two([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
    
    input("Press enter to end:")

if __name__ == '__main__':
    main()