From 5f66554cdb1e1b30e672d0ed3b4080928244622f Mon Sep 17 00:00:00 2001
From: Felix Martin <mail@felixm.de>
Date: Mon, 13 Dec 2021 21:24:24 -0500
Subject: [PATCH] Eod commit. Debugging Q6.

---
 p4_tracking/inference.py | 53 ++++++++++++++++++++++++++++++++++------
 1 file changed, 45 insertions(+), 8 deletions(-)

diff --git a/p4_tracking/inference.py b/p4_tracking/inference.py
index 0cdd0f6..0e39a0a 100644
--- a/p4_tracking/inference.py
+++ b/p4_tracking/inference.py
@@ -425,7 +425,10 @@ class JointParticleFilter:
         Storing your particles as a Counter (where there could be an associated
         weight with each position) is incorrect and may produce errors.
         """
-        "*** YOUR CODE HERE ***"
+        tuples = list(itertools.product(*[self.legalPositions for _ in range(self.numGhosts)]))
+        random.shuffle(tuples)
+        numTuples = len(tuples)
+        self.particles = [tuples[i % numTuples] for i in range(self.numParticles)]
 
     def addGhostAgent(self, agent):
         """
@@ -466,13 +469,44 @@ class JointParticleFilter:
         a list, edited, and then converted back to a tuple. This is a common
         operation when placing a ghost in jail.
         """
-        pacmanPosition = gameState.getPacmanPosition()
         noisyDistances = gameState.getNoisyGhostDistances()
-        if len(noisyDistances) < self.numGhosts:
-            return
-        emissionModels = [busters.getObservationDistribution(dist) for dist in noisyDistances]
+        # if len(noisyDistances) < self.numGhosts:
+        #     return
+        pacmanPosition = gameState.getPacmanPosition()
+        emissionModels = [busters.getObservationDistribution(dist)
+                          for dist in noisyDistances]
 
-        "*** YOUR CODE HERE ***"
+        distribution, samples = [], []
+        for particle in self.particles:
+            weight = 1 # weight is likelihood over whole particle
+            for ghostIndex in range(self.numGhosts):
+                noisyDistance = noisyDistances[ghostIndex]
+                emissionModel = emissionModels[ghostIndex]
+                if noisyDistance is None:
+                    # Ghost is in jail.
+                    particle = self.getParticleWithGhostInJail(particle, ghostIndex)
+                else:
+                    # Find probability and update weight.
+                    ghostPosition = particle[ghostIndex]
+                    trueDistance = util.manhattanDistance(ghostPosition, pacmanPosition)
+                    probability = emissionModel[trueDistance]
+                    weight = weight * probability
+            distribution.append(weight)
+            samples.append(particle)
+
+        if not [p for p in distribution if p > 0.0]:
+            # All probabilities are zero so we have to reinitialize.
+            self.initializeParticles()
+            newParticles = []
+            # But then make sure that we move jailed ghosts into jail.
+            for particle in self.particles:
+                for ghostIndex in range(self.numGhosts):
+                    if noisyDistances[ghostIndex] is None:
+                        particle = self.getParticleWithGhostInJail(particle, ghostIndex)
+                newParticles.append(particle)
+            self.particles = newParticles
+        else:
+            self.particles = util.nSample(distribution, samples, self.numParticles)
 
     def getParticleWithGhostInJail(self, particle, ghostIndex):
         """
@@ -539,8 +573,11 @@ class JointParticleFilter:
         self.particles = newParticles
 
     def getBeliefDistribution(self):
-        "*** YOUR CODE HERE ***"
-        util.raiseNotDefined()
+        allPossible = util.Counter()
+        for particle in self.particles:
+            allPossible[particle] += 1
+        allPossible.normalize()
+        return allPossible
 
 # One JointInference module is shared globally across instances of MarginalInference
 jointInference = JointParticleFilter()