//
//  Test_3x3transposeTimes.cpp
//  BulletTest
//
//  Copyright (c) 2011 Apple Inc.
//

#include "LinearMath/btScalar.h"
#if defined(BT_USE_SSE_IN_API) || defined(BT_USE_NEON)

#include "Test_3x3transposeTimes.h"
#include "vector.h"
#include "Utils.h"
#include "main.h"
#include <math.h>
#include <string.h>

#include <LinearMath/btMatrix3x3.h>

#define LOOPCOUNT 1000
#define ARRAY_SIZE 128

static inline btSimdFloat4 rand_f4(void)
{
	return btAssign128(RANDF_01, RANDF_01, RANDF_01, BT_NAN);  // w channel NaN
}

static btMatrix3x3 TransposeTimesReference(const btMatrix3x3 &in, const btMatrix3x3 &m)
{
	btVector3 m_el[3] = {in[0], in[1], in[2]};
	btSimdFloat4 r0 = btAssign128(m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
								  m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
								  m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
								  0.0f);
	btSimdFloat4 r1 = btAssign128(m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
								  m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
								  m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
								  0.0f);
	btSimdFloat4 r2 = btAssign128(m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
								  m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
								  m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z(),
								  0.0f);
	return btMatrix3x3(r0, r1, r2);
}

static int operator!=(const btMatrix3x3 &a, const btMatrix3x3 &b)
{
	if (a.getRow(0) != b.getRow(0))
		return 1;
	if (a.getRow(1) != b.getRow(1))
		return 1;
	if (a.getRow(2) != b.getRow(2))
		return 1;
	return 0;
}

int Test_3x3transposeTimes(void)
{
	// Init an array flanked by guard pages
	btMatrix3x3 in1[ARRAY_SIZE];
	btMatrix3x3 in2[ARRAY_SIZE];
	btMatrix3x3 out[ARRAY_SIZE];
	btMatrix3x3 out2[ARRAY_SIZE];

	float maxRelativeError = 0.f;
	// Init the data
	size_t i, j;
	for (i = 0; i < ARRAY_SIZE; i++)
	{
		in1[i] = btMatrix3x3(rand_f4(), rand_f4(), rand_f4());
		in2[i] = btMatrix3x3(rand_f4(), rand_f4(), rand_f4());

		out[i] = TransposeTimesReference(in1[i], in2[i]);
		out2[i] = in1[i].transposeTimes(in2[i]);

		if (out[i] != out2[i])
		{
			float relativeError = 0.f;

			for (int column = 0; column < 3; column++)
				for (int row = 0; row < 3; row++)
					relativeError = btMax(relativeError, btFabs(out2[i][row][column] - out[i][row][column]) / out[i][row][column]);

			if (relativeError > 1e-6)
			{
				vlog("failure @ %ld\n", i);
				btVector3 m0, m1, m2;
				m0 = out[i].getRow(0);
				m1 = out[i].getRow(1);
				m2 = out[i].getRow(2);

				vlog(
					"\ncorrect = (%10.4f, %10.4f, %10.4f, %10.4f) "
					"\n          (%10.4f, %10.4f, %10.4f, %10.4f) "
					"\n          (%10.4f, %10.4f, %10.4f, %10.4f) \n",
					m0.m_floats[0], m0.m_floats[1], m0.m_floats[2], m0.m_floats[3],
					m1.m_floats[0], m1.m_floats[1], m1.m_floats[2], m1.m_floats[3],
					m2.m_floats[0], m2.m_floats[1], m2.m_floats[2], m2.m_floats[3]);

				m0 = out2[i].getRow(0);
				m1 = out2[i].getRow(1);
				m2 = out2[i].getRow(2);

				vlog(
					"\ntested  = (%10.4f, %10.4f, %10.4f, %10.4f) "
					"\n          (%10.4f, %10.4f, %10.4f, %10.4f) "
					"\n          (%10.4f, %10.4f, %10.4f, %10.4f) \n",
					m0.m_floats[0], m0.m_floats[1], m0.m_floats[2], m0.m_floats[3],
					m1.m_floats[0], m1.m_floats[1], m1.m_floats[2], m1.m_floats[3],
					m2.m_floats[0], m2.m_floats[1], m2.m_floats[2], m2.m_floats[3]);

				return -1;
			}
			else
			{
				if (relativeError > maxRelativeError)
					maxRelativeError = relativeError;
			}
		}
	}

	if (maxRelativeError)
	{
		printf("Warning: maxRelativeError = %e\n", maxRelativeError);
	}
	uint64_t scalarTime, vectorTime;
	uint64_t startTime, bestTime, currentTime;
	bestTime = -1LL;
	scalarTime = 0;
	for (j = 0; j < LOOPCOUNT; j++)
	{
		startTime = ReadTicks();
		for (i = 0; i < ARRAY_SIZE; i++)
			out[i] = TransposeTimesReference(in1[i], in2[i]);
		currentTime = ReadTicks() - startTime;
		scalarTime += currentTime;
		if (currentTime < bestTime)
			bestTime = currentTime;
	}
	if (0 == gReportAverageTimes)
		scalarTime = bestTime;
	else
		scalarTime /= LOOPCOUNT;

	bestTime = -1LL;
	vectorTime = 0;
	for (j = 0; j < LOOPCOUNT; j++)
	{
		startTime = ReadTicks();
		for (i = 0; i < ARRAY_SIZE; i++)
			out[i] = in1[i].transposeTimes(in2[i]);
		currentTime = ReadTicks() - startTime;
		vectorTime += currentTime;
		if (currentTime < bestTime)
			bestTime = currentTime;
	}
	if (0 == gReportAverageTimes)
		vectorTime = bestTime;
	else
		vectorTime /= LOOPCOUNT;

	vlog("Timing:\n");
	vlog("\t    scalar\t    vector\n");
	vlog("\t%10.2f\t%10.2f\n", TicksToCycles(scalarTime) / ARRAY_SIZE, TicksToCycles(vectorTime) / ARRAY_SIZE);

	return 0;
}

#endif  //BT_USE_SSE