def angle_to_chord(radius, angle):
    return (2 * radius * np.sin((angle / 57.3) / 2))

def to_euclid(xyz):
	return (np.sqrt(xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2]))


radius = 2
angle_list = [57, 143, 160]
des_dist_list = [0, 0, 0]
for idx in range(0,len(angle_list)):
	des_dist_list[idx] = angle_to_chord(radius, angle_list[idx])

act_dist_list = [0, 0, 0]
qc_list = [q1, q2, q3]
err_dist_list = [0, 0, 0]
unit_vector_list = [[0,0,0],[0,0,0],[0,0,0]]

for t in time:

    q1.broadcast_rel_xyz(qt)
    q2.broadcast_rel_xyz(qt)
    q3.broadcast_rel_xyz(qt)
    for idx in range(0,len(qc_list)):
        act_dist_list[idx] = to_euclid(qc_list[idx].calc_neighbour_vector(qc_list[(idx + 1) % len(qc_list)]))

    for idx in range(0, len(qc_list)):
        err_dist_list[idx] = act_dist_list[idx] - des_dist_list[idx]

    for idx in range(0, len(qc_list)):
        unit_vector_list[idx][0] = qc_list[idx].calc_neighbour_vector(qc_list[(idx + 1) % len(qc_list)])[0] / act_dist_list[idx]
        unit_vector_list[idx][1] = qc_list[idx].calc_neighbour_vector(qc_list[(idx + 1) % len(qc_list)])[1] / act_dist_list[idx]
        unit_vector_list[idx][2] = qc_list[idx].calc_neighbour_vector(qc_list[(idx + 1) % len(qc_list)])[2] / act_dist_list[idx]

    print("Desired: ", des_dist_list)
    print("Actual: ", act_dist_list)
    print("Errors: ", err_dist_list)
    print("unit_vectors: " , unit_vector_list)



    #'''
    def broadcast_rel_xyz(self, qt):
        self.rel_xyz = qt.xyz - self.xyz
        return self.rel_xyz
    #'''
    #'''
    def calc_neighbour_vector(self, qx):
        self.neighbour_vector = self.rel_xyz - qx.rel_xyz
        return self.neighbour_vector
    #'''