/*
 *
 * Copyright (C) 2020-2022 Qing.zhou
 *
 *
 */
/* -------------------------------- Includes -------------------------------- */
/* external head files */

/* internal head files */
#include "UDS_Services_Common.h"
#include "DiagnosticR/Comp_ISO_15765_3/Iso15765_3.h"
/* -------------------------------- Defines --------------------------------- */

/* -------------------------------- Macros ---------------------------------- */

/* ------------------------------- Data Types ------------------------------- */

/* -------------------------- Function declaration -------------------------- */
static BOOL isSessionAllowed(t_UDS_Session did_session);
static boolean UDS_isSecurityUnlocked(uint8 did_security_level);

/* ----------------------------- Local Variables ---------------------------- */

/* ---------------------------- Global Variables ---------------------------- */

/* --------------------------- Private Variables ---------------------------- */

/* --------------------------- Private Functions ---------------------------- */
static BOOL isSessionAllowed(t_UDS_Session did_session)
{
    UI_8 curr_session;
    BOOL ret = FALSE;

    curr_session = Iso15765_3QueryActiveSession();

    ret = ((curr_session & did_session) == curr_session) ? TRUE : FALSE;

    return ret;
}

static boolean UDS_isSecurityUnlocked(uint8 did_security_level)
{
    boolean result = FALSE;

    if (UDS_ECU_LOCKED == did_security_level) {
        if (UDS_ECU_LOCKED == UdsSecurityTaskQueryAccessStatus()) {
            result = FALSE;
        }
        else{
            result = TRUE;
        }
    }
    else {
        result = TRUE;
    }

    result = TRUE;

    return result;
}


/* ---------------------------- Public Functions ---------------------------- */
uint8 UDS_ReadDiD_CB(uint8 id_h, uint8 id_l)
{
    uint16_t   requested_id;
    uint16_t   resp_pos;
    tp_uds_read_data_by_identifier_resp resp;
    t_UDS_ERR response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;

    resp_pos = 0;//( - (UI_16) 1);

    resp = ISO15765_3_GET_RESP_DATA(tp_uds_read_data_by_identifier_resp);
    /* Send correct Ids */
    resp->buffer_dades[resp_pos + 0] = id_h;
    resp->buffer_dades[resp_pos + 1] = id_l;

    resp_pos = resp_pos + 2;

    /* Get the complete id */
    requested_id = ((uint16_t) id_h) << 8;
    requested_id = requested_id | (uint16_t) id_l;

    for (uint8 idx = 0; idx < GetReadDidServiceNumber(); idx++) {

        if (requested_id == uds_did_read_commands[idx].did) {

            if (TRUE == isSessionAllowed(uds_did_read_commands[idx].allowed_session)) {

                (void)uds_did_read_commands[idx].Getter((t_UDS_ERR*)&resp->buffer_dades[resp_pos]);
                Iso15765_3IncrementResponseSize(uds_did_read_commands[idx].size);
                /* assuming no errors reading */
                response_mode = ISO15765_3_POSITIVE_RESPONSE;

            }
            else {
                response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
            }
            break;
        }
    }

    /* Finally, in case of positive response, we set the identifier in the response */
    if (response_mode == ISO15765_3_POSITIVE_RESPONSE) {
        Iso15765_3IncrementResponseSize((UI_8) 2);
    } else {

        /* Do nothing */
    }

    /* Send the response to ISO_3 layer */
    return (uint8)(response_mode);
}

uint8 UDS_WriteDiD_CB(uint8 id_h, uint8 id_l, uint8 *buf_data_rx, uint16_t size)
{
    uint16_t   requested_id;
    tp_uds_write_data_by_identifier resp;
    t_UDS_ERR response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;


    resp = ISO15765_3_GET_RESP_DATA(tp_uds_write_data_by_identifier);

    /* Get the complete id */
    requested_id = ((((uint16_t) (id_h)) << 8) | ((uint16_t) (id_l)));

    for (uint8 idx = 0; idx < GetWriteDidServiceNumber(); idx++) {
        if (requested_id == uds_did_write_commands_by_reference[idx].did) {

            if (TRUE == isSessionAllowed(uds_did_write_commands_by_reference[idx].allowed_session)) {
                if (TRUE == UDS_isSecurityUnlocked(uds_did_write_commands_by_reference[idx].security)) {
                    if (NULL != uds_did_write_commands_by_reference[idx].Setter) {

                        if(size == uds_did_write_commands_by_reference[idx].size) {
                            response_mode = uds_did_write_commands_by_reference[idx].Setter(buf_data_rx);
                        }
                        else {
                            /** Wrong size **/
                            response_mode = UDS_ERR_INVALID_FORMAT;
                        }

                    }
                    else {
                        /** Callback not implemented **/
                        response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                    }
                }
                else {
                    /** Security is not unlocked **/
                    response_mode = UDS_ERR_SECURITY_ACCESS_DENIED;
                }
            }
            else {
                /** Wrong Session **/
                response_mode = UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION;
            }
            break;
        }
    }

    /* Finally in case of positive response we set the identifier in the response */
    if (response_mode == ISO15765_3_POSITIVE_RESPONSE) {
        resp->id_high = id_h;
        resp->id_low = id_l;
        Iso15765_3IncrementResponseSize((UI_8) 2);

    } else {

        /* Do nothing */
    }

    /* Send the response to ISO_3 layer */
    Iso15765_3SendResponse(response_mode);

    return response_mode;
}


/*!
 * @brief Routine functional unit
 *
 * @param[in] routine_ctrl_type: routineControlType
 * @param[in] id_high: ID MSB
 * @param[in] id_low: ID LSB
 * @param[in] routine_entry_option: routineControlOption
 * @param[in] size: data size
 * @return None
 */
void UDS_RoutineControl_CB(uint8 routine_ctrl_type, uint8 id_high, uint8 id_low, uint8 *routine_entry_option, uint16 size)
{
    tp_uds_control_routine resp;
    uint8 response_mode = ISO15765_3_POSITIVE_RESPONSE;
    uint16 id;

    resp = ISO15765_3_GET_RESP_DATA(tp_uds_control_routine);
     /* Get the complete id */
    id = ((uint16) id_high << 8) | (uint16) id_low;

    for (uint8 idx =0; idx < GetRoutineServiceNumber(); idx++){
        if (id ==  uds_routine_control_commands[idx].did){
            if (size == uds_routine_control_commands[idx].size){
                if (TRUE == isSessionAllowed(uds_routine_control_commands[idx].allowed_session)){
                    if (TRUE == UDS_isSecurityUnlocked(uds_routine_control_commands[idx].security)){
                        switch(routine_ctrl_type){
                        case UDS_CTRL_ROUTINE_START:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_routine_control_commands[idx].func_impl.ImplStart()){
                                response_mode = uds_routine_control_commands[idx].func_impl.ImplStart();
                            }
                            else{
                                response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }
                        break;
                        case UDS_CTRL_ROUTINE_STOP:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_routine_control_commands[idx].func_impl.ImplStop()){
                                response_mode = uds_routine_control_commands[idx].func_impl.ImplStop();
                            }
                            else{
                                response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }
                        break;
                        case UDS_CTRL_ROUTINE_RESULTS:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_routine_control_commands[idx].func_impl.ImplReq()){
                                response_mode = uds_routine_control_commands[idx].func_impl.ImplReq();
                            }
                            else{
                                response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }                        
                        break;
                        default:
                        response_mode = UDS_ERR_INVALID_FORMAT;
                        break;   
                        }                     
                    }
                    else{
                        /** Security is not unlocked **/
                        response_mode = UDS_ERR_SECURITY_ACCESS_DENIED;
                    }
                }
                else{
                    /** Wrong Session **/
                    response_mode = UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION;
                }
            }
            else{
                 /** Wrong size **/
                response_mode = UDS_ERR_INVALID_FORMAT;
            }
            break;
        }
    }

/* Finally in case of positive response we set the identifier in the response */
    if (ISO15765_3_POSITIVE_RESPONSE == response_mode) {
        resp->routine_ctrl_type = routine_ctrl_type;
        resp->routine_id_high = id_high;
        resp->routine_id_low = id_low;
        Iso15765_3IncrementResponseSize(3);
    }
    /* Send the response to ISO_3 layer */
    Iso15765_3SendResponse(response_mode);
}

/*!
 * @brief InputOutput Control functional unit
 *
 * @param[in] id_h: ID MSB
 * @param[in] id_l: ID LSB
 * @param[in] io_ctrl_type: iocontrol type
 * @param[in] buf_data_rx: controlOptionRecord
 * @param[in] size: data size of buf_data_rx
 * @return None
 */
void UDS_IOControl_CB(uint8 id_h, uint8 id_l, uint8 io_ctrl_type, uint8* buf_data_rx, uint16 size)
{
    uint16 io_ctl_id;
    tp_uds_input_output_control_by_id resp;
    t_UDS_ERR response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;

    resp = ISO15765_3_GET_RESP_DATA(tp_uds_input_output_control_by_id);
    /* Get the complete id */
    io_ctl_id = ((((uint16) (id_h)) << 8) | ((uint16) (id_l)));

    for (uint8 idx = 0; idx < GetIoControlServiceNumber(); idx++) {
        if (io_ctl_id == uds_io_control_commands[idx].did) {
            if (size == uds_io_control_commands[idx].size){
                if (TRUE == isSessionAllowed(uds_io_control_commands[idx].allowed_session)) {
                    if (TRUE == UDS_isSecurityUnlocked(uds_io_control_commands[idx].security)) {
                        switch (io_ctrl_type){
                        case IO_CTL_RETURN_CONTOL:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_io_control_commands[idx].func_impl.ImplReturn(buf_data_rx)){
                                response_mode = uds_io_control_commands[idx].func_impl.ImplReturn(buf_data_rx);
                            } 
                            else{
                                response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }
                            break;
                        case IO_CTL_RESET:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_io_control_commands[idx].func_impl.ImplDefault(buf_data_rx)){
                                response_mode = uds_io_control_commands[idx].func_impl.ImplDefault(buf_data_rx);
                            } 
                            else{
                             response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }
                            break;
                        case IO_CTL_FREEZE:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_io_control_commands[idx].func_impl.ImplFreeze(buf_data_rx)){
                                response_mode = uds_io_control_commands[idx].func_impl.ImplFreeze(buf_data_rx);
                            } 
                            else{
                                response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }
                            break;
                        case IO_CTL_CONTROL:  //DamonChange 20230117  0 replace NULL
                            if (0 != uds_io_control_commands[idx].func_impl.ImplControl(buf_data_rx)){
                                response_mode = uds_io_control_commands[idx].func_impl.ImplControl(buf_data_rx);
                            } 
                            else{
                                response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
                            }
                            break;                           
                        default:
                            response_mode = UDS_ERR_INVALID_FORMAT;
                            break;
                        }
                    }
                    else{
                         /** Security is not unlocked **/
                        response_mode = UDS_ERR_SECURITY_ACCESS_DENIED;
                    }
                }
                else{
                /** Wrong Session **/
                    response_mode = UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION;
                }
            }
            else{
                 /** Wrong size **/
                response_mode = UDS_ERR_INVALID_FORMAT;
            }
            break;             
        }
    }   

        /* Finally in case of positive response we set the identifier in the response */
    if (response_mode == ISO15765_3_POSITIVE_RESPONSE) {
        resp->id_high = id_h;
        resp->id_low = id_l;
        resp->io_ctrl_type = io_ctrl_type;
        Iso15765_3IncrementResponseSize((UI_8) 3);
    } else {

        /* Do nothing */
    }

    /* Send the response to ISO_3 layer */
    Iso15765_3SendResponse(response_mode);
}


