From d650a7a2688a60045782ab98212981586de93071 Mon Sep 17 00:00:00 2001
From: kub <derkub@gmail.com>
Date: Tue, 7 Dec 2021 21:13:23 +0100
Subject: [PATCH] core, some test code for team player and 4way play

---
 pico/memory.c | 79 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 pico/pico.h   |  2 ++
 2 files changed, 80 insertions(+), 1 deletion(-)

diff --git a/pico/memory.c b/pico/memory.c
index 85e2e377..f98375f4 100644
--- a/pico/memory.c
+++ b/pico/memory.c
@@ -256,6 +256,65 @@ out:
   return value;
 }
 
+static u32 read_pad_team(int i, u32 out_bits)
+{
+  u32 pad;
+  int phase = Pico.m.padTHPhase[i];
+  u32 value;
+
+  if (phase == 0) {
+    value = 0x03;
+    goto out;
+  }
+  if (phase == 1) {
+    value = 0x0f;
+    goto out;
+  }
+
+  pad = ~PicoIn.padInt[0]; // Get inverse of pad MXYZ SACB RLDU
+  if (phase == 8) {
+    value = pad & 0x0f;                          // ?x?x RLDU
+    goto out;
+  }
+  else if(phase == 9) {
+    value = (pad & 0xf0) >>  4;                  // ?x?x SACB
+    goto out;
+  }
+
+  pad = ~PicoIn.padInt[1]; // Get inverse of pad MXYZ SACB RLDU
+  if (phase == 12) {
+    value = pad & 0x0f;                          // ?x?x RLDU
+    goto out;
+  }
+  else if(phase == 13) {
+    value = (pad & 0xf0) >>  4;                  // ?x?x SACB
+    goto out;
+  }
+
+  if (phase >= 8 && pad < 16) {
+    value = 0x0f;
+    goto out;
+  }
+
+  value = 0;
+
+out:
+  value |= (out_bits & 0x40) | ((out_bits & 0x20)>>1);
+  return value;
+}
+
+static u32 read_pad_4way(int i, u32 out_bits)
+{
+  u32 pad = (PicoMem.ioports[2] & 0x70) >> 4;
+  u32 value = 0;
+
+  if (i == 0 && !(pad & 1))
+    value = read_pad_3btn(pad >> 1, out_bits);
+
+  value |= (out_bits & 0x40);
+  return value;
+}
+
 static u32 read_nothing(int i, u32 out_bits)
 {
   return 0xff;
@@ -307,6 +366,14 @@ void PicoSetInputDevice(int port, enum input_device device)
     func = read_pad_6btn;
     break;
 
+  case PICO_INPUT_PAD_TEAM:
+    func = read_pad_team;
+    break;
+
+  case PICO_INPUT_PAD_4WAY:
+    func = read_pad_4way;
+    break;
+
   default:
     func = read_nothing;
     break;
@@ -337,7 +404,17 @@ NOINLINE void io_ports_write(u32 a, u32 d)
   if (1 <= a && a <= 2)
   {
     Pico.m.padDelay[a - 1] = 0;
-    if (!(PicoMem.ioports[a] & 0x40) && (d & 0x40))
+    if (port_readers[a - 1] == read_pad_team) {
+      if (d & 0x40)
+        Pico.m.padTHPhase[a - 1] = 0;
+      else if ((d^PicoMem.ioports[a]) & 0x60)
+        Pico.m.padTHPhase[a - 1]++;
+    } else if (port_readers[a - 1] == read_pad_4way) {
+      if (a == 2 && ((PicoMem.ioports[a] ^ d) & 0x70))
+        Pico.m.padTHPhase[0] = 0;
+      if (a == 1 && !(PicoMem.ioports[a] & 0x40) && (d & 0x40))
+        Pico.m.padTHPhase[0]++;
+    } else if (!(PicoMem.ioports[a] & 0x40) && (d & 0x40))
       Pico.m.padTHPhase[a - 1]++;
   }
 
diff --git a/pico/pico.h b/pico/pico.h
index b35ce2fe..39fd838a 100644
--- a/pico/pico.h
+++ b/pico/pico.h
@@ -308,6 +308,8 @@ enum input_device {
   PICO_INPUT_NOTHING,
   PICO_INPUT_PAD_3BTN,
   PICO_INPUT_PAD_6BTN,
+  PICO_INPUT_PAD_TEAM,
+  PICO_INPUT_PAD_4WAY,
 };
 void PicoSetInputDevice(int port, enum input_device device);
 
-- 
2.39.2