#include "CAN_FW.h"
#include "Can.h"
#include <string.h>

void DiagReq (TCanFwMem * vars, uint8_t mode, uint8_t pid, TCanChannel channel)
{
    TCanPkt req;
    req.id = 0x7DF;
    req.SetData ("\x02\x00\x00");
    req.data_len = 8;
    req.data[1] = mode;
    req.data[2] = pid;
    CoreFunc->CanSend (channel, &req, 0);

    vars->Diag.mode = mode;
    vars->Diag.pid = pid;
}


void DiagRcv (TCanFwMem * vars, TCanPkt * pkt, TCanChannel channel)
{
    uint8_t * buf = pkt->data;

    // заголовок мультипакета
    if (buf[0] == 0x10)
    {
        uint8_t mode = buf[2] & (~0x40);
        uint8_t pid = buf[3];

        // отправить flow control
        if (mode == vars->Diag.mode &&
        pid == vars->Diag.pid)
        {
            TCanPkt fc;
            fc.id = 0x7E0;
            fc.SetData ("\x30\x00\x00");
            fc.data_len = 8;
            CoreFunc->CanSend (channel, &fc, 0);

            vars->Diag.multipkt = true;
        }
        else
        {
            // сбросить автомат
            vars->Diag.mode = vars->Diag.pid = 0;
            return;
        }

        // заголовок мультипакета с VIN-кодом
        if (mode == 9 && pid == 2)
        memcpy (vars->Diag.vin, &buf[5], 3);

    }
    // тело мультипакета
    else
    if (((buf[0] & 0xF0) == 0x20) && vars->Diag.multipkt)
    {
        uint8_t cnt = buf[0] & 0x0F;

        // мультипакет с VIN-кодом
        if (vars->Diag.mode == 9 && vars->Diag.pid == 2)
        {
            if (cnt == 1)
            memcpy (&vars->Diag.vin[3], &buf[1], 7);
            if (cnt == 2)
            {
                memcpy (&vars->Diag.vin[10], &buf[1], 7);

                // сообщить в ядро о приёме пакета
                CoreFunc->DataValueArr (CanData_VIN, vars->Diag.vin);

                // сбросить автомат
                vars->Diag.mode = vars->Diag.pid = 0;
            }
        }
    }
    else
    vars->Diag.multipkt = false;
}
