#include <stdlib.h>
#include <string.h>
#include <sys/time.h>

#include "vcasdo_fsm.h"
#include "can_vca.h"
#include "vca_canopen.h"

static const char *sdofsmErrorMessages[] = {"none", "SDO transfer time out.", "SDO FSM of this port & node exists."};
static const int sdofsmErrorMessagesCnt = 3;

/**
 * vcasdo_error_msg - translates err_no to the string message
 * @err_no: number of error, if FSM state == %sdofsmError
 */
const char* vcasdo_error_msg(int err_no)
{
    if(err_no < 0 || err_no >= sdofsmErrorMessagesCnt) return "unknown error.";
    return sdofsmErrorMessages[err_no];
}

//======================== logging support ==========================

// -------------- log levels ------------------
#define LOG_FATAL   0
#define LOG_ERR     1
#define LOG_MSG     2
#define LOG_INF     3
#define LOG_DEB     4
/*
 * sdo_log - write message to the log if level <= log_level
 * @level: importance of message (0 is the highest)
 * @format: common printf format
 */
static void sdo_log(int level, const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    vca_vlog("vcasdo", level, format, ap);
    va_end(ap);
}

static void sdo_log_msg(int level, const struct canmsg_t *msg)
{
    char msgbuff[50];
    vca_msg2str(msg, msgbuff, 50);
    vca_log("vcasdo", level, "msg dump: %s\n", msgbuff);
}

static void sdo_error(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    if(!fsm) return;
    // copy msg to the out_msg
    memcpy(&(fsm->out_msg), msg, sizeof(canmsg_t));
    fsm->state = sdofsmError;
    sdo_log_msg(LOG_ERR, msg);
}

int sdo_check_abort(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int head = msg->data[0];
    int cmd = VCA_SDO_GET_COMMAND(head);
    if(cmd == VCA_SDO_ABORT) {
        // copy msg to the out_msg
        memcpy(&(fsm->out_msg), msg, sizeof(canmsg_t));
        fsm->state = sdofsmAbort;
        return 1;
    }
    return 0;
}

void sdo_fill_multiplexor(byte *mult, unsigned index, unsigned subindex)
{
    mult[0] = index % 256;
    mult[1] = (index >> 8) % 256;
    mult[2] = (byte)subindex;    
}
//===================== state functions =============================
//--------------------- UPLOAD --------------------------------------
int vcasdo_fsm_state_upload_init(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg);
int vcasdo_fsm_state_upload_segment(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg);

/*
 * vcasdo_fsm_state_upload_init - SDO upload init state function 
 *
 * Returns 0 if no answer is in fsm->out_msg, 
 */
int vcasdo_fsm_state_upload_init(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int ret = 0;
    // is it init domain upload response ?
    int head = msg->data[0];
    int cmd = VCA_SDO_GET_COMMAND(head);
    sdo_log(LOG_DEB, "fsm(%p): entering vcasdo_fsm_state_upload_init()\n", fsm);
    while(1) {  // not real loop, just for break if error occures
        if(sdo_check_abort(fsm, msg)) break;
        if(cmd != VCA_SDO_INIT_UPLOAD_C) {
            sdo_log(LOG_ERR, "fsm(%p): unexpected command (VCA_SDO_INIT_UPLOAD_C expected)\n", fsm);
            sdo_error(fsm, msg);
            break;
        }
        if(head & VCA_SDO_EXPEDITED_TRANSFER) {
            ul_dbuff_set_len(&fsm->data, 0);
            //expedited transfer
            int len = VCA_SDO_EXPEDITED_SIZE(head);
            ul_dbuff_cat(&(fsm->data), msg->data + 4, len);
            sdo_log(LOG_INF, "fsm(%p): %i bytes uploaded\n", fsm, fsm->data.len);
            fsm->state = sdofsmDone;
            break;
        } 
        
        // segmented transfer
        if(head & VCA_SDO_SIZE_INDICATOR) {
            //read size (byte4: LSB, byte7:MSB)
            fsm->bytes_to_load = msg->data[7]; 
            fsm->bytes_to_load = msg->data[4] + (fsm->bytes_to_load << 8);
            ul_dbuff_set_capacity(&fsm->data, fsm->bytes_to_load);
            ul_dbuff_set_len(&fsm->data, 0);
        }
        else {
            // e=0, s=0 reserved for future use
            sdo_log(LOG_ERR, "fsm(%p): e=0, s=0 reserved for future use\n", fsm);
            sdo_error(fsm, msg);
            break;
        }
        
        // send upload SDO segment msg
        fsm->toggle_bit = 0;
        byte *data = fsm->out_msg.data;
        memset(data, 0, 8);
        data[0] = VCA_SDO_COMMAND(VCA_SDO_UPLOAD_SEGMENT_R);
        fsm->statefnc = vcasdo_fsm_state_upload_segment;
        ret = 1;

        break;
    }
    gettimeofday(&(fsm->last_activity), NULL);
    
    return ret;
}

//-------------------------------------------------------------------
/*
 * vcasdo_fsm_state_upload_segment - SDO upload segment state function 
 *
 * Returns 0 if no answer is in fsm->out_msg, 
 */
int vcasdo_fsm_state_upload_segment(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int ret = 0;
    // is it init domain upload response ?
    int head = msg->data[0];
    int cmd = VCA_SDO_GET_COMMAND(head);
    sdo_log(LOG_INF, "fsm(%p): entering vcasdo_fsm_state_upload_segment()\n", fsm);
    while(1) {  // not real loop, just for break if error occures
        int n;
        if(sdo_check_abort(fsm, msg)) break;
        if(cmd != VCA_SDO_UPLOAD_SEGMENT_C) {
            sdo_log(LOG_ERR, "fsm(%p): unexpected command (VCA_SDO_UPLOAD_SEGMENT_C expected)\n", fsm);
            sdo_error(fsm, msg);
            break;
        }
        // check toggle bit
        if(!((head & VCA_SDO_TOGGLE_BIT) ^ fsm->toggle_bit)) {
            sdo_log(LOG_ERR, "toggle bit error\n");
            sdo_error(fsm, msg);
            break;
        }
        // toggle toggle bit
        fsm->toggle_bit ^= VCA_SDO_TOGGLE_BIT;
        // get number of data bytes
        n = VCA_SDO_SEGMENTED_SIZE_(head);
        // store received bytes to fsm->data
        if(fsm->data.len + n > fsm->data.capacity) n = fsm->data.capacity - fsm->data.len;
        ul_dbuff_cat(&(fsm->data), msg->data + 1, n);
        // check if this is the last segment
        if(head & VCA_SDO_LAST_SEGMENT_INDICATOR) {
            sdo_log(LOG_INF, "fsm(%p): %i bytes of %i expected uploaded\n", fsm, fsm->data.len, fsm->bytes_to_load);
            fsm->state = sdofsmDone;
        }
        else {
            // prepare request for the next segment
            byte *data = fsm->out_msg.data;
            memset(data, 0, 8);
            data[0] = VCA_SDO_COMMAND(VCA_SDO_UPLOAD_SEGMENT_R);
            data[0] &= ~VCA_SDO_TOGGLE_BIT;
            data[0] |= fsm->toggle_bit;
            ret = 1;
        }
        break;
    }
    gettimeofday(&(fsm->last_activity), NULL);
    return ret;
}
//--------------------- DOWNLOAD -----------------------------------
int vcasdo_fsm_state_download_init(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg);
int vcasdo_fsm_state_download_segment(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg);

/*
 * vcasdo_fsm_state_download_init - SDO FSM download init state function 
 *
 * Returns 0 if no answer is in fsm->out_msg, 
 */
int vcasdo_fsm_state_download_init(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int ret = 0;
    // is it init domain down response ?
    int head = msg->data[0];
    int cmd = VCA_SDO_GET_COMMAND(head);
    sdo_log(LOG_DEB, "fsm(%p): entering vcasdo_fsm_state_download_init()\n", fsm);
    while(1) {  // not real loop, just for break if error occures
        if(sdo_check_abort(fsm, msg)) break;
        if(cmd != VCA_SDO_INIT_DOWNLOAD_C) {
            sdo_log(LOG_ERR, "fsm(%p): unexpected command (VCA_SDO_INIT_DOWNLOAD_C expected)\n", fsm);
            sdo_error(fsm, msg);
            break;
        }
        if(fsm->bytes_to_load <= 4) {
            // expedited transfer
            ul_dbuff_set_len(&fsm->data, 0);
            sdo_log(LOG_INF, "fsm(%p): %i bytes downloaded\n", fsm, fsm->data.len);
            fsm->state = sdofsmDone;
            break;
        } 
        
        // segmented transfer
        if(fsm->bytes_to_load > 0) {
            byte *msg = fsm->out_msg.data;
            msg[0] = VCA_SDO_COMMAND(VCA_SDO_DOWNLOAD_SEGMENT_R);
            byte n = 7;
            if(fsm->bytes_to_load <= 7) n = fsm->bytes_to_load;
            msg[0] |= ((7 - n) << 1);
            memcpy(msg + 1, fsm->data.data + (fsm->data.len - fsm->bytes_to_load), n);
            fsm->bytes_to_load -= n;
            if(fsm->bytes_to_load == 0) msg[0] |= VCA_SDO_LAST_SEGMENT_INDICATOR;
            fsm->toggle_bit = 0;
            fsm->statefnc = vcasdo_fsm_state_upload_segment;
            ret = 1;
        }
        break;
    }
    gettimeofday(&(fsm->last_activity), NULL);
    return ret;
}

//-------------------------------------------------------------------
/*
 * vcasdo_fsm_state_download_segment - SDO upload segment state function 
 *
 * Returns 0 if no answer is in fsm->out_msg, 
 */
int vcasdo_fsm_state_download_segment(struct vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int ret = 0;
    // is it init domain download response ?
    int head = msg->data[0];
    int cmd = VCA_SDO_GET_COMMAND(head);
    sdo_log(LOG_DEB, "fsm(%p): entering vcasdo_fsm_state_download_segment()\n", fsm);
    while(1) {  // not real loop, just for break if error occures
        //int n;
        if(sdo_check_abort(fsm, msg)) break;
        if(cmd != VCA_SDO_DOWNLOAD_SEGMENT_C) {
            sdo_log(LOG_ERR, "fsm(%p): unexpected command (VCA_SDO_DOWNLOAD_SEGMENT_C expected)\n", fsm);
            sdo_error(fsm, msg);
            break;
        }
        // check toggle bit
        if(!((head & VCA_SDO_TOGGLE_BIT) ^ fsm->toggle_bit)) {
            sdo_log(LOG_ERR, "toggle bit error\n");
            sdo_error(fsm, msg);
            break;
        }
        // toggle toggle bit
        fsm->toggle_bit ^= VCA_SDO_TOGGLE_BIT;
        // check if this is the last segment
        if(fsm->bytes_to_load == 0) {
            sdo_log(LOG_DEB, "fsm(%p): %i bytes of %i expected uploaded\n", fsm, fsm->data.len, fsm->bytes_to_load);
            fsm->state = sdofsmDone;
        }
        else {
            // prepare request for the next segment
            byte *msg = fsm->out_msg.data;
            msg[0] = VCA_SDO_COMMAND(VCA_SDO_DOWNLOAD_SEGMENT_R);
            byte n = 7;
            if(fsm->bytes_to_load <= 7) n = fsm->bytes_to_load;
            msg[0] |= ((7 - n) << 1);
            memcpy(msg + 1, fsm->data.data + (fsm->data.len - fsm->bytes_to_load), n);
            fsm->bytes_to_load -= n;
            if(fsm->bytes_to_load == 0) msg[0] |= VCA_SDO_LAST_SEGMENT_INDICATOR;
            msg[0] |= fsm->toggle_bit;
            fsm->statefnc = vcasdo_fsm_state_upload_segment;
            ret = 1;
        }
        break;
    }
    gettimeofday(&(fsm->last_activity), NULL);
    return ret;
}
//-------------------------------------------------------------------

//========================= FSM functions ===========================

/**
 * vcasdo_init_fsm -  init SDO FSM 
 * @fsm: fsm to init, if NULL, function creates and inits a new one
 * @type: fsm type, could be VCASDO_UPLOADER, VCASDO_DOWNLOADER
 * @server_port: SDO server port number (default value 0x580 is set if parameter == 0)
 * @client_port: SDO client port number (default value 0x600 is set if parameter == 0)
 * @node: number of node on CAN bus to communicate with
 *
 * Return Value: pointer to the (created) inited FSM,
 *      NULL in case of allocation error.
 */
vcasdo_fsm_t * vcasdo_init_fsm(vcasdo_fsm_t *fsm, unsigned server_port, unsigned client_port, unsigned node)
{
    if(!fsm) {
        fsm = (vcasdo_fsm_t*)malloc(sizeof(vcasdo_fsm_t));
        if(fsm) ul_dbuff_init(&fsm->data, 0);
    }
    if(!fsm) {
        sdo_log(LOG_FATAL, "vcasdo_init_fsm - can't allocate memory for FSM\n");
        exit(EXIT_FAILURE);
    }
    ul_dbuff_set_len(&fsm->data, 0);
    fsm->state = sdofsmIdle;
    if(server_port == 0) fsm->server_port = 0x580;
    else fsm->server_port = server_port;
    if(client_port == 0) fsm->client_port = 0x600;
    else fsm->client_port = client_port;
    fsm->node = node;
    fsm->statefnc = NULL;
    return fsm;
}

/**
 * vcasdo_destroy_fsm -  frees SDO FSM and also frees all its resources 
 * @fsm: fsm to destroy
 */
void vcasdo_destroy_fsm(vcasdo_fsm_t *fsm)
{
    if(!fsm) return;
    ul_dbuff_destroy(&fsm->data);
    free(fsm);
}

/**
 * vcasdo_run - starts SDO communication protocol for this FSM
 * @fsm: SDO FSM
 *
 * Return Value: not 0 if fsm->out_msg contains CAN message to send
 */
int vcasdo_run(vcasdo_fsm_t *fsm)
{
    if(!fsm) return 0;

    fsm->out_msg.id = fsm->client_port | fsm->node;
    sdo_fill_multiplexor(fsm->out_msg.data + 1, fsm->index, fsm->subindex); // fills out_msg.data[1-3]
    if(fsm->type == sdofsmUploader) {
        fsm->out_msg.data[0] = VCA_SDO_COMMAND(VCA_SDO_INIT_UPLOAD_R);
        fsm->out_msg.length = 4;
        fsm->statefnc = vcasdo_fsm_state_upload_init;
        sdo_log(LOG_DEB, "fsm(%p): UPLOAD started\n", fsm);
    }
    else if(fsm->type == sdofsmDownloader) {
        byte *head = fsm->out_msg.data;
        *head = VCA_SDO_COMMAND(VCA_SDO_INIT_DOWNLOAD_R);
        *head |= VCA_SDO_SIZE_INDICATOR;
        if(fsm->data.len <= 4) {
            // expedited transfer
            *head |= VCA_SDO_EXPEDITED_TRANSFER;
            // fill number og bytes not containing data
            byte n = (4 - fsm->data.len) & 0x3;
            *head |= n << 2;
            fsm->bytes_to_load = 4 - n;
            memcpy(head + 4, fsm->data.data, fsm->bytes_to_load);
            fsm->out_msg.length = fsm->bytes_to_load + 4;
        }
        else {
            // segmented transfer
            // fill LSB than MSB data len
            fsm->bytes_to_load = fsm->data.len & 0xFFFF;
            head[4] = fsm->bytes_to_load % 256;
            head[7] = fsm->bytes_to_load / 256;
            fsm->out_msg.length = 8;
            fsm->toggle_bit = 0;
        }
        fsm->statefnc = vcasdo_fsm_state_download_init;
        sdo_log(LOG_DEB, "fsm(%p): DOWNLOAD started\n", fsm);
    }
    else {
        sdo_log(LOG_ERR, "fsm(%p): unknown type: %i\n", fsm, fsm->type);
        return 0;
    }
    gettimeofday(&(fsm->last_activity), NULL);
    fsm->state = sdofsmRun;
    return 1;
}

/*
 * vcasdo_upload - uploads object using SDO 
 *
 * @index: index of object in Object dictionary
 * @subindex: subindex of object in Object dictionary
 *
 * Returns true if fsm->out_msg contains CAN message to send
 *
 *
int vcasdo_upload(vcasdo_fsm_t *fsm, unsigned index, unsigned subindex)
{
    if(!fsm) return 0;

    fsm->out_msg.id = fsm->client_port | fsm->node;
    fsm->out_msg.data[0] = VCA_SDO_COMMAND(VCA_SDO_INIT_UPLOAD_R);
    sdo_fill_multiplexor(fsm->out_msg.data + 1, index, subindex);
    fsm->out_msg.length = 4;
    fsm->type = sdofsmUploader;
    fsm->state = sdofsmRun;
    fsm->statefnc = vcasdo_fsm_state_upload_init;
    sdo_log(LOG_INF, "fsm(%p): Uploaded started\n", fsm);
    return 1;
}
*/
//====================== fsm state functions ========================

// 0 not for me
static int is_for_me(const vcasdo_fsm_t *fsm, const struct canmsg_t *msg)
{
    int ret = 0;
    if(!fsm) return ret;
    if(!msg) return ret;
    if(!msg->length) return ret;
    
//    unsigned char cmd = msg->data[0];
//    unsigned char sdonumber = cmd >> VCA_SDO_COMMAND_OFFSET;
    
    // check if message is for me
    if(VCA_SDO_GET_PORT(msg->id) != fsm->server_port) return ret;
    if(VCA_SDO_GET_NODE(msg->id) != fsm->node) return ret;
    return 1;
}

//-------------------------------------------------------------------
/**
 * vcasdo_fsm_taste_msg - try to process msg in FSM
 * @fsm: fsm to process msg
 * @msg: tried msg
 *
 * Return Value: %sdofsmMsgRefuseif FSM refuses %msg, 
 *         %sdofsmMsgEat       if FSM proceses %msg, 
 *         %sdofsmMsgEatAnswer if FSM proceses %msg and it has answer in %fsm->out_msg prepared
 *         %sdofsmMsgEatError  %msg was eaten, but it does not match communication protocol or abort msg was detected
 */
int vcasdo_fsm_taste_msg(vcasdo_fsm_t *fsm, const canmsg_t *msg)
{
    int ret;
    if(!is_for_me(fsm, msg)) return sdofsmMsgRefuse;
    
    if(fsm->state != sdofsmRun) return sdofsmMsgEatError;
    
    if(fsm->statefnc) {
        ret = fsm->statefnc(fsm, msg);
        if(fsm->state == sdofsmError) return sdofsmMsgEatError;
        if(fsm->state == sdofsmAbort) return sdofsmMsgEatError;
        if(ret) return sdofsmMsgEatAnswer;
    }
    return sdofsmMsgEat;
}