library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

library grlib;
use grlib.amba.all;
use grlib.stdlib.all;
use grlib.devices.all;

library techmap;
use techmap.gencomp.all;

entity yccrgb is
   generic (
      memtech : integer := DEFMEMTECH;
      hirq   : integer := 0;       
      mhindex : integer := 0;
      chprot : integer := 3);
   port (
      rst   : in std_ulogic;
      clk   : in std_ulogic;
      ahbmi : in ahb_mst_in_type;
      ahbmo : out ahb_mst_out_type;
      kready  : out std_logic;
      kstrobe : in std_logic;
      kdata   : in std_logic_vector(23 downto 0);
      xmcumax : in std_logic_vector(5 downto 0);
      ymcumax : in std_logic_vector(4 downto 0);
      incaddy  : in std_logic_vector(15 downto 0);
      incaddmcux : in std_logic_vector(15 downto 0);
      incaddmcuy : in std_logic_vector(10 downto 0);
      fbstartadd : in std_logic_vector(31 downto 0);
      startgen   : in std_logic
   );
end;

architecture rtl of yccrgb is


constant mhconfig : ahb_config_type := (
 0 => ahb_device_reg( VENDOR_CONTRIB, CONTRIB_CORE1, 0, 0, 0),
 others => zero32);
 
constant fdepth : integer :=256;
          
function mysigned_mul(a,b : std_logic_vector) return std_logic_vector is
variable z : std_logic_vector(a'length + b'length -1 downto 0);
begin
    z := std_logic_vector(signed(a) * signed(b));
    return(z);
end;

function mysigned_mul2(b,a : std_logic_vector) return std_logic_vector is
variable sum : std_logic_vector(15 downto 0);
variable p0 : std_logic_vector(15 downto 0);
variable p1 : std_logic_vector(15 downto 0);
variable p2 : std_logic_vector(15 downto 0);
variable p3 : std_logic_vector(15 downto 0);
variable p4 : std_logic_vector(15 downto 0);
variable p5 : std_logic_vector(15 downto 0);
variable p6 : std_logic_vector(15 downto 0);
variable p7 : std_logic_vector(15 downto 0);
variable p_plus : std_logic_vector(15 downto 0);
begin
    p0 := "00000001" & not(a(7) and b(0)) & (a(6) and b(0))& (a(5) and b(0)) & (a(4) and b(0))
           & (a(3) and b(0)) & (a(2) and b(0))& (a(1) and b(0)) & (a(0) and b(0));
    p1 := "0000000" & not(a(7) and b(1)) & (a(6) and b(1))& (a(5) and b(1)) & (a(4) and b(1))
          & (a(3) and b(1)) & (a(2) and b(1))& (a(1) and b(1)) & (a(0) and b(1)) & '0';           
    p2 := "000000" & not(a(7) and b(2)) & (a(6) and b(2))& (a(5) and b(2)) & (a(4) and b(2))
          & (a(3) and b(2)) & (a(2) and b(2))& (a(1) and b(2)) & (a(0) and b(2)) & "00";
    p3 := "00000" & not(a(7) and b(3)) & (a(6) and b(3))& (a(5) and b(3)) & (a(4) and b(3))
          & (a(3) and b(3)) & (a(2) and b(3))& (a(1) and b(3)) & (a(0) and b(3)) & "000";
    p4 := "0000" & not(a(7) and b(4)) & (a(6) and b(4))& (a(5) and b(4)) & (a(4) and b(4))
          & (a(3) and b(4)) & (a(2) and b(4))& (a(1) and b(4)) & (a(0) and b(4)) & "0000";    
    p5 := "000" & not(a(7) and b(5)) & (a(6) and b(5))& (a(5) and b(5)) & (a(4) and b(5))
          & (a(3) and b(5)) & (a(2) and b(5))& (a(1) and b(5)) & (a(0) and b(5)) & "00000";    
    p6 := "00" & not(a(7) and b(6)) & (a(6) and b(6))& (a(5) and b(6)) & (a(4) and b(6))
          & (a(3) and b(6)) & (a(2) and b(6))& (a(1) and b(6)) & (a(0) and b(6)) & "000000";          
    p7 := '1' & (a(7) and b(7)) & not(a(6) and b(7))& not(a(5) and b(7)) & not(a(4) and b(7))
          & not(a(3) and b(7)) & not(a(2) and b(7))& not(a(1) and b(7)) & not(a(0) and b(7)) 
          & "0000000";
    sum := std_logic_vector((unsigned(p0) + unsigned(p1)) + (unsigned(p2) + unsigned(p3)) + (unsigned(p4) 
          + unsigned(p5)) + (unsigned(p6) + unsigned(p7)));
    return(sum);
end;  

function mysigned_add(a,b : std_logic_vector) return std_logic_vector is
variable ex_a : std_logic_vector(a'length downto 0);
variable ex_b : std_logic_vector(b'length downto 0);
variable z1 : std_logic_vector(a'length downto 0);
variable z2 : std_logic_vector(b'length downto 0);
begin
    ex_a := a(a'left) & a;
    ex_b := b(b'left) & b;
    if( a'length > b'length)then
       z1 := std_logic_vector(signed(ex_a) + signed(ex_b));
       return(z1);
    else
       z2 := std_logic_vector(signed(ex_a) + signed(ex_b));
       return(z2);
    end if;
end;

type fstate_type is (ready, stuck);
type mstate_type is (idle, busreq, grant, nonseq, seq);

type control_reg is record
   ycc    : std_logic_vector(23 downto 0);
   written : std_ulogic;
   fifo_state : fstate_type;
   fifo_rp : std_logic_vector(7 downto 0);
   fifo_wp : std_logic_vector(7 downto 0);
   gen_add : std_logic_vector(31 downto 0);
   temp_data : std_logic_vector(15 downto 0);
   mstate : mstate_type;
   yxpoint : std_logic_vector( 6 downto 0);
   xmcu : std_logic_vector(5 downto 0);
   ymcu : std_logic_vector(4 downto 0);
   keep_data : std_logic_vector(31 downto 0);
   dhready : std_logic;
end record;

signal r, rin : control_reg;
signal read_en_fifo, write_en_fifo : std_logic;
signal read_pointer_fifo : std_logic_vector(7 downto 0);
signal write_pointer_fifo : std_logic_vector(7 downto 0);
signal data_out_fifo : std_logic_vector(31 downto 0);
signal data_in_fifo : std_logic_vector(31 downto 0);

begin
ram0 : syncram_2p generic map(tech => memtech, abits => 8, dbits => 32, sepclk => 0)
            port map( clk, read_en_fifo, read_pointer_fifo, data_out_fifo,
                clk, write_en_fifo, write_pointer_fifo, data_in_fifo);

comb : process (r, rst, ahbmi, kstrobe, kdata, xmcumax, ymcumax, incaddmcuy, fbstartadd, startgen, data_out_fifo)
      variable v : control_reg;
      variable virq : std_logic_vector(NAHBIRQ-1 downto 0);
      variable vsready : std_logic;
      variable fifo_write : std_logic;
      variable fifo_read :std_logic;
      variable write_point : integer;
      variable read_point : integer;
      variable num_ele : integer;
      variable mhtrans : std_logic_vector(1 downto 0);
      variable mhbusreq : std_ulogic;
      variable mhprot : std_logic_vector(3 downto 0);
      variable add_inc : std_logic;
      variable vmhwdata : std_logic_vector(31 downto 0);
      variable incval : std_logic_vector(15 downto 0);
      variable refresh : std_logic;
      variable rout : std_logic_vector(4 downto 0);
      variable gout : std_logic_vector(5 downto 0);
      variable bout : std_logic_vector(4 downto 0);     
      variable node1 : std_logic_vector(9 downto 0);
      variable node2 : std_logic_vector(9 downto 0);
      variable node3 : std_logic_vector(15 downto 0);
      variable node4 : std_logic_vector(15 downto 0);
      variable node5 : std_logic_vector(15 downto 0);
      variable node6 : std_logic_vector(15 downto 0);
      variable node7 : std_logic_vector(9 downto 0);
      variable node8 : std_logic_vector(8 downto 0);
      variable node9 : std_logic_vector(9 downto 0);
      variable node10 : std_logic_vector(9 downto 0);     
      constant c_01011010 : std_logic_vector(7 downto 0) := "01011010";
      constant c_11101010 : std_logic_vector(7 downto 0) := "11101010";
      constant c_11010010 : std_logic_vector(7 downto 0) := "11010010";
      constant c_01110001 : std_logic_vector(7 downto 0) := "01110001";      
    begin

   v := r;
   virq := (others => '0');
   mhprot := conv_std_logic_vector(chprot,4);
   v.written := '0';
   fifo_write := '0';
   vsready := '1';
   mhbusreq := '0';
   mhtrans := HTRANS_IDLE;
      
   if kstrobe = '1' then
        v.ycc(23 downto 0) := kdata;
        v.written := '1';            
   end if;

-- register write   
      node1 := mysigned_add(('0' & r.ycc(15 downto 8)), "110000000");
      node2 := mysigned_add(('0' & r.ycc(7 downto 0)), "110000000");
      node3 := mysigned_mul2(c_01011010, node2(7 downto 0));
      node4 := mysigned_mul2(c_11101010, node1(7 downto 0));
      node5 := mysigned_mul2(c_11010010, node2(7 downto 0));
      node6 := mysigned_mul2(c_01110001, node1(7 downto 0));
      node7 := mysigned_add(('0' & r.ycc(23 downto 16)), (node3(14 downto 7) & '0'));
      node8 := mysigned_add((node4(13 downto 7) & '0'), (node5(13 downto 7) & '0'));
      node9 := mysigned_add(('0' & r.ycc(23 downto 16)), ( node6(14 downto 7) & '0'));
      node10 := mysigned_add(('0' & r.ycc(23 downto 16)), node8);
      if(node7(9) = '1') then
         rout := "00000";
      elsif(node7(8) = '1') then
         rout := "11111";
      else
         rout := node7(7 downto 3);
      end if;
     
      if(node10(9) = '1') then
         gout := "000000";
      elsif(node10(8) = '1') then
         gout := "111111";
      else
         gout := node10(7 downto 2);
      end if;
      
      if(node9(9) = '1') then
         bout := "00000";
      elsif(node9(8) = '1') then
         bout := "11111";
      else
         bout := node9(7 downto 3);
      end if;

-- Fifo part
   write_point := to_integer(unsigned(r.fifo_wp));
   read_point := to_integer(unsigned(r.fifo_rp));
   if write_point >= read_point then
       num_ele := write_point - read_point;
   else
       num_ele := fdepth - read_point + write_point;
   end if;
   if num_ele > fdepth/2 then
       vsready := '0';
   end if;
   if (num_ele = fdepth/2) and ( r.written = '1' or r.fifo_state = stuck) then
       vsready := '0';
   end if;
   
   case r.fifo_state is
   when ready =>
      if r.written = '1' then
         v.temp_data := rout & gout & bout;
         v.fifo_state := stuck;
      end if;
   when stuck =>
      if r.written = '1' then
         fifo_write := '1';
         write_point := write_point + 1;
         if write_point = fdepth then
            write_point := 0;
         end if;
         v.fifo_state := ready;
      end if;
   end case; 
      
   data_in_fifo <= r.temp_data & rout & gout & bout; 
   write_en_fifo <= fifo_write;
   v.fifo_wp := std_logic_vector(to_unsigned(write_point,8));

-- AHB master part
   fifo_read := '0';
   add_inc := '0';

   case r.mstate is
   when idle =>
       mhbusreq := '0';
       mhtrans := HTRANS_IDLE;
       if (num_ele >= 8) then
           v.mstate := busreq;
       end if;
   when busreq =>
       mhbusreq := '1';
       mhtrans := HTRANS_NONSEQ;
       if  (ahbmi.hready = '1') and (ahbmi.hgrant(mhindex) = '1') then
           v.mstate := nonseq;
       end if;
   when nonseq =>
       mhbusreq := '1';
       mhtrans := HTRANS_NONSEQ;
       if ahbmi.hready = '1' then
           fifo_read := '1';
           add_inc := '1';
           v.mstate := seq;
       end if;
   when seq =>
       mhbusreq := '1';
       mhtrans := HTRANS_SEQ;
       if ahbmi.hready = '1' then
           fifo_read := '1';
           add_inc := '1';
           if (r.yxpoint(2 downto 0) = "111") then
               v.mstate := idle;
           end if;
       end if;
   when others =>
   end case;
   
   
   refresh := '0';
   incval := (others => '0');
   if add_inc = '1' then
   v.yxpoint := r.yxpoint + '1';
       if r.yxpoint(2 downto 0) /= "111" then
          incval := "0000000000000100";
       else 
          if r.yxpoint(6 downto 3) /= "1111" then
             incval := incaddy;
          else 
             v.xmcu := r.xmcu + '1';
             if r.xmcu /= xmcumax then
                incval := incaddmcux;
             else   
                v.xmcu := "000000";
                v.ymcu := r.ymcu + '1';
                if r.ymcu /= ymcumax then
                   incval := "00000" & incaddmcuy;
                else
                   v.ymcu := "00000";
                   refresh := '1';
                end if;
             end if;
          end if;
       end if;
   end if;
  
   if refresh = '1' then
       v.gen_add := fbstartadd;
   else
       v.gen_add := std_logic_vector(signed(r.gen_add) + signed(incval));
   end if;

   if fifo_read = '1' then
      read_point := read_point + 1;
      if read_point = fdepth then
          read_point := 0;
      end if;
   end if;
   v.fifo_rp :=  std_logic_vector(to_unsigned(read_point,8));
   
-- for the hready assert
   v.dhready := ahbmi.hready;
   if  (ahbmi.hready = '1') or (ahbmi.hgrant(mhindex) = '0') then
      v.keep_data := data_out_fifo;
   end if;
   if (r.dhready = '0') and (ahbmi.hgrant(mhindex) = '1') then
      vmhwdata := r.keep_data;
   else
      vmhwdata := data_out_fifo;
   end if; 

-- soft reset
   if startgen = '1' then
         v.gen_add := fbstartadd;
         v.written := '0';
         v.fifo_state := ready;
         v.fifo_rp := (others => '0');
         v.fifo_wp := (others => '0');
         v.mstate := idle;
         v.yxpoint := (others => '0');
         v.xmcu := (others => '0');
         v.ymcu := (others => '0');
   end if;      
-- reset part
   if rst = '0' then
         v.ycc := (others => '0');
         v.written := '0';
         v.fifo_state := ready;
         v.fifo_rp := (others => '0');
         v.fifo_wp := (others => '0');
         v.mstate := idle;
         v.yxpoint := (others => '0');
         v.xmcu := (others => '0');
         v.ymcu := (others => '0');  
         v.temp_data := (others => '0');
         v.gen_add := (others => '0'); 
   end if;
   
-- signal
   rin <= v;   
   kready <= vsready;
   read_en_fifo <= '1';
   write_pointer_fifo <= r.fifo_wp(7 downto 0);
   read_pointer_fifo <= r.fifo_rp(7 downto 0);
   ahbmo.haddr <= r.gen_add;
   ahbmo.htrans <= mhtrans;
   ahbmo.hbusreq <= mhbusreq;
   ahbmo.hprot <= mhprot;
   ahbmo.hwdata <= vmhwdata; 
   

end process;

   ahbmo.hconfig <= mhconfig;
   ahbmo.hlock <= '0';
   ahbmo.hwrite <= '1';
   ahbmo.hsize <= "010";
   ahbmo.hburst <= HBURST_INCR;   
   ahbmo.hirq <= (others => '0');
   ahbmo.hindex <= mhindex;

   
-- registers 
reg : process(clk)
begin
   if rising_edge(clk) then
        r <= rin;
   end if;
end process;


end;