# Functions to compute properties of 3d vectors, including angles,
# torsions, out-of-plane angles.  Several return False if the operation
# cannot be completed numerically, as for example a torsion in which 3
# points are collinear.
import logging
from math import acos, asin, fabs, fsum, sin, sqrt

import numpy as np

from .exceptions import AlgError, OptError
from . import op

# a couple of obscure parameters used in torsion computation:
#  phi_lim = op.Params.v3d_tors_angle_lim
#  tors_cos_tol = op.Params.v3d_tors_cos_tol

DOT_PARALLEL_LIMIT = 1.0e-10
logger = logging.getLogger(__name__)


def norm(v):
    return np.linalg.norm(v)
    # return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])


def dot(v1, v2, length=None):
    """
    Asks numpy to perform dot prodct, with the option (not used?) to return part of
    the resulting vector
    """
    if length is None:
        return np.dot(v1, v2)
        # return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]
    else:
        return fsum([v1[i] * v2[i] for i in range(length)])
        # I'll have to look closer before removing this


# avoid numerical problems by limiting range to [-1,+1]
def dot_unit(v1, v2):
    dot = np.dot(v1, v2)
    return max(min(1.0, dot), -1.0)


def dist(v1, v2):
    return np.linalg.norm(v1 - v2)
    # return sqrt((v2[0] - v1[0])**2 + (v2[1] - v1[1])**2 + (v2[2] - v1[2])**2)


def normalize(v1, Rmin=1.0e-8, Rmax=1.0e15):
    """
    Normalize vector in place.  If norm exceeds thresholds, don't normalize and return False..
    """
    n = norm(v1)
    if n < Rmin or n > Rmax:
        raise AlgError("Could not normalize vector. Vector norm beyond tolerance")
    else:
        v1 /= n


# def axpy(a, X, Y):
#    Z = np.zeros(Y.shape)
#    Z = a * X + Y
#    return Z


# Compute and return normalized vector from point p1 to point p2.
# If norm is too small, don't normalize and return check as False.
def eAB(p1, p2):
    eAB = p2 - p1
    normalize(eAB)
    return eAB


# Compute and return cross-product.
def cross(u, v):
    # X = np.zeros(3)
    X = np.cross(u, v)
    # X[0] = u[1] * v[2] - u[2] * v[1]
    # X[1] = -u[0] * v[2] + u[2] * v[0]
    # X[2] = u[0] * v[1] - u[1] * v[0]
    return X


def are_parallel(u, v):
    """Determines if two vectors are parallel within tolerance (1e-10)"""
    if fabs(dot(u, v) - 1.0e0) < DOT_PARALLEL_LIMIT:
        return True
    else:
        return False


def are_antiparallel(u, v):
    """Determines if two vectors are antiparallel within tolerance (1e-10)"""
    if fabs(dot(u, v) + 1.0e0) < DOT_PARALLEL_LIMIT:
        return True
    else:
        return False


def are_parallel_or_antiparallel(u, v):
    """
    Determines if two vectors are parallel and or antiparallal

    Returns
    -------

    boolean
        if vectors are either parallel or antiparallel
    """

    return are_parallel(u, v) or are_antiparallel(u, v)


def angle(A, B, C, tol=1.0e-14):
    """Compute and return angle in radians A-B-C (between vector B->A and vector B->C)
    If points are absurdly close or far apart, returns False

    Parameters
    ----------
    A : np.ndarray
        number of atom in fragment system. uses 1 indexing
    B : np.ndarray
    C : np.ndarray

    Returns
    -------
    double
        angle in radians
    """
    try:
        eBA = eAB(B, A)
    except AlgError as error:
        logger.warning("Could not normalize eBA in angle()\n")
        raise AlgError from error

    try:
        eBC = eAB(B, C)
    except AlgError as error:
        logger.warning("Could not normalize eBC in angle()\n")
        raise AlgError from error

    return _calc_angle(eBA, eBC, tol)


def _calc_angle(vec_1, vec_2, tol=1.0e-14):
    """
    Computes and returns angle in radians A-B_B (between vector B->A and vector B->C

    Should only be called by tors or angle. Error checking and vector creation
    is performed in angle() or tors() previously

    Paramters
    ---------
    vec_1 : ndarray
        first vector of an angle
    vec_2 : ndarray
        second vector on an angle
    tol : double
        nearness of cos to 1/-1 to set angle 0/pi.
    """

    dotprod = dot(vec_1, vec_2)

    if dotprod > 1.0 - tol:
        phi = 0.0
    elif dotprod < -1.0 + tol:
        phi = acos(-1.0)
    else:
        phi = acos(dotprod)

    return phi


def tors(A, B, C, D, indices):
    """
    Compute and return angle in dihedral angle in radians A-B-C-d
    Raises AlgError exception if bond angles are too large for good torsion definition

    Parameters
    ----------
    A : np.ndarray
    B : np.ndarray
    C : np.ndarray
    D : np.ndarray
    indices: [int]
        atomic indices of the torsion to be computed (used for printing)

    Returns
    -------
    float

    """
    phi_lim = op.Params.v3d_tors_angle_lim
    tors_cos_tol = op.Params.v3d_tors_cos_tol

    # Form e vectors
    try:
        EBA = eAB(B, A)
        EAB = -1 * EBA
    except AlgError: # as error:
        logger.warning(f"Could not normalize {A}, {B} vector in tors()\n")
        raise
    try:
        EBC = eAB(B, C)
    except AlgError: # as error:
        logger.warning(f"Could not normalize {B}, {C} vector in tors()\n")
        raise
    try:
        ECB = eAB(C, B)
        EBC = -1 * ECB
    except AlgError: # as error:
        logger.warning(f"Could not normalize {C}, {B} vector in tors()\n")
        raise
    try:
        ECD = eAB(C, D)
    except AlgError: # as error:
        logger.warning(f"Could not normalize {C}, {D} vector in tors()\n")
        raise

    # Compute bond angles
    phi_123 = _calc_angle(EBA, EBC)
    phi_234 = _calc_angle(ECB, ECD)

    linear_torsion_check(phi_123, phi_234, phi_lim, indices)
    tmp = cross(EAB, EBC)
    tmp2 = cross(EBC, ECD)
    tval = dot(tmp, tmp2) / (sin(phi_123) * sin(phi_234))

    if tval >= 1.0 - tors_cos_tol:  # accounts for numerical leaking out of range
        tau = 0.0
    elif tval <= -1.0 + tors_cos_tol:
        tau = acos(-1)
    else:
        tau = acos(tval)

    # determine sign of torsion ; this convention matches Wilson, Decius and Cross
    if tau != acos(-1):  # no torsion will get value of -pi; Range is (-pi,pi].
        tmp = cross(EBC, ECD)
        tval = dot(EAB, tmp)
        if tval < 0:
            tau *= -1

    return tau

def linear_torsion_check(phi_123, phi_234, phi_lim, indices):

    up_lim = acos(-1) - phi_lim

    # Check that both bends are within range
    phi_123_bad = not phi_lim < phi_123 < up_lim
    phi_234_bad = not phi_lim < phi_234 < up_lim
    bad_bends = []
    linear_bends = []
    old_bends = []

    # Print specific message of which bend has become problematic in torsion
    if phi_123_bad:
        val = phi_123 * 180.0 / np.pi
        logger.warning(
            f"Interior angle of {val:5.1f} for bend B({indices[:-1]}) can't work in good torsion"
        )
        bad_bends.append(phi_123)
        a, b, c = indices[:-1]
        linear_bends.append(indices[:-1])
        # Just add all combinations to be safe (avoids checking connectivity)
        old_bends.append([a, b, c])
        old_bends.append([a, c, b])
        old_bends.append([a, b, c])

    if phi_234_bad:
        val = phi_234 * 180.0 / np.pi
        logger.warning(
            f"Interior angle of {val:5.1f} for bend B({indices[1:]}) can't work in good torsion"
        )
        bad_bends.append(phi_234)
        a, b, c = indices[1:]
        linear_bends.append(indices[1:])
        # Just add all combinations to be safe (avoids checking connectivity)
        old_bends.append([a, b, c])
        old_bends.append([a, c, b])
        old_bends.append([a, b, c])

    if bad_bends:
        raise AlgError(
            f"Could not compute T({indices}). Problem computing interior bend.",
            new_linear_bends=linear_bends,
            old_bends=old_bends,
        )

    # if phi_123 < phi_lim or phi_123 > up_lim or phi_234 < phi_lim or phi_234 > up_lim:
    #     raise AlgError(
    #         "Interior angle of {:5.1f} or {:5.1f}  can't work in good torsion.".format(
    #             180.0 * phi_123 / np.pi, 180.0 * phi_234 / np.pi
    #         )
    #     )

def oofp(A, B, C, D):
    """
    Compute and return angle in dihedral angle in radians A-B-C-d
    returns false if bond angles are too large for good torsion definition

    Parameters
    ----------
    A : np.ndarray
    B : np.ndarray
    C : np.ndarray
    D : np.ndarray

    Returns
    -------
    float

    """
    try:
        eBA = eAB(B, A)
    except AlgError as error:
        logger.warning(f"Could not normalize {B}, {C} vector in tors()\n")
        raise
    try:
        eBC = eAB(B, C)
    except AlgError as error:
        logger.warning(f"Could not normalize {B}, {C} vector in tors()\n")
        raise
    try:
        eBD = eAB(B, D)
    except AlgError as error:
        logger.warning(f"Could not normalize {B}, {D} vector in tors()\n")
        raise

    phi_CBD = _calc_angle(eBC, eBD)

    # This shouldn't happen unless angle B-C-d -> 0,
    if sin(phi_CBD) < op.Params.v3d_tors_cos_tol:  # reusing parameter
        raise AlgError(f"Angle: {C}, {B}, {D} is to close to zero in oofp\n")

    dotprod = dot(cross(eBC, eBD), eBA) / sin(phi_CBD)

    if dotprod > 1.0:
        tau = acos(-1)
    elif dotprod < -1.0:
        tau = -1 * acos(-1)
    else:
        tau = asin(dotprod)
    return tau


def are_collinear(A, B, C, threshold=1.0e-4):
    try:
        eab = eAB(A, B)
        eac = eAB(A, C)
    except AlgError as error:
        return True  # points on top of each other

    cr = cross(eab, eac)
    N = sqrt(dot(cr, cr))
    # print("N {}".format(N))
    if N < threshold:
        return True
    else:
        return False
