/*
 * packet.c
 *
 * Packet processing.
 *
 *  Created on: 2023/09/18
 *      Author: BestTechnology
 */

#include "hal_data.h"

#include <stdlib.h>
#include <stdint.h>
#include <usart.h>

//#define DBG_TRACE
#include "trace.h"

#include "packet.h"
#include "pid.h"
#include "control_table.h"
#include "global_variables.h"

////////////////////////////////////////////////////////////
// Packet processing
////////////////////////////////////////////////////////////

extern int get_motor_temp (void);
extern int get_hvamp_temp (void);
extern int get_cpu_temp (void);

extern void led_on (void);
extern void led_off (void);

extern void __aeabi_memclr8 (void *dest, size_t n);


// Packet Robustness Level
typedef enum { pl_ignore = 0, pl_unilateral, pl_convenience, pl_strict, pl_target } tplevel;

// Flag to be processed after transmission is complete
bool packet_req_initalize = false;
bool packet_req_init_id_baud = false;

static bool FlagOfIDs[256] = {false};
static bool MachedID = false;
static char Ack[2000] = {0};
static bool AckSend = false;
static bool MakeAck = false;
static int PreviousID = -1;

// Filter packets from the host
int packet_receive (char *packet) {
//    uint8_t rboot  = 0;
    bool result = false;
    tplevel plevel = pl_ignore;
    int len = 0;
    for (;;) {
        while (us_rx_buff () > 0) {
            char c = us_getc ();
            switch (c) {
                case '[':
                    if (plevel == pl_ignore) {
                        if (len == 0) {
                            packet[len++] = c;
                            plevel = pl_unilateral;
                        } else {
                            len = 0;
                            plevel = pl_ignore;
                        }
                    } else {
                        len = 0;
                        plevel = pl_ignore;
                    }
                    break;
                case '<':
                    if (plevel == pl_ignore) {
                        if (len == 0) {
                            packet[len++] = c;
                            plevel = pl_convenience;
                        } else {
                            len = 0;
                            plevel = pl_ignore;
                        }
                    } else {
                        len = 0;
                        plevel = pl_ignore;
                    }
                    break;
                case '{':
                    if (plevel == pl_ignore) {
                        if (len == 0) {
                            packet[len++] = c;
                            plevel = pl_strict;
                        } else {
                            len = 0;
                            plevel = pl_ignore;
                        }
                    } else {
                        len = 0;
                        plevel = pl_ignore;
                    }
                    break;
                case '(':
                    if (plevel == pl_ignore) {
                        if (len == 0) {
                            packet[len++] = c;
                            plevel = pl_target;
                        } else {
                            len = 0;
                            plevel = pl_ignore;
                        }
                    } else {
                        len = 0;
                        plevel = pl_ignore;
                    }
                    break;

                case ']':
                    if (len > 0 && plevel == pl_unilateral) {
                        packet[len++] = c;
                        packet[len] = 0;
                        return len;
                    }
                    len = 0;
                    plevel = pl_ignore;
                    break;
                case '>':
                    if (len > 0 && plevel == pl_convenience) {
                        packet[len++] = c;
                        packet[len] = 0;
                        return len;
                    }
                    len = 0;
                    plevel = pl_ignore;
                    break;
                case '}':
                    if (len > 0 && plevel == pl_strict) {
                        packet[len++] = c;
                        packet[len] = 0;
                        return len;
                    }
                    len = 0;
                    plevel = pl_ignore;
                    break;
                case ')':
                    if (len > 0 && plevel == pl_target) {
                        packet[len++] = c;
                        packet[len] = 0;
                        return len;
                    }
                    len = 0;
                    plevel = pl_ignore;
                    break;
                case ',':
                case ';':
                case ':':
                case '=':
                case '#':
                case 'A'...'Z':
                case 'a'...'z':
                case '0'...'9':
                case '+':
                case '-':
                    if (len > 0) packet[len++] = c;
                    break;
#if 0
                case '!':
                    if (rboot++ > 2) NVIC_SystemReset ();
                    break;
#endif
                default:
                    return 0;
                    break;
            }
        }
//        __WFI();
    }
    return result;
}

// Binaryize decimal string
inline int getnum (char *s, int *num) {
    char *p;
    *num = strtol(s, &p, 10);
    return p - s;
}

static char* mystrcat (char* dest, const char* src) {
    while (*dest) dest++;
    while ((*dest++ = *src++)) ;
    return --dest;
}

static char* mystrcat2 (char* dest, const char* src, bool ini) {
    if (ini) while (*dest) dest++;
    while ((*dest++ = *src++)) ;
    return --dest;
}

// Concatenate response ACK
static char *concack (char *buf, const char *cmd, bool comma, bool ack, bool *sent) {
    if (MakeAck) {
        if (*sent) buf = mystrcat2 (buf, comma ? "," : "", true);
        mystrcat2 (mystrcat2 (buf, cmd, !(*sent)), ack ? "=OK" : "=NG", false);
        *sent = true;
    }
    return buf;
}

// Concatenate response ID(signed int)
static char *concdecid (char *buf, const char *cmd, int32_t dec, bool *setid) {
    if (!(*setid) && MakeAck) {
        char tmp[20];
        __aeabi_memclr8 (tmp, sizeof (tmp));
        __itoa (dec, tmp, 10);
        mystrcat2 (mystrcat2 (buf, cmd, true), tmp, false);
        *setid = true;
    }
    return buf;
}

// Concatenate response signed int
static char *concdec (char *buf, const char *cmd, bool comma, int32_t dec, bool *sent) {
    if (MakeAck) {
        char tmp[20];
        __aeabi_memclr8 (tmp, sizeof (tmp));
        if (*sent) buf = mystrcat2 (buf, comma ? "," : "", true);
        __itoa (dec, tmp, 10);
        mystrcat2 (mystrcat2 (mystrcat2 (buf, cmd, !(*sent)), "=", false), tmp, false);
        *sent = true;
    }
    return buf;
}

// Concatenate response unsigned int
static char *concudec (char *buf, const char *cmd, bool comma, uint32_t dec, bool *sent) {
    if (MakeAck) {
        char tmp[20];
        __aeabi_memclr8 (tmp, sizeof (tmp));
        if (*sent) buf = mystrcat2 (buf, comma ? "," : "", true);
        __utoa (dec, tmp, 10);
        mystrcat2 (mystrcat2 (mystrcat2 (buf, cmd, !(*sent)), "=", false), tmp, false);
        *sent = true;
    }
    return buf;
}

// Arguments to be passed to each command
typedef struct {
    int *id;            // id
    bool *matchid;      // Matching id
    bool *comma;        // Granted if the host contains commas
    bool *setid;        // Set id for ACK

    const char *cmd;    // command string
    bool *sent;         // Whether one or more responses are described
    char **ack;         // Response string buffer
    char **src;         // String buffer for commands from host
    bool *silent;       // No response
} TCmdInfoArg;


// TODO:Response processing when multiple IDs are specified
static bool cmd_id (TCmdInfoArg *p) {
    bool result = false;
    int n, dec;
    if ((n = getnum (*(p->src), &dec))) {
        if (dec <= 254) {
            *(p->id) = (uint8_t)dec;
            *(p->matchid) = (*(p->id) == ControlTable.ID);
            *(p->src) += n;
            if (*(p->matchid)) {
                concdecid (*(p->ack), (p->cmd), *(p->id), (p->setid));
                MachedID = true;
            }
            if (!FlagOfIDs[dec] && !MachedID) {
                PreviousID = *(p->id);
            }
            FlagOfIDs[dec] = true;
        } else {
            *(p->matchid) = false;
            *(p->src) += n;
        }
    }
    return result;
}

void write_item_callback (uint16_t idx, int data) {
    switch (idx) {
        case _ITEM_IDX_LED:
            if (data == 0) led_off (); else led_on ();
            break;

        case _ITEM_IDX_ANGFLEX_NCOMP ... _ITEM_IDX_ANGFLEX_PCOMP:
            interp_angle_compliance_1d[idx - _ITEM_IDX_ANGFLEX_NCOMP] = data;
            break;
        case _ITEM_IDX_CURFLEX_NCOMP ... _ITEM_IDX_CURFLEX_PCOMP:
            interp_current_compliance_1d[idx - _ITEM_IDX_CURFLEX_NCOMP] = data;
            break;

        case _ITEM_IDX_ANGLECTRLGAIN:
            pidparam_ang.Kp = data * 0.001;
            break;
        case _ITEM_IDX_ANGLECTRLGAIN + 1:
            pidparam_ang.Ki = data * 0.001;
            break;
        case _ITEM_IDX_ANGLECTRLGAIN + 2:
            pidparam_ang.Kd = data * 0.001;
            break;
        case _ITEM_IDX_ANGLECTRLGAIN + 3:
            pidparam_ang.delta_t = data * 0.001;
            break;
        case _ITEM_IDX_VELOCITYCTRLGAIN:
            pidparam_velo.Kp = data * 0.001;
            break;
        case _ITEM_IDX_VELOCITYCTRLGAIN + 1:
            pidparam_velo.Ki = data * 0.001;
            break;
        case _ITEM_IDX_VELOCITYCTRLGAIN + 2:
            pidparam_velo.Kd = data * 0.001;
            break;
        case _ITEM_IDX_VELOCITYCTRLGAIN + 3:
            pidparam_velo.delta_t = data * 0.001;
            break;
        case _ITEM_IDX_CURRENTCTRLGAIN:
            pidparam_cur.Kp = data * 0.001;
            break;
        case _ITEM_IDX_CURRENTCTRLGAIN  + 1:
            pidparam_cur.Ki = data * 0.001;
            break;
        case _ITEM_IDX_CURRENTCTRLGAIN + 2:
            pidparam_cur.Kd = data * 0.001;
            break;
        case _ITEM_IDX_CURRENTCTRLGAIN + 3:
            pidparam_cur.delta_t = data * 0.001;
            break;
        case _ITEM_IDX_ANGLECORRECTION ... _ITEM_IDX_ANGLECORRECTION + 36:
            interp_tad2141_1d[idx - _ITEM_IDX_ANGLECORRECTION + interp_tad2141_ar_w] = data;
            break;
    }
}

static bool cmd_wr (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    // offset
    if ((n = getnum (*(p->src), &dec))) {
        int ofs = dec, num = 0;
        bool ok = true;
        *(p->src) += n;
        if (**(p->src) == '=') {
            *(p->src) += 1;
            while ((n = getnum (*(p->src), &dec)) > 0) {
                *(p->src) += n;
                // Support for updating id and baudrate
                if (ofs + num == 0 || ofs + num == 1) packet_req_init_id_baud = true;
                if (!write_item ((uint16_t)(ofs + num++), dec, &write_item_callback)) {
                    ok = false;
                    break;
                }
                if (**(p->src) == ';') *(p->src) += 1;
            }
        }
        result = ok;
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_wd (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    result = SaveCcontroltableToNVM ();
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_rr (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    static char cmd_r[1000];
    char s_tmp[15];
    bool result = false;
    Tdatatype dtype;
    int n, dec;
    char *pcmd = cmd_r;

    __aeabi_memclr8 (cmd_r, sizeof (cmd_r));
    strcpy (cmd_r, p->cmd);
    // offset
    if ((n = getnum (*(p->src), &dec))) {
        __itoa (dec, s_tmp, 10);
        //mystrcat (cmd_r, s_tmp);
        pcmd = mystrcat2 (cmd_r, s_tmp, true);
        int ofs = dec;
        *(p->src) += n;
        // multi
        if (**(p->src) == ';') {
            *(p->src) += 1;
            if ((n = getnum (*(p->src), &dec))) {
                *(p->src) += n;
                if ((n > 0) && (dec > 0)) {
                    int num = dec;
                    bool ok = true;
                    //mystrcat (cmd_r, "=");
                    pcmd = mystrcat2 (pcmd, "=", false);
                    for (int i = 0; i < num; i++) {
                        int32_t v;
                        if (read_item ((uint16_t)(ofs + i), &dtype, &v)) {
                            if ((dtype == dt_uint8) || (dtype == dt_uint16) || (dtype == dt_uint32))
                                __utoa ((uint32_t)v, s_tmp, 10);
                            else
                                __itoa ((int32_t)v, s_tmp, 10);
                            //mystrcat (cmd_r, s_tmp);
                            pcmd = mystrcat2 (pcmd, s_tmp, false);
                            //if (i != num - 1) mystrcat (cmd_r, ";");
                            if (i != num - 1) pcmd = mystrcat2 (pcmd, ";", false);
                        } else {
                            ok = false;
                            break;
                        }
                    }
                    if (ok) {
                        if (*(p->sent) && !*p->silent) mystrcat (*(p->ack), *(p->comma) ? "," : "");
                        if (!*p->silent) mystrcat (*(p->ack), cmd_r);
                        *(p->sent) = true;
                        result = true;
                    } else
                        if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
                } else
                    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
            } else
                if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
        // single
        } else {
            int32_t v;
            if (read_item ((uint16_t)ofs, &dtype, &v)) {
                if (!*p->silent) *(p->ack) = concdec (*(p->ack), cmd_r, *(p->comma), v, p->sent);
            }
        }
    } else
        if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_ta (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;

        int m = 0;
        bool ok = true;
        while ((n = getnum (*(p->src), &dec)) > 0) {
            *(p->src) += n;
            if (m == 0) {
                if (!write_item (_ITEM_IDX_TARGETANGLE, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else if (m == 1) {
                if (!write_item (_ITEM_IDX_TRANSITIONMODE, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else if (m == 2) {
                if (!write_item (_ITEM_IDX_TRANSITIONTIME, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else {
                ok = false;
                break;
            }
            if (**(p->src) == ';') *(p->src) += 1;
            m++;
        }
        result = ok;
        *(p->src) += n;
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_tv (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;

        int m = 0;
        bool ok = true;
        while ((n = getnum (*(p->src), &dec)) > 0) {
            *(p->src) += n;
            if (m == 0) {
                if (!write_item (_ITEM_IDX_TARGETVELOCITY, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else if (m == 1) {
                if (!write_item (_ITEM_IDX_TRANSITIONMODE, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else if (m == 2) {
                if (!write_item (_ITEM_IDX_TRANSITIONTIME, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else {
                ok = false;
                break;
            }
            if (**(p->src) == ';') *(p->src) += 1;
            m++;
        }
        result = ok;
        *(p->src) += n;
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_tc (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;

        int m = 0;
        bool ok = true;
        while ((n = getnum (*(p->src), &dec)) > 0) {
            *(p->src) += n;
            if (m == 0) {
                if (!write_item (_ITEM_IDX_TARGETCURRENT, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else if (m == 1) {
                if (!write_item (_ITEM_IDX_TRANSITIONMODE, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else if (m == 2) {
                if (!write_item (_ITEM_IDX_TRANSITIONTIME, dec, NULL)) {
                    ok = false;
                    break;
                }
            } else {
                ok = false;
                break;
            }
            if (**(p->src) == ';') *(p->src) += 1;
            m++;
        }
        result = ok;
        *(p->src) += n;
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_tp (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;
        if ((n = getnum (*(p->src), &dec))) result = write_item (_ITEM_IDX_TARGETPWM, dec, NULL);
        *(p->src) += n;
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_ca (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concdec (*(p->ack), p->cmd, *(p->comma), ControlTable.CurrentAngle, p->sent);
    return true;
}

static bool cmd_cv (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concdec (*(p->ack), p->cmd, *(p->comma), ControlTable.CurrentVelocity, p->sent);
    return true;
}

static bool cmd_cc (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concdec (*(p->ack), p->cmd, *(p->comma), ControlTable.CurrentCurrent, p->sent);
    return true;
}

static bool cmd_cp (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concdec (*(p->ack), p->cmd, *(p->comma), ControlTable.CurrentPWM, p->sent);
    return true;
}

static bool cmd_ct (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (**(p->src) == '0') {
        *(p->src) += 1;
        ControlTable.CurrentMPUTemp = (int16_t)get_cpu_temp ();
        if (!*p->silent) *(p->ack) = concdec (*(p->ack), "CT0", *(p->comma), ControlTable.CurrentMPUTemp, p->sent);
        return true;
    } else if (**(p->src) == '1') {
        *(p->src) += 1;
        ControlTable.CurrentAMPTemp = (int16_t)get_hvamp_temp ();
        if (!*p->silent) *(p->ack) = concdec (*(p->ack), "CT1", *(p->comma), ControlTable.CurrentAMPTemp, p->sent);
        return true;
    } else if (**(p->src) == '2') {
        *(p->src) += 1;
        ControlTable.CurrentMotorTemp = (int16_t)get_motor_temp ();
        if (!*p->silent) *(p->ack) = concdec (*(p->ack), "CT2", *(p->comma), ControlTable.CurrentMotorTemp, p->sent);
        return true;
    }
    return false;
}

static bool cmd_cb (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concdec (*(p->ack), p->cmd, *(p->comma), ControlTable.CurrentBusVoltage, p->sent);
    return true;
}

static bool cmd_ex (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;
        if ((n = getnum (*(p->src), &dec))) result = write_item (_ITEM_IDX_EXCITATION, dec, NULL);
        *(p->src) += n;
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_sg0 (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;
        // p
        if ((n = getnum (*(p->src), &dec))) {
            int P = dec;
            *(p->src) += n;
            if (**(p->src) == ';') {
                *(p->src) += 1;
                // i
                if ((n = getnum (*(p->src), &dec))) {
                    int I = dec;
                    *(p->src) += n;
                    if (**(p->src) == ';') {
                        *(p->src) += 1;
                        // d
                        if ((n = getnum (*(p->src), &dec))) {
                            int D = dec;
                            *(p->src) += n;
                            result = write_item (_ITEM_IDX_ANGLECTRLGAIN, P, &write_item_callback) && write_item (_ITEM_IDX_ANGLECTRLGAIN + 1, I, &write_item_callback) && write_item (_ITEM_IDX_ANGLECTRLGAIN + 2, D, &write_item_callback);
                        }
                    }
                }
            }
        }
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_sg1 (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;
        // p
        if ((n = getnum (*(p->src), &dec))) {
            int P = dec;
            *(p->src) += n;
            if (**(p->src) == ';') {
                *(p->src) += 1;
                // i
                if ((n = getnum (*(p->src), &dec))) {
                    int I = dec;
                    *(p->src) += n;
                    if (**(p->src) == ';') {
                        *(p->src) += 1;
                        // d
                        if ((n = getnum (*(p->src), &dec))) {
                            int D = dec;
                            *(p->src) += n;
                            result = write_item (_ITEM_IDX_VELOCITYCTRLGAIN, P, &write_item_callback) && write_item (_ITEM_IDX_VELOCITYCTRLGAIN + 1, I, &write_item_callback) && write_item (_ITEM_IDX_VELOCITYCTRLGAIN + 2, D, &write_item_callback);
                        }
                    }
                }
            }
        }
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_sg2 (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    int n, dec;
    if (**(p->src) == '=') {
        *(p->src) += 1;
        // p
        if ((n = getnum (*(p->src), &dec))) {
            int P = dec;
            *(p->src) += n;
            if (**(p->src) == ';') {
                *(p->src) += 1;
                // i
                if ((n = getnum (*(p->src), &dec))) {
                    int I = dec;
                    *(p->src) += n;
                    if (**(p->src) == ';') {
                        *(p->src) += 1;
                        // d
                        if ((n = getnum (*(p->src), &dec))) {
                            int D = dec;
                            *(p->src) += n;
                            result =  write_item (_ITEM_IDX_CURRENTCTRLGAIN, P, &write_item_callback) && write_item (_ITEM_IDX_CURRENTCTRLGAIN + 1, I, &write_item_callback) && write_item (_ITEM_IDX_CURRENTCTRLGAIN + 2, D, &write_item_callback);
                        }
                    }
                }
            }
        }
    }
    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), result, p->sent);
    return result;
}

static bool cmd_rb (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), true, p->sent);
    NVIC_SystemReset ();
    return true;
}

static bool cmd_st (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    bool result = false;
    if (!*p->silent) *(p->ack) = concudec (*(p->ack), p->cmd, *(p->comma), ControlTable.SystemStatus.all, p->sent);
    return result;
}

static bool cmd_ini (TCmdInfoArg *p) {
    if (!*(p->matchid)) return false;

    if (!*p->silent) *(p->ack) = concack (*(p->ack), p->cmd, *(p->comma), true, p->sent);
    packet_req_initalize = true;
    return true;
}

// List of commands
typedef struct {
    char cmd[10];
    bool (*fp)(TCmdInfoArg *p);
} TCmdInfo;

#define sizeofrec(d)      (sizeof(d) / sizeof(d[0]))

static const TCmdInfo CmdInfo[] = {
    { "#",  &cmd_id },   // target id           #{0~65535}
    { "WR", &cmd_wr },   // write item          WR<addr=value1>[;value2[;value3...]]
    { "WD", &cmd_wd },   // write data flash    WD
    { "RR", &cmd_rr },   // read item           RR<addr;datanum>
    { "TA", &cmd_ta },   // target angle        TA<=value[[;mode];time]>
    { "TV", &cmd_tv },   // target velocity     TV<=value[[;mode];time]>
    { "TC", &cmd_tc },   // target torquXe      TT<=value[[;mode];time]>
    { "TP", &cmd_tp },   // target pwm          TP<=value[[;mode];time]>

    { "CA", &cmd_ca },   // current angle       CA
    { "CV", &cmd_cv },   // current velocity    CV
    { "CC", &cmd_cc },   // current current     CC
    { "CP", &cmd_cp },   // current pwm duty    CP
    { "CT", &cmd_ct },   // current temperature CT{0|1|2}
    { "CB", &cmd_cb },   // current bus voltage CB
    { "ST", &cmd_st },   // current status      ST

    { "EX", &cmd_ex },   // excitation          EX<={0|1|2|3|4|5|6|7|8}>
    { "SG0", &cmd_sg0 }, // set gain            SG0<=p_value;i_value;d_value>
    { "SG1", &cmd_sg1 }, // set gain            SG1<=p_value;i_value;d_value>
    { "SG2", &cmd_sg2 }, // set gain            SG2<=p_value;i_value;d_value>
    { "RB", &cmd_rb },   // reboot              RB
    { "INI", &cmd_ini }, // initalize           INI
};

bool packet_handling (char *s) {
    int id = -1;
    bool sent = false, comma = false, setid = false, matchid = false, silent= false;
    char *p_ack = Ack;
    bool ackrcv = false;
    int packetid = -1;
    TCmdInfoArg info = {
        &id,
        &matchid,
        &comma,
        &setid,
        NULL,//const char *cmd;
        &sent,
        &p_ack,
        NULL,//char **src;
        &silent
    };

    MakeAck = false;
    while (*s != 0) {
        if (*s == '[') {
            // New Commands packet
            __aeabi_memclr8 (Ack, sizeof (Ack));
            __aeabi_memclr8 (FlagOfIDs, sizeof (FlagOfIDs));
            ControlTable.NoCommTime = 0;
            MachedID = false;
            AckSend = false;
            PreviousID = -1;
            MakeAck = true;
            sent = true;
            silent = true;
        }
        else if (*s == '<') {
            // New Commands packet
            __aeabi_memclr8 (Ack, sizeof (Ack));
            __aeabi_memclr8 (FlagOfIDs, sizeof (FlagOfIDs));
            ControlTable.NoCommTime = 0;
            MachedID = false;
            AckSend = false;
            PreviousID = -1;
            MakeAck = true;
            sent = true;
            strcpy (Ack, "(");
            silent = false;
        }
        else if (*s == '{') {
            // New Commands packet
            __aeabi_memclr8 (Ack, sizeof (Ack));
            __aeabi_memclr8 (FlagOfIDs, sizeof (FlagOfIDs));
            ControlTable.NoCommTime = 0;
            MachedID = false;
            AckSend = false;
            PreviousID = -1;
            MakeAck = true;
            sent = true;
            strcpy (Ack, "(");
            silent = false;
        }

        if (*s == '(') {
            ackrcv = true;
        }
        bool m = false;
        if (*s == ',') {
            comma = true;
        }

        // make ack
        if (ackrcv){
            size_t cmdlen = strlen (CmdInfo[0].cmd);
            if ((strncmp (s, CmdInfo[0].cmd, cmdlen) == 0)) {
                m = true;
                s += cmdlen;
                info.cmd = CmdInfo[0].cmd;
                info.src = &s;
                CmdInfo[0].fp (&info);
                if (matchid) {
                    packetid = *(info.id);
                    AckSend = true;
                }
                if (ackrcv) {
                    packetid = *(info.id);
                }
                break;
            }
        } else {
            for (size_t i = 0; i < sizeofrec(CmdInfo); i++) {
                size_t cmdlen = strlen (CmdInfo[i].cmd);
                if ((strncmp (s, CmdInfo[i].cmd, cmdlen) == 0)) {
                    m = true;
                    s += cmdlen;
                    info.cmd = CmdInfo[i].cmd;
                    info.src = &s;
                    CmdInfo[i].fp (&info);
                    if (matchid) {
                        packetid = *(info.id);
                        AckSend = true;
                    }
                    if (ackrcv) {
                        packetid = *(info.id);
                    }
                    break;
                }
            }
        }
        if (!m) s++;
    }
    if (MakeAck) {
        if (PreviousID == -1) {
            ackrcv = true;
            PreviousID = packetid;
        }
        mystrcat(Ack, ")");
    }
    // Send when it's my turn
    if (ackrcv && (PreviousID == packetid) && AckSend && !silent) {
        // Waiting for bus switching delay
        R_BSP_SoftwareDelay (ControlTable.ReturnDelayTime, BSP_DELAY_UNITS_MICROSECONDS);
        us_putsb ((uint8_t *)Ack, strlen(Ack));
    }

    return false;
}
