const a0   = 3.56679     # a0 for diamond   [A]
const felE = 10000.
const felAngle = 30 * pi/180
const hc = 12398.42               # physical constant [eV * A]

# Useful constants
const lxr    = hc / felE
const lxr4pi = lxr / (4*pi)

# Defines k0 in lab frame
const k0     = 2*pi/lxr * [sin(felAngle); 0; cos(felAngle)]

# mRadians calculated with XOP (could be improved)
const laue_cutoff111 = 21.35e-6 * 20
const laue_cutoff220 = 15.16e-6 * 20
const laue_cutoff311 = 10.53e-6 * 20
const laue_cutoff400 = 12.34e-6 * 20

const p111 = [
             1  1  1;
            -1  1  1;
             1 -1  1;
             1  1 -1;
            -1 -1 -1;
             1 -1 -1;
            -1  1 -1;
            -1 -1  1]

const p220 = [
             2  2  0;
            -2  2  0;
             2  0  2;
            -2  0  2;
             0  2  2;
             0 -2  2;
            -2 -2  0;
             2 -2  0;
            -2  0 -2;
             2  0 -2;
             0 -2 -2;
             0  2 -2]

function reussFit(a, invCmat, sigma_h, t, N, dalpha, i_range, j_min,
                  hist_results=false, texture=false, Ph=0)
    # Set up stress tensor in lab frame
    S_samp = -1. * [
            sigma_h-t/3  0             0;
            0            sigma_h-t/3   0;
            0            0             sigma_h+2*t/3]

    # Initialize arrays
    data111 = Float64[]
    data220 = Float64[]
    data311 = Float64[]
    data400 = Float64[]
    samp    = Float64[]

    stre_vec = [
        S_samp[1,1]; S_samp[2,2]; S_samp[3,3];
        S_samp[2,3]; S_samp[3,1]; S_samp[1,2]]

    #println("S_samp:   $S_samp")
    #println("invCmat:  $invCmat")
    #println("stre_vec: $stre_vec")

    # Calculate strain vector
    stra_vec = invCmat * stre_vec
    #println("stra_vec $stra_vec")

    for i in i_range
        for j in j_min:i+1
            xp, yp  = project(j,i,N)
            h, k, l = normalize([j; i; N])

            push!(samp, xp, yp)

            # Calculate rotation matrix, add eps to avoid div by 0
            for alpha in (0:dalpha:360) * pi/180
                RM    = fullR(alpha, h,k,l)

                #println("RM: $RM")

                # Transform stress tensor into crystal coordinates
                S_crys = RM * (S_samp * RM')

                # Stress vector in crystal coordinates
                stress_vec = [
                    S_crys[1,1]; S_crys[2,2]; S_crys[3,3];
                    S_crys[2,3]; S_crys[3,1]; S_crys[1,2]]

                #println("stress_vec: $stress_vec")

                # Calculated strain of crystal vectors
                e_xx, e_yy, e_zz, e_yz, e_zx, e_xy = invCmat * stress_vec

                #println("e_xx, e_yy, e_zz, e_yz, e_zx, e_xy: $e_xx, $e_yy, $e_zz, $e_yz, $e_zx, $e_xy")

                # Calculate deformed lattice vectors using strains
                def_a = (1+e_xx)*a * [
                        cos(e_xy)*cos(e_zx);
                        sin(e_xy);
                        sin(e_zx)]
                def_b = (1+e_yy)*a * [
                        sin(e_xy);
                        cos(e_xy)*cos(e_yz);
                        sin(e_yz)]
                def_c = (1+e_zz)*a * [
                        sin(e_zx),
                        sin(e_yz),
                        cos(e_zx)*cos(e_yz)]

                #println("def_a, def_b, def_c, $def_a, $def_b, $def_c")

                # Calculate compressed cell volume, compression
                V    = abs(dot(def_a, cross(def_b, def_c)))
                comp = a^3 / V
                rnf  = 2*pi / V

                # Calculate reciprocal lattice
                rda = rnf * cross(def_b, def_c)
                rdb = rnf * cross(def_c, def_a)
                rdc = rnf * cross(def_a, def_b)

                #println("rda, rdb, rdc: $rda, $rdb, $rdc")

                # Analyze compressed planes
                for p in 1:size(p111)[1]
                    d, ks, fitv = analyze_plane(p111[p,:], rda,rdb,rdc, RM)

                    kx, ky, kz = ks

                    if fitv < laue_cutoff111
                        push!(data111, fitv, d, kx, ky, kz, comp, h, k, l)
                    end
                end

                for p in 1:size(p220)[1]
                    d, ks, fitv = analyze_plane(p220[p,:], rda,rdb,rdc, RM)

                    kx, ky, kz = ks

                    if fitv < laue_cutoff220
                        push!(data220, fitv, d, kx, ky, kz, comp, h, k, l)
                    end
                end

                #= Skip 311 and 400 planes for now
                for p in arange(len(p311)):
                    [d, ks, fitv] = analyze_plane(p311[p,:], rda,rdb,rdc, RM)

                    kx,ky,kz = ks

                    if fitv < laue_cutoff311:
                        data311.append([fitv, d, kx, ky, kz, comp, h, k, l])

                for p in arange(len(p400)):
                    [d, ks, fitv] = analyze_plane(p400[p,:], rda,rdb,rdc, RM)

                    kx,ky,kz = ks

                    if fitv < laue_cutoff400:
                        data400.append([fitv, d, kx, ky, kz, comp, h, k, l])
                =#
            end
        end
    end

    data111 = reshape(data111, (9, div(length(data111),9)))
    data220 = reshape(data220, (9, div(length(data220),9)))
    samp    = reshape(samp, (2, div(length(samp),2)))

    return data111, data220, samp
end

function analyze_plane(hkl, rda, rdb, rdc, R)
    # Return d spacing, outgoing wavevector, and fit value for plane hkl
    Gp      = hkl[1]*rda + hkl[2]*rdb + hkl[3]*rdc
    GpNorm  = norm(Gp)
    d       = 2*pi / GpNorm

    # Convert G to lab frame
    G   = R' * Gp
    k   = k0 + G
    fit = abs(asin(lxr4pi*GpNorm) + asin(dot(k0,G) / (2*pi/lxr*GpNorm)))

    return d, k, fit
end

function project(x, y, z)
    # Project (xyz) to (xy) in inverse pole figure
    den = sqrt(x^2 + y^2 + z^2) + z
    xp  = y/den
    yp  = x/den

    return xp, yp
end

function R_y(theta)
    # Rotation matrix for angle theta about y axis
    return [
            cos(theta)    0   sin(theta);
            0             1   0;
            -sin(theta)   0   cos(theta)]
end

function R_z(theta)
    # Rotation matrix for angle theta about z axis
    return [
            cos(theta)   -sin(theta)   0;
            sin(theta)   cos(theta)    0;
            0            0             1]
end

function fullR(alpha, h,k,l)
    # Rotation matrix for crystallite with [hkl] normal to surface
    # with rotation alpha, where alpha = 0 defined by ...
    beta  = acos(l / sqrt(h^2 + k^2 + l^2))
    gamma = acos(h / sqrt(h^2 + k^2 + eps()))

    return R_z(alpha) * (R_y(beta) * R_z(gamma))
end