From 26cce7c58fd1770ad61e97ceb5325c6d4ce5af81 Mon Sep 17 00:00:00 2001
From: wheremyfoodat <gponiris2004@gmail.com>
Date: Sun, 12 Mar 2023 04:55:06 +0200
Subject: [PATCH] [PICA] Implement madi

---
 include/PICA/shader.hpp              |  1 +
 src/core/PICA/shader_interpreter.cpp | 31 ++++++++++++++++++++++++++--
 2 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/include/PICA/shader.hpp b/include/PICA/shader.hpp
index 2f9c4688..f249cd0a 100644
--- a/include/PICA/shader.hpp
+++ b/include/PICA/shader.hpp
@@ -103,6 +103,7 @@ class PICAShader {
 	void jmpu(u32 instruction);
 	void loop(u32 instruction);
 	void mad(u32 instruction);
+	void madi(u32 instruction);
 	void max(u32 instruction);
 	void min(u32 instruction);
 	void mov(u32 instruction);
diff --git a/src/core/PICA/shader_interpreter.cpp b/src/core/PICA/shader_interpreter.cpp
index bb5fd1d6..de23d05c 100644
--- a/src/core/PICA/shader_interpreter.cpp
+++ b/src/core/PICA/shader_interpreter.cpp
@@ -37,6 +37,10 @@ void PICAShader::run() {
 			case ShaderOpcodes::RCP: rcp(instruction); break;
 			case ShaderOpcodes::RSQ: rsq(instruction); break;
 
+			case 0x30: case 0x31: case 0x32: case 0x33: case 0x34: case 0x35: case 0x36: case 0x37:
+				madi(instruction);
+				break;
+
 			case 0x38: case 0x39: case 0x3A: case 0x3B: case 0x3C: case 0x3D: case 0x3E: case 0x3F:
 				mad(instruction);
 				break;
@@ -158,12 +162,12 @@ void PICAShader::add(u32 instruction) {
 
 void PICAShader::mul(u32 instruction) {
 	const u32 operandDescriptor = operandDescriptors[instruction & 0x7f];
-	const u32 src1 = (instruction >> 12) & 0x7f;
+	u32 src1 = (instruction >> 12) & 0x7f;
 	const u32 src2 = (instruction >> 7) & 0x1f; // src2 coming first because PICA moment
 	const u32 idx = (instruction >> 19) & 3;
 	const u32 dest = (instruction >> 21) & 0x1f;
 
-	if (idx) Helpers::panic("[PICA] MUL: idx != 0");
+	src1 = getIndexedSource(src1, idx);
 	vec4f srcVec1 = getSourceSwizzled<1>(src1, operandDescriptor);
 	vec4f srcVec2 = getSourceSwizzled<2>(src2, operandDescriptor);
 
@@ -379,6 +383,29 @@ void PICAShader::mad(u32 instruction) {
 	}
 }
 
+void PICAShader::madi(u32 instruction) {
+	const u32 operandDescriptor = operandDescriptors[instruction & 0x1f];
+	const u32 src1 = (instruction >> 17) & 0x1f;
+	const u32 src2 = (instruction >> 12) & 0x1f;
+	u32 src3 = (instruction >> 5) & 0x7f;
+	const u32 idx = (instruction >> 22) & 3;
+	const u32 dest = (instruction >> 24) & 0x1f;
+
+	src3 = getIndexedSource(src3, idx);
+
+	auto srcVec1 = getSourceSwizzled<1>(src1, operandDescriptor);
+	auto srcVec2 = getSourceSwizzled<2>(src2, operandDescriptor);
+	auto srcVec3 = getSourceSwizzled<3>(src3, operandDescriptor);
+	auto& destVector = getDest(dest);
+
+	u32 componentMask = operandDescriptor & 0xf;
+	for (int i = 0; i < 4; i++) {
+		if (componentMask & (1 << i)) {
+			destVector[3 - i] = srcVec1[3 - i] * srcVec2[3 - i] + srcVec3[3 - i];
+		}
+	}
+}
+
 void PICAShader::cmp(u32 instruction) {
 	const u32 operandDescriptor = operandDescriptors[instruction & 0x7f];
 	const u32 src1 = (instruction >> 12) & 0x7f;