#include <iostream>
#include <random>

#include <nvmath.h>

int main(int argc, char** argv)
{
	std::mt19937 e(time(nullptr));
	std::uniform_real_distribution<float> posGenerator(-10.f, 10.f);
	std::uniform_real_distribution<float> scaleGenerator(.5f, 5.f);
	std::uniform_real_distribution<float> angleGenerator(.0f, nv_two_pi);

	nvmath::vec3f rayOrigin{ posGenerator(e), posGenerator(e), posGenerator(e) };
	nvmath::vec3f rayTarget{ posGenerator(e), posGenerator(e), posGenerator(e) };
	nvmath::vec3f rayDir = nvmath::normalize(rayTarget - rayOrigin);
	std::cout << "Generated ray with origin: (" << rayOrigin.x << ", " << rayOrigin.y << ", " << rayOrigin.z << ") and direction: (" << rayDir.x << ", " << rayDir.y << ", " << rayDir.z << ")" << std::endl;

	float scalar = nvmath::length(rayOrigin) - 1.f;
	nvmath::vec3f normal = rayOrigin / nvmath::length(rayOrigin);
	nvmath::vec3f tracedPos = rayOrigin - normal * scalar;
	std::cout << "Calculated traced position on sphere: (" << tracedPos.x << ", " << tracedPos.y << ", " << tracedPos.z << ")" << std::endl;
	if (abs(nvmath::length(tracedPos) - 1.f) <= 1e-5)
	{
		std::cout << "It is examined on sphere surface. Algorithm succeeded with error = " << abs(nvmath::length(tracedPos) - 1.f) << "." << std::endl;

		nvmath::vec3f one = nvmath::normalize(nvmath::vec3f_one);
		auto matrixTRS = nvmath::translation_mat4(nvmath::vec3f{ posGenerator(e), posGenerator(e), posGenerator(e) });
		matrixTRS *= nvmath::rotation_yaw_pitch_roll(angleGenerator(e), angleGenerator(e), angleGenerator(e));
		matrixTRS *= nvmath::scale_mat4(nvmath::vec3f{ scaleGenerator(e), scaleGenerator(e), scaleGenerator(e) });
		auto inverseTRS = nvmath::inverse(matrixTRS);

		nvmath::vec4f objectPos = inverseTRS * nvmath::vec4f{ rayOrigin, 1.f };
		nvmath::vec3f originObjPos = { objectPos.x, objectPos.y, objectPos.z };
		scalar = nvmath::length(originObjPos) - 1.f;
		normal = originObjPos / nvmath::length(originObjPos);
		tracedPos = originObjPos - normal * scalar;
		
		auto worldPos = matrixTRS * nvmath::vec4f{ tracedPos, 1.f };
		tracedPos = { worldPos.x, worldPos.y, worldPos.z };
		float realScalar = nvmath::length(tracedPos - rayOrigin);

		nvmath::vec4f objectVec = matrixTRS * nvmath::vec4f{ one, .0f };
		float transformedScalar = scalar * nvmath::length(objectVec);

		//std::cout << "Distance in object space = " << scalar << std::endl;
		//std::cout << "Real distance = " << realScalar << ", with distance calculated from transforming = " << transformedScalar << std::endl;
		std::cout << "norm(M*pos_traced - rayorigin) = " << realScalar << std::endl;
		std::cout << "norm(-M*grad_pos) = " << nvmath::length(matrixTRS * nvmath::vec4f{ normal, .0f }) << ", norm(-M*grad_pos) * scalar_pos = " << nvmath::length(matrixTRS * nvmath::vec4f{ normal, .0f }) * scalar << std::endl;
		std::cout << "distance calculated from transforming = " << transformedScalar << std::endl;
	}
	else
		std::cout << "It is not on sphere surface. Algorithm failed" << std::endl;

	return 0;
}