Fix color bug

Oh man, that was way harder to track down that I want to admit.
Couple of hours wasted, yeeeah.

src/cpu.rs

@@ -938,13 +938,25 @@ impl CPU {
             instruction = self.read_byte(self.ip);
             if self.debug {
                 print!(
-                    "{:#06x}: [SP: {:#04X}] i={:02X}. ",
+                    "{:#06X}: [SP: {:#04X}] i={:02X}. ",
                     &self.ip, &self.sp, &instruction
                 );
+                /*
                 for (idx, reg) in REG_NAMES.iter().enumerate() {
                     print!("{}: {:02X} ", reg, self.get_8bit_reg(idx));
                 }
                 print!("A: {:02X}  ", self.regs[REG_A]);
+                */
+
+                print!("AF={:02X}{:02X} BC={:02X}{:02X} DE={:02X}{:02X} HL={:02X}{:02X}  ",
+                       self.get_8bit_reg(REG_N_A),
+                       self.get_8bit_reg(REG_N_F),
+                       self.get_8bit_reg(REG_N_B),
+                       self.get_8bit_reg(REG_N_C),
+                       self.get_8bit_reg(REG_N_D),
+                       self.get_8bit_reg(REG_N_E),
+                       self.get_8bit_reg(REG_N_H),
+                       self.get_8bit_reg(REG_N_L));
                 print!("I: {:02X}  ", self.interconnect.read_byte(0xFFFF));
 
                 // Flags

src/display.rs

@@ -73,15 +73,35 @@ impl CgbPalette {
         let r = (v & 0b1_1111) as u8;
         let g = ((v >> 5) & 0b1_1111) as u8;
         let b = ((v >> 10) & 0b1_1111) as u8;
-        // According to some code:
+
-        // Real colors:
+        if false {
-        let r = r as u16;
+            sdl2::pixels::Color::RGB(r << 3, g << 3, b << 3)
-        let g = g as u16;
+        } else {
-        let b = b as u16;
+            // According to some code:
-        let mapped_r = ((r * 13 + g * 2 + b) >> 1) as u8;
+            // Real colors:
-        let mapped_g = ((g * 3 + b) << 1) as u8;
+            let r = r as u16;
-        let mapped_b = ((r * 3 + g * 2 + b * 11) >> 1) as u8;
+            let g = g as u16;
-        sdl2::pixels::Color::RGB(mapped_r, mapped_g, mapped_b)
+            let b = b as u16;
+            let mapped_r = ((r * 13 + g * 2 + b) >> 1) as u8;
+            let mapped_g = ((g * 3 + b) << 1) as u8;
+            let mapped_b = ((r * 3 + g * 2 + b * 11) >> 1) as u8;
+            sdl2::pixels::Color::RGB(mapped_r, mapped_g, mapped_b)
+        }
+    }
+}
+
+
+impl std::fmt::Display for CgbPalette {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        write!(f, "Palette: ")?;
+        for n in 0..4 {
+            let v = ((self.0[2 * n + 1] as u16) << 8) | (self.0[2 * n] as u16);
+            let r = (v & 0b1_1111) as u8;
+            let g = ((v >> 5) & 0b1_1111) as u8;
+            let b = ((v >> 10) & 0b1_1111) as u8;
+            write!(f, "{:02X}{:02X}{:02X} ", r, g, b)?;
+        }
+        write!(f, "")
     }
 }
 
@@ -127,7 +147,7 @@ impl Default for Pixel {
     }
 }
 
-#[derive(Debug)]
+#[derive(Debug, PartialEq)]
 enum DisplayMode {
     ReadOAMMemory,
     ReadFullMemory,
@@ -368,6 +388,10 @@ impl Display {
                 self.background_palette_autoinc = (val >> 7) != 0;
             }
             0xFF69 => {
+                if self.current_mode == DisplayMode::ReadFullMemory {
+                    println!("Trying to write to palette memory while being accessed!");
+                }
+
                 let idx = self.background_palette_index as usize;
                 if idx < 64 {
                     self.background_palette_cgb[idx / 8].0[idx % 8] = val;
@@ -656,7 +680,7 @@ impl Display {
                             PixelOrigin::Sprite,
                         );
                         */
-                        let c = ((b1 as u8) * 2 + b2 as u8) as usize;
+                        let c = ((b2 as u8) * 2 + b1 as u8) as usize;
                         if c == 0 {
                             continue;
                         }
@@ -766,8 +790,7 @@ impl Display {
 
                 // Obtain tile information
                 let tile_base_addr: usize = self.get_bg_window_tile_addr(tile_id);
-                // TODO: Colored background
+                let addr = tile_base_addr + (tile_offset_y as usize) * 2; // two bytes per row
-                let addr = tile_base_addr + (tile_offset_y as usize) * 2;
 
                 let tile_line_1 = vram[addr];
                 let tile_line_2 = vram[addr + 1];
@@ -787,7 +810,7 @@ impl Display {
                 let b2: bool = (tile_line_2 & b2) != 0;
 
                 // Lookup the color
-                let c = ((b1 as u8) * 2 + b2 as u8) as usize;
+                let c = ((b2 as u8) * 2 + b1 as u8) as usize;
                 let origin = match c {
                     0 => PixelOrigin::Empty, // Hack so that objects will be in front of it.
                     _ => PixelOrigin::Background,
@@ -873,7 +896,7 @@ impl Display {
                     let b1: bool = (tile_line_1 & 1 << (7 - tile_offset_x)) > 0;
                     let b2: bool = (tile_line_2 & 1 << (7 - tile_offset_x)) > 0;
 
-                    let c = (b1 as u8) * 2 + b2 as u8;
+                    let c = (b2 as u8) * 2 + b1 as u8;
                     let origin = match c {
                         0 => PixelOrigin::Empty, // Hack so that objects will be in front of it.
                         _ => PixelOrigin::Window,