LAB3/src/LFO.vhd

@@ -1,40 +1,154 @@
-    library IEEE;
+LIBRARY IEEE;
-use IEEE.STD_LOGIC_1164.ALL;
+USE IEEE.STD_LOGIC_1164.ALL;
 
 -- Uncomment the following library declaration if using
 -- arithmetic functions with Signed or Unsigned values
-use IEEE.NUMERIC_STD.ALL;
+USE IEEE.NUMERIC_STD.ALL;
 
-entity LFO is
+ENTITY LFO IS
-    generic(
+    GENERIC (
-        CHANNEL_LENGHT  : integer := 24;
+        CHANNEL_LENGHT : INTEGER := 24;
-        JOYSTICK_LENGHT  : integer := 10;
+        JOYSTICK_LENGHT : INTEGER := 10;
-        CLK_PERIOD_NS   : integer := 10;
+        CLK_PERIOD_NS : INTEGER := 10;
-        TRIANGULAR_COUNTER_LENGHT    : integer := 10 -- Triangular wave period length
+        TRIANGULAR_COUNTER_LENGHT : INTEGER := 10 -- Triangular wave period length
     );
-    Port (
+    PORT (
 
-            aclk			: in std_logic;
+        aclk : IN STD_LOGIC;
-            aresetn			: in std_logic;
+        aresetn : IN STD_LOGIC;
 
-            lfo_period      : in std_logic_vector(JOYSTICK_LENGHT-1 downto 0);
+        lfo_period : IN STD_LOGIC_VECTOR(JOYSTICK_LENGHT - 1 DOWNTO 0);
 
-            lfo_enable      : in std_logic;
+        lfo_enable : IN STD_LOGIC;
 
-            s_axis_tvalid	: in std_logic;
+        s_axis_tvalid : IN STD_LOGIC;
-            s_axis_tdata	: in std_logic_vector(CHANNEL_LENGHT-1 downto 0);
+        s_axis_tdata : IN STD_LOGIC_VECTOR(CHANNEL_LENGHT - 1 DOWNTO 0);
-            s_axis_tlast    : in std_logic;
+        s_axis_tlast : IN STD_LOGIC;
-            s_axis_tready	: out std_logic;
+        s_axis_tready : OUT STD_LOGIC;
 
-            m_axis_tvalid	: out std_logic;
+        m_axis_tvalid : OUT STD_LOGIC;
-            m_axis_tdata	: out std_logic_vector(CHANNEL_LENGHT-1 downto 0);
+        m_axis_tdata : OUT STD_LOGIC_VECTOR(CHANNEL_LENGHT - 1 DOWNTO 0);
-            m_axis_tlast	: out std_logic;
+        m_axis_tlast : OUT STD_LOGIC;
-            m_axis_tready	: in std_logic
+        m_axis_tready : IN STD_LOGIC
     );
-end entity LFO;
+END ENTITY LFO;
 
-architecture Behavioral of LFO is
+ARCHITECTURE Behavioral OF LFO IS
 
-begin  
+    CONSTANT LFO_COUNTER_BASE_PERIOD_NS : INTEGER := 100000;
+    CONSTANT ADJUSTMENT_FACTOR : INTEGER := 90;
 
-end architecture;
+    SIGNAL step_counter : INTEGER := 1;
+    SIGNAL tri_counter : signed(CHANNEL_LENGHT - 1 DOWNTO 0) := (OTHERS => '0');
+    SIGNAL direction_up : STD_LOGIC := '1';
+    SIGNAL lfo_tick : STD_LOGIC := '0';
+
+    SIGNAL lfo_period_int : INTEGER := LFO_COUNTER_BASE_PERIOD_NS;
+    SIGNAL m_axis_tvalid_i : STD_LOGIC := '0';
+    SIGNAL m_axis_tdata_i : STD_LOGIC_VECTOR(CHANNEL_LENGHT - 1 DOWNTO 0) := (OTHERS => '0');
+    SIGNAL m_axis_tlast_i : STD_LOGIC := '0';
+    SIGNAL s_axis_tready_i : STD_LOGIC := '1';
+    SIGNAL temp : STD_LOGIC_VECTOR(CHANNEL_LENGHT + TRIANGULAR_COUNTER_LENGHT - 1 DOWNTO 0) := (OTHERS => '0');
+
+BEGIN
+
+    PROCESS (aclk)
+    BEGIN
+        IF rising_edge(aclk) THEN
+            lfo_period_int <= LFO_COUNTER_BASE_PERIOD_NS - ADJUSTMENT_FACTOR * to_integer(unsigned(lfo_period));
+        END IF;
+    END PROCESS;
+
+    -- Optimized single process for LFO step and triangular waveform generation
+    PROCESS (aclk)
+    BEGIN
+        IF rising_edge(aclk) THEN
+            IF aresetn = '0' THEN
+                step_counter <= 0;
+                tri_counter <= (OTHERS => '0');
+                direction_up <= '1';
+                lfo_tick <= '0';
+            ELSIF lfo_enable = '1' THEN
+                IF step_counter < lfo_period_int THEN
+                    step_counter <= step_counter + 1;
+                    lfo_tick <= '0';
+                ELSE
+                    step_counter <= 0;
+                    lfo_tick <= '1';
+
+                    IF direction_up = '1' THEN
+                        IF tri_counter = 2 ** TRIANGULAR_COUNTER_LENGHT - 1 THEN
+                            direction_up <= '0';
+                            tri_counter <= tri_counter - 1;
+                        ELSE
+                            tri_counter <= tri_counter + 1;
+                        END IF;
+                    ELSE
+                        IF tri_counter = 0 THEN
+                            direction_up <= '1';
+                            tri_counter <= tri_counter + 1;
+                        ELSE
+                            tri_counter <= tri_counter - 1;
+                        END IF;
+                    END IF;
+                END IF;
+            ELSE
+                lfo_tick <= '0';
+                direction_up <= '1';
+                tri_counter <= (OTHERS => '0');
+                step_counter <= 0;
+            END IF;
+        END IF;
+    END PROCESS;
+    PROCESS (aclk)
+    BEGIN
+        IF rising_edge(aclk) THEN
+            IF aresetn = '0' THEN
+                temp <= (OTHERS => '0');
+            ELSIF s_axis_tvalid = '1' AND m_axis_tready = '1' AND lfo_enable = '1' THEN
+                temp <= STD_LOGIC_VECTOR(
+                    resize(
+                    signed(s_axis_tdata) * signed(resize(tri_counter, s_axis_tdata'length)),
+                    temp'length
+                    )
+                    );
+            END IF;
+        END IF;
+    END PROCESS;
+
+    PROCESS (aclk)
+    BEGIN
+        IF rising_edge(aclk) THEN
+            IF aresetn = '0' THEN
+                m_axis_tvalid_i <= '0';
+                m_axis_tdata_i <= (OTHERS => '0');
+                m_axis_tlast_i <= '0';
+            ELSE
+                IF s_axis_tvalid = '1' AND m_axis_tready = '1' THEN
+                    IF lfo_enable = '1' THEN
+                        m_axis_tdata_i <= temp(temp'high DOWNTO temp'high - (CHANNEL_LENGHT - 1));
+                    ELSE
+                        m_axis_tdata_i <= s_axis_tdata;
+                    END IF;
+                    s_axis_tready_i <= '0';
+                    m_axis_tvalid_i <= '1';
+                    m_axis_tlast_i <= s_axis_tlast;
+                END IF;
+                IF m_axis_tvalid_i = '1' THEN
+                    IF m_axis_tready = '0' THEN
+                        s_axis_tready_i <= '0';
+                    ELSE
+                        s_axis_tready_i <= '1';
+                    END IF;
+                    m_axis_tvalid_i <= '0';
+                END IF;
+            END IF;
+        END IF;
+    END PROCESS;
+
+    s_axis_tready <= s_axis_tready_i;
+    m_axis_tdata <= m_axis_tdata_i;
+    m_axis_tvalid <= m_axis_tvalid_i;
+    m_axis_tlast <= m_axis_tlast_i;
+
+END ARCHITECTURE Behavioral;

LAB3/src/all_pass_filter.vhd

@@ -1,29 +1,69 @@
-library IEEE;
+LIBRARY IEEE;
-use IEEE.STD_LOGIC_1164.ALL;
+USE IEEE.STD_LOGIC_1164.ALL;
-use ieee.numeric_std.all;
+USE ieee.numeric_std.ALL;
 
-entity all_pass_filter is
+ENTITY all_pass_filter IS
-	generic (
+	GENERIC (
-		TDATA_WIDTH		: positive := 24
+		TDATA_WIDTH : POSITIVE := 24
 	);
-	Port (
+	PORT (
-		aclk			: in std_logic;
+		aclk : IN STD_LOGIC;
-		aresetn			: in std_logic;
+		aresetn : IN STD_LOGIC;
 
-		s_axis_tvalid	: in std_logic;
+		s_axis_tvalid : IN STD_LOGIC;
-		s_axis_tdata	: in std_logic_vector(TDATA_WIDTH-1 downto 0);
+		s_axis_tdata : IN STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
-		s_axis_tlast	: in std_logic;
+		s_axis_tlast : IN STD_LOGIC;
-		s_axis_tready	: out std_logic;
+		s_axis_tready : OUT STD_LOGIC;
 
-		m_axis_tvalid	: out std_logic;
+		m_axis_tvalid : OUT STD_LOGIC;
-		m_axis_tdata	: out std_logic_vector(TDATA_WIDTH-1 downto 0);
+		m_axis_tdata : OUT STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
-		m_axis_tlast	: out std_logic;
+		m_axis_tlast : OUT STD_LOGIC;
-		m_axis_tready	: in std_logic
+		m_axis_tready : IN STD_LOGIC
 	);
-end all_pass_filter;
+END all_pass_filter;
 
-architecture Behavioral of all_pass_filter is
+ARCHITECTURE Behavioral OF all_pass_filter IS
 
-begin
+	SIGNAL s_axis_tready_int : STD_LOGIC := '0';
+	SIGNAL m_axis_tvalid_int : STD_LOGIC := '0';
 
-end Behavioral;
+BEGIN
+
+	-- Output assignments
+	s_axis_tready <= s_axis_tready_int;
+	m_axis_tvalid <= m_axis_tvalid_int;
+
+	PROCESS (aclk)
+	BEGIN
+		IF rising_edge(aclk) THEN
+
+			IF aresetn = '0' THEN
+				s_axis_tready_int <= '0';
+				m_axis_tvalid_int <= '0';
+
+			ELSE
+				-- Clear valid flag when master interface is ready
+				IF m_axis_tready = '1' THEN
+					m_axis_tvalid_int <= '0';
+				END IF;
+
+				IF s_axis_tvalid = '1' AND s_axis_tready_int = '1' THEN
+					IF m_axis_tvalid_int = '0' OR m_axis_tready = '1' THEN
+						s_axis_tready_int <= '1'; -- Keep reading from slave interface
+						m_axis_tvalid_int <= '1'; -- Set valid flag for master interface
+                        m_axis_tdata <= s_axis_tdata;
+						m_axis_tlast <= s_axis_tlast;
+
+                    ELSE
+                        s_axis_tready_int <= '0'; -- Block slave interface to avoid data loss
+
+                    END IF;
+				END IF;
+
+			END IF;
+
+	END IF;
+
+END PROCESS;
+
+END Behavioral;

LAB3/src/moving_average_filter.vhd

@@ -1,32 +1,99 @@
-library IEEE;
+LIBRARY IEEE;
-use IEEE.STD_LOGIC_1164.ALL;
+USE IEEE.STD_LOGIC_1164.ALL;
-use ieee.numeric_std.all;
+USE ieee.numeric_std.ALL;
 
-entity moving_average_filter is
+ENTITY moving_average_filter IS
-	generic (
+	GENERIC (
 		-- Filter order expressed as 2^(FILTER_ORDER_POWER)
-		FILTER_ORDER_POWER	: integer := 5;
+		FILTER_ORDER_POWER : INTEGER := 5;
 
-		TDATA_WIDTH			: positive := 24
+		TDATA_WIDTH : POSITIVE := 24
 	);
-	Port (
+	PORT (
-		aclk			: in std_logic;
+		aclk : IN STD_LOGIC;
-		aresetn			: in std_logic;
+		aresetn : IN STD_LOGIC;
 
-		s_axis_tvalid	: in std_logic;
+		s_axis_tvalid : IN STD_LOGIC;
-		s_axis_tdata	: in std_logic_vector(TDATA_WIDTH-1 downto 0);
+		s_axis_tdata : IN STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
-		s_axis_tlast	: in std_logic;
+		s_axis_tlast : IN STD_LOGIC;
-		s_axis_tready	: out std_logic;
+		s_axis_tready : OUT STD_LOGIC;
 
-		m_axis_tvalid	: out std_logic;
+		m_axis_tvalid : OUT STD_LOGIC;
-		m_axis_tdata	: out std_logic_vector(TDATA_WIDTH-1 downto 0);
+		m_axis_tdata : OUT STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
-		m_axis_tlast	: out std_logic;
+		m_axis_tlast : OUT STD_LOGIC;
-		m_axis_tready	: in std_logic
+		m_axis_tready : IN STD_LOGIC
 	);
-end moving_average_filter;
+END moving_average_filter;
 
-architecture Behavioral of moving_average_filter is
+ARCHITECTURE Behavioral OF moving_average_filter IS
 
-begin
+	CONSTANT FILTER_ORDER : INTEGER := 2 ** FILTER_ORDER_POWER;
 
-end Behavioral;
+	TYPE sample_array IS ARRAY (0 TO FILTER_ORDER - 1) OF signed(TDATA_WIDTH - 1 DOWNTO 0);
+	SIGNAL samples : sample_array := (OTHERS => (OTHERS => '0'));
+
+	SIGNAL sum : signed(TDATA_WIDTH + FILTER_ORDER_POWER - 1 DOWNTO 0) := (OTHERS => '0');
+	SIGNAL sample_count : INTEGER RANGE 0 TO FILTER_ORDER := 0;
+
+	SIGNAL m_axis_tvalid_int : STD_LOGIC := '0';
+
+BEGIN
+
+	-- Output assignments
+	m_axis_tvalid <= m_axis_tvalid_int;
+	s_axis_tready <= m_axis_tready OR NOT m_axis_tvalid_int;
+
+	PROCESS (aclk)
+
+		VARIABLE new_sum : signed(TDATA_WIDTH + FILTER_ORDER_POWER - 1 DOWNTO 0);
+		VARIABLE oldest_sample : signed(TDATA_WIDTH - 1 DOWNTO 0);
+		VARIABLE avg : signed(TDATA_WIDTH - 1 DOWNTO 0);
+		VARIABLE wr_ptr : INTEGER RANGE 0 TO FILTER_ORDER - 1 := 0;
+
+	BEGIN
+
+		IF rising_edge(aclk) THEN
+
+			IF aresetn = '0' THEN
+				samples <= (OTHERS => (OTHERS => '0'));
+				sum <= (OTHERS => '0');
+				sample_count <= 0;
+				m_axis_tvalid_int <= '0';
+				m_axis_tlast <= '0';
+				m_axis_tdata <= (OTHERS => '0');
+				wr_ptr := 0;
+
+			ELSE
+				m_axis_tvalid_int <= '0';
+				m_axis_tlast <= '0';
+
+				IF s_axis_tvalid = '1' AND (m_axis_tready = '1' OR m_axis_tvalid_int = '0') THEN
+					-- Circular buffer
+					oldest_sample := samples(wr_ptr);
+					samples(wr_ptr) <= signed(s_axis_tdata);
+					wr_ptr := (wr_ptr + 1) MOD FILTER_ORDER;
+
+					-- Aggiorna la somma
+					new_sum := sum - oldest_sample + signed(s_axis_tdata);
+					sum <= new_sum;
+
+					-- Aggiorna il conteggio solo fino a 32
+					IF sample_count < FILTER_ORDER THEN
+						sample_count <= sample_count + 1;
+					END IF;
+
+					-- Calcola la media sempre su 32 (anche se sample_count < 32)
+					avg := new_sum(TDATA_WIDTH + FILTER_ORDER_POWER - 1 DOWNTO FILTER_ORDER_POWER);
+
+					m_axis_tdata <= STD_LOGIC_VECTOR(avg);
+					m_axis_tvalid_int <= '1';
+					m_axis_tlast <= s_axis_tlast;
+
+				END IF;
+			END IF;
+
+		END IF;
+
+	END PROCESS;
+
+END Behavioral;

LAB3/src/moving_average_filter_en.vhd

@@ -1,34 +1,148 @@
-library IEEE;
+LIBRARY IEEE;
-use IEEE.STD_LOGIC_1164.ALL;
+USE IEEE.STD_LOGIC_1164.ALL;
-use ieee.numeric_std.all;
+USE ieee.numeric_std.ALL;
 
-entity moving_average_filter_en is
+ENTITY moving_average_filter_en IS
-	generic (
+	GENERIC (
 		-- Filter order expressed as 2^(FILTER_ORDER_POWER)
-		FILTER_ORDER_POWER	: integer := 5;
+		FILTER_ORDER_POWER : INTEGER := 5;
 
-		TDATA_WIDTH		: positive := 24
+		TDATA_WIDTH : POSITIVE := 24
 	);
-	Port (
+	PORT (
-		aclk			: in std_logic;
+		aclk : IN STD_LOGIC;
-		aresetn			: in std_logic;
+		aresetn : IN STD_LOGIC;
 
-		s_axis_tvalid	: in std_logic;
+		s_axis_tvalid : IN STD_LOGIC;
-		s_axis_tdata	: in std_logic_vector(TDATA_WIDTH-1 downto 0);
+		s_axis_tdata : IN STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
-		s_axis_tlast	: in std_logic;
+		s_axis_tlast : IN STD_LOGIC;
-		s_axis_tready	: out std_logic;
+		s_axis_tready : OUT STD_LOGIC;
 
-		m_axis_tvalid	: out std_logic;
+		m_axis_tvalid : OUT STD_LOGIC;
-		m_axis_tdata	: out std_logic_vector(TDATA_WIDTH-1 downto 0);
+		m_axis_tdata : OUT STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
-		m_axis_tlast	: out std_logic;
+		m_axis_tlast : OUT STD_LOGIC;
-		m_axis_tready	: in std_logic;
+		m_axis_tready : IN STD_LOGIC;
 
-		enable_filter	: in std_logic
+		enable_filter : IN STD_LOGIC
 	);
-end moving_average_filter_en;
+END moving_average_filter_en;
 
-architecture Behavioral of moving_average_filter_en is
+ARCHITECTURE Behavioral OF moving_average_filter_en IS
 
-begin
+	-- Component declarations
+	COMPONENT all_pass_filter IS
+		GENERIC (
+			TDATA_WIDTH : POSITIVE := 24
+		);
+		PORT (
+			aclk : IN STD_LOGIC;
+			aresetn : IN STD_LOGIC;
 
-end Behavioral;
+			s_axis_tvalid : IN STD_LOGIC;
+			s_axis_tdata : IN STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
+			s_axis_tlast : IN STD_LOGIC;
+			s_axis_tready : OUT STD_LOGIC;
+
+			m_axis_tvalid : OUT STD_LOGIC;
+			m_axis_tdata : OUT STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
+			m_axis_tlast : OUT STD_LOGIC;
+			m_axis_tready : IN STD_LOGIC
+		);
+	END COMPONENT;
+
+	COMPONENT moving_average_filter IS
+		GENERIC (
+			FILTER_ORDER_POWER : INTEGER := 5;
+			TDATA_WIDTH : POSITIVE := 24
+		);
+		PORT (
+			aclk : IN STD_LOGIC;
+			aresetn : IN STD_LOGIC;
+
+			s_axis_tvalid : IN STD_LOGIC;
+			s_axis_tdata : IN STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
+			s_axis_tlast : IN STD_LOGIC;
+			s_axis_tready : OUT STD_LOGIC;
+			
+			m_axis_tvalid : OUT STD_LOGIC;
+			m_axis_tdata : OUT STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
+			m_axis_tlast : OUT STD_LOGIC;
+			m_axis_tready : IN STD_LOGIC
+		);
+	END COMPONENT;
+
+	-- Internal signals for the all-pass filter
+	SIGNAL all_pass_s_tvalid : STD_LOGIC;
+	SIGNAL all_pass_s_tready : STD_LOGIC;
+
+	SIGNAL all_pass_m_tvalid : STD_LOGIC;
+	SIGNAL all_pass_m_tdata : STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
+	SIGNAL all_pass_m_tlast : STD_LOGIC;
+	SIGNAL all_pass_m_tready : STD_LOGIC;
+
+	-- Internal signals for the moving average filter
+	SIGNAL moving_avg_s_tvalid : STD_LOGIC;
+	SIGNAL moving_avg_s_tready : STD_LOGIC;
+	
+	SIGNAL moving_avg_m_tvalid : STD_LOGIC;
+	SIGNAL moving_avg_m_tdata : STD_LOGIC_VECTOR(TDATA_WIDTH - 1 DOWNTO 0);
+	SIGNAL moving_avg_m_tlast : STD_LOGIC;
+	SIGNAL moving_avg_m_tready : STD_LOGIC;
+
+BEGIN
+
+	-- Instantiate the all-pass filter
+	all_pass_inst : all_pass_filter
+	GENERIC MAP(
+		TDATA_WIDTH => TDATA_WIDTH
+	)
+	PORT MAP(
+		aclk => aclk,
+		aresetn => aresetn,
+
+		s_axis_tvalid => all_pass_s_tvalid,
+		s_axis_tdata => s_axis_tdata,
+		s_axis_tlast => s_axis_tlast,
+		s_axis_tready => all_pass_s_tready,
+
+		m_axis_tvalid => all_pass_m_tvalid,
+		m_axis_tdata => all_pass_m_tdata,
+		m_axis_tlast => all_pass_m_tlast,
+		m_axis_tready => all_pass_m_tready
+	);
+
+	-- Instantiate the moving average filter
+	moving_avg_inst : moving_average_filter
+	GENERIC MAP(
+		FILTER_ORDER_POWER => FILTER_ORDER_POWER,
+		TDATA_WIDTH => TDATA_WIDTH
+	)
+	PORT MAP(
+		aclk => aclk,
+		aresetn => aresetn,
+
+		s_axis_tvalid => moving_avg_s_tvalid,
+		s_axis_tdata => s_axis_tdata,
+		s_axis_tlast => s_axis_tlast,
+		s_axis_tready => moving_avg_s_tready,
+
+		m_axis_tvalid => moving_avg_m_tvalid,
+		m_axis_tdata => moving_avg_m_tdata,
+		m_axis_tlast => moving_avg_m_tlast,
+		m_axis_tready => moving_avg_m_tready
+	);
+
+	-- Assign filter control signals based on enable_filter
+	all_pass_s_tvalid    <= s_axis_tvalid WHEN enable_filter = '0' ELSE '0';
+	moving_avg_s_tvalid  <= s_axis_tvalid WHEN enable_filter = '1' ELSE '0';
+
+	all_pass_m_tready    <= m_axis_tready WHEN enable_filter = '0' ELSE '0';
+	moving_avg_m_tready  <= m_axis_tready WHEN enable_filter = '1' ELSE '0';
+
+	-- Main AXIS assignments based on enable_filter
+	s_axis_tready        <= all_pass_s_tready   WHEN enable_filter = '0' ELSE moving_avg_s_tready;
+	m_axis_tvalid        <= all_pass_m_tvalid   WHEN enable_filter = '0' ELSE moving_avg_m_tvalid;
+	m_axis_tdata         <= all_pass_m_tdata    WHEN enable_filter = '0' ELSE moving_avg_m_tdata;
+	m_axis_tlast         <= all_pass_m_tlast    WHEN enable_filter = '0' ELSE moving_avg_m_tlast;
+
+END Behavioral;

LAB3/test/uart_viewer.py

@@ -5,6 +5,9 @@ import queue
 import threading
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
+import io
+from PIL import Image
+import numpy as np
 
 # CONFIGURAZIONE
 BASYS3_PID = 0x6010
@@ -48,37 +51,71 @@ def receive_graph_mode(ser):
     reader_thread = threading.Thread(target=serial_reader, daemon=True)
     reader_thread.start()
 
-    latest_point = [64, 64]  # Punto iniziale al centro del grafico
+    point = [64, 64]  # Punto iniziale al centro del grafico
-    latest_color = 'blue'
+    color = 'blue'
-    latest_size = 100
+    size = 100
+
+    release = True
+    rgb = [0, 0, 0]
 
     fig, ax = plt.subplots()
-    sc = ax.scatter([latest_point[0]], [latest_point[1]], c=[latest_color], s=[latest_size])
+
+    # Load PNG directly
+    png_path = r'LAB3\test\Color_circle_(RGB).png'
+    img = Image.open(png_path).convert('RGB')
+    img = img.resize((127, 127), Image.LANCZOS)  # Ensure image is 127x127
+    img_np = np.array(img)
+
+    # Show the image as background
+    ax.imshow(img, extent=[0, 127, 0, 127], aspect='auto', zorder=0)
+
+    sc = ax.scatter([point[0]], [point[1]], s=[size], zorder=1)
     ax.set_xlim(0, 127)
     ax.set_ylim(0, 127)
     ax.set_xlabel("X")
     ax.set_ylabel("Y")
     ax.set_title("Coordinate in tempo reale")
 
+    def send_rgb_over_serial(ser, rgb):
+        """
+        Send RGB values over serial with a fixed first byte (0xC0).
+        """
+        bytes_to_send = [0xC0]  # Primo byte fisso
+        for part in rgb:
+            val = int(part)
+            bytes_to_send.append(val)
+        ser.write(bytearray(bytes_to_send))
+        print(f"Inviato: {' '.join(f'{b:02X}' for b in bytes_to_send)}")
+
     def update(frame):
-        nonlocal latest_point, latest_color, latest_size
+        nonlocal point, color, size, release, rgb
+
         while not q.empty():
             x, y, flags = q.get()
-            latest_point[0] = x
+            point[0] = x
-            latest_point[1] = y
+            point[1] = y
-            # Bit 0: red if set, else blue
+
             if flags & 0b00000001:
-                latest_color = 'red'
+                rgb = [0, 0, 0]
+                size = 50
+                if release:
+                    send_rgb_over_serial(ser, rgb)
+                    release = False
+
+            elif flags & 0b00000010:
+                flipped_y = 127 - y # Flip y to match image coordinates
+                rgb = img_np[flipped_y, x]
+                size = 300
+                if release:
+                    send_rgb_over_serial(ser, rgb)
+                    release = False
+
             else:
-                latest_color = 'blue'
+                size = 100
-            # Bit 1: bigger if set
+                release = True  # Reset release when no button is pressed
-            if flags & 0b00000010:
+
-                latest_size = 300
+        sc.set_offsets([point])
-            else:
+        sc.set_sizes([size])
-                latest_size = 100
-        sc.set_offsets([latest_point])
-        sc.set_color([latest_color])
-        sc.set_sizes([latest_size])
         return sc,
 
     ani = animation.FuncAnimation(fig, update, interval=10, blit=True, cache_frame_data=False)