import numpy as np #type: ignore
from bikinematicsolver.dtypes import Pos  

def find_intersection(a1,a2,b1,b2):
    """
    Finds intersection of lines a and b, described by points a1,a2,b1,b2
    Accepts vector input of shape (2,N)
    Returns (x_int,y_int)
    """
    #Eqn first line
    m_a = np.subtract( a2.y , a1.y ) / np.subtract( a2.x , a1.x ) 
    c_a = np.subtract( a1.y,  m_a * a1.x ) 
    #Eqn second line
    m_b = np.subtract( b2.y , b1.y ) / np.subtract( b2.x , b1.x ) 
    c_b = np.subtract( b1.y, m_b * b1.x )
    #Intersction point
    x0 = np.subtract( c_b , c_a ) / np.subtract( m_a, m_b )
    y0 = np.add( m_b * x0  , c_b ) 

    ic = Pos(x0,y0)      
    return ic

def tangent_eqn(cen,r1,r2):
    """
    Finds equation of line in form ax + by = c, that is tangent to two circles. The first circle is at the origin, and the other is at an arbitrary point

    cen: centrepoint of offset circle (or vector between the centres of both circles)
    r1: radius of first circle
    r2: radius of second circle
    """
    delta_r = r2-r1
    z = np.add( np.power(cen[0],2) , np.power(cen[1],2))
    d = np.sqrt( np.subtract( z , np.power(delta_r,2) ) )
    
    a = np.add( cen[0]*delta_r, cen[1]*d  ) / z
    b = np.subtract( cen[1]*delta_r, cen[0]*d  ) / z
    c = r1
    ret = [a,b,c]
    return ret

def find_common_circle_tangent(cen1,r1,cen2,r2):
    """
    Finds all the tangent lines between two circles in form ax + by = c
    """
    tan_lines = []
    for i in [-1,1]:
        for j in [-1,1]: 
            cen = np.subtract(cen2 , cen1)
            #print(cen)
            tan_lines.append(tangent_eqn(cen, r1 * i, r2 * j))
    for line in tan_lines:
        s = np.add( line[0] * cen1[0] , line[1] * cen1[1])
        line[2] = np.subtract( line[2] , s )
    return tan_lines

def find_circle_tangent_intersection(cen,tangent_line):
    """
    Finds the intersection point of a circle and a line TANGENT!!!!!!!!!!!!!!!!!!!!!! to the circle
    """
    line = tangent_line[:]
    s = np.add( line[0] * cen[0] , line[1] * cen[1])
    line[2] = np.add( line[2] , s )
    
    z = np.add( np.power(line[0],2) , np.power(line[1],2) )
    x0 = -(line[0] * line[2]) / z + cen[0]
    y0 = -(line[1] * line[2] ) / z + cen[1]

    ic = Pos(x0,y0)
    return ic

def find_upper_tangent_points(t_lines,cen1,cen2):
    """
    Given a set of circles and their appropriate tangent lines (ax + by = c format) this finds the intersection points between the 'upper' tangent line and the circles - this is the chainline on a bike
    """
    p1 =[]
    p2 = []

    for line in t_lines:
        p1.append(find_circle_tangent_intersection(cen1,line))
        p2.append(find_circle_tangent_intersection(cen2,line))

    positive_inds_1 = [p1.index(point) for point in p1
                        if point.y-cen1[1] > 0]
    positive_inds_2 = [p2.index(point) for point in p2
                        if point.y-cen2[1] > 0]

    ind = [i for i in positive_inds_1 if i in positive_inds_2]
    if ind:
        ind = ind[0]
        return [p1[ind],p2[ind]]
    else:
        return [Pos(0,0),Pos(0,0)]