`timescale 1ns / 1ps
(* KEEP = "TRUE" *)
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 		Churbanov S.
// 
// Create Date:    15:24:31 08/20/2019 
// Design Name: 
// Module Name:    gain_master 
// Project Name: 
// Target Devices: 
// Tool versions: 
// Description: 
//
// Dependencies: 
//
// Revision: 
// Revision 0.02 - File Modified
// Additional Comments: 16.09.2019 file modified in assotiate with task.
//
//////////////////////////////////////////////////////////////////////////////////
module GainControl
#(	
	parameter	AdcNcoMultWidth	=	35,
	parameter	ThresholdWidth	=	24,
	parameter	AdcDataWidth	=	14,
	parameter	MeasPeriod		=	32
)	
(
	input	Rst_i,
	input	Clk_i,	
	input	StartMeas_i,
	input	GainAutoEn_i,
	
	input	signed	[AdcNcoMultWidth-1:0]	AdcCos_i,
	input	signed	[AdcNcoMultWidth-1:0]	AdcSin_i,
	
	input	[ThresholdWidth-1:0]	GainLowThreshold_i,
	input	[ThresholdWidth-1:0]	GainHighThreshold_i,
	
	output	GainNewState_o,
	output	SensEn_o,
	output	MeasStart_o
);

//================================================================================
//  LOCALPARAMS
	localparam	CntWidth		=	32;
	localparam	Delay			=	100;
	localparam	AverageDelay	=	MeasPeriod+Delay-1;
	localparam	SumWidth		=	AdcNcoMultWidth+6-1;
//================================================================================
//  REG/WIRE
	reg		[CntWidth-1:0]	measCnt;
	
	reg		signed	[SumWidth-1:0]	adcSinSum;			
	reg		signed	[SumWidth-1:0]	adcCosSum;	
	
	reg		measWind;
	wire	measEnd	=	(measCnt==AverageDelay-1)&measWind;
	
	reg	gainNewStateR;
	reg		gainNewState;
	wire	sensEn	=	((gainNewStateR& (!gainNewState))|(!gainNewStateR&gainNewState));
	
	reg		signed	[SumWidth-1:0]	sinShifted;
	reg		signed	[SumWidth-1:0]	cosShifted;
	
	wire	signed	[ThresholdWidth-5:0]		sinShiftedCut	=	sinShifted	[(SumWidth-1)-:20];		//width is 20
	wire	signed	[ThresholdWidth-5:0]		cosShiftedCut	=	cosShifted	[(SumWidth-1)-:20];		//width is 20
	
	wire	signed	[(ThresholdWidth*2)-9:0]	sinSumSquared	=	(sinShiftedCut*sinShiftedCut);	// width is 40
	wire	signed	[(ThresholdWidth*2)-9:0]	cosSumSquared	=	(cosShiftedCut*cosShiftedCut);	// width is 40
	
	wire	signed	[(ThresholdWidth*2)-9:0]	sumSquared	=	(cosSumSquared+sinSumSquared);	//width is 40	
	
	wire	[(ThresholdWidth*2)-9:0]	lowThresholdCompl	=	{10'b0,GainLowThreshold_i,6'b0};
	wire	[(ThresholdWidth*2)-9:0]	highThresholdCompl	=	{10'b0,GainHighThreshold_i,6'b0};
	
	wire	accWind	=	(measCnt>0	&	measCnt	<=MeasPeriod-2);
//================================================================================
//  ASSIGNMENTS
	assign	GainNewState_o	=	gainNewState;
	assign	SensEn_o		=	sensEn;
	assign	MeasStart_o		=	GainAutoEn_i?	measEnd:StartMeas_i;
//================================================================================
//  CODING

always	@(posedge	Clk_i)	begin
	if	(!Rst_i)	begin
		if	(GainAutoEn_i)	begin
			if	(StartMeas_i)	begin
				measWind	<=	1'b1;
			end	else	if	(measEnd)	begin
				measWind	<=	1'b0;
			end
		end	else	begin
			measWind	<=	1'b0;
		end
	end	else	begin
		measWind	<=	1'b0;
	end
end

always	@(posedge	Clk_i)	begin
	if	(measWind)	begin
		if	(measCnt	==	MeasPeriod-2)	begin
			sinShifted	<=	adcSinSum>>>2;
			cosShifted	<=	adcCosSum>>>2;
		end
	end	
end

always	@(posedge	Clk_i)	begin
	if	(!Rst_i)	begin
		if	(measWind)	begin
			if	(measCnt	!= AverageDelay-1)	begin
				measCnt	<=	measCnt	+	3'd1;	
			end
		end	else	begin
			measCnt	<=	3'd0;
		end
	end	else	begin
		measCnt	<=	3'd0;
	end
end

always	@(posedge	Clk_i)	begin
	if	(!Rst_i)	begin
		if	(!accWind)	begin
			adcSinSum	<=	AdcSin_i;
			adcCosSum	<=	AdcCos_i;
		end	else	begin
			adcSinSum	<=	adcSinSum	+	AdcSin_i;
			adcCosSum	<=	adcCosSum	+	AdcCos_i;
		end
	end	else	begin
		adcSinSum	<=	0;	
		adcCosSum	<=	0;
	end
end


always	@(posedge	Clk_i)	begin	
	if	(!Rst_i)	begin
		if	(GainAutoEn_i)	begin
			if	(measCnt	==	MeasPeriod-1)	begin
				if	(gainNewState)	begin
					if	(sumSquared	>	highThresholdCompl)	begin
						gainNewState	<=	1'b0;
					end	else	begin
						gainNewState	<=	gainNewState;
					end
				end	else	begin
					if	(sumSquared	<	lowThresholdCompl)	begin
						gainNewState	<=	1'b1;
					end	else	begin
						gainNewState	<=	gainNewState;
					end
				end
			end
		end	else	begin
			gainNewState	<=	1'b0;
		end
	end	else	begin
		gainNewState	<=	1'b0;
	end
	
	gainNewStateR	<=	gainNewState;
end

endmodule