#!/usr/bin/python3
# ===================================================================
# given three (3D) points, calculate the (3D) angle formed by them
#
# from: itecnote.com/tecnote/
#       python-code-to-calculate-angle-between-
#       three-point-using-their-3d-coordinates/
# ===================================================================

import sys
import numpy as np
import user_interface as ui

np.seterr(all='raise')


interactive_description = r'''
----------------------------------------------------------------------
In interactive mode, the origin (0,0,0) is asummed to be
the middle point used in the calculation (point B).

            A
           / 
          /
         B
          \ 
           \
            C

Points A are C entered by the user and the angle calculated.

The smaller angle is returned - no angles over 180 degrees is returned

if an error occurs 0.0 is returned
----------------------------------------------------------------------
'''


# --------------------------------------------------------------------
# ---- get 3D angle in degrees from three points a,b,c
# --------------------------------------------------------------------

def get_3d_angle(a, b, c):

    try:
        a = np.array(a)
        b = np.array(b)
        c = np.array(c)

        ba = a - b
        bc = c - b

        cosine_angle = np.dot(ba, bc) / \
                       (np.linalg.norm(ba) * np.linalg.norm(bc))
        ang = np.arccos(cosine_angle)

    except Exception as e:
        ##print(e)
        ang = 0.0

    ang = np.degrees(ang)       # radians to degree
    
    return ang


# -------------------------------------------------------------------
# ---- pre-defined angle calculation tests
# -------------------------------------------------------------------

def predefined_angle_tests():

    tests_3d = [
       [ 'clockwise tests' ],      
       [ (5,0,0),(0,0,0),(5,-5,0),     'a. Test 45   degrees' ],
       [ (5,0,0),(0,0,0),(0,-5,0),     'b. Test 90   degrees' ],
       [ (5,0,0),(0,0,0),(-5,-5,0),    'c. Test 135  degrees' ],
       [ (5,0,0),(0,0,0),(-5,0,0),     'd. Test 180  degrees' ],
       [ (5,0,0),(0,0,0),(-5,5,0),     'e. Test 225  degrees' ],
       [ (5,0,0),(0,0,0),(0,5,0),      'f. Test 270  degrees' ],
       [ (5,0,0),(0,0,0),(5,5,0),      'g. Test 315  degrees' ],
       [ 'special case tests - illegal points/angles ?' ],       
       [ (5,0,0),(0,0,0),(5,0,0),      'x. Test 0    degrees' ], 
       ##[ (0,0,0),(0,0,0),(0,0,0),      'y. Test (0,0,0)' ],
       ##[ (5,5,0),(5,5,0),(5,5,0),      'z. Test (5,5,0)' ],
       [ 'counter clockwise tests' ],       
       [ (5,-5,0), (0,0,0),(5,0,0),    'a. Test -315 degrees' ],
       [ (0,-5,0), (0,0,0),(5,0,0),    'b. Test -270 degrees' ],
       [ (-5,-5,0),(0,0,0),(5,0,0),    'c. Test -225 degrees' ],
       [ (-5,0,0), (0,0,0),(5,0,0),    'd. Test -180 degrees' ],
       [ (-5,5,0), (0,0,0),(5,0,0),    'e. Test -135 degrees' ],
       [ (0,5,0),  (0,0,0),(5,0,0),    'f. Test -90  degrees' ],
       [ (5,5,0),  (0,0,0),(5,0,0),    'g. Test -45  degrees' ]
               ]

    for t  in tests_3d:

        if len(t) == 1:
            print()
            print(t[0])
            print()
            continue

        p0 = np.array(t[0])
        p1 = np.array(t[1])
        p2 = np.array(t[2])

        print(f'----------------------- {t[3]}')
        
        ang = get_3d_angle(p0,p1,p2)
        
        print(f'returned angle   = {ang:<6.2f}  degrees')

# --------------------------------------------------------------------
# ---- ask the user for a point's coordinates
# --------------------------------------------------------------------

def get_point_coordinates(title):

    print()
    print(title)

    while True:

        print()
        s = ui.get_user_input('Enter x,y,z coordinates: ')

        if not s: sys.exit()
        
        s = s.replace(',', ' ')
        s = s.replace(r'/', ' ')
        s = s.replace(':', ' ')
        sx,sy,sz = s.split()
        
        tf,x = ui.is_float(sx)
        if not tf:
            print('bad X coord entered')
            continue

        tf,y = ui.is_float(sy)
        if not tf:
            print('bad Y coord entered')
            continue

        tf,z = ui.is_float(sz)
        if not tf:
            print('bad Z coord entered')
            continue

        return (x,y,z)
    
# --------------------------------------------------------------------
# ---- interactive angle calculation tests
# --------------------------------------------------------------------

def interactive_angle_tests(center_point):

    print(interactive_description)

    while True:

        a = get_point_coordinates(
            'Enter x,y,z coordinates for point A')

        c = get_point_coordinates(
            'Enter x,y,z coordinates for point C')

        ang = get_3d_angle(a,center_point,c)

        print()
        print(f'Angle is {ang:.4} degrees')

        ##ui.pause()

# --------------------------------------------------------------------
# ---- main
# --------------------------------------------------------------------

if __name__ == '__main__':

    # ---- pre-defined tests
    
    ##predefined_angle_tests()

    # ---- interactive tests
    
    interactive_angle_tests((0,0,0))