#include "finger.h"
#include "finger_para.h"
#include "fingerCmd.h"
#include "./iic/hal.h"
#include "pca.h"
#include "debug.h"

typedef struct
{
    uint32_t    capChannelData[MAX_CAP_CHANNEL_NUM];
    ydds        yddsData[MAX_YDDS_NUM];
    uint32_t    sProxCapData[MAX_S_PROX_NUM];
    uint32_t    mProxCapData[MAX_M_PROX_NUM];
    uint8_t     rcvBuf[128];
}fingerCapData;

typedef enum
{
    FINGER_READ_STEP_READ_ADDR = 0,
    FINGER_READ_STEP_READ_PN,
    FINGER_READ_STEP_SET_OFFSET,
    FINGER_READ_STEP_SET_DATATYPE,
    FINGER_READ_STEP_WAIT_CYC,
    FINGER_READ_STEP_READ_CAP_DATA,
    FINGER_READ_STEP_SET_SYNC
}eFingerReadStep;


typedef struct
{
    bool            connectFlg;     ///< 连接标志
    uint8_t         packIdx;        ///< 读取计数，用于判断是否数据有更新
    uint8_t         pcaIndx;        ///< pca序号
    uint8_t         addr;           ///< iic地址
    uint32_t        connectTime;    ///< 用于检查是否连接的计时

    uint32_t        readTime;       ///< 读取计时
    uint32_t        syncTime;       ///< 同步计时
    uint8_t         readStep;

    eFingerReadStep connnectStep;

    fingerCapData   capData;
    finger_param_ts para;
    
    I2C_PlatformInterface_t comPort;
}finger, *pfinger;

 

typedef struct
{
    uint32_t        syncTime;
    uint8_t         connectFingerNum;
    finger          finger[MAX_FINGER];

    pcaTypedef      pca;
}fingerTypeDef;

static fingerTypeDef fingerData;

static void capDataRead(pfinger finger);
bool fingerInit(bool isUsePca, const uint8_t *pcaIndex)
{
    memset((void *)&fingerData, 0, sizeof(fingerData));
    
    fingerData.pca.isUsePca = isUsePca;

    if(isUsePca == true)
    {
        I2C_PlatformInterface_t com;
        I2C_Init(0, &com);
        for(uint8_t i=0; i<MAX_FINGER; i++)
        {
            fingerData.finger[i].comPort = com;
            fingerData.finger[i].pcaIndx = pcaIndex[i];
        }

        fingerData.pca.pcaCom = com;
        fingerData.pca.pcaAddr = 0x70;
    }
    else
    {
        for(uint8_t i=0; i<MAX_FINGER; i++)
        {
            I2C_Init(i, &fingerData.finger[i].comPort);
        }
    }
    
    return true;
}


static void fingerConnect(pfinger finger, uint8_t addr)
{
    finger->addr = addr;
    finger->connectFlg = true;
    finger->connectTime = finger->comPort.getTimeMs();
    finger->packIdx = 0;
}

static void fingerDisconnect(pfinger finger)
{
    finger->addr = 0xff;
    finger->connectFlg = false;
    finger->connectTime = finger->comPort.getTimeMs();
    finger->packIdx = 0;
    finger->connnectStep = FINGER_READ_STEP_READ_ADDR;
}



bool fingerCheckConnect(pfinger finger)
{
    uint8_t pn = 0xff;
    bool err = false;
    I2C_Transfer_Status comErr = I2C_TRANSFER_BUSY;
    
    switch(finger->connnectStep)
    {
    case FINGER_READ_STEP_READ_ADDR:
        comErr = fingerCmdReadAddr(&finger->comPort, 0, &finger->addr);
        if(comErr == I2C_TRANSFER_DONE)
        {
            if(finger->addr == 0)
            {
                err = false;
            }
            else
            {
                finger->connnectStep = FINGER_READ_STEP_READ_PN;   ///< 正确读取到地址
            }
        }
        else if(comErr == I2C_TRANSFER_ERROR)
        {
            err = false;
        }
        
        break;

    case FINGER_READ_STEP_READ_PN:
        comErr = fingerCmdReadPN(&finger->comPort,0, &pn);
        if(comErr == I2C_TRANSFER_DONE)
        {
            finger->connnectStep = FINGER_READ_STEP_SET_OFFSET;   ///< 正确读取到pn

            findFingerPara(pn, &finger->para);
        }
        else if(comErr == I2C_TRANSFER_ERROR)
        {
            /// 这里为了兼容老版本固件，可能读取不到pn
            //err = false;
            pn = 0;
            findFingerPara(pn, &finger->para);

            finger->connnectStep = FINGER_READ_STEP_SET_OFFSET;   ///< 正确读取到pn
            //break;
        }
        
        break;

    case FINGER_READ_STEP_SET_OFFSET:
        comErr = fingerCmdWriteSampleOffset(&finger->comPort, finger->addr, finger->addr%finger->para.sensorNum);
        if(comErr == I2C_TRANSFER_DONE)
        {
            finger->connnectStep = FINGER_READ_STEP_SET_DATATYPE;   ///< 正确设置offset
        }
        else if(comErr == I2C_TRANSFER_ERROR)
        {
            err = false;
            break;
        }

        break;
    case FINGER_READ_STEP_SET_DATATYPE:
        comErr = fingerCmdWriteDataType(&finger->comPort, finger->addr, 0);
        if(comErr == I2C_TRANSFER_DONE)
        {
            fingerConnect(finger, finger->addr);
            finger->connnectStep = FINGER_READ_STEP_WAIT_CYC;   ///< 正确设置数据类型
        }
        else if(comErr == I2C_TRANSFER_ERROR)
        {
            err = false;
            break;
        }

        break;
    case FINGER_READ_STEP_WAIT_CYC:///< 等待读取周期到
        if(finger->comPort.getTimeMs() > (finger->readTime + READ_CAP_CYC))
        {
            finger->connnectStep = FINGER_READ_STEP_READ_CAP_DATA;
            finger->readTime = finger->comPort.getTimeMs();
        }
        else
        {
            break;
        }
        
    case FINGER_READ_STEP_READ_CAP_DATA:
        comErr = fingerCmdReadCapData(&finger->comPort, finger->addr, finger->capData.rcvBuf, finger->para.packLen);
        if(comErr == I2C_TRANSFER_DONE)
        {
            finger->connnectStep = FINGER_READ_STEP_WAIT_CYC;
            capDataRead(finger);
            finger->connectTime = finger->comPort.getTimeMs();
        }
        else if(comErr == I2C_TRANSFER_ERROR)
        {
            finger->connnectStep = FINGER_READ_STEP_WAIT_CYC;
        }
        break;
    case FINGER_READ_STEP_SET_SYNC:
        comErr = fingerCmdWriteSampleSync(&finger->comPort, finger->addr);
        if(comErr == I2C_TRANSFER_DONE)
        {
            finger->connnectStep = FINGER_READ_STEP_WAIT_CYC;   ///< 正确发送同步消息
        }
        else if(comErr == I2C_TRANSFER_ERROR)
        {
            err = false;
            break;
        }
        else
        {
            break;
        }
    default:
        break;
    }

    return err;
}

static void capDataRead(pfinger finger)
{
    uint8_t *dataP = (uint8_t*)(finger->capData.rcvBuf + 6);

    ydds_com_ts    *p2;
    ydds_u8_ts     *p3;
    ydds_u16_ts    *p4;
    ydds_float_ts  *p5;

    /// 读取原始值
    for (uint16_t i=0; i<finger->para.sensorNum; i++) {
        int32_t data = *(int32_t*)dataP & (0xFFFFFFFF >> (32 - finger->para.capByte * 8));
        dataP += finger->para.capByte;
        finger->capData.capChannelData[i] = data;
    }

    /// 三维力和接触数据
    int yddsSize = 0;

    switch(finger->para.yddsType)
    {
    case 2:
        p2 = (ydds_com_ts*)dataP;

        finger->capData.yddsData[0].nf = p2->nf;
        finger->capData.yddsData[0].tf = p2->tf;
        finger->capData.yddsData[0].tfDir = p2->tfDir;
        finger->capData.sProxCapData[0] = p2->prox;
        break;
    case 3:
        yddsSize = sizeof(ydds_u8_ts);
        for (uint16_t i = 0; i<finger->para.yddsNum; i++)
        {
            p3 = (ydds_u8_ts*)dataP;

            finger->capData.yddsData[i].nf = p3->nf / 10.0f;
            finger->capData.yddsData[i].tf = p3->tf / 10.0f;
            finger->capData.yddsData[i].tfDir = p3->tfDir;

            dataP += yddsSize;
        }

        for (uint16_t i = 0; i < finger->para.sProxNum; i++) {
            uint32_t data = *(int32_t*)dataP & 0xFFFFFF;
            dataP += 3;
            finger->capData.sProxCapData[i] = data;
        }
        for (uint16_t i = 0; i < finger->para.mProxNum; i++) {
            uint32_t data = *(int32_t*)dataP & 0xFFFFFF;
            dataP += 3;
            finger->capData.mProxCapData[i] = data;
        }
        
        break;
    case 4:
        yddsSize = sizeof(ydds_u16_ts);
        for (uint16_t i = 0; i<finger->para.yddsNum; i++)
        {
            p4 = (ydds_u16_ts*)dataP;

            finger->capData.yddsData[i].nf = p4->nf / 100.0f;
            finger->capData.yddsData[i].tf = p4->tf / 100.0f;
            finger->capData.yddsData[i].tfDir = p4->tfDir;

            dataP += yddsSize;
        }

        for (uint16_t i = 0; i < finger->para.sProxNum; i++) {
            uint32_t data = *(int32_t*)dataP & 0xFFFFFF;
            dataP += 3;
            finger->capData.sProxCapData[i] = data;
        }
        for (uint16_t i = 0; i < finger->para.mProxNum; i++) {
            uint32_t data = *(int32_t*)dataP & 0xFFFFFF;
            dataP += 3;
            finger->capData.mProxCapData[i] = data;
        }
        
        break;
    case 5:
        yddsSize = sizeof(ydds_float_ts);
        for (uint16_t i = 0; i<finger->para.yddsNum; i++)
        {
            p5 = (ydds_float_ts*)dataP;

            finger->capData.yddsData[i].nf = p5->nf;
            finger->capData.yddsData[i].tf = p5->tf;
            finger->capData.yddsData[i].tfDir = p5->tfDir;

            dataP += yddsSize;
        }

        for (uint16_t i = 0; i < finger->para.sProxNum; i++) {
            uint32_t data = *(int32_t*)dataP & 0xFFFFFF;
            dataP += 3;
            finger->capData.sProxCapData[i] = data;
        }
        for (uint16_t i = 0; i < finger->para.mProxNum; i++) {
            uint32_t data = *(int32_t*)dataP & 0xFFFFFF;
            dataP += 3;
            finger->capData.mProxCapData[i] = data;
        }
        
        break;
    default:
        break;
    }

    /// 打印
    debugPrintfNoFile(0, "finger=%d, capChannelDat = ", finger->addr);
    for(uint16_t i=0; i<finger->para.sensorNum; i++)
    {
        /// 电容原始值
        debugPrintfNoFile(0, "%d ", finger->capData.capChannelData[i]);
    }
    debugPrintfNoFile(0, "\n");

    for (uint16_t i = 0; i<finger->para.yddsNum; i++)
    {
        /// 三维力
        debugPrintfNoFile(0, "ydds[%d]: nf=%d, tf=%d, tfdif=%d\r\n", i, (uint32_t)(finger->capData.yddsData[i].nf*1000), (uint32_t)(finger->capData.yddsData[i].tf*1000), finger->capData.yddsData[i].tfDir);
    }

    for (uint16_t i = 0; i<finger->para.sProxNum; i++)
    {
        /// 互自电容接近
        debugPrintfNoFile(0, "sProx[%d]: sProx=%d ", i, finger->capData.sProxCapData[i]);
    }

    for (uint16_t i = 0; i<finger->para.mProxNum; i++)
    {
        /// 互电容接近
        debugPrintfNoFile(0, "sProx[%d]: mProx=%d ", i, finger->capData.mProxCapData[i]);
    }
    debugPrintfNoFile(0, "\n");
}


static void checkSync(void)
{
    uint8_t notBusyNum = 0;
    fingerData.connectFingerNum = 0;
    uint8_t pcaIndex = 0;
    if(fingerData.finger[0].comPort.getTimeMs() > (fingerData.syncTime + 1000))
    {
        for(uint8_t i=0; i<MAX_FINGER; i++)
        {
            pfinger finger = &fingerData.finger[i];

            if(finger->connectFlg == true)
            {
                fingerData.connectFingerNum++;

                if(finger->connnectStep == FINGER_READ_STEP_WAIT_CYC)///< 空闲状态
                {
                    notBusyNum++;

                    pcaIndex |= (uint8_t)1<<finger->pcaIndx;
                }
            }
        }

        /// 如果所有连接的手指都空闲
        if(fingerData.connectFingerNum > 1)
        {
            if(fingerData.connectFingerNum == notBusyNum)
            {
                if(fingerData.pca.isUsePca == true)
                {
                    if(pcaSwitch(&fingerData.pca, pcaIndex) == true)
                        fingerCmdWriteSampleSync(&fingerData.pca.pcaCom, 0);///< pca模式可以使用广播方式
                }
                else
                {
                    for(uint8_t i=0; i<MAX_FINGER; i++)
                    {
                        pfinger finger = &fingerData.finger[i];

                        finger->connnectStep = FINGER_READ_STEP_SET_SYNC;
                    }
                }
                
                debugPrintf(0, "sync \n");
                
                fingerData.syncTime = fingerData.finger[0].comPort.getTimeMs();
            }
        }
        else
        {
            fingerData.syncTime = fingerData.finger[0].comPort.getTimeMs();
        }
    }
}

void fingerDeal(void)
{
    for(uint8_t i=0; i<MAX_FINGER; i++)
    {
        pfinger finger = &fingerData.finger[i];
        
        if (pcaSwitch(&fingerData.pca, finger->pcaIndx) == false)
        {
            continue;
        }

        fingerCheckConnect(finger);

        /// 断开重连
        if((finger->connectFlg == true) && (finger->comPort.getTimeMs() > (finger->connectTime + 1000)))
        {
            fingerDisconnect(finger);
        }
    }
    
    //checkSync();
}
