#include "stm32f4xx.h"
#include "ucos_ii.h"
#include "os.h"
#include "string.h"
#include "modbus_parse.h"
#include "modbus_rec_send.h"

extern OS_EVENT* ModbusWriteFlash;      /*接收到modbus写入coil请求报文*/
extern OS_EVENT* ModbusConnect;         /*已接收到modbus正确报文*/

uint8_t ModbusErr;
/*0x270F + 1 = K10000 */
uint8_t ModbusCoil[1250];   /*10000/8=1250*/
uint16_t ModbusReg[10000];

/*附加功能：页寄存器*/
uint16_t ModbusExtPage[1] = {0};    /*设置地址为0xFFFE*/
uint16_t ModbusExtReg[1];           /*地址映射为0x10000*/
/*附加功能：读取所有单数个寄存器*/
uint16_t ModbusExtOdd[1] = {0};     /*设置地址为0xFFFF*/

uint8_t ModbusRxBuf[MODBUS_RX_BUF_SIZE];    /*接收数组*/
uint16_t ModbusRxLen = 0;                   /*接收数据长度*/
uint8_t ModbusTxBuf[MODBUS_TX_BUF_SIZE];    /*发送数组*/
uint16_t ModbusTxLen = 0;                   /*发送数据长度*/


/*modbus协议帧解析任务*/
void AppTaskModbusParse (void *p_arg)
{
    OS_ERR err;
    MODBUS_FRAME frame;
    uint16_t rxLen;
    uint8_t rxBuf[256];

    while(1)
    {
        /*无限期等待接收到数据帧信号量*/
        OSSemPend(ModbusReceived, 0, &err);
        
        /*数据接收，防止下一帧覆盖*/
        rxLen = ModbusRxLen;
        if(rxLen > MODBUS_RX_BUF_SIZE)
        {
            rxLen = 0;
        }
        memcpy(rxBuf, ModbusRxBuf, rxLen);

        /*帧分解*/
        frame.slaveAddres = rxBuf[0];
        frame.func = rxBuf[1];
        frame.crc = ((uint16_t)(rxBuf[rxLen - 2])) | ((uint16_t)rxBuf[rxLen - 1] << 8);
        frame.opeAddres = ((uint16_t)rxBuf[2] << 8) | ((uint16_t)rxBuf[3]);
        frame.opeNum = ((uint16_t)rxBuf[4] << 8) | ((uint16_t)rxBuf[5]);

        /*crc校验判断*/
        if(ChcekCrc16(frame.crc, rxBuf, rxLen) != 0)
        {   
            /*crc校验错误，直接丢弃*/
            ModbusErr = ERR_CRC;
            continue;
        }
        /*从机地址判断*/
        if(frame.slaveAddres != SLAVE_ADDRES)
        {
            /*从机地址错误，直接丢弃*/
            ModbusErr = ERR_SLAVE_ADDRES;
            continue;
        }
        /*释放正确接收信号量*/
        OSSemPost(ModbusConnect);

        /*功能码分类*/
        switch(frame.func)
        {
            case 0x01:/*读线圈*/
                ModbusErr = ModbusReadCoils(frame.opeAddres, frame.opeNum);
                break;

            case 0x03:/*读寄存器*/
                ModbusErr = ModbusReadRegs(frame.opeAddres, frame.opeNum);
                break;

            case 0x0F:/*写多个线圈*/
                frame.wNum = rxBuf[6];
                memcpy(frame.wData, &rxBuf[7], frame.wNum);
                ModbusErr = ModbusWriteCoils(frame.opeAddres, frame.opeNum, frame.wData);         
                break;

            case 0x10:/*写多个寄存器*/
                frame.wNum = rxBuf[6];
                memcpy(frame.wData, &rxBuf[7], frame.wNum);
                ModbusErr = ModbusWriteRegs(frame.opeAddres, frame.opeNum, frame.wData);
                break;
            
            case 0x05:/*写单个线圈*/
                memcpy(frame.wData, &rxBuf[4], 2);
                ModbusErr = ModbusWriteCoil(frame.opeAddres, frame.wData);
                break;

            case 0x06:/*写单个寄存器*/
                memcpy(frame.wData, &rxBuf[4], 2);
                ModbusErr = ModbusWriteReg(frame.opeAddres, frame.wData);
                break;

            default:/*功能码错误*/
                ModbusErr = ERR_FUNC;
                ModbusErr = ModbusErrReply(frame.func, ERR_FUNC);
                break;
        }
        /*发送响应帧*/
        ModbusSend(ModbusTxBuf, ModbusTxLen);

        if(ModbusErr == 0 && (frame.func == 0x0F || frame.func == 0x10 
                                || frame.func == 0x05 || frame.func == 0x06))
        {
            /*释放写FLASH信号量*/
            OSSemPost(ModbusWriteFlash);
        }
    }
}

uint8_t ModbusReadCoils(uint16_t addres, uint16_t num)
{
    uint16_t byteIndI;  /*字节索引*/
    uint8_t bitIndI;    /*位索引*/
    uint16_t byteIndO;
    uint8_t bitIndO;

    uint8_t dataLen;    /*响应帧返回字节数*/

    uint8_t index;      /*发送数组索引*/
    uint8_t readVal;    /*读取到的形式值*/
    uint8_t writeVal;   /*写入的形式值*/

    if(ModbusRxLen < 8)
    {
        return ModbusErrReply(0x01, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        return ModbusErrReply(0x01, ERR_OPE_ADDRES);
    }
    /*操作数量超限*/
    if(num == 0 || num > 2000)
    {
        return ModbusErrReply(0x01, ERR_DATA);
    }

    /*不能num/8+1，当为8的整数时会多一个*/
    dataLen = (num + 7) / 8;

    /*清零发送数组*/
    memset(&ModbusTxBuf[3], 0, 253);

    for(uint16_t i = 0; i < num; i++)
    {
        byteIndO = i / 8;
        bitIndO = i % 8;

        byteIndI = (addres + i) / 8;
        bitIndI = (addres + i) % 8;

        /*位掩码读取线圈值*/
        readVal = ModbusCoil[byteIndI] & ((uint8_t)1 << bitIndI);
        
        /*读取到的值为0则不进行操作*/
        if(readVal != 0)
        {
            index = 3 + byteIndO;
            /*将读取到的值移位到要写入的位*/
            writeVal = (uint8_t)1u << bitIndO;

            ModbusTxBuf[index] |= writeVal;
        }
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;  /*从机地址*/
    ModbusTxBuf[1] = 0x01;          /*功能码*/
    ModbusTxBuf[2] = dataLen;       /*响应帧返回数据字节数*/
    AddCrc16(ModbusTxBuf, 3 + dataLen);
    ModbusTxLen = 5 + dataLen;

    return 0;
}

uint8_t ModbusWriteCoils(uint16_t addres, uint16_t num, uint8_t *data)
{
    uint16_t byteIndI;
    uint8_t bitIndI;
    uint16_t byteIndO;
    uint8_t bitIndO;

    uint8_t readVal;
    uint8_t writeVal;

    if(ModbusRxLen < 10)
    {
        return ModbusErrReply(0x0F, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        return ModbusErrReply(0x0F, ERR_OPE_ADDRES);
    }
    /*操作数量超限*/
    if(num == 0 || num > 1968)
    {
        return ModbusErrReply(0x0F, ERR_DATA);
    }
   
    for(uint16_t i = 0; i < num; i++)
    {
        byteIndI = (addres + i) / 8;
        bitIndI = (addres + i) % 8;
    
        byteIndO = i / 8;
        bitIndO = i % 8;

        /*位掩码读取要写入的值*/
        readVal = data[byteIndO] & ((uint8_t)1 << bitIndO);
        /*将写入值移位到要写入的位*/
        writeVal = (uint8_t)1 << bitIndI;

        /*对应位写入1*/
        if(readVal != 0)
        {
            ModbusCoil[byteIndI] |= writeVal;
        }
        /*对应位写入0*/
        else
        {
            ModbusCoil[byteIndI] &= (~writeVal);
        }
    }
    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x0F;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入的数量*/
    ModbusTxBuf[4] = (uint8_t)(num >> 8);
    ModbusTxBuf[5] = (uint8_t)(num & 0x00FF);
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;

    return 0;
}

uint8_t ModbusReadRegs(uint16_t addres, uint16_t num)
{
    uint8_t index;
    uint8_t readValH;
    uint8_t readValL;
    uint8_t dataLen;

    if(ModbusRxLen < 8)
    {
        return ModbusErrReply(0x03, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        if(addres == 0xFFFF && num == 1)
        {
            ;
        }
        else
        {
            return ModbusErrReply(0x03, ERR_OPE_ADDRES);
        } 
    }
    /*操作数量超限*/
    if(num == 0 || num > 125)
    {
        return ModbusErrReply(0x03, ERR_DATA);
    }

    dataLen = num * 2;
    /*附加功能：读取0x10000寄存器*/
    if(ModbusExtPage[0] == 1 && addres == 0x0000)
    {
        /*将16位寄存器值拆分为两个8位*/
        readValH = (uint8_t)(ModbusExtReg[0] >> 8);
        readValL = (uint8_t)(ModbusExtReg[0] & 0x00FF);

        ModbusTxBuf[3] = readValH;
        ModbusTxBuf[4] = readValL;
    }
    else
    {
        for(uint16_t i = 0; i < num; i++)
        {
            index = 3 + (i * 2);
            /*将16位寄存器值拆分为两个8位*/
            readValH = (uint8_t)(ModbusReg[addres + i] >> 8);
            readValL = (uint8_t)(ModbusReg[addres + i] & 0x00FF);

            ModbusTxBuf[index] = readValH;
            ModbusTxBuf[index + 1] = readValL;
        }
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x03;
    ModbusTxBuf[2] = dataLen;
    AddCrc16(ModbusTxBuf, 3 + dataLen);
    ModbusTxLen = 5 + dataLen;

    return 0;
}

uint8_t ModbusWriteRegs(uint16_t addres, uint16_t num, uint8_t *data)
{
    uint8_t index;

    uint8_t writeValH;
    uint8_t writeValL;
    uint16_t writeVal;

    if(ModbusRxLen < 11)
    {
        return ModbusErrReply(0x10, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        if((addres == 0xFFFE || addres == 0xFFFF) && num == 1)
        {
            ;
        }
        else
        {
            return ModbusErrReply(0x10, ERR_OPE_ADDRES);
        }
    }
    /*操作数量超限*/
    if(num == 0 || num > 123)
    {
        return ModbusErrReply(0x10, ERR_DATA);
    }

    /*附加功能：读取0x10000寄存器*/
    if(addres == 0xFFFE && num == 1)
    {
        writeValH = data[0];
        writeValL = data[1];
        
        /*将要写入两个8位值拼接为16位*/
        writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
        ModbusExtPage[0] = writeVal;
    }
    /*附加功能：读取大量奇数寄存器*/
    else if(addres == 0xFFFF && num == 1)
    {
        writeValH = data[0];
        writeValL = data[1];
        
        /*将要写入两个8位值拼接为16位*/
        writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
        ModbusExtOdd[0] = writeVal;
    }
    /*正常功能*/
    else
    {
        for(uint16_t i = 0; i < num; i++)
        {
            index = i * 2;

            writeValH = data[index];
            writeValL = data[index + 1];
        
            /*将要写入两个8位值拼接为16位*/
            writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
            ModbusReg[addres + i] = writeVal;
        }
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x10;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入数量*/
    ModbusTxBuf[4] = (uint8_t)(num >> 8);
    ModbusTxBuf[5] = (uint8_t)(num & 0x00FF);
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;

    return 0;
}

uint8_t ModbusWriteCoil(uint16_t addres, uint8_t *data)
{
    uint16_t byteIndI;
    uint8_t bitIndI;

    uint16_t readVal;
    uint8_t writeVal;

    if(ModbusRxLen < 8)
    {
        return ModbusErrReply(0x05, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F)
    {
        return ModbusErrReply(0x05, ERR_OPE_ADDRES);
    }
   
    byteIndI = addres / 8;
    bitIndI = addres % 8;
    
    /*位掩码读取要写入的值*/
    readVal = ((uint16_t)data[0] << 8) | ((uint16_t)data[1] & 0x00FF);
    /*将写入值移位到要写入的位*/
    writeVal = (uint8_t)1 << bitIndI;

    /*置on*/
    if(readVal == 0xFF00)
    {
        ModbusCoil[byteIndI] |= writeVal;
    }
    /*置off*/
    else if(readVal == 0x0000)
    {
        ModbusCoil[byteIndI] &= (~writeVal);
    }
    else
    {
        return ModbusErrReply(0x05, ERR_DATA);
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x05;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入*/
    ModbusTxBuf[4] = data[0];
    ModbusTxBuf[5] = data[1];
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;

    return 0;
}

uint8_t ModbusWriteReg(uint16_t addres, uint8_t *data)
{
    uint8_t writeValH;
    uint8_t writeValL;
    uint16_t writeVal;
    
    if(ModbusRxLen < 8)
    {
        return ModbusErrReply(0x06, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F)
    {
        return ModbusErrReply(0x06, ERR_OPE_ADDRES);
    }

    writeValH = data[0];
    writeValL = data[1];
    /*将要写入两个8位值拼接为16位*/
    writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
    ModbusReg[addres] = writeVal;
    
    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x06;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入*/
    ModbusTxBuf[4] = writeValH;
    ModbusTxBuf[5] = writeValL;
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;
    
    return 0;
}



uint8_t ModbusErrReply(uint8_t func, uint8_t err)
{
    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = func | 0x80;
    ModbusTxBuf[2] = err;
    AddCrc16(ModbusTxBuf, 3);
    ModbusTxLen = 5;

    return err;
}