library work;
use work.synchro.all;
use work.op_pkg.all;
----------------------------------------------------------------
entity pid is 
port (
	reset             : in bit;
	Fsignin           : in bit;
	HostInterrupt     : in bit;
	PositionChange    : in bit;
	Irefkout          : out real );
end pid;
-----------------------------------------------------------------
architecture behavior of pid is 

  component fu_fpu
  port
   (	clock   : in bit;
	reset   : in bit;
	input1  : in real;
	input2  : in real;
	sel     : in bit;
	com     : in int3bit;
	output  : out real;
	outdone : out bit);
  end component;

  signal sig_in1,sig_in2 :real;
  signal sig_out : real;
  signal sig_sel,sig_done :bit;
  signal sig_com : int3bit;
  signal clock : bit;-- <= '1';
  for all:fu_fpu use entity work.fixedpointunit(behavior);   

begin 
  Inst_FU :fu_fpu
  port map(clock   => clock,
		   reset   => reset,
		   input1  => sig_in1,
		   input2  => sig_in2,
		   sel     => sig_sel,
		   com     => sig_com,
		   output  => sig_out,
		   outdone => sig_done);

  process
	variable N,	Ek_1,Ik,Ek,Kp,Ki,Kd,Fref: real;
	variable Fk,Dek,Irefk, Temp: real;
	variable done : bit;
    procedure mul(a,b : in real) is
	begin
	  sig_in1 <= a;
	  sig_in2 <= b;
	  sig_sel <= '1';
	  sig_com <= 2;
	  wait until rising_edge(clock);
	  sig_sel <= '0';
	  return;
    end mul;
	procedure rep(a: in real) is
	begin 
	  sig_in1 <= a;
	  sig_sel <= '1';
	  sig_com <= 1;
	  wait until rising_edge(clock);
	  sig_sel <= '0';
	  return;
    end rep;
	procedure waitresult(x:out real; y:out bit) is
	begin
	  x := sig_out;
	  y := sig_done;
	  wait until rising_edge(clock);
	  return;
    end waitresult;

	type ROM is array(0 to 4) of  real;
	variable val_rom : ROM := (2.0* 2.0**(-20), 2.0, 3.0*2.0**(-20),
					4.0*2.0**(20),2.0*2.0**(-20));
    procedure getconstKp(x :out real) is
	begin
	  x := val_rom(0);
	end getconstKp;
    procedure getconstKi(x :out real) is
	begin 
	  x := val_rom(1);
	end getconstKi;
    procedure getconstKd(x : out real) is
	begin
	  x := val_rom(2);
	end getconstKd;
    procedure getFref(x :out real) is
	begin
	  x := val_rom(3);
	end getFref;
    procedure getN(x :out real) is 
	begin 
	 x := val_rom(4);
	end getN;
------------------------------------------------------------
  begin
    getConstKp(Kp);
    getConstKi(Ki);
    getConstKd(Kd);
    getFref(Fref);
    Ik :=0.0;
    Ek := 0.0;
    Irefk := 0.0;
    --wait for 50 ns;
    wait until (HostInterrupt = '0');
    while (HostInterrupt = '0') loop
      wait until(PositionChange = '1');
	getN(N);
	rep(N);
	Ek_1 := Ek;
	--wait until(done = '1')
	waitresult(Fk,done);
	while (done /= '1') loop waitresult(Fk,done); end loop;
	if(Fsignin = '0')
	then Ek := Fref - Fk;
	else Ek := Fref + Fk;
	end if;
	mul(Kp,Ek);
	Dek := Ek - Ek_1;
	waitresult(Irefk,done);
	while(done /= '1') loop waitresult(Irefk,done); end loop;
	mul(Dek,Fk);
	waitresult(Temp,done);
	while(done /= '1') loop waitresult(Temp,done); end loop;
	mul(Temp,Kd);
	waitresult(Temp,done);
	while(done /= '1') loop waitresult(Temp,done); end loop;
        Irefk := Irefk + Temp;
	mul(Ek,N);
        waitresult(Temp,done);
	while(done /= '1') loop waitresult(Temp,done); end loop;
	Ik := Ik + Temp;
	mul(Ik,Ki);
	waitresult(Temp,done);
	while (done /= '1') loop waitresult(Temp ,done); end loop;
	--  Irefkout <= Irefk + Temp;
	Irefk := Irefk + Temp;
    end loop;
    Irefkout <= Irefk;
  end process;
  clock <= not clock after 50 ns;
end behavior;