#!/usr/bin/env python
#
# calculate covariance from neutron events
# @author Tobias Weber <tweber@ill.fr>
# @date 30-mar-2019
# @license GPLv3, see 'LICENSE' file
#

import os
import numpy as np
import numpy.linalg as la
import tascalc as tas


np.set_printoptions(
	floatmode = "fixed", 
	precision = 4)

verbose = True			# console outputs
plot_results = True		# show plot window
plot_neutrons = True	# also plot neutron events
centre_on_Q = False		# centre plots on Q or zero?
ellipse_points = 128	# number of points to draw ellipses
symsize = 32.
dpi = 600

# column indices in kf,kf file
ki_start_idx = 0	# start index of ki 3-vector
kf_start_idx = 3	# start index of kf 3-vector
wi_idx = 9			# start index of ki weight factor
wf_idx = 10			# start index of kf weight factor

# column indices in Q,E file
Q_start_idx = 0
E_idx = 3
w_idx = 4


# constants
sig2hwhm = np.sqrt(2. * np.log(2.))
sig2fwhm = 2.*sig2hwhm


#
# normalises events and filters out too low probabilities
#
def filter_events(Q, E, w):
	# normalise intensity/probability
	w /= np.max(w)


	# filter out too low probabilities
	theeps = 1e-4
	beloweps = lambda d: np.abs(d) <= theeps
	nonzero_idx = [i for i in range(len(w)) if not beloweps(w[i])]

	Q = Q[nonzero_idx]
	E = E[nonzero_idx]
	w = w[nonzero_idx]


	return [Q, E, w]



#
# loads a list of neutron events in the [ ki_vec, kf_vec, pos_vec, wi, wf ] format
#
def load_events_kikf(filename):
	dat = np.loadtxt(filename)
	ki = dat[:, ki_start_idx:ki_start_idx+3]
	kf = dat[:, kf_start_idx:kf_start_idx+3]
	wi = dat[:, wi_idx]
	wf = dat[:, wf_idx]

	w = wi * wf
	Q = ki - kf
	E = tas.k2_to_E * (np.multiply(ki, ki).sum(1) - np.multiply(kf, kf).sum(1))

	return filter_events(Q, E, w)



#
# loads a list of neutron events in the [ h, k, l, E, w ] format
#
def load_events_QE(filename):
	dat = np.loadtxt(filename)

	Q = dat[:, Q_start_idx:Q_start_idx+3]
	E = dat[:, E_idx]
	w = dat[:, w_idx]

	return filter_events(Q, E, w)



#
# calculates the covariance matrix of the (Q, E) 4-vectors
#
def calc_covar(Q, E, w, Qpara, Qperp):
	# make a [Q, E] 4-vector
	Q4 = np.insert(Q, 3, E, axis=1)

	# calculate the mean Q 4-vector
	Qmean = [ np.average(Q4[:,i], weights = w) for i in range(4) ]
	if verbose:
		print("Mean (Q, E) vector in lab system:\n%s\n" % Qmean)

	# get the weighted covariance matrix
	Qcov = np.cov(Q4, rowvar = False, aweights = w, ddof = 0)
	if verbose:
		print("Covariance matrix in lab system:\n%s\n" % Qcov)

	# the resolution is the inverse of the covariance
	Qres = la.inv(Qcov)
	if verbose:
		print("Resolution matrix in lab system:\n%s\n" % Qres)


	# create a matrix to transform into the coordinate system with Q along x
	# choose given coordinate system
	if len(Qpara) == 3 and len(Qperp) == 3:
		Qnorm = Qpara / la.norm(Qpara)
		Qside = Qperp / la.norm(Qperp)
		Qup = np.cross(Qnorm, Qside)
	else:
		Qnorm = Qmean[0:3] / la.norm(Qmean[0:3])
		Qup = np.array([0, 1, 0])
		Qside = np.cross(Qup, Qnorm)

	if verbose:
		print("Qpara = %s\nQperp = %s\nQup = %s\n" % (Qnorm, Qside, Qup))

	# trafo matrix
	T = np.transpose(np.array([
		np.insert(Qnorm, 3, 0),
		np.insert(Qside, 3, 0),
		np.insert(Qup, 3, 0),
		[0, 0, 0, 1] ]))

	if verbose:
		print("Transformation into (Qpara, Qperp, Qup, E) system:\n%s\n" % T)

	# transform mean Q vector
	Qmean_Q = np.dot(np.transpose(T), Qmean)
	if verbose:
		print("Mean (Q, E) vector in (Qpara, Qperp, Qup, E) system:\n%s\n" % Qmean_Q)

	# transform the covariance matrix
	Qcov_Q = np.dot(np.transpose(T), np.dot(Qcov, T))
	if verbose:
		print("Covariance matrix in (Qpara, Qperp, Qup, E) system:\n%s\n" % Qcov_Q)

	# the resolution is the inverse of the covariance
	Qres_Q = la.inv(Qcov_Q)
	if verbose:
		print("Resolution matrix in (Qpara, Qperp, Qup, E) system:\n%s\n" % Qres_Q)

	#[ evals, evecs ] = la.eig(Qcov_Q)
	#print("Ellipsoid fwhm radii:\n%s\n" % (np.sqrt(np.abs(evals)) * sig2fwhm))

	# transform all neutron events
	Q4_Q = np.array([])
	if plot_neutrons:
		Q4_Q = np.dot(Q4, T)
		if not centre_on_Q:
			Q4_Q -= Qmean_Q


	return [Qres_Q, Q4_Q, Qmean_Q]



#
# calculates the characteristics of a given ellipse by principal axis trafo
#
def descr_ellipse(quadric):
	[ evals, evecs ] = la.eig(quadric)
	#print("Evals: %s" % evals)

	fwhms = 1./np.sqrt(np.abs(evals)) * sig2fwhm

	angles = np.array([])
	if len(quadric) == 2:
		angles = np.array([ np.arctan2(evecs[1][0], evecs[0][0]) ])

	return [fwhms, angles/np.pi*180., evecs]



#
# projects along one axis of the quadric
#
def proj_quad(_E, idx):
	E = np.delete(np.delete(_E, idx, axis=0), idx, axis=1)
	if np.abs(_E[idx, idx]) < 1e-8:
		return E

	v = (_E[idx,:] + _E[:,idx]) * 0.5
	vv = np.outer(v, v) / _E[idx, idx]
	vv = np.delete(np.delete(vv, idx, axis=0), idx, axis=1)

	return E - vv


#
# describes the ellipsoid by a principal axis trafo and by 2d cuts
#
def calc_ellipses(Qres_Q):
	# 4d ellipsoid
	[fwhms, angles, rot] = descr_ellipse(Qres_Q)

	if verbose:
		print("4d resolution ellipsoid diagonal elements fwhm (coherent-elastic scattering) lengths:\n%s\n" \
			% (1./np.sqrt(np.abs(np.diag(Qres_Q))) * sig2fwhm))

		print("4d resolution ellipsoid principal axes fwhm lengths:\n%s\n" % fwhms)

		Qres_proj = proj_quad(Qres_Q, 2)
		Qres_proj = proj_quad(Qres_proj, 1)
		Qres_proj = proj_quad(Qres_proj, 0)
		print("Incoherent-elastic fwhm width: %.4f meV\n" % (1./np.sqrt(np.abs(Qres_proj[0,0])) * sig2fwhm))


	# 2d sliced ellipses
	Qres_QxE = np.delete(np.delete(Qres_Q, 2, axis=0), 2, axis=1)
	Qres_QxE = np.delete(np.delete(Qres_QxE, 1, axis=0), 1, axis=1)
	[fwhms_QxE, angles_QxE, rot_QxE] = descr_ellipse(Qres_QxE)
	if verbose:
		print("2d Qx,E sliced ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QxE, angles_QxE[0]))

	Qres_QyE = np.delete(np.delete(Qres_Q, 2, axis=0), 2, axis=1)
	Qres_QyE = np.delete(np.delete(Qres_QyE, 0, axis=0), 0, axis=1)
	[fwhms_QyE, angles_QyE, rot_QyE] = descr_ellipse(Qres_QyE)
	if verbose:
		print("2d Qy,E sliced ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QyE, angles_QyE[0]))

	Qres_QzE = np.delete(np.delete(Qres_Q, 1, axis=0), 1, axis=1)
	Qres_QzE = np.delete(np.delete(Qres_QzE, 0, axis=0), 0, axis=1)
	[fwhms_QzE, angles_QzE, rot_QzE] = descr_ellipse(Qres_QzE)
	if verbose:
		print("2d Qz,E sliced ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QzE, angles_QzE[0]))

	Qres_QxQy = np.delete(np.delete(Qres_Q, 3, axis=0), 3, axis=1)
	Qres_QxQy = np.delete(np.delete(Qres_QxQy, 2, axis=0), 2, axis=1)
	[fwhms_QxQy, angles_QxQy, rot_QxQy] = descr_ellipse(Qres_QxQy)
	if verbose:
		print("2d Qx,Qy sliced ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QxQy, angles_QxQy[0]))


	# 2d projected ellipses
	Qres_QxE_proj = np.delete(np.delete(Qres_Q, 2, axis=0), 2, axis=1)
	Qres_QxE_proj = proj_quad(Qres_QxE_proj, 1)
	[fwhms_QxE_proj, angles_QxE_proj, rot_QxE_proj] = descr_ellipse(Qres_QxE_proj)
	if verbose:
		print("2d Qx,E projected ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QxE_proj, angles_QxE_proj[0]))

	Qres_QyE_proj = np.delete(np.delete(Qres_Q, 2, axis=0), 2, axis=1)
	Qres_QyE_proj = proj_quad(Qres_QyE_proj, 0)
	[fwhms_QyE_proj, angles_QyE_proj, rot_QyE_proj] = descr_ellipse(Qres_QyE_proj)
	if verbose:
		print("2d Qy,E projected ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QyE_proj, angles_QyE_proj[0]))

	Qres_QzE_proj = np.delete(np.delete(Qres_Q, 1, axis=0), 1, axis=1)
	Qres_QzE_proj = proj_quad(Qres_QzE_proj, 0)
	[fwhms_QzE_proj, angles_QzE_proj, rot_QzE_proj] = descr_ellipse(Qres_QzE_proj)
	if verbose:
		print("2d Qz,E projected ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QzE_proj, angles_QzE_proj[0]))

	Qres_QxQy_proj = proj_quad(Qres_Q, 3)
	Qres_QxQy_proj = np.delete(np.delete(Qres_QxQy_proj, 2, axis=0), 2, axis=1)
	[fwhms_QxQy_proj, angles_QxQy_proj, rot_QxQy_proj] = descr_ellipse(Qres_QxQy_proj)
	if verbose:
		print("2d Qx,Qy projected ellipse fwhm lengths and slope angle:\n%s, %.4f\n" % (fwhms_QxQy_proj, angles_QxQy_proj[0]))



	return [fwhms_QxE, rot_QxE, fwhms_QyE, rot_QyE, \
		fwhms_QzE, rot_QzE, fwhms_QxQy, rot_QxQy, \
		fwhms_QxE_proj, rot_QxE_proj, fwhms_QyE_proj, rot_QyE_proj, \
		fwhms_QzE_proj, rot_QzE_proj, fwhms_QxQy_proj, rot_QxQy_proj]



#
# shows the 2d ellipses
#
def plot_ellipses(file, Q4, w, Qmean, \
	fwhms_QxE, rot_QxE, fwhms_QyE, rot_QyE, \
	fwhms_QzE, rot_QzE, fwhms_QxQy, rot_QxQy, \
	fwhms_QxE_proj, rot_QxE_proj, fwhms_QyE_proj, rot_QyE_proj, \
	fwhms_QzE_proj, rot_QzE_proj, fwhms_QxQy_proj, rot_QxQy_proj):

	import mpl_toolkits.mplot3d as mplot3d
	import matplotlib.pyplot as plot

	ellfkt = lambda rad, rot, phi, Qmean2d : \
		np.dot(rot, np.array([ rad[0]*np.cos(phi), rad[1]*np.sin(phi) ])) + Qmean2d


	# centre plots on zero or mean Q vector ?
	QxE = np.array([[0], [0]])
	QyE = np.array([[0], [0]])
	QzE = np.array([[0], [0]])
	QxQy = np.array([[0], [0]])

	if centre_on_Q:
		QxE = np.array([[Qmean[0]], [Qmean[3]]])
		QyE = np.array([[Qmean[1]], [Qmean[3]]])
		QzE = np.array([[Qmean[2]], [Qmean[3]]])
		QxQy = np.array([[Qmean[0]], [Qmean[1]]])


	phi = np.linspace(0, 2.*np.pi, ellipse_points)

	ell_QxE = ellfkt(fwhms_QxE*0.5, rot_QxE, phi, QxE)
	ell_QyE = ellfkt(fwhms_QyE*0.5, rot_QyE, phi, QyE)
	ell_QzE = ellfkt(fwhms_QzE*0.5, rot_QzE, phi, QzE)
	ell_QxQy = ellfkt(fwhms_QxQy*0.5, rot_QxQy, phi, QxQy)

	ell_QxE_proj = ellfkt(fwhms_QxE_proj*0.5, rot_QxE_proj, phi, QxE)
	ell_QyE_proj = ellfkt(fwhms_QyE_proj*0.5, rot_QyE_proj, phi, QyE)
	ell_QzE_proj = ellfkt(fwhms_QzE_proj*0.5, rot_QzE_proj, phi, QzE)
	ell_QxQy_proj = ellfkt(fwhms_QxQy_proj*0.5, rot_QxQy_proj, phi, QxQy)


	thesymsize = symsize * w
	themarker = "."


	# Qpara, E axis
	fig = plot.figure()
	subplot_QxE = fig.add_subplot(221)
	subplot_QxE.set_xlabel("Qpara (1/A)")
	subplot_QxE.set_ylabel("E (meV)")
	if len(Q4.shape)==2 and len(Q4)>0 and len(Q4[0])==4:
		subplot_QxE.scatter(Q4[:, 0], Q4[:, 3], marker=themarker, s=thesymsize)
	subplot_QxE.plot(ell_QxE[0], ell_QxE[1], c="black", linestyle="dashed")
	subplot_QxE.plot(ell_QxE_proj[0], ell_QxE_proj[1], c="black", linestyle="solid")

	# Qperp, E axis
	subplot_QyE = fig.add_subplot(222)
	subplot_QyE.set_xlabel("Qperp (1/A)")
	subplot_QyE.set_ylabel("E (meV)")
	if len(Q4.shape)==2 and len(Q4)>0 and len(Q4[0])==4:
		subplot_QyE.scatter(Q4[:, 1], Q4[:, 3], marker=themarker, s=thesymsize)
	subplot_QyE.plot(ell_QyE[0], ell_QyE[1], c="black", linestyle="dashed")
	subplot_QyE.plot(ell_QyE_proj[0], ell_QyE_proj[1], c="black", linestyle="solid")

	# Qup, E axis
	subplot_QzE = fig.add_subplot(223)
	subplot_QzE.set_xlabel("Qup (1/A)")
	subplot_QzE.set_ylabel("E (meV)")
	if len(Q4.shape)==2 and len(Q4)>0 and len(Q4[0])==4:
		subplot_QzE.scatter(Q4[:, 2], Q4[:, 3], marker=themarker, s=thesymsize)
	subplot_QzE.plot(ell_QzE[0], ell_QzE[1], c="black", linestyle="dashed")
	subplot_QzE.plot(ell_QzE_proj[0], ell_QzE_proj[1], c="black", linestyle="solid")

	# Qpara, Qperp axis
	subplot_QxQy = fig.add_subplot(224)
	subplot_QxQy.set_xlabel("Qpara (1/A)")
	subplot_QxQy.set_ylabel("Qperp (1/A)")
	if len(Q4.shape)==2 and len(Q4)>0 and len(Q4[0])==4:
		subplot_QxQy.scatter(Q4[:, 0], Q4[:, 1], marker=themarker, s=thesymsize)
	subplot_QxQy.plot(ell_QxQy[0], ell_QxQy[1], c="black", linestyle="dashed")
	subplot_QxQy.plot(ell_QxQy_proj[0], ell_QxQy_proj[1], c="black", linestyle="solid")
	plot.tight_layout()


	# 3d plot
	fig3d = plot.figure()
	subplot3d = fig3d.add_subplot(111, projection="3d")

	subplot3d.set_xlabel("Qpara (1/A)")
	subplot3d.set_ylabel("Qperp (1/A)")
	subplot3d.set_zlabel("E (meV)")

	subplot3d.scatter(Q4[:,0], Q4[:,1], Q4[:,3], marker=themarker, s=thesymsize)
	# xE
	subplot3d.plot(ell_QxE[0], ell_QxE[1], zs=0., zdir="y", c="black", linestyle="dashed")
	subplot3d.plot(ell_QxE_proj[0], ell_QxE_proj[1], zs=0., zdir="y", c="black", linestyle="solid")
	# yE
	subplot3d.plot(ell_QyE[0], ell_QyE[1], zs=0., zdir="x", c="black", linestyle="dashed")
	subplot3d.plot(ell_QyE_proj[0], ell_QyE_proj[1], zs=0., zdir="x", c="black", linestyle="solid")
	# xy
	subplot3d.plot(ell_QxQy[0], ell_QxQy[1], zs=0., zdir="z", c="black", linestyle="dashed")
	subplot3d.plot(ell_QxQy_proj[0], ell_QxQy_proj[1], zs=0., zdir="z", c="black", linestyle="solid")


	if file != "":
		splitext = os.path.splitext(file)
		file3d = splitext[0] + "_3d" + splitext[1]

		if verbose:
			print("Saving 2d plot to \"%s\"." % file)
			print("Saving 3d plot to \"%s\"." % file3d)
		fig.savefig(file, dpi=dpi)
		fig3d.savefig(file3d, dpi=dpi)

	if plot_results:
		plot.show()



#
# checks versions of needed packages
#
def check_versions():
	npver = np.version.version.split(".")
	if int(npver[0]) >= 2:
		return
	if int(npver[0]) < 1 or int(npver[1]) < 10:
		print("Numpy version >= 1.10 is required, but installed version is %s." % np.version.version)
		exit(-1)



#
# main
#
if __name__ == "__main__":
	print("This is a covariance matrix calculator using neutron events, written by T. Weber <tweber@ill.fr>, 30 March 2019.\n")
	check_versions()

	import argparse as arg

	args = arg.ArgumentParser(description="Calculates the covariance matrix of neutron scattering events.")
	args.add_argument("file", type=str, help="input file")
	args.add_argument("-s", "--save", default="", type=str, nargs="?", help="save plot to file")
	args.add_argument("--ellipse", default=ellipse_points, type=int, nargs="?", help="number of points to draw ellipses")
	args.add_argument("--ki", default=ki_start_idx, type=int, nargs="?", help="index of ki vector's first column in kikf file")
	args.add_argument("--kf", default=kf_start_idx, type=int, nargs="?", help="index of kf vector's first column in kikf file")
	args.add_argument("--wi", default=wi_idx, type=int, nargs="?", help="index of ki weight factor column in kikf file")
	args.add_argument("--wf", default=wf_idx, type=int, nargs="?", help="index of kf weight factor column in kikf file")
	args.add_argument("--w", default=w_idx, type=int, nargs="?", help="index of neutron weight factor column in QE file")
	args.add_argument("--Q", default=Q_start_idx, type=int, nargs="?", help="index of Q vector's first column in QE file")
	args.add_argument("--E", default=E_idx, type=int, nargs="?", help="index of E column in QE file")
	args.add_argument("--kikf", action="store_true", help="use the kikf file type")
	args.add_argument("--centreonQ", action="store_true", help="centre plots on mean Q")
	args.add_argument("--noverbose", action="store_true", help="don't show console logs")
	args.add_argument("--noplot", action="store_true", help="don't show any plot windows")
	args.add_argument("--noneutrons", action="store_true", help="don't show neutron events in plots")
	args.add_argument("--symsize", default=symsize, type=float, nargs="?", help="size of the symbols in plots")
	args.add_argument("--ax", default=None, type=float, nargs="?", help="x component of first orientation vector")
	args.add_argument("--ay", default=None, type=float, nargs="?", help="y component of first orientation vector")
	args.add_argument("--az", default=None, type=float, nargs="?", help="z component of first orientation vector")
	args.add_argument("--bx", default=None, type=float, nargs="?", help="x component of second orientation vector")
	args.add_argument("--by", default=None, type=float, nargs="?", help="y component of second orientation vector")
	args.add_argument("--bz", default=None, type=float, nargs="?", help="z component of second orientation vector")
	args.add_argument("--a", "--as", "-a", default=None, type=float, nargs="?", help="lattice constant a (only needed in case data is in rlu)")
	args.add_argument("--b", "--bs", "-b", default=None, type=float, nargs="?", help="lattice constant b (only needed in case data is in rlu)")
	args.add_argument("--c", "--cs", "-c", default=None, type=float, nargs="?", help="lattice constant c (only needed in case data is in rlu)")
	args.add_argument("--aa", "--alpha", default=90., type=float, nargs="?", help="lattice angle alpha (only needed in case data is in rlu)")
	args.add_argument("--bb", "--beta", default=90., type=float, nargs="?", help="lattice angle beta (only needed in case data is in rlu)")
	args.add_argument("--cc", "--gamma", default=90., type=float, nargs="?", help="lattice angle gamma (only needed in case data is in rlu)")
	args.add_argument("--dpi", default=dpi, type=int, nargs="?", help="DPI of output plot file")
	argv = args.parse_args()

	verbose = (argv.noverbose==False)
	plot_results = (argv.noplot==False)
	plot_neutrons = (argv.noneutrons==False)
	centre_on_Q = argv.centreonQ
	use_kikf_file = argv.kikf
	dpi = argv.dpi

	B = []
	if argv.a!=None and argv.b!=None and argv.c!=None and argv.aa!=None and argv.bb!=None and argv.cc!=None:
		lattice = np.array([ argv.a, argv.b, argv.c,  ])
		angles = np.array([ argv.aa, argv.bb, argv.cc ]) / 180.*np.pi

		B = tas.get_B(lattice, angles)

		if verbose:
			print("Crystal B matrix:\n%s\n" % B)


	infile = argv.file
	outfile = argv.save
	ellipse_points = argv.ellipse
	ki_start_idx = argv.ki
	kf_start_idx = argv.kf
	wi_idx = argv.wi
	wf_idx = argv.wf
	symsize = argv.symsize
	avec = [ argv.az, argv.ay, argv.az ]
	bvec = [ argv.bx, argv.by, argv.bz ]

	if use_kikf_file:
		[Q, E, w] = load_events_kikf(infile)
	else:
		[Q, E, w] = load_events_QE(infile)

		# convert rlu to 1/A 
		if len(B) != 0:
			Q = np.dot(Q, np.transpose(B))

	if avec[0]!=None and avec[1]!=None and avec[2]!=None and bvec[0]!=None and bvec[1]!=None and bvec[2]!=None:
		Qpara = np.array(avec)
		Qperp = np.array(bvec)

		# convert rlu to 1/A 
		if len(B) != 0:
			Qpara = np.dot(B, Qpara)
			Qperp = np.dot(B, Qperp)
	else:
		Qpara = np.array([])
		Qperp = np.array([])

	[Qres, Q4, Qmean] = calc_covar(Q, E, w, Qpara, Qperp)
	[fwhms_QxE, rot_QxE, fwhms_QyE, rot_QyE, \
	fwhms_QzE, rot_QzE, fwhms_QxQy, rot_QxQy, \
	fwhms_QxE_proj, rot_QxE_proj, fwhms_QyE_proj, rot_QyE_proj, \
	fwhms_QzE_proj, rot_QzE_proj, fwhms_QxQy_proj, rot_QxQy_proj] \
		= calc_ellipses(Qres)

	if plot_results or outfile!="":
		plot_ellipses(outfile, Q4, w, Qmean, \
			fwhms_QxE, rot_QxE, fwhms_QyE, rot_QyE, \
			fwhms_QzE, rot_QzE, fwhms_QxQy, rot_QxQy, \
			fwhms_QxE_proj, rot_QxE_proj, fwhms_QyE_proj, rot_QyE_proj, \
			fwhms_QzE_proj, rot_QzE_proj, fwhms_QxQy_proj, rot_QxQy_proj)