module CPLD_Con (
    input i_sys_clk,
    input i_rst_n,
    input i_con_exec,
    input [23:0]i_freq_relay1,
    input [23:0]i_freq_relay2,
    input [23:0]i_freq_relay3,
    input [23:0]i_freq_relay4,
    output o_con_done,
    output [7:0]o_RC_F,           //Ref: Schematic
    output [15:0]o_RC_S,
    output [31:0]o_RC_T,
    output [3:0] o_OC_x09,
    output [3:0] o_OC_x25
);

    //Output Relay Reg
    reg [3:0] OC_x09; 
    reg [3:0] OC_x25;

    //Relay Reg
    reg [7:0]relay_l1;
    reg [15:0]relay_l2 ;
    reg [31:0]relay_l3 ;

    //Flags
    reg exec_done_flag;
    reg exec_flag;
	
	reg [3:0]slot_exec_flag;
	reg err_flag;

    //Counter
    reg [15:0]delay_cnt;

    wire [8:0] freq_relay1 = i_freq_relay1[9:1];
    wire [8:0] freq_relay2 = i_freq_relay2[9:1];
    wire [8:0] freq_relay3 = i_freq_relay3[9:1];
    wire [8:0] freq_relay4 = i_freq_relay4[9:1];

    always @(posedge i_sys_clk) begin
        if (i_rst_n == 1'b0) begin
            delay_cnt <= 'd0;
        end
        else begin
            if (exec_flag == 1'b1) begin
                delay_cnt <= delay_cnt + 1'd1;
            end
            else begin
                delay_cnt <= 'd0;
            end
        end
    end

    always @(posedge i_sys_clk) begin
        if (i_rst_n == 1'b0) begin
            relay_l1 <= 'd0;
            relay_l2 <= 'd0;
            relay_l3 <= 'd0;
            exec_flag <= 1'b0;
            exec_done_flag <= 1'b1;
			slot_exec_flag <= 'd0;
			err_flag <= 'd0;
        end
        else begin
            if ((i_con_exec == 1'b1) & (exec_flag == 1'b0)) begin
                exec_done_flag <= 'd0;
                exec_flag <= 1'b1;
                //To DO: More Sanity Check?
                // Add the bits together. Verilog handles the width expansion automatically for the comparison.
                if ( (i_freq_relay1[0] + i_freq_relay2[0] + i_freq_relay3[0] + i_freq_relay4[0]) > 2'd1 ) begin
                    err_flag <= 1'b1;
                end
                else begin
                    slot_exec_flag[0] <= i_freq_relay1[0];
                    slot_exec_flag[1] <= i_freq_relay2[0];
                    slot_exec_flag[2] <= i_freq_relay3[0];
                    slot_exec_flag[3] <= i_freq_relay4[0];
                end
            end
            else if (exec_flag == 1'b1) begin
                //Output Relay
                case (slot_exec_flag)
                    4'b0001: begin
                        relay_l1 <= {'d0,i_freq_relay1[22],i_freq_relay1[23]};
                        if (delay_cnt == 'd500) begin
                            relay_l2 <= {'d0,i_freq_relay1[18],i_freq_relay1[19],i_freq_relay1[20],i_freq_relay1[21]};
                        end
                        else if (delay_cnt == 'd1000) begin
                            relay_l3 <= {'d0,i_freq_relay1[10],i_freq_relay1[11],i_freq_relay1[12],i_freq_relay1[13],i_freq_relay1[14],i_freq_relay1[15],i_freq_relay1[16],i_freq_relay1[17]};
                        end
                        else if (delay_cnt == 'd1080) begin
                            exec_flag <= 'd0;
                            exec_done_flag <= 1'b1;
                        end
                        if ((freq_relay1 > 64) & (freq_relay1 < 73)) begin
                            OC_x09 <= 4'b0001;
                        end
                        else if ((freq_relay1 > 192) & (freq_relay1 < 201)) begin
                            OC_x25 <= 4'b0001;
                        end
                        else begin
                            OC_x09 <= 'd0;
                            OC_x25 <= 'd0;
                        end
                    end
                    4'b0010: begin
                        relay_l1 <= {4'd0,i_freq_relay2[22],i_freq_relay2[23],2'd0};
                        if (delay_cnt == 'd500) begin
                            relay_l2 <= {8'd0,i_freq_relay2[18],i_freq_relay2[19],i_freq_relay2[20],i_freq_relay2[21],4'd0};
                        end
                        else if (delay_cnt == 'd1000) begin
                            relay_l3 <= {16'd0,i_freq_relay2[10],i_freq_relay2[11],i_freq_relay2[12],i_freq_relay2[13],i_freq_relay2[14],i_freq_relay2[15],i_freq_relay2[16],i_freq_relay2[17],8'd0};
                        end
                        else if (delay_cnt == 'd1080) begin
                            exec_flag <= 'd0;
                            exec_done_flag <= 1'b1;
                        end
                        if ((freq_relay2 > 64) & (freq_relay2 < 73)) begin
                            OC_x09 <= 4'b0010;
                        end
                        else if ((freq_relay2 > 192) & (freq_relay2 < 201)) begin
                            OC_x25 <= 4'b0010;
                        end
                        else begin
                            OC_x09 <= 'd0;
                            OC_x25 <= 'd0;
                        end
                    end
                    4'b0100: begin
                        relay_l1 <= {2'd0,i_freq_relay3[22],i_freq_relay3[23],4'd0};
                        if (delay_cnt == 'd500) begin
                            relay_l2 <= {4'd0,i_freq_relay3[18],i_freq_relay3[19],i_freq_relay3[20],i_freq_relay3[21],8'd0};
                        end
                        else if (delay_cnt == 'd1000) begin
                            relay_l3 <= {8'd0,i_freq_relay3[12],i_freq_relay3[13],i_freq_relay3[10],i_freq_relay3[11],i_freq_relay3[16],i_freq_relay3[17],i_freq_relay3[14],i_freq_relay3[15],16'd0};
                        end
                        else if (delay_cnt == 'd1080) begin
                            exec_flag <= 'd0;
                            exec_done_flag <= 1'b1;
                        end
                        if ((freq_relay3 > 64) &(freq_relay3 <73)) begin
                            OC_x09 <= 4'b0100;
                        end
                        else if ((freq_relay3 > 192) & (freq_relay3 < 201)) begin
                            OC_x25 <= 4'b0100;
                        end
                        else begin
                            OC_x09 <= 'd0;
                            OC_x25 <= 'd0;
                        end
                    end
                    4'b1000: begin
                        relay_l1 <= {i_freq_relay4[22],i_freq_relay4[23],6'd0};
                        if (delay_cnt == 'd500) begin
                            relay_l2 <= {i_freq_relay4[18],i_freq_relay4[19],i_freq_relay4[20],i_freq_relay4[21],12'd0};
                        end
                        else if (delay_cnt == 'd1000) begin
                            relay_l3 <= {i_freq_relay4[10],i_freq_relay4[11],i_freq_relay4[12],i_freq_relay4[13],i_freq_relay4[14],i_freq_relay4[15],i_freq_relay4[16],i_freq_relay4[17],24'd0};
                        end
                        else if (delay_cnt == 'd1080) begin
                            exec_flag <= 'd0;
                            exec_done_flag <= 1'b1;
                        end
                        if ((freq_relay4 > 64) & (freq_relay4 < 73)) begin
                            OC_x09 <= 4'b1000;
                        end
                        else if ((freq_relay4 > 192) & (freq_relay4 < 201)) begin
                            OC_x25 <= 4'b1000;
                        end
                        else begin
                            OC_x09 <= 'd0;
                            OC_x25 <= 'd0;
                        end
                    end
                    default: begin 
                        OC_x09 <= 4'b0000;
                        OC_x25 <= 4'b0000;
					end
                endcase
            end
        end
    end 

    assign o_RC_F = relay_l1;
    assign o_RC_S = relay_l2;
    assign o_RC_T = relay_l3;
    assign o_con_done = exec_done_flag;
    assign o_OC_x09 = OC_x09;
    assign o_OC_x25 = OC_x25;

endmodule