/*! Register map:
 *!
 *! |-----|-------------|-------------------------------------------------------|
 *! | Adr | Name        | Description                                           |
 *! |-----|-------------|-------------------------------------------------------|
 *! | 00  | CamRegHi    | High byte of camera register address                  |
 *! | 01  | CamRegLo    | Low byte of camera register address                   |
 *! | 02  | CamVal      | New value to write to camera register                 |
 *! |-----|-------------|-------------------------------------------------------|
 *! | 10  | PulseHi     | High byte of LED pulse length                         |
 *! | 11  | PulseLo     | Low byte of LED pulse length                          |
 *! | 12  | PrePulseHi  | High byte of LED pre-pulse length                     |
 *! | 13  | PrePulseLo  | Low byte of LED pre-pulse length                      |
 *! |-----|-------------|-------------------------------------------------------|
 *! | 70  | I_L_R_Hi    | High byte of left LED current measurement, running    |
 *! | 71  | I_L_R_Lo    | Low byte " " "                                        |
 *! | 72  | I_L_I_Hi    | High byte of left LED current measurement, idle       |
 *! | 73  | I_L_I_Lo    | Low byte " " "                                        |
 *! | 74  | I_R_R_Hi    | High byte of right LED current measurement, running   |
 *! | 75  | I_R_R_Lo    | Low byte " " "                                        |
 *! | 76  | I_R_I_Hi    | High byte of right LED current measurement, idle      |
 *! | 77  | I_R_I_Lo    | Low byte " " "                                        |
 *! | 78  | I_Errors    | Current limit errors. Bitmap. <br> Top 4 bits are latched, bottom are live from latest measurement. <br> Order, from LSB: Left running, left idle, right running, right idle.|
 *! |-----|-------------|-------------------------------------------------------|
 *! | 80  | I_Lim_R_Hi  | High byte of running LED current limit                |
 *! | 81  | I_Lim_R_Lo  | Low byte " " "                                        |
 *! | 83  | I_Lim_I_Hi  | High byte of idle LED current limit                   |
 *! | 84  | I_Lim_I_Lo  | Low byte " " "                                        |
 *! |-----|-------------|-------------------------------------------------------|
 *! | A0  | Mode        | Application mode. "A" = app (camera), "B" = bootloader|
 *! | A1  | SWVer_Hi    | High byte of software version (BCD)                   |
 *! | A2  | SWVer_Lo    | Low byte " " "                                        |
 *! |-----|-------------|-------------------------------------------------------|
 *! | B0  | Debug       | Bitmap. Bit0 = reset_usb (tells if cameras attached)  |
 *! |-----|-------------|-------------------------------------------------------|
 */
module i2c_system_control
#(
    parameter INITIAL_PRE_PULSE = 16'd2250      // 2,250 / 60,000,000 * 8 = 166 us
,   parameter INITIAL_PULSE_COUNT = 16'd7500    // 7,500 / 60,000,000 * 8 = 1000 us

    // Setting limit for running LED current: 
    // BCR421U set for ~13mA
    // 1 ohm current sense resistor (V = I)
    // Current sense gain = 316/15 = 21.07
    // ADC is 12-bit (max 2047) fractional part
    // Values over 1V will start to read over 2048
    //      Note that the ADC counts are inflated by 1.024x
    //      So input 0.5V will become 1048 (0.5*2048*1.024)
    // 
    // Current-to-counts calculation:
    //      ADC_DATA = I (mA) * 21.07 * 2048 * 1.024
    //
    // If we want a limit of 30mA for current (over 2x the BCR421U setting)
    // We should impose ADC counts limit of 1325
    // And a small 3mA limit for idle ==> 132
,   parameter INITIAL_RUN_LIMIT = 16'd1325
,   parameter INITIAL_IDLE_LIMIT = 16'd132
)
(
    input CLK
,   input RESET

    // I2C registers
,   input [7:0] REG_ADDR
,   input [7:0] REG_NEW_VALUE
,   input REG_WR_EN
,   output [7:0] REG_CUR_VALUE

    // control outputs
    // Camera I2C commands
,   output [15:0] CAM_REG_ADDR
,   output [7:0] CAM_REG_VALUE
,   output CAM_REG_WRITE_START
,   input CAM_REG_WRITE_IN_PROGRESS

    // IR LED pulse control
,   output [15:0] PRE_PULSE_COUNT
,   output [15:0] PULSE_LENGTH_COUNT
,   output PRE_PULSE_UPDATE
,   output PULSE_LENGTH_UPDATE

    // Current measurements
,   input [13:0] I_LEFT_RUN
,   input [13:0] I_RIGHT_RUN
,   input [13:0] I_LEFT_IDLE
,   input [13:0] I_RIGHT_IDLE
,   input [7:0] I_ERRORS

    // Current limits
,   output [13:0] RUN_CURRENT_LIMIT
,   output [13:0] IDLE_CURRENT_LIMIT

    // Debug stuff
,   input   RESET_USB
);

`include "sw_ver.v"

reg [15:0] cam_reg_addr;
reg [7:0] cam_reg_value;
reg cam_reg_write_start;

// reg [15:0] pwm_duty_cycle;
// reg pwm_update;
reg [15:0] pre_pulse_count;
reg [15:0] pulse_length_count;
reg pre_pulse_update;
reg pulse_length_update;

reg [7:0] reg_cur_value;

reg [7:0] module_resets;

reg [15:0] run_current_limit;
reg [15:0] idle_current_limit;
reg current_limit_update;

localparam [15:0] i2c_sw_ver = `SW_VER;
localparam [7:0] application_id_code = "A";

// set the camera control registers
// setting register 0x02 (cam register value)
// also triggers the i2c transaction to the camera(s)
// the transaction trigger is lowered as soon as the I2C
// interface reports it is busy ("CAM_REG_WRITE_IN_PROGRESS" goes high)
always @(posedge CLK or posedge RESET) begin
    if(RESET) begin
        cam_reg_addr <= 16'd0;
        cam_reg_value <= 8'd0;
        cam_reg_write_start <= 1'b0;
    end else begin
        if(REG_WR_EN) begin
            case(REG_ADDR)
            8'h00: cam_reg_addr[15:8] <= REG_NEW_VALUE;
            8'h01: cam_reg_addr[7:0]  <= REG_NEW_VALUE;
            8'h02: cam_reg_value <= REG_NEW_VALUE;
            endcase
        end
        if(REG_WR_EN && (REG_ADDR == 8'h02)) begin
            cam_reg_write_start <= 1'b1;
        end else begin
            if(cam_reg_write_start) begin
                if(CAM_REG_WRITE_IN_PROGRESS) cam_reg_write_start <= 1'b0;
            end
        end
    end
end

// set the pwm control registers
// assigning the LSB of pwm duty cycle also triggers the 
// update externally
// the update flag stays high only one clock cycle
always @(posedge CLK or posedge RESET) begin
    if(RESET) begin
        pre_pulse_count <= INITIAL_PULSE_COUNT;
        pulse_length_count <= INITIAL_PULSE_COUNT;
        pre_pulse_update <= 1'b0;
        pulse_length_update <= 1'b0;
    end else begin
        if(REG_WR_EN) begin
            case(REG_ADDR)
            8'h10: begin
                pulse_length_count[15:8] <= REG_NEW_VALUE;
                pulse_length_update <= 1'b0;
            end
            8'h11: begin
                pulse_length_count[7:0] <= REG_NEW_VALUE;
                pulse_length_update <= 1'b1;
            end
            8'h12: begin
                pre_pulse_count[15:8] <= REG_NEW_VALUE;
                pre_pulse_update <= 1'b0;
            end
            8'h13: begin
                pre_pulse_count[7:0] <= REG_NEW_VALUE;
                pre_pulse_update <= 1'b1;
            end
            default: begin
                pre_pulse_update <= 1'b0;
                pulse_length_update <= 1'b0;
            end
            endcase
        end else begin
            if(pulse_length_update) pulse_length_update <= 1'b0;
            if(pre_pulse_update) pre_pulse_update <= 1'b0;
        end
    end
end

// set current limits
always @(posedge CLK or posedge RESET) begin
    if(RESET) begin
        run_current_limit <= INITIAL_RUN_LIMIT;
        idle_current_limit <= INITIAL_IDLE_LIMIT;
        current_limit_update <= 1'b0;
    end
    else begin
        if(REG_WR_EN) begin
            case(REG_ADDR)
            8'h80: begin
                run_current_limit[15:8] <= REG_NEW_VALUE;
                current_limit_update <= 1'b0;
            end
            8'h81: begin
                run_current_limit[7:0] <= REG_NEW_VALUE;
                current_limit_update <= 1'b1;
            end
            8'h82: begin
                idle_current_limit[15:8] <= REG_NEW_VALUE;
                current_limit_update <= 1'b0;
            end
            8'h83: begin
                idle_current_limit[7:0] <= REG_NEW_VALUE;
                current_limit_update <= 1'b1;
            end
            default:
                current_limit_update <= 1'b0;
            endcase
        end
        else begin
            current_limit_update <= 1'b0;
        end
    end
end

reg [7:0] reg_addr_r;
wire reg_rd_en;

// check for change in register address
// used to latch the current measurements
always @(posedge CLK or posedge RESET) begin
    if(RESET) begin
        reg_addr_r <= 8'h0;
    end else begin
        reg_addr_r <= REG_ADDR;
    end
end

assign reg_rd_en = REG_ADDR != reg_addr_r;

reg [ 7:0] i_errors_shad;
reg [15:0] i_left_run_shad;
reg [15:0] i_left_idle_shad;
reg [15:0] i_right_run_shad;
reg [15:0] i_right_idle_shad;

always @(posedge CLK or posedge RESET) begin
    if(RESET) begin
        i_errors_shad <= 8'h0;
        i_left_run_shad <= 16'h0;
        i_left_idle_shad <= 16'h0;
        i_right_run_shad <= 16'h0;
        i_right_idle_shad <= 16'h0;
    end else begin
        if(reg_rd_en) begin
            if(REG_ADDR == 8'h70)
                i_left_run_shad <= {2'b0, I_LEFT_RUN};
            if(REG_ADDR == 8'h72)
                i_left_idle_shad <= {2'b0, I_LEFT_IDLE};
            if(REG_ADDR == 8'h74)
                i_right_run_shad <= {2'b0, I_RIGHT_RUN};
            if(REG_ADDR == 8'h76)
                i_right_idle_shad <= {2'b0, I_RIGHT_IDLE};                            
            if(REG_ADDR == 8'h78)
                i_errors_shad <= I_ERRORS;
        end
    end
end


// read registers
// updates synchronously with clock as soon as address changes
always @(posedge CLK) begin
  case (REG_ADDR)
    8'h00: reg_cur_value <= cam_reg_addr[15:8];  
    8'h01: reg_cur_value <= cam_reg_addr[7:0];  
    8'h02: reg_cur_value <= cam_reg_value; 
    8'h10: reg_cur_value <= pulse_length_count[15:8];
    8'h11: reg_cur_value <= pulse_length_count[7:0];
    8'h12: reg_cur_value <= pre_pulse_count[15:8];
    8'h13: reg_cur_value <= pre_pulse_count[7:0];
    8'h20: reg_cur_value <= module_resets;
    8'h30: reg_cur_value <= 8'h00; // DFU reset always reads zero

    8'h70: reg_cur_value <= i_left_run_shad[15:8];
    8'h71: reg_cur_value <= i_left_run_shad[7:0];
    8'h72: reg_cur_value <= i_left_idle_shad[15:8];
    8'h73: reg_cur_value <= i_left_idle_shad[7:0];   
    8'h74: reg_cur_value <= i_right_run_shad[15:8];
    8'h75: reg_cur_value <= i_right_run_shad[7:0];
    8'h76: reg_cur_value <= i_right_idle_shad[15:8];
    8'h77: reg_cur_value <= i_right_idle_shad[7:0];  
    8'h78: reg_cur_value <= i_errors_shad;

    8'h80: reg_cur_value <= run_current_limit[15:8];
    8'h81: reg_cur_value <= run_current_limit[7:0];
    8'h82: reg_cur_value <= idle_current_limit[15:8];
    8'h83: reg_cur_value <= idle_current_limit[7:0];

    8'hA0: reg_cur_value <= application_id_code;
    8'hA1: reg_cur_value <= i2c_sw_ver[15:8];
    8'hA2: reg_cur_value <= i2c_sw_ver[7:0];

    8'hB0: reg_cur_value <= {7'b0, RESET_USB};
    default: reg_cur_value <= 8'hFF;
  endcase
end

reg [15:0] run_limit_shad;
reg [15:0] idle_limit_shad;
always @(posedge CLK or posedge RESET) begin
    if(RESET) begin
        run_limit_shad <= INITIAL_RUN_LIMIT;
        idle_limit_shad <= INITIAL_IDLE_LIMIT;
    end
    else begin
        if(current_limit_update) begin
            run_limit_shad <= run_current_limit;
            idle_limit_shad <= idle_current_limit;
        end
    end
end

assign CAM_REG_ADDR = cam_reg_addr;
assign CAM_REG_VALUE = cam_reg_value;
assign CAM_REG_WRITE_START = cam_reg_write_start;

assign PULSE_LENGTH_COUNT = pulse_length_count;
assign PRE_PULSE_COUNT = pre_pulse_count;
assign PULSE_LENGTH_UPDATE = pulse_length_update;
assign PRE_PULSE_UPDATE = pre_pulse_update;
assign REG_CUR_VALUE = reg_cur_value;

assign RUN_CURRENT_LIMIT = run_limit_shad[13:0];
assign IDLE_CURRENT_LIMIT = idle_limit_shad[13:0];

endmodule