module CPLD_Con (
    input i_sys_clk,
    input i_rst_n,
    input i_con_exec,

// Freq Relay Control
    input [23:0]i_freq_relay1,
    input [23:0]i_freq_relay2,
    input [23:0]i_freq_relay3,
    input [23:0]i_freq_relay4,

// DC related Control
    input [23:0]i_DC_Con1,
    input [23:0]i_DC_Con2,
    input [23:0]i_DC_Con3,
    input [23:0]i_DC_Con4,

    //Output RELAYs
    output [18:0]o_DMM_EN,
    output o_IO_RC1,
    output [5:0]o_RC_Tx,

    output [31:0]o_PMU_OC_1 ,
    output [31:0]o_PMU_OC_2 ,
    output [31:0]o_PMU_OC_3 ,
    output [31:0]o_PMU_OC_4 ,

//  DMM Mode Sel
    output o_RC_VSel,           
    output o_RC_ISel,

//  Force/Sense Control
    output [1:0]o_RC_LOFS,    

// Output RLoad Sel
    output [17:0]o_RC_RLSel,

//  FLAGs
    output o_con_done,
    output o_err
);

    //Relay Reg
    reg [3:0]OC_x01 ;
    reg [3:0]OC_x17 ;
    reg [18:0]DMM_en ;
	
	reg[5:0]RC_Tx;
    reg io_RC;

    reg RC_VSel;
    reg RC_ISel;
    reg [1:0]RC_LOFS;
    reg [17:0]RC_RLSel ;
    reg [31:0]PMU_OC_1 ;
    reg [31:0]PMU_OC_2 ;
    reg [31:0]PMU_OC_3 ;
    reg [31:0]PMU_OC_4 ;

    //Condition Tester
    reg [3:0]en_t ;
    reg [3:0]freq_t;
    reg [3:0]dc_t;

    //Flags
    reg exec_flag;
    reg err_flag;
    reg [3:0]freq_slot_flag;
    reg [3:0]dc_slot_flag ;
    reg done_flag;

    //Counter
    reg [15:0]delay_cnt;

    //Shift Counter
    wire [4:0]OC_Shifter1 = i_DC_Con1[8:4];     // Channel Number div 8
    wire [2:0]DMM_Shifter1 = i_DC_Con1[8:7];    // Channel Number div 64
    wire [4:0]RC_RLShifter1 = i_DC_Con1[21:17]; // RC_RL Shifter
    wire [4:0]OC_Shifter2 = i_DC_Con2[8:4];     // Channel Number div 8
    wire [2:0]DMM_Shifter2 = i_DC_Con2[8:7];    // Channel Number div 64
    wire [4:0]RC_RLShifter2 = i_DC_Con2[21:17]; // RC_RL Shifter
    wire [4:0]OC_Shifter3 = i_DC_Con3[8:4];     // Channel Number div 8
    wire [2:0]DMM_Shifter3 = i_DC_Con3[8:7];    // Channel Number div 64
    wire [4:0]RC_RLShifter3 = i_DC_Con3[21:17]; // RC_RL Shifter
    wire [4:0]OC_Shifter4 = i_DC_Con4[8:4];     // Channel Number div 8
    wire [2:0]DMM_Shifter4 = i_DC_Con4[8:7];    // Channel Number div 64
    wire [4:0]RC_RLShifter4 = i_DC_Con4[21:17]; // RC_RL Shifter
    wire [3:0]DC_OC1 = {PMU_OC_4[0],PMU_OC_3[0],PMU_OC_2[0],PMU_OC_1[0]} ;
    wire [3:0]DC_OC17 = {PMU_OC_4[16],PMU_OC_3[16],PMU_OC_2[16],PMU_OC_1[16]} ;

    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

//Function Identify
    always @(posedge i_sys_clk) begin
        if (i_rst_n == 1'b0) begin
            en_t <= 'd0;
            freq_t <= 'd0;
            dc_t <= 'd0;
        end
        else begin
            en_t <= (i_freq_relay1[0] + i_freq_relay2[0] + i_freq_relay3[0] + i_freq_relay4[0]) + (i_DC_Con1[0] +i_DC_Con2[0] +i_DC_Con3[0] +i_DC_Con4[0] );
            freq_t <= i_freq_relay1[0] + i_freq_relay2[0] + i_freq_relay3[0] + i_freq_relay4[0];
            dc_t <= i_DC_Con1[0] +i_DC_Con2[0] +i_DC_Con3[0] +i_DC_Con4[0];
        end
    end

//  Main Controller Function
    always @(posedge i_sys_clk) begin
        if (i_rst_n == 1'b0) begin
            exec_flag <= 1'b0;
            err_flag <= 1'b0;

            freq_slot_flag <= 'd0;
            dc_slot_flag <= 'd0;

            DMM_en            <= 'd0;
            
            RC_VSel            <= 'd1;      //Vpk HIGH Disable, LOW Enable
            RC_ISel            <= 'd1;      //Apk HIGH Disable, LOW Enable
            RC_LOFS             <= 'hf;      //HIGH Disable, LOW Enable
            RC_RLSel           <='hfffff;

            PMU_OC_1       <= 'd0 ;
            PMU_OC_2       <= 'd0 ;
            PMU_OC_3       <= 'd0 ;
            PMU_OC_4       <= 'd0 ;

            done_flag <= 'd1;
        end
        else begin
            if ((i_con_exec == 1'b1) & (exec_flag == 1'b0)) begin
                //To DO: More Sanity Check?
                if (en_t > 'd1) begin                                                   //Only ONE Enable bit should be set!
                    err_flag <= 1'b1;
                end
                else begin
                    exec_flag <= 1'b1;
                    done_flag <= 1'b0;
                    err_flag <= 1'b0;
                    freq_slot_flag[0] <= i_freq_relay1[0];
                    freq_slot_flag[1] <= i_freq_relay2[0];
                    freq_slot_flag[2] <= i_freq_relay3[0];
                    freq_slot_flag[3] <= i_freq_relay4[0];
                    
                    RC_Tx <= i_freq_relay4[17:12];                      //Special Relay for slot4

                    dc_slot_flag[0] <= i_DC_Con1[0];
                    dc_slot_flag[1] <= i_DC_Con2[0];
                    dc_slot_flag[2] <= i_DC_Con3[0];
                    dc_slot_flag[3] <= i_DC_Con4[0];

                end
            end
            else if (exec_flag == 1'b1) begin
                if (freq_t == 1'b1) begin
                    io_RC <= i_freq_relay1[0] || i_freq_relay2[0] || i_freq_relay3[0] || i_freq_relay4[0];
                    case (freq_slot_flag)
                        4'b0001: begin
                            if (i_freq_relay1[9:1] < 9) begin
                                PMU_OC_1 <= 'd1;
                                DMM_en <= 19'b000_0000_0000_0000_0001;
                            end
                            else if (i_freq_relay1[9:1] < 73) begin
                                DMM_en <= 19'b000_0000_0000_0000_0010;
                                PMU_OC_1 <= 'd0;
                            end
                            else if (i_freq_relay1[9:1] < 137) begin
                                PMU_OC_1 <= 1'b1 << 16;
                                DMM_en <= 19'b000_0000_0000_0000_0100;
                            end
                            else begin
                                DMM_en <= 19'b000_0000_0000_0000_1000;
                                PMU_OC_1 <= 'd0;
                            end
                        end
                        4'b0010: begin
                            if (i_freq_relay2[9:1] < 9) begin
                                PMU_OC_2 <= 'd1;
                                DMM_en <= 19'b000_0000_0000_0001_0000;
                            end
                            else if (i_freq_relay2[9:1] < 73) begin
                                DMM_en <= 19'b000_0000_0000_0010_0000;
                                PMU_OC_2 <= 'd0;
                            end
                            else if (i_freq_relay2[9:1] < 137) begin
                                PMU_OC_2 <= 1'b1 << 16;
                                DMM_en <= 19'b000_0000_0000_0100_0000;
                            end
                            else begin
                                PMU_OC_2 <= 'd0;
                                DMM_en <= 19'b000_0000_0000_1000_0000;
                            end
                        end
                        4'b0100: begin
                            if (i_freq_relay3[9:1] < 9) begin
                                PMU_OC_3 <= 'd1;
                                DMM_en <= 19'b000_0000_0001_0000_0000;
                            end
                            else if ((i_freq_relay3[9:1] > 64)& (i_freq_relay3[9:1] < 73)) begin
                                PMU_OC_3 <= 'd0;
                                DMM_en <= 19'b000_0000_0010_0000_0000;
                            end
                            else if (i_freq_relay3[9:1] < 137) begin
                                DMM_en <= 19'b000_0000_0100_0000_0000;
                                PMU_OC_3 <= 1'b1 << 16;
                            end
                            else if (i_freq_relay3[9:1] < 201) begin
                                PMU_OC_3 <= 'd0;
                                DMM_en <= 19'b000_0000_1000_0000_0000; 
                            end
                        end
                        4'b1000: begin
                            if (i_freq_relay4[9:1] < 9) begin
                                PMU_OC_4 <= 'd1;
                                DMM_en <= 19'b000_0001_0000_0000_0000;
                            end
                            else if (i_freq_relay4[9:1] < 72) begin
                                DMM_en <= 19'b000_0010_0000_0000_0000;
                                PMU_OC_4 <= 'd0;
                            end
                            else if (i_freq_relay4[9:1] < 137) begin
                                PMU_OC_4 <= 1'b1 << 16;
                                DMM_en <= 19'b000_0100_0000_0000_0000;
                            end
                            else begin
                                PMU_OC_4 <= 'd0;
                                DMM_en <= 19'b000_1000_0000_0000_0000;
                            end
                        end
                        default: begin                                                                                  // You Should NOT be HERE
                            PMU_OC_1 <= 'd0;
                            PMU_OC_2 <= 'd0;
                            PMU_OC_3 <= 'd0;
                            PMU_OC_4 <= 'd0;
                            DMM_en <= 'd0;
                        end
                    endcase
                end
                else if (dc_t == 1'b1) begin
                    case (dc_slot_flag)
                        4'b0001: begin
                            //Here Set Mode and F/S
                            if (i_DC_Con1[22] == 1'b1) begin
                                RC_VSel <= 1'b1;                                    //Select Current
                                RC_ISel <= 1'b0;
                                RC_LOFS <= 2'b01;
                            end
                            else begin
                                RC_VSel <= 1'b0;                                    //Select Voltage
                                RC_ISel <= 1'b1;
                                RC_LOFS <= 2'b10;
                            end
                            //Sanity Check
                            if ((i_DC_Con1[21:17] < 1) || (i_DC_Con1[21:17] > 18) || (i_DC_Con1[9:1] < 1) || (i_DC_Con1[9:1]  > 256)) begin
                                err_flag <= 1'b1;
                            end
                            else begin
                                RC_RLSel <= (RC_RLShifter1 >= 1 && RC_RLShifter1 <= 18) 
                                            ? ~(18'b1 << (RC_RLShifter1 - 1))
                                            : 18'h3FFFF;                        //Set RLoad
                                if (i_DC_Con1[23] == 1'b0) begin                //Select PMU
                                    PMU_OC_1 <= (32'b0000_0000_0000_0000_0000_0000_0000_0001)<<(OC_Shifter1);        
                                    DMM_en <= (19'b000_0000_0000_0000_0001)<<(DMM_Shifter1);
                                end
                                else if (i_DC_Con1[23] == 1'b1) begin       //Select DPS
                                    
                                end
                            end
                        end
                        4'b0010: begin
                            if (i_DC_Con2[22] == 1'b1) begin
                                RC_VSel <= 1'b1;                                    //Select Current
                                RC_ISel <= 1'b0;
                                RC_LOFS <= 2'b01;
                            end
                            else begin
                                RC_VSel <= 1'b0;                                    //Select Voltage
                                RC_ISel <= 1'b1;
                                RC_LOFS <= 2'b10;
                            end
                             //Sanity Check
                            if ((i_DC_Con2[21:17] < 1) || (i_DC_Con2[21:17] > 18) || (i_DC_Con2[9:1] < 1) || (i_DC_Con2[9:1]  > 256)) begin
                                err_flag <= 1'b1;
                            end
                            else begin
                                RC_RLSel <= (RC_RLShifter2 >= 1 && RC_RLShifter2 <= 18) 
                                            ? ~(18'b1 << (RC_RLShifter2 - 1))
                                            : 18'h3FFFF;   // 否则�1（即不置 0�
                                if (i_DC_Con2[23] == 1'b0) begin                //Select PMU
                                    PMU_OC_2 <= (32'b0000_0000_0000_0000_0000_0000_0000_0001)<<(OC_Shifter2);
                                    DMM_en <= (19'b000_0000_0000_0001_0000)<<(DMM_Shifter2);
                                end
                                else if (i_DC_Con2[23] == 1'b1) begin       //Select DPS
                                    
                                end
                            end
                        end
                        4'b0100:begin
                            if (i_DC_Con3[22] == 1'b1) begin
                                RC_VSel <= 1'b1;                                    //Select Current
                                RC_ISel <= 1'b0;
                                RC_LOFS <= 2'b01;
                            end
                            else begin
                                RC_VSel <= 1'b0;                                    //Select Voltage
                                RC_ISel <= 1'b1;
                                RC_LOFS <= 2'b10;
                            end
                             //Sanity Check
                            if ((i_DC_Con3[21:17] < 1) || (i_DC_Con3[21:17] > 18) || (i_DC_Con3[9:1] < 1) || (i_DC_Con3[9:1]  > 256)) begin
                                err_flag <= 1'b1;
                            end
                            else begin
                                RC_RLSel <= (RC_RLShifter3 >= 1 && RC_RLShifter3 <= 18) 
                                            ? ~(18'b1 << (RC_RLShifter3 - 1))
                                            : 18'h3FFFF;   // 否则�1（即不置 0�
                                if (i_DC_Con3[23] == 1'b0) begin                //Select PMU
                                    PMU_OC_3 <= (32'b0000_0000_0000_0000_0000_0000_0000_0001)<<(OC_Shifter3);
                                    DMM_en <= (19'b000_0000_0001_0000_0000)<<(DMM_Shifter3);
                                end
                                else if (i_DC_Con3[23] == 1'b1) begin       //Select DPS
                                    
                                end
                            end
                        end
                        4'b1000:begin
                            if (i_DC_Con4[22] == 1'b1) begin
                                RC_VSel <= 1'b1;                                    //Select Current
                                RC_ISel <= 1'b0;
                                RC_LOFS <= 2'b01;
                            end
                            else begin
                                RC_VSel <= 1'b0;                                    //Select Voltage
                                RC_ISel <= 1'b1;
                                RC_LOFS <= 2'b10;
                            end
                             //Sanity Check
                            if ((i_DC_Con4[21:17] < 1) || (i_DC_Con4[21:17] > 18) || (i_DC_Con4[9:1] < 1) || (i_DC_Con4[9:1]  > 256)) begin
                                err_flag <= 1'b1;
                            end
                            else begin
                                RC_RLSel <= (RC_RLShifter4 >= 1 && RC_RLShifter4 <= 18) 
                                            ? ~(18'b1 << (RC_RLShifter4 - 1))
                                            : 18'h3FFFF;   // 否则�1（即不置 0�
                                if (i_DC_Con4[23] == 1'b0) begin                //Select PMU
                                    PMU_OC_4 <= (32'b0000_0000_0000_0000_0000_0000_0000_0001)<<(OC_Shifter4);
                                    DMM_en <= (19'b000_0001_0000_0000_0000)<<(DMM_Shifter4);
                                end
                                else if (i_DC_Con4[23] == 1'b1) begin       //Select DPS
                                    
                                end
                            end
                        end
                        default:begin                           // You Should NOT be HERE
                            RC_VSel            <= 'd1;           
                            RC_ISel            <= 'd1;
                            RC_RLSel           <= 'hf;
                            RC_LOFS            <= 'hf;

                            PMU_OC_1       <= 'd0 ;
                            PMU_OC_2       <= 'd0 ;
                            PMU_OC_3       <= 'd0 ;
                            PMU_OC_4       <= 'd0 ;
                            DMM_en            <= 'd0;
                        end
                    endcase
                end
                done_flag <= 1'b1;
            end
        end
        if (done_flag == 1) begin
            exec_flag <= 1'b0; 
        end
    end 

    assign o_con_done = done_flag;
    assign o_err = err_flag;

    assign o_DMM_EN = DMM_en;
    assign o_IO_RC1 = io_RC;
    assign o_RC_Tx = RC_Tx; 

    assign o_RC_VSel = RC_VSel;
    assign o_RC_ISel = RC_ISel;

    assign o_RC_LOFS = RC_LOFS;

    assign o_RC_RLSel = RC_RLSel;

    assign o_PMU_OC_1   = PMU_OC_1;
    assign o_PMU_OC_2   = PMU_OC_2;
    assign o_PMU_OC_3   = PMU_OC_3;
    assign o_PMU_OC_4   = PMU_OC_4;


endmodule