# INSert membrANE
# A simple, versatile tool for building coarse-grained simulation systems
# Copyright (C) 2017  Tsjerk A. Wassenaar and contributors
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

import math


def vvadd(a, b):
    if type(b) in (int, float):
        return [i+b for i in a]
    return [i+j for i, j in zip(a, b)]

def vvsub(a, b):
    if type(b) in (int, float):
        return [i-b for i in a]
    return [i-j for i, j in zip(a, b)]

def norm2(a):
    return sum([i*i for i in a])

def norm(a):
    return math.sqrt(norm2(a))

def cos_angle(a, b):
    p = sum([i*j for i, j in zip(a, b)])
    q = math.sqrt(sum([i*i for i in a])*sum([j*j for j in b]))
    return min(max(-1, p/q), 1)

## MIJN EIGEN ROUTINE ##

# Quite short piece of code for diagonalizing symmetric 3x3 matrices :)

# Analytic solution for third order polynomial
def solve_p3( a, b, c ):
    Q, R, a3 = (3*b-a**2)/9.0, (-27*c+a*(9*b-2*a**2))/54.0, a/3.0
    if Q**3 + R**2:
        t, R13 = math.acos(R/math.sqrt(-Q**3))/3, 2*math.sqrt(-Q)
        u, v, w = math.cos(t), math.sin(t+math.pi/6), math.cos(t+math.pi/3)
        return R13*u-a3, -R13*v-a3, -R13*w-a3
    else:
        R13   = math.sqrt3(R)
        return 2*R13-a3, -R13-a3, -R13-a3

# Normalization of 3-vector
def normalize(a):
    f = 1.0/math.sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2])
    return f*a[0], f*a[1], f*a[2]

# Eigenvectors for a symmetric 3x3 matrix:
# For symmetric matrix A the eigenvector v with root r satisfies
#   v.Aw = Av.w = rv.w = v.rw
#   v.(A-rI)w = v.Aw - v.rw = 0 for all w
# This means that for any two vectors p, q the eigenvector v follows from:
#   (A-rI)p x (A-rI)q
# The input is var(x), var(y), var(z), cov(x, y), cov(x, z), cov(y, z)
# The routine has been checked and yields proper eigenvalues/-vectors
def mijn_eigen_sym_3x3(a, d, f, b, c, e):
    a, d, f, b, c, e=1, d/a, f/a, b/a, c/a, e/a
    b2, c2, e2, df = b*b, c*c, e*e, d*f
    roots = list(solve_p3(-a-d-f, df-b2-c2-e2+a*(f+d), a*e2+d*c2+f*b2-a*df-2*b*c*e))
    roots.sort(reverse=True)
    ux, uy, uz = b*e-c*d, b*c-a*e, a*d-b*b
    u = (ux+roots[0]*c, uy+roots[0]*e, uz+roots[0]*(roots[0]-a-d))
    v = (ux+roots[1]*c, uy+roots[1]*e, uz+roots[1]*(roots[1]-a-d))
    w = u[1]*v[2]-u[2]*v[1], u[2]*v[0]-u[0]*v[2], u[0]*v[1]-u[1]*v[0] # Cross product
    return normalize(u), normalize(v), normalize(w), roots
