/*
   This software is being provided to you, the licensee, by Ville Pulkki,
   under the following license. By obtaining, using and/or copying this
   software, you agree that you have read, understood, and will comply
   with these terms and conditions: Permission to use, copy, modify and
   distribute, including the right to grant others rights to distribute
   at any tier, this software and its documentation for any purpose and
   without fee or royalty is hereby granted, provided that you agree to
   comply with the following copyright notice and statements, including
   the disclaimer, and that the same appear on ALL copies of the software
   and documentation, including modifications that you make for internal
   use or for distribution:

   Copyright 1998 by Ville Pulkki, Helsinki University of Technology.  All
   rights reserved.

   The software may be used, distributed, and included to commercial
   products without any charges. When included to a commercial product,
   the method "Vector Base Amplitude Panning" and its developer Ville
   Pulkki must be referred to in documentation.

   This software is provided "as is", and Ville Pulkki or Helsinki
   University of Technology make no representations or warranties,
   expressed or implied. By way of example, but not limitation, Helsinki
   University of Technology or Ville Pulkki make no representations or
   warranties of merchantability or fitness for any particular purpose or
   that the use of the licensed software or documentation will not
   infringe any third party patents, copyrights, trademarks or other
   rights. The name of Ville Pulkki or Helsinki University of Technology
   may not be used in advertising or publicity pertaining to distribution
   of the software.
*/

#include <algorithm>
#include <cmath>
#include <stdlib.h>

#include "pbd/cartesian.h"

#include "vbap_speakers.h"

using namespace ARDOUR;
using namespace PBD;
using namespace std;

const double VBAPSpeakers::MIN_VOL_P_SIDE_LGTH = 0.01;

VBAPSpeakers::VBAPSpeakers (std::shared_ptr<Speakers> s)
	: _dimension (2)
	, _parent (s)
{
	_parent->Changed.connect_same_thread (speaker_connection, std::bind (&VBAPSpeakers::update, this));
	update ();
}

VBAPSpeakers::~VBAPSpeakers ()
{
}

void
VBAPSpeakers::update ()
{
	int dim = 2;

	_speakers = _parent->speakers ();

	for (vector<Speaker>::const_iterator i = _speakers.begin (); i != _speakers.end (); ++i) {
		if ((*i).angles ().ele != 0.0) {
			dim = 3;
			break;
		}
	}

	_dimension = dim;

	if (_speakers.size () < 2) {
		/* nothing to be done with less than two speakers */
		return;
	}

	if (_dimension == 3) {
		ls_triplet_chain* ls_triplets = 0;
		choose_speaker_triplets (&ls_triplets);
		if (ls_triplets) {
			calculate_3x3_matrixes (ls_triplets);
			free (ls_triplets);
		}
	} else {
		choose_speaker_pairs ();
	}
}

void
VBAPSpeakers::choose_speaker_triplets (struct ls_triplet_chain** ls_triplets)
{
	/* Selects the loudspeaker triplets, and
	 * calculates the inversion matrices for each selected triplet.
	 * A line (connection) is drawn between each loudspeaker. The lines
	 * denote the sides of the triangles. The triangles should not be
	 * intersecting. All crossing connections are searched and the
	 * longer connection is erased. This yields non-intesecting triangles,
	 * which can be used in panning.
	 */

#if 0 // DEVEL/DEBUG
	for (vector<Speaker>::iterator i = _speakers.begin(); i != _speakers.end(); ++i) {
		cout << "Speaker " << (*i).id << " @ "
		  << (*i).coords().x << ", " << (*i).coords().y << ", " << (*i).coords().z
		  << " azimuth " << (*i).angles().azi
		  << " elevation " << (*i).angles().ele
		  << " distance " << (*i).angles().length
		  << endl;
	}
#endif

	int i, j, k, l, table_size;
	int n_speakers = _speakers.size ();

	if (n_speakers < 3) {
		fprintf (stderr, "VBAP: at least 3 speakers need to be defined.");
		return;
	}

	/* variable length arrays arrived in C99, became optional in C11, and
	 * are only planned for C++14. Use alloca which is functionally
	 * identical (but uglier to read).
	 */
	int*   connections      = (int*)alloca (sizeof (int) * n_speakers * n_speakers);
	float* distance_table   = (float*)alloca (sizeof (float) * ((n_speakers * (n_speakers - 1)) / 2));
	int*   distance_table_i = (int*)alloca (sizeof (int) * ((n_speakers * (n_speakers - 1)) / 2));
	int*   distance_table_j = (int*)alloca (sizeof (int) * ((n_speakers * (n_speakers - 1)) / 2));
	float  distance;

	struct ls_triplet_chain *trip_ptr, *prev, *tmp_ptr;

	for (i = 0; i < n_speakers * n_speakers; i++) {
		connections[i] = 0;
	}

	for (i = 0; i < n_speakers; i++) {
		for (j = i + 1; j < n_speakers; j++) {
			for (k = j + 1; k < n_speakers; k++) {
				if (vol_p_side_lgth (i, j, k, _speakers) > MIN_VOL_P_SIDE_LGTH) {
					connections[(i * n_speakers) + j] = 1;
					connections[(j * n_speakers) + i] = 1;
					connections[(i * n_speakers) + k] = 1;
					connections[(k * n_speakers) + i] = 1;
					connections[(j * n_speakers) + k] = 1;
					connections[(k * n_speakers) + j] = 1;
					add_ldsp_triplet (i, j, k, ls_triplets);
				}
			}
		}
	}

	/*calculate distancies between all speakers and sorting them*/
	table_size = (((n_speakers - 1) * (n_speakers)) / 2);
	for (i = 0; i < table_size; i++) {
		distance_table[i] = 100000.0;
	}

	for (i = 0; i < n_speakers; i++) {
		for (j = i + 1; j < n_speakers; j++) {
			if (connections[(i * n_speakers) + j] == 1) {
				distance = fabs (vec_angle (_speakers[i].coords (), _speakers[j].coords ()));
				k        = 0;
				while (distance_table[k] < distance) {
					k++;
				}
				for (l = table_size - 1; l > k; l--) {
					distance_table[l]   = distance_table[l - 1];
					distance_table_i[l] = distance_table_i[l - 1];
					distance_table_j[l] = distance_table_j[l - 1];
				}
				distance_table[k]   = distance;
				distance_table_i[k] = i;
				distance_table_j[k] = j;
			} else
				table_size--;
		}
	}

	/* disconnecting connections which are crossing shorter ones,
	 * starting from shortest one and removing all that cross it,
	 * and proceeding to next shortest */
	for (i = 0; i < table_size; i++) {
		int fst_ls = distance_table_i[i];
		int sec_ls = distance_table_j[i];
		if (connections[(fst_ls * n_speakers) + sec_ls] == 1) {
			for (j = 0; j < n_speakers; j++) {
				for (k = j + 1; k < n_speakers; k++) {
					if ((j != fst_ls) && (k != sec_ls) && (k != fst_ls) && (j != sec_ls)) {
						if (lines_intersect (fst_ls, sec_ls, j, k) == 1) {
							connections[(j * n_speakers) + k] = 0;
							connections[(k * n_speakers) + j] = 0;
						}
					}
				}
			}
		}
	}

	/* remove triangles which had crossing sides
	 * with smaller triangles or include loudspeakers*/
	trip_ptr = *ls_triplets;
	prev     = 0;
	while (trip_ptr != 0) {
		i = trip_ptr->ls_nos[0];
		j = trip_ptr->ls_nos[1];
		k = trip_ptr->ls_nos[2];
		if (connections[(i * n_speakers) + j] == 0 ||
		    connections[(i * n_speakers) + k] == 0 ||
		    connections[(j * n_speakers) + k] == 0 ||
		    any_ls_inside_triplet (i, j, k) == 1) {
			if (prev != 0) {
				prev->next = trip_ptr->next;
				tmp_ptr    = trip_ptr;
				trip_ptr   = trip_ptr->next;
				free (tmp_ptr);
			} else {
				*ls_triplets = trip_ptr->next;
				tmp_ptr      = trip_ptr;
				trip_ptr     = trip_ptr->next;
				free (tmp_ptr);
			}
		} else {
			prev     = trip_ptr;
			trip_ptr = trip_ptr->next;
		}
	}
}

int
VBAPSpeakers::any_ls_inside_triplet (int a, int b, int c)
{
	/* returns 1 if there is loudspeaker(s) inside given ls triplet */
	float                  invdet;
	const CartesianVector* lp1;
	const CartesianVector* lp2;
	const CartesianVector* lp3;
	float                  invmx[9];
	int                    i, j;
	float                  tmp;
	bool                   any_ls_inside;
	bool                   this_inside;
	int                    n_speakers = _speakers.size ();

	lp1 = &(_speakers[a].coords ());
	lp2 = &(_speakers[b].coords ());
	lp3 = &(_speakers[c].coords ());

	/* matrix inversion */
	invdet = 1.0 / (  lp1->x * ((lp2->y * lp3->z) - (lp2->z * lp3->y))
	                - lp1->y * ((lp2->x * lp3->z) - (lp2->z * lp3->x))
	                + lp1->z * ((lp2->x * lp3->y) - (lp2->y * lp3->x)));

	invmx[0] = ((lp2->y * lp3->z) - (lp2->z * lp3->y)) * invdet;
	invmx[3] = ((lp1->y * lp3->z) - (lp1->z * lp3->y)) * -invdet;
	invmx[6] = ((lp1->y * lp2->z) - (lp1->z * lp2->y)) * invdet;
	invmx[1] = ((lp2->x * lp3->z) - (lp2->z * lp3->x)) * -invdet;
	invmx[4] = ((lp1->x * lp3->z) - (lp1->z * lp3->x)) * invdet;
	invmx[7] = ((lp1->x * lp2->z) - (lp1->z * lp2->x)) * -invdet;
	invmx[2] = ((lp2->x * lp3->y) - (lp2->y * lp3->x)) * invdet;
	invmx[5] = ((lp1->x * lp3->y) - (lp1->y * lp3->x)) * -invdet;
	invmx[8] = ((lp1->x * lp2->y) - (lp1->y * lp2->x)) * invdet;

	any_ls_inside = false;
	for (i = 0; i < n_speakers; i++) {
		if (i != a && i != b && i != c) {
			this_inside = true;
			for (j = 0; j < 3; j++) {
				tmp = _speakers[i].coords ().x * invmx[0 + j * 3];
				tmp += _speakers[i].coords ().y * invmx[1 + j * 3];
				tmp += _speakers[i].coords ().z * invmx[2 + j * 3];
				if (tmp < -0.001) {
					this_inside = false;
				}
			}
			if (this_inside) {
				any_ls_inside = true;
			}
		}
	}

	return any_ls_inside;
}

void
VBAPSpeakers::add_ldsp_triplet (int i, int j, int k, struct ls_triplet_chain** ls_triplets)
{
	/* adds i,j,k triplet to triplet chain*/

	struct ls_triplet_chain *trip_ptr, *prev;
	trip_ptr = *ls_triplets;
	prev     = 0;

	while (trip_ptr != 0) {
		prev     = trip_ptr;
		trip_ptr = trip_ptr->next;
	}

	trip_ptr = (struct ls_triplet_chain*)malloc (sizeof (struct ls_triplet_chain));

	if (prev == 0) {
		*ls_triplets = trip_ptr;
	} else {
		prev->next = trip_ptr;
	}

	trip_ptr->next      = 0;
	trip_ptr->ls_nos[0] = i;
	trip_ptr->ls_nos[1] = j;
	trip_ptr->ls_nos[2] = k;
}

double
VBAPSpeakers::vec_angle (CartesianVector v1, CartesianVector v2)
{
	double inner = ((v1.x * v2.x + v1.y * v2.y + v1.z * v2.z) /
	                (vec_length (v1) * vec_length (v2)));

	if (inner > 1.0) {
		inner = 1.0;
	}

	if (inner < -1.0) {
		inner = -1.0;
	}

	return fabs (acos (inner));
}

double
VBAPSpeakers::vec_length (CartesianVector v1)
{
	double rv = sqrt (v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
	if (rv > 1e-14)
		return rv;
	return 0;
}

double
VBAPSpeakers::vec_prod (CartesianVector v1, CartesianVector v2)
{
	return (v1.x * v2.x + v1.y * v2.y + v1.z * v2.z);
}

double
VBAPSpeakers::vol_p_side_lgth (int i, int j, int k, const vector<Speaker>& speakers)
{
	/* calculate volume of the parallelepiped defined by the loudspeaker
	 * direction vectors and divide it with total length of the triangle sides.
	 * This is used when removing too narrow triangles. */

	double          volper, lgth;
	CartesianVector xprod;
	cross_prod (speakers[i].coords (), speakers[j].coords (), &xprod);
	volper = fabs (vec_prod (xprod, speakers[k].coords ()));
	lgth   = (fabs (vec_angle (speakers[i].coords (), speakers[j].coords ())) + fabs (vec_angle (speakers[i].coords (), speakers[k].coords ())) + fabs (vec_angle (speakers[j].coords (), speakers[k].coords ())));
	if (lgth > 0.00001) {
		return volper / lgth;
	} else {
		return 0.0;
	}
}

void
VBAPSpeakers::cross_prod (CartesianVector v1, CartesianVector v2, CartesianVector* res)
{
	double length;

	res->x = (v1.y * v2.z) - (v1.z * v2.y);
	res->y = (v1.z * v2.x) - (v1.x * v2.z);
	res->z = (v1.x * v2.y) - (v1.y * v2.x);

	length = vec_length (*res);
	if (length > 0) {
		res->x /= length;
		res->y /= length;
		res->z /= length;
	} else {
		res->x = 0;
		res->y = 0;
		res->z = 0;
	}
}

int
VBAPSpeakers::lines_intersect (int i, int j, int k, int l)
{
	/* checks if two lines intersect on 3D sphere
	 * see theory in paper Pulkki, V. Lokki, T. "Creating Auditory Displays
	 * with Multiple Loudspeakers Using VBAP: A Case Study with
	 * DIVA Project" in International Conference on
	 * Auditory Displays -98. E-mail Ville.Pulkki@hut.fi
	 * if you want to have that paper.
	 */

	CartesianVector v1;
	CartesianVector v2;
	CartesianVector v3, neg_v3;

	float dist_ij, dist_kl, dist_iv3, dist_jv3, dist_inv3, dist_jnv3;
	float dist_kv3, dist_lv3, dist_knv3, dist_lnv3;

	cross_prod (_speakers[i].coords (), _speakers[j].coords (), &v1);
	cross_prod (_speakers[k].coords (), _speakers[l].coords (), &v2);
	cross_prod (v1, v2, &v3);

	neg_v3.x = 0.0 - v3.x;
	neg_v3.y = 0.0 - v3.y;
	neg_v3.z = 0.0 - v3.z;

	dist_ij   = (vec_angle (_speakers[i].coords (), _speakers[j].coords ()));
	dist_kl   = (vec_angle (_speakers[k].coords (), _speakers[l].coords ()));
	dist_iv3  = (vec_angle (_speakers[i].coords (), v3));
	dist_jv3  = (vec_angle (v3, _speakers[j].coords ()));
	dist_inv3 = (vec_angle (_speakers[i].coords (), neg_v3));
	dist_jnv3 = (vec_angle (neg_v3, _speakers[j].coords ()));
	dist_kv3  = (vec_angle (_speakers[k].coords (), v3));
	dist_lv3  = (vec_angle (v3, _speakers[l].coords ()));
	dist_knv3 = (vec_angle (_speakers[k].coords (), neg_v3));
	dist_lnv3 = (vec_angle (neg_v3, _speakers[l].coords ()));

	/* if one of loudspeakers is close to crossing point, don't do anything*/
	if (fabsf (dist_iv3) <= 0.01 || fabsf (dist_jv3) <= 0.01 ||
	    fabsf (dist_kv3) <= 0.01 || fabsf (dist_lv3) <= 0.01 ||
	    fabsf (dist_inv3) <= 0.01 || fabsf (dist_jnv3) <= 0.01 ||
	    fabsf (dist_knv3) <= 0.01 || fabsf (dist_lnv3) <= 0.01) {
		return (0);
	}

	/* if crossing point is on line between both loudspeakers return 1 */
	if (((fabsf (dist_ij - (dist_iv3 + dist_jv3)) <= 0.01) &&
	     (fabsf (dist_kl - (dist_kv3 + dist_lv3)) <= 0.01)) ||
	    ((fabsf (dist_ij - (dist_inv3 + dist_jnv3)) <= 0.01) &&
	     (fabsf (dist_kl - (dist_knv3 + dist_lnv3)) <= 0.01))) {
		return (1);
	} else {
		return (0);
	}
}

void
VBAPSpeakers::calculate_3x3_matrixes (struct ls_triplet_chain* ls_triplets)
{
	/* Calculates the inverse matrices for 3D */
	float                    invdet;
	const CartesianVector*   lp1;
	const CartesianVector*   lp2;
	const CartesianVector*   lp3;
	float*                   invmx;
	struct ls_triplet_chain* tr_ptr        = ls_triplets;
	int                      triplet_count = 0;
	int                      triplet;

	assert (tr_ptr);

	/* counting triplet amount */

	while (tr_ptr != 0) {
		triplet_count++;
		tr_ptr = tr_ptr->next;
	}

#if 0 // DEVEL/DEBUG
	cerr << "@@@ VBAP triplets generate " << triplet_count << " of speaker tuples\n";
#endif

	triplet = 0;

	_matrices.clear ();
	_speaker_tuples.clear ();

	for (int n = 0; n < triplet_count; ++n) {
		_matrices.push_back (threeDmatrix ());
		_speaker_tuples.push_back (tmatrix ());
	}

	tr_ptr = ls_triplets;
	while (tr_ptr != 0) {
		lp1 = &(_speakers[tr_ptr->ls_nos[0]].coords ());
		lp2 = &(_speakers[tr_ptr->ls_nos[1]].coords ());
		lp3 = &(_speakers[tr_ptr->ls_nos[2]].coords ());

		/* matrix inversion */
		invmx  = tr_ptr->inv_mx;
		invdet = 1.0 / (  lp1->x * ((lp2->y * lp3->z) - (lp2->z * lp3->y))
		                - lp1->y * ((lp2->x * lp3->z) - (lp2->z * lp3->x))
		                + lp1->z * ((lp2->x * lp3->y) - (lp2->y * lp3->x)));

		invmx[0] = ((lp2->y * lp3->z) - (lp2->z * lp3->y)) * invdet;
		invmx[3] = ((lp1->y * lp3->z) - (lp1->z * lp3->y)) * -invdet;
		invmx[6] = ((lp1->y * lp2->z) - (lp1->z * lp2->y)) * invdet;
		invmx[1] = ((lp2->x * lp3->z) - (lp2->z * lp3->x)) * -invdet;
		invmx[4] = ((lp1->x * lp3->z) - (lp1->z * lp3->x)) * invdet;
		invmx[7] = ((lp1->x * lp2->z) - (lp1->z * lp2->x)) * -invdet;
		invmx[2] = ((lp2->x * lp3->y) - (lp2->y * lp3->x)) * invdet;
		invmx[5] = ((lp1->x * lp3->y) - (lp1->y * lp3->x)) * -invdet;
		invmx[8] = ((lp1->x * lp2->y) - (lp1->y * lp2->x)) * invdet;

		/* copy the matrix */

		_matrices[triplet][0] = invmx[0];
		_matrices[triplet][1] = invmx[1];
		_matrices[triplet][2] = invmx[2];
		_matrices[triplet][3] = invmx[3];
		_matrices[triplet][4] = invmx[4];
		_matrices[triplet][5] = invmx[5];
		_matrices[triplet][6] = invmx[6];
		_matrices[triplet][7] = invmx[7];
		_matrices[triplet][8] = invmx[8];

		_speaker_tuples[triplet][0] = tr_ptr->ls_nos[0];
		_speaker_tuples[triplet][1] = tr_ptr->ls_nos[1];
		_speaker_tuples[triplet][2] = tr_ptr->ls_nos[2];

#if 0 // DEVEL/DEBUG
		cerr << "Triplet[" << triplet << "] = "
		     << tr_ptr->ls_nos[0] << " + "
		     << tr_ptr->ls_nos[1] << " + "
		     << tr_ptr->ls_nos[2] << endl;
#endif

		triplet++;

		tr_ptr = tr_ptr->next;
	}
}

void
VBAPSpeakers::choose_speaker_pairs ()
{
	/* selects the loudspeaker pairs, calculates the inversion
	 * matrices and stores the data to a global array
	*/
	const int n_speakers = _speakers.size ();

	if (n_speakers < 2) {
		fprintf (stderr, "VBAP: at least 2 speakers need to be defined.");
		return;
	}

	const double AZIMUTH_DELTA_THRESHOLD_DEGREES = (180.0 / M_PI) * (M_PI - 0.175);
	/* variable length arrays arrived in C99, became optional in C11, and
	 * are only planned for C++14. Use alloca which is functionally
	 * identical (but uglier to read).
	 */
	int*    sorted_speakers = (int*)alloca (sizeof (int) * n_speakers);
	bool*   exists          = (bool*)alloca (sizeof (bool) * n_speakers);
	double* inverse_matrix  = (double*)alloca (sizeof (double) * n_speakers * 4);
	int     expected_pairs  = 0;
	int     pair;
	int     speaker;

	for (speaker = 0; speaker < n_speakers; ++speaker) {
		exists[speaker] = false;
	}

	/* sort loudspeakers according their aximuth angle */
#ifdef __clang_analyzer__
	// sort_2D_lss() assigns values to all of sorted_speakers
	// "uninitialized value"
	memset (sorted_speakers, 0, sizeof (*sorted_speakers));
#endif
	sort_2D_lss (sorted_speakers);

	/* adjacent loudspeakers are the loudspeaker pairs to be used.*/
	for (speaker = 0; speaker < n_speakers - 1; speaker++) {
		if ((_speakers[sorted_speakers[speaker + 1]].angles ().azi -
		     _speakers[sorted_speakers[speaker]].angles ().azi) <= AZIMUTH_DELTA_THRESHOLD_DEGREES) {
			if (calc_2D_inv_tmatrix (_speakers[sorted_speakers[speaker]].angles ().azi,
			                         _speakers[sorted_speakers[speaker + 1]].angles ().azi,
			                         &inverse_matrix[4 * speaker]) != 0) {
				exists[speaker] = true;
				expected_pairs++;
			}
		}
	}

	if (((6.283 - _speakers[sorted_speakers[n_speakers - 1]].angles ().azi) + _speakers[sorted_speakers[0]].angles ().azi) <= AZIMUTH_DELTA_THRESHOLD_DEGREES) {
		if (calc_2D_inv_tmatrix (_speakers[sorted_speakers[n_speakers - 1]].angles ().azi,
		                         _speakers[sorted_speakers[0]].angles ().azi,
		                         &inverse_matrix[4 * (n_speakers - 1)]) != 0) {
			exists[n_speakers - 1] = true;
			expected_pairs++;
		}
	}

	pair = 0;

	_matrices.clear ();
	_speaker_tuples.clear ();

	for (int n = 0; n < expected_pairs; ++n) {
		_matrices.push_back (twoDmatrix ());
		_speaker_tuples.push_back (tmatrix ());
	}

	for (speaker = 0; speaker < n_speakers - 1; speaker++) {
		if (exists[speaker]) {
			_matrices[pair][0] = inverse_matrix[(speaker * 4) + 0];
			_matrices[pair][1] = inverse_matrix[(speaker * 4) + 1];
			_matrices[pair][2] = inverse_matrix[(speaker * 4) + 2];
			_matrices[pair][3] = inverse_matrix[(speaker * 4) + 3];

			_speaker_tuples[pair][0] = sorted_speakers[speaker];
			_speaker_tuples[pair][1] = sorted_speakers[speaker + 1];

			pair++;
		}
	}

	if (exists[n_speakers - 1]) {
		_matrices[pair][0] = inverse_matrix[(speaker * 4) + 0];
		_matrices[pair][1] = inverse_matrix[(speaker * 4) + 1];
		_matrices[pair][2] = inverse_matrix[(speaker * 4) + 2];
		_matrices[pair][3] = inverse_matrix[(speaker * 4) + 3];

		_speaker_tuples[pair][0] = sorted_speakers[n_speakers - 1];
		_speaker_tuples[pair][1] = sorted_speakers[0];
	}
}

void
VBAPSpeakers::sort_2D_lss (int* sorted_speakers)
{
	vector<Speaker>           tmp = _speakers;
	vector<Speaker>::iterator s;
	azimuth_sorter            sorter;
	unsigned int              n;

	sort (tmp.begin (), tmp.end (), sorter);

	for (n = 0, s = tmp.begin (); s != tmp.end (); ++s, ++n) {
		sorted_speakers[n] = (*s).id;
	}
	assert (n == _speakers.size ());
}

int
VBAPSpeakers::calc_2D_inv_tmatrix (double azi1, double azi2, double* inverse_matrix)
{
	double x1, x2, x3, x4;
	double det;

	x1  = cos (azi1 * (M_PI / 180.0));
	x2  = sin (azi1 * (M_PI / 180.0));
	x3  = cos (azi2 * (M_PI / 180.0));
	x4  = sin (azi2 * (M_PI / 180.0));
	det = (x1 * x4) - (x3 * x2);

	if (fabs (det) <= 0.001) {
		inverse_matrix[0] = 0.0;
		inverse_matrix[1] = 0.0;
		inverse_matrix[2] = 0.0;
		inverse_matrix[3] = 0.0;

		return 0;

	} else {
		inverse_matrix[0] = x4 / det;
		inverse_matrix[1] = -x3 / det;
		inverse_matrix[2] = -x2 / det;
		inverse_matrix[3] = x1 / det;

		return 1;
	}
}