#include "hciemulator.h"
#define CHECKCHANGEDSET(Target,Value,Flag) if((Target)!=(Value)){Target=Value;Flag=true;}
int hciloglevel = DEFAULTLOGLEVEL;

#ifdef SOFTSERIAL
#define Log(Level,Message) LogCore(Level,Message)
#define Log3(Level,Message,Buffer, Len) LogCore(Level,Message,Buffer,Len)
//LOGLEVEL
void LogCore(int Level, const char* msg, const unsigned char * data=NULL, size_t datalen=0){
    if(Level>hciloglevel){
        return;
    }
    if(data!=NULL && datalen>0){ 
        String newmsg(msg);
        char str[4];
        for (size_t i = 0; i < datalen; i++){
            snprintf(str,sizeof(str),"%02x ", data[i]);
            newmsg+=str;
        }
        Serial.println(newmsg);        
    }else{
        Serial.println(msg);
    }
}
#else 
    #define Log(Level,Message) 
    #define Log3(Level,Message,Buffer, Len) 
#endif


int HCIEmulator::getLogLevel(){
    return hciloglevel;
    
}
void HCIEmulator::setLogLevel(int level){
    hciloglevel=level;
}

//modbus crc calculation borrowed from:
//https://github.com/yaacov/ArduinoModbusSlave
#define MODBUS_CRC_LENGTH 2
#define readCRC(arr, length) word(arr[(length - MODBUS_CRC_LENGTH) + 1], arr[length - MODBUS_CRC_LENGTH])
#define readUInt16(arr, index) word(arr[index], arr[index + 1])
/**
 * Calculate the CRC of the passed byte array from zero up to the passed length.
 *
 * @param buffer The byte array containing the data.
 * @param length The length of the byte array.
 *
 * @return The calculated CRC as an unsigned 16 bit integer.
 * 
 * Calculate and add the CRC.
 *       uint16_t crc = Modbus::calculateCRC(_responseBuffer, _responseBufferLength - MODBUS_CRC_LENGTH);
 *       _responseBuffer[_responseBufferLength - MODBUS_CRC_LENGTH] = crc & 0xFF;
 *       _responseBuffer[(_responseBufferLength - MODBUS_CRC_LENGTH) + 1] = crc >> 8;
 * 
 * 
 * #define MODBUS_FRAME_SIZE 4
 * #define MODBUS_CRC_LENGTH 2
 * uint16_t crc = readCRC(_requestBuffer, _requestBufferLength);
 * #define readUInt16(arr, index) word(arr[index], arr[index + 1])
 * #define readCRC(arr, length) word(arr[(length - MODBUS_CRC_LENGTH) + 1], arr[length - MODBUS_CRC_LENGTH])
 */
uint16_t calculateCRC(uint8_t *buffer, int length)
{
    int i, j;
    uint16_t crc = 0xFFFF;
    uint16_t tmp;

    // Calculate the CRC.
    for (i = 0; i < length; i++)
    {
        crc = crc ^ buffer[i];
        for (j = 0; j < 8; j++)
        {
            tmp = crc & 0x0001;
            crc = crc >> 1;
            if (tmp)
            {
                crc = crc ^ 0xA001;
            }
        }
    }
    return crc;
}

HCIEmulator::HCIEmulator(Stream * port) { 
    m_state.valid = false; 
    m_statemachine=WAITING;
    m_rxlen = m_txlen = 0;
    m_recvTime=m_lastStateTime=0;
    m_skipFrame=false;
    m_port = port;
    m_statusCallback = NULL;
    setLogLevel(DEFAULTLOGLEVEL);
};

void HCIEmulator::poll(){
    
    if(m_port==NULL) return;

    // receive Data
    if(m_port->available() >0)
    {
        m_rxlen+= m_port->readBytes((char*)(m_rxbuffer+m_rxlen), _min((int)(255-m_rxlen),m_port->available()));
        if(m_rxlen > 254)
        {
            Log(LL_ERROR,"RX Bufferoverflow, skip next Frame");   
            Log3(LL_DEBUG,"Buffer Data: ", m_rxbuffer, m_rxlen);
            m_rxlen=0;               
            m_skipFrame = true;
        }
        m_recvTime = micros();             
    }        
    

    // check frame, process frame
    if(m_rxlen>0 && (micros()-m_recvTime > T3_5)) 
    {
        // check last action timeout -> reset > then 2sec
        if(m_statemachine!= WAITING && m_lastStateTime+2000<millis()){
            m_statemachine = WAITING;
        }

        if(!m_skipFrame){

            processFrame();

            // send response
            if(m_txlen > 0){
                
                // fix crc
                uint16_t crc = calculateCRC(m_txbuffer, m_txlen - MODBUS_CRC_LENGTH);
                m_txbuffer[m_txlen - MODBUS_CRC_LENGTH] = crc & 0xFF;
                m_txbuffer[(m_txlen - MODBUS_CRC_LENGTH) + 1] = crc >> 8;                

                // send data
                m_lastSendTime = micros()-m_recvTime;

                //Log(LL_DEBUG, ("ST:"+String(m_lastSendTime)).c_str());
                
                m_port->write(m_txbuffer, m_txlen);
                Log3(LL_DEBUG,"Response: ", m_txbuffer, m_txlen);
                m_txlen = 0;
            }                
        }

        m_skipFrame = false;
        m_rxlen=0;            
    }    
}

void HCIEmulator::processFrame(){
    m_txlen = 0;    // clear send buffer

    if(m_rxlen<5) {
        Log(LL_ERROR,"Frame skipped, invalid frame len");      
        Log3(LL_ERROR,"Data:", m_rxbuffer,m_rxlen);
        return;
    }

    // check device id, pass only device id 2 and 0 (broadcast)
    if(m_rxbuffer[0] != BROADCASTID && m_rxbuffer[0] != DEVICEID){      
        Log(LL_DEBUG,"Frame skipped, unsupported device id");      
        Log3(LL_DEBUG,"Data:", m_rxbuffer,m_rxlen);
        return;
    }

    // check crc
    uint16_t crc = readCRC(m_rxbuffer, m_rxlen);        
    if(crc != calculateCRC(m_rxbuffer,m_rxlen-MODBUS_CRC_LENGTH)){
        Log3(LL_ERROR,"Frame skipped, wrong crc", m_rxbuffer,m_rxlen);
        return;
    }

    Log3(LL_DEBUG,"Incomming Data: ", m_rxbuffer, m_rxlen);

    // dispatch modbus function
    switch(m_rxbuffer[1]){ 
        case 0x10:{  //  Write Multiple registers                            
            if(m_rxlen == 0x1b && m_rxbuffer[0] == BROADCASTID)
            {
                processBroadcastStatusFrame();
                return;
            }  
            break;
        }
        
        case 0x17:{ // Read/Write Multiple registers
            if(m_rxbuffer[0] == DEVICEID){
                switch(m_rxlen){
                    case 0x11:{
                        processDeviceStatusFrame();
                        return;
                    }

                    case 0x13:
                        processDeviceBusScanFrame();
                        return;;
                }
            }
            break;                               
        }
    }
    Log3(LL_ERROR,"Frame skipped, unexpected data: ", m_rxbuffer, m_rxlen);
}

const unsigned char ResponseTemplate_Fcn17_Cmd03_L08 []= {0x02,0x17,0x10,0x3E,0x00,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x74,0x1B};
const unsigned char ResponseTemplate_Fcn17_Cmd04_L02 []= {0x02,0x17,0x04,0x0F,0x00,0x04,0xFD,0x0A,0x72};
void HCIEmulator::processDeviceStatusFrame(){
    if(m_rxlen==0x11){  
        unsigned char counter = m_rxbuffer[11];
        unsigned char cmd = m_rxbuffer[12];
        if(m_rxbuffer[5] == 0x08){
                // expose internal state
            //      00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16
            //0011: 02 17 9C B9 00 08 9C 41 00 02 04 3E 03 00 00 EB CC
            //res=> 02 17 10 3E 00 03 01 00 00 00 00 00 00 00 00 00 00 00 00 74 1B                 
            memcpy_P(m_txbuffer, ResponseTemplate_Fcn17_Cmd03_L08, sizeof(ResponseTemplate_Fcn17_Cmd03_L08));  
            m_txbuffer[0] = m_rxbuffer[0];
            m_txbuffer[3] = counter;
            m_txbuffer[5] = cmd;
            m_txlen = sizeof(ResponseTemplate_Fcn17_Cmd03_L08);   

            
            switch(m_statemachine)
            {
                // open Door
                case STARTOPENDOOR:
                    m_txbuffer[7]= 0x02;
                    m_txbuffer[8]= 0x10;
                    m_statemachine = STARTOPENDOOR_RELEASE;
                    m_lastStateTime = millis();
                    break;
                case STARTOPENDOOR_RELEASE:
                    if(m_lastStateTime+SIMULATEKEYPRESSDELAYMS<millis()){
                        m_txbuffer[7]= 0x01;
                        m_txbuffer[8]= 0x10;
                        m_statemachine = WAITING; 
                    }
                    break;

                // close Door
                case STARTCLOSEDOOR:
                    m_txbuffer[7]= 0x02;
                    m_txbuffer[8]= 0x20;
                    m_statemachine = STARTCLOSEDOOR_RELEASE;
                    m_lastStateTime = millis();
                    break;
                case STARTCLOSEDOOR_RELEASE:
                    if(m_lastStateTime+SIMULATEKEYPRESSDELAYMS<millis()){
                        m_txbuffer[7]= 0x01;
                        m_txbuffer[8]= 0x20;
                        m_statemachine = WAITING; 
                    }
                    break;   

                // stop Door
                case STARTSTOPDOOR:
                    m_txbuffer[7]= 0x02;
                    m_txbuffer[8]= 0x40;
                    m_statemachine = STARTSTOPDOOR_RELEASE;
                    m_lastStateTime = millis();
                    break;
                case STARTSTOPDOOR_RELEASE:
                    if(m_lastStateTime+SIMULATEKEYPRESSDELAYMS<millis()){
                        m_txbuffer[7]= 0x01;
                        m_txbuffer[8]= 0x40;
                        m_statemachine = WAITING; 
                    }
                    break;

                // Ventilation
                case STARTVENTPOSITION:
                    m_txbuffer[7]= 0x02;
                    m_txbuffer[9]= 0x40;
                    m_statemachine = STARTVENTPOSITION_RELEASE;
                    m_lastStateTime = millis();
                    break;
                case STARTVENTPOSITION_RELEASE:
                    if(m_lastStateTime+SIMULATEKEYPRESSDELAYMS<millis()){
                        m_txbuffer[7]= 0x01;
                        m_txbuffer[9]= 0x40;
                        m_statemachine = WAITING; 
                    }
                    break;        


                // Half Position
                case STARTOPENDOORHALF:
                    m_txbuffer[7]= 0x02;
                    m_txbuffer[9]= 0x04;
                    m_statemachine = STARTOPENDOORHALF_RELEASE;
                    m_lastStateTime = millis();
                    break;

                case STARTOPENDOORHALF_RELEASE:
                    if(m_lastStateTime+SIMULATEKEYPRESSDELAYMS<millis()){
                        m_txbuffer[7]= 0x01;
                        m_txbuffer[9]= 0x04;
                        m_statemachine = WAITING; 
                    }
                    break;                                          

                // Toggle Lamp
                case STARTTOGGLELAMP:
                    m_txbuffer[7]= 0x10;
                    m_txbuffer[9]= 0x02;
                    m_statemachine = STARTTOGGLELAMP_RELEASE;
                    m_lastStateTime = millis();
                    break;
                case STARTTOGGLELAMP_RELEASE:
                    if(m_lastStateTime+SIMULATEKEYPRESSDELAYMS<millis()){
                        m_txbuffer[7]= 0x08;
                        m_txbuffer[9]= 0x02;
                        m_statemachine = WAITING; 
                    }
                    break;  
                    
                case WAITING:
                    break;            
            }                                 
            return;             
        }
        else if(m_rxbuffer[5] == 0x02){
            //      00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16
            //0011: 02 17 9C B9 00 02 9C 41 00 02 04 0F 04 17 00 7B 21
            //res=> 02 17 04 0F 00 04 FD 0A 72     
            memcpy_P(m_txbuffer, ResponseTemplate_Fcn17_Cmd04_L02, sizeof(ResponseTemplate_Fcn17_Cmd04_L02));  
            m_txbuffer[0] = m_rxbuffer[0];
            m_txbuffer[3] = counter;
            m_txbuffer[5] = cmd;
            m_txlen = sizeof(ResponseTemplate_Fcn17_Cmd04_L02);   
            return;
        }
    }

    Log3(LL_ERROR,"Frame skipped, unexpected data: ", m_rxbuffer, m_rxlen); 
}

const unsigned char ResponseTemplate_Fcn17_Cmd02_L05 []= {0x02,0x17,0x0a,0x00,0x00,0x02,0x05,0x04,0x30,0x10,0xff,0xa8,0x45,0x0e,0xdf};
void HCIEmulator::processDeviceBusScanFrame(){
    //      00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16
    //0013: 02 17 9C B9 00 05 9C 41 00 03 06 00 02 00 00 01 02 f8 35
    //res=> 02 17 0a 00 00 02 05 04 30 10 ff a8 45 0e df
    unsigned char counter = m_rxbuffer[11];
    unsigned char cmd = m_rxbuffer[12];
    memcpy_P(m_txbuffer, ResponseTemplate_Fcn17_Cmd02_L05, sizeof(ResponseTemplate_Fcn17_Cmd02_L05));  
    m_txbuffer[0] = m_rxbuffer[0];
    m_txbuffer[3] = counter;
    m_txbuffer[5] = cmd;
    m_txlen = sizeof(ResponseTemplate_Fcn17_Cmd02_L05); 

    Log(LL_INFO,"Busscan received");  
}

void HCIEmulator::processBroadcastStatusFrame(){
    //001B: 00 10 9D 31 00 09 12 64 00 00 00 40 60 00 00 00 00 00 00 00 00 00 01 00 00 CA 22
    bool hasChanged = false;
    CHECKCHANGEDSET(m_state.lampOn,m_rxbuffer[20] == 0x14,hasChanged);      
    CHECKCHANGEDSET(m_state.doorCurrentPosition,m_rxbuffer[10],hasChanged);
    CHECKCHANGEDSET(m_state.doorTargetPosition, m_rxbuffer[9],hasChanged);
    CHECKCHANGEDSET(m_state.doorState, m_rxbuffer[11],hasChanged);
    CHECKCHANGEDSET(m_state.reserved, m_rxbuffer[17],hasChanged);
    CHECKCHANGEDSET(m_state.valid, true,hasChanged);   
    
    if(hasChanged){
        Log3(LL_INFO,"New State: ",m_rxbuffer,m_rxlen);
        if(m_statusCallback != NULL){
            m_statusCallback(m_state);
        }  
    }             
}    

void HCIEmulator::openDoor(){
    if(m_statemachine != WAITING){
        return;
    }
    m_lastStateTime = millis();
    m_statemachine = STARTOPENDOOR;
}

void HCIEmulator::openDoorHalf(){
    if(m_statemachine != WAITING){
        return;
    }
    m_lastStateTime = millis();
    m_statemachine = STARTOPENDOORHALF;
}

void HCIEmulator::closeDoor(){
    if(m_statemachine != WAITING){
        return;
    }
    m_lastStateTime = millis();    
    m_statemachine = STARTCLOSEDOOR;
}

void HCIEmulator::stopDoor(){
    if(m_statemachine != WAITING){
        return;
    }
    m_lastStateTime = millis();    
    m_statemachine = STARTSTOPDOOR;
}

void HCIEmulator::toggleLamp(){
    if(m_statemachine != WAITING){
        return;
    }
    m_lastStateTime = millis();
    m_statemachine = STARTTOGGLELAMP;
}

void HCIEmulator::ventilationPosition(){
    if(m_statemachine != WAITING){
        return;
    }
    m_lastStateTime = millis();
    m_statemachine = STARTVENTPOSITION;
}

void HCIEmulator::onStatusChanged(callback_function_t handler) {
    m_statusCallback = handler;
}