#ifndef CAMA_SM30
#define CAMA_SM30

#include "SensorSM30Controller.h"
#include "CamaSM30Commands.h"
#include "Streaming.h"
#include <Arduino.h>

#define COMMAND_DATA_LEN 44
#define TIMEOUT 1500

// HardwareSerial SensorSerial(2);

SensorSM30Controller::SensorSM30Controller(HardwareSerial *_hwSerial)
{
    hwSerial = _hwSerial;
    sensorSerial = hwSerial;
    sensorSerial->setTimeout(TIMEOUT);
}

void SensorSM30Controller::begin()
{
    hwSerial->begin(115200);
}

void SensorSM30Controller::cancel()
{
    Serial << "Reset sensor command" << endl;
    String cmd = getCommand(SM30_CMD_FP_CANCEL);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    long begin = millis();
    readSerialPort();
    // sensorSerial->readBytes(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    long diff = millis() - begin;
    Serial << "conversion_time:" << diff << endl;
}

/**
 * @brief CHECK SENSOR
 *
 */
bool SensorSM30Controller::camaTest()
{
    Serial << "Test sensor" << endl;
    String cmd = getCommand(SM30_CMD_TEST_CONNECTION);
    //Serial << cmd << endl;
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    long begin = millis();
    readSerialPort();
    // sensorSerial->readBytes(inbuffer, SM30_REGULAR_PACKET_LENGTH);

    long diff = millis() - begin;
    Serial << "conversion_time:" << diff << endl;

    if (inbuffer[23] == 0 && inbuffer[24] == 0)
    {
        return false;
    }
    return true;
}

/**
 * @brief Detect finger
 * Returns 1 if finger detected, 0 if not finger present, 0xFF in communication error
 *
 *  @return uint8_t
 */
uint8_t SensorSM30Controller::detectFinger()
{
    String cmd = getCommand(SM30_CMD_FINGER_DETECT);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    sensorSerial->flush();
    if (readSerialPort())
    {
        if (inbuffer[8] == 1)
        {
            return 1;
        }
    }

    return 0;
    // ledOFF();
}

// void SensorSM30Controller::readSensor(uint8_t *buffer, uint16_t len)
// {

//     for (int i = 0; i < len; i++)
//     {
//         buffer[i] = sensorSerial->read();
//     }
// }

bool SensorSM30Controller::ledON()
{
    String cmd = getCommand(SM30_CMD_SENSOR_LED_CONTROLL, 2, "01");
    return sendCommand(cmd);
}

bool SensorSM30Controller::ledOFF()
{
    String cmd = getCommand(SM30_CMD_SENSOR_LED_CONTROLL, 2, "00");
    return sendCommand(cmd);
}

bool SensorSM30Controller::sendCommand(String cmd)
{
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    sensorSerial->flush();
    return readSerialPort();
}

/**
 * @brief Save template from host to sensor
 * Host send Write Template command to cause module wait for receive Command Data Packet to get
template data from Host
1) Module check the Command packet whether is correct or not
if size of Template record is incorrectness,result=ERR_INVALID_PARAM
else return response packet and prepare receive template data (Command Data Packet)
2) Host send Command Data Packet with template No. and Template Data
3) after receive Command Data Packet is successful,
if the appoint template No. is invalid,result= ERR_INVALID_TMPL_NO
if CheckSum of Template Data is incorrectness,result=ERR_INVALID_PARAM
4) else Template Data that received write to Flash Memory
 * @param slot
 * @param template_chars
 * @param template_len
 * @return true
 * @return false
 */
bool SensorSM30Controller::saveTemplate(uint16_t slot, String template_chars, uint16_t template_len)
{
    String cmd = getCommand(SM30_CMD_WRITE_TEMPLATE, 2, SM30_TEMPLATE_DATA_PACKET_LENGTH);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    tools.dump_byte_array(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    if (readSerialPort())
    {
        tools.dump_byte_array(inbuffer, SM30_REGULAR_PACKET_LENGTH);
        Serial.println("^ write template command response");
    }
    else
    {
        return false;
    }

    cmd = getDataTemplate(SM30_CMD_WRITE_TEMPLATE, SM30_TEMPLATE_DATA_PACKET_LENGTH + 2, slot, template_chars);
    tools.convertHexStringToByteArray(template_buffer, SM30_TEMPLATE_BUFFER_LENGHT, cmd);
    tools.dump_byte_array(template_buffer, SM30_TEMPLATE_BUFFER_LENGHT);
    Serial.println("^ write template data sent");
    sensorSerial->write(template_buffer, SM30_TEMPLATE_BUFFER_LENGHT);

    long t = millis();
    // Timeout
    // 12 bytes response
    while (sensorSerial->available() < SM30_DATA_PACKET_RESPONSE_LENGTH)
    {
        if (millis() - t > TIMEOUT)
        {
            Serial << "Sensor timeout" << endl;
            break;
        }
    }
    // Serial2.setTimeout();
    sensorSerial->readBytes(inbuffer, SM30_DATA_PACKET_RESPONSE_LENGTH);
    if (!isGoodCRC(inbuffer, SM30_DATA_PACKET_RESPONSE_LENGTH))
    {
        Serial << "Bad CRC" << endl;
        return false;
    }

    tools.dump_byte_array(inbuffer, SM30_DATA_PACKET_RESPONSE_LENGTH);
    Serial.println("^ write template data response");
    return true;
}

/**
 * @brief Delete template
 *
 * @param slot
 * @return true
 * @return false
 */
bool SensorSM30Controller::deleteTemplate(uint16_t slot)
{
    String cmd = getCommand(SM30_CMD_CLEAR_TEMPLATE, 2, slot);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    readSerialPort();
    tools.dump_byte_array(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    Serial.println();
    if (inbuffer[8] != 1)
    {
        return false;
    }
    return true;
}

/**
 * @brief Identify finger after detection
[Operation Sequence]
1) if Template Data Base is null, result= ERR_ALL_TMPL_EMPTY;
2) detect finger whether press on sensor or not repeatedly;
if no any finger press on sensor in the period of timeout, result= ERR_TIME_OUT;
3) else check quality of the captured fingerprint image,
if image is not good, result= ERR_BAD_QUALITY
4) else result=GD_NEED_RELEASE_FINGER, denote that lift finger
5) identify the captured fingerprint with the all template data in FLASH memory,
if identify ok ,result=matched template No.; else result= ERR_IDENTIFY
6) In the period of process identify command, if module received “FP Cancel “command,
The module then stop identify command and return ACK that is ERR_FP_CANCEL
Please refer to appendix about the flow of Verify & Identify
 * @return uint16_t
 *

 */
uint16_t SensorSM30Controller::identify()
{
    String cmd = getCommand(SM30_CMD_IDENTIFY);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    sensorSerial->flush();
    // read result
    // uint8_t size = Serial2.readBytes(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    if (readSerialPort())
    {
        // lift finger - ready for identification - GD_NEED_RELEASE_FINGER
        if (inbuffer[8] == 0xF4 && inbuffer[9] == 0xFF)
        {
            Serial << "finger up" << endl;
        }
    }
    else
    {
        return 0xFFFF;
    }

    if (!readSerialPort())
    {
        return 0xFFFF;
    }

    if (inbuffer[6] == 1)
    {
        if (inbuffer[8] == 0x12 && inbuffer[9] == 0x00)
        {
            Serial << "no matching found" << endl;
        }
        else if (inbuffer[8] == 0x15 && inbuffer[9] == 0x00)
        {
            Serial << "all empty templates" << endl;
        }
        else if (inbuffer[8] == 0x23 && inbuffer[9] == 0x00)
        {
            Serial << "timeout identify" << endl;
        }
        else if (inbuffer[8] == 0x21 && inbuffer[9] == 0x00)
        {
            Serial << "bad quality" << endl;
        }
        return 0; // not recognized
    }

    uint16_t fnd = inbuffer[9];
    fnd <<= 8;
    fnd += inbuffer[8];
    Serial << "Found finger:" << fnd << endl;
    return fnd;
}

uint16_t SensorSM30Controller::getTemplatesCount()
{
    String cmd = getCommand(SM30_CMD_GET_ENROLL_COUNT);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    readSerialPort();
    uint16_t fnd = inbuffer[9];
    fnd <<= 8;
    fnd += inbuffer[8];
    Serial << "Fingers count:" << fnd << endl;
    return fnd;
}

/**
 * @brief Read CAMA response pack
 *
 * @return true
 * @return false
 */
bool SensorSM30Controller::readSerialPort()
{
    long t = millis();
    // Timeout
    // sensorSerial->setTimeout()
    while (sensorSerial->available() < 24)
    {
        if (millis() - t > TIMEOUT)
        {
            Serial << "Sensor timeout" << endl;
            break;
        }
    }
    // Serial2.setTimeout();
    sensorSerial->readBytes(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    if (!isGoodCRC(inbuffer, SM30_REGULAR_PACKET_LENGTH))
    {
        Serial << "Bad CRC. Sensor needs restart" << endl;
        return false;
    }
    return true;
}

/**
 * Command creation - command and data
 * @param cmd command
 * @param len length
 * @param slot_h slot MSB
 * @param slot_l slot LSB
 * @return
 */
String SensorSM30Controller::getCommand(uint16_t cmd, uint16_t len, String data)
{
    // Total length of the command packet is 24 Bytes   //
    String command = "55AA";                             // PREFIX + command
    command += tools.byteToHex((uint8_t)(cmd & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cmd >> 8));     // MSB
    command += tools.byteToHex((uint8_t)(len & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(len >> 8));     // MSB
    command += data;                                     // data
    while (command.length() < COMMAND_DATA_LEN)
    {
        command += "0";
    }
    uint16_t cks = tools.calculateCKS(command);
    command += tools.byteToHex((uint8_t)(cks & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cks >> 8));     // MSB
    return command;
}

/**
 * @brief Generate template data packet
 *
 * @param cmd
 * @param len
 * @param slot
 * @param templ
 * @return String
 */
String SensorSM30Controller::getDataTemplate(uint16_t cmd, uint16_t len, uint16_t slot, String templ)
{
    String command = "5AA5";                              // PREFIX + command
    command += tools.byteToHex((uint8_t)(cmd & 0x00ff));  // LSB
    command += tools.byteToHex((uint8_t)(cmd >> 8));      // MSB
    command += tools.byteToHex((uint8_t)(len & 0x00ff));  // LSB
    command += tools.byteToHex((uint8_t)(len >> 8));      // MSB
    command += tools.byteToHex((uint8_t)(slot & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(slot >> 8));     // MSB
    command += templ;                                     // data

    uint16_t cks = tools.calculateCKS(command);
    command += tools.byteToHex((uint8_t)(cks & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cks >> 8));     // MSB
    return command;
}

String SensorSM30Controller::getCommand(uint16_t cmd, uint16_t len, uint16_t slot)
{
    // Total length of the command packet is 24 Bytes   //
    String command = "55AA";                              // PREFIX + command
    command += tools.byteToHex((uint8_t)(cmd & 0x00ff));  // LSB
    command += tools.byteToHex((uint8_t)(cmd >> 8));      // MSB
    command += tools.byteToHex((uint8_t)(len & 0x00ff));  // LSB
    command += tools.byteToHex((uint8_t)(len >> 8));      // MSB
    command += tools.byteToHex((uint8_t)(slot & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(slot >> 8));     // MSB                                   // data
    while (command.length() < COMMAND_DATA_LEN)
    {
        command += "0";
    }
    uint16_t cks = tools.calculateCKS(command);
    command += tools.byteToHex((uint8_t)(cks & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cks >> 8));     // MSB
    return command;
}

/**
 * @brief Simple command with len 0
 *
 * @param cmd
 * @return String
 */
String SensorSM30Controller::getCommand(uint16_t cmd)
{
    // Total length of the command packet is 24 Bytes
    uint16_t len = 0;                                    //
    String command = "55AA";                             // PREFIX + command
    command += tools.byteToHex((uint8_t)(cmd & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cmd >> 8));     // MSB
    command += tools.byteToHex((uint8_t)(len & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(len >> 8));     // MSB
    while (command.length() < COMMAND_DATA_LEN)
    {
        command += "0";
    }
    uint16_t cks = tools.calculateCKS(command);
    command += tools.byteToHex((uint8_t)(cks & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cks >> 8));     // MSB
    return command;
}

/**
 * @brief Set duplication parameter
 *
 * @param duplication
 */
void SensorSM30Controller::setDuplicationCheck(bool duplication)
{
    String data = "00";
    if (duplication)
        data = "01";
    String cmd = getCommand(SM30_CMD_SET_DUPLICATION_CHECK, (uint16_t)2, data);
    Serial << cmd << endl;

    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    sensorSerial->flush();
    readSerialPort();
}

/**
 * @brief Set security parameter
 *
 * @param securityLevel
 */
void SensorSM30Controller::setSecurityLevel(uint8_t securityLevel)
{
    String data = "0";
    data += securityLevel; // from 1 to 5
    String cmd = getCommand(SM30_CMD_SET_SECURITY_LEVEL, (uint16_t)2, data);
    // Serial << cmd << endl;

    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    sensorSerial->flush();
    readSerialPort();
    Serial.println();
}

/**
 * Command for getting data creation
 * @param cmd command
 * @param len length
 * @param slot_h slot MSB
 * @param slot_l slot LSB
 * @return
 */
String SensorSM30Controller::getData(uint16_t cmd, uint16_t len, String data)
{
    String command = "5AA5";                             // PREFIX
    command += tools.byteToHex((uint8_t)(cmd & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cmd >> 8));     // MSB
    command += tools.byteToHex((uint8_t)(len & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(len >> 8));     // MSB
    command += data;                                     // data
    uint16_t cks = tools.calculateCKS(command);
    command += tools.byteToHex((uint8_t)(cks & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(cks >> 8));     // MSB
    return command;
}

/**
 * @brief Check CRC
 *
 * @param buffer
 * @param len
 * @return true
 * @return false
 */
bool SensorSM30Controller::isGoodCRC(uint8_t *buffer, uint16_t len)
{
    if ((buffer[0] + buffer[1]) != 0xFF)
    {
        return false;
    }
    uint16_t sum = 0;

    // without crc and additional 2 bytes
    //  Lenght -1 to start from 0 and 2 bytes crc
    //   buffer len is 2 bites longer to
    for (uint16_t i = 0; i < len - 3; i++)
    {
        sum += buffer[i];
    }

    if (sum >> 8 == buffer[len - 1] && sum % 256 == buffer[len - 2])
    {
        return true;
    }
    return false;
}

/**
 * @brief Get finget tineout
 *
 * @return uint16_t
 */
uint16_t SensorSM30Controller::getFingerTimeout()
{
    String cmd = getCommand(SM30_CMD_GET_FINGER_TIMEOUT);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    readSerialPort();
    // tools.dump_byte_array(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    // Serial.println();
    uint16_t fnd = inbuffer[9];
    fnd <<= 8;
    fnd += inbuffer[8];
    Serial << "Timeout:" << fnd << endl;
    return fnd;
}

/**
 * @brief Set finger timeout
 *
 * @param time
 * @return true
 * @return false
 */
bool SensorSM30Controller::setFingerTimeout(uint8_t time)
{
    String cmd = getCommand(SM30_CMD_SET_FINGER_TIMEOUT);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    readSerialPort();
    // tools.dump_byte_array(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    // Serial.println();
    return true;
}

/**
 * @brief Enroll
 *
 * @return true
 * @return false
 */
bool SensorSM30Controller::enroll(uint16_t slot)
{
    String cmd = getCommand(SM30_CMD_ENROLL, 2, slot);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    for (uint8_t i = 0; i < 10; i++)
    {
        /* code */
        if (!readSerialPort())
        {
            return false;
        }
        tools.dump_byte_array(inbuffer, SM30_REGULAR_PACKET_LENGTH);
        Serial.println();
        switch (inbuffer[8])
        {
        case 0xF1:
            Serial << "Need first sweep" << endl;
            break;
        case 0xF4:
            Serial << "Lift finger" << endl;
            break;
        case 0xF2:
            Serial << "Need seccond sweep" << endl;
            break;
        case 0xF3:
            Serial << "Need third sweep" << endl;
            break;
        default:
            break;
        }
        if (inbuffer[4] == 0x06)
        {
            Serial << "End enroll" << endl;
            break;
        }
    }
    return true;
}

/**
 * @brief Delete all templates
 *
 * @param time
 * @return true
 * @return false
 */
bool SensorSM30Controller::deleteAllTemplates()
{
    String cmd = getCommand(SM30_CMD_CLEAR_ALL_TEMPLATES);
    tools.convertHexStringToByteArray(outbuffer, SM30_REGULAR_PACKET_LENGTH, cmd);
    sensorSerial->write(outbuffer, SM30_REGULAR_PACKET_LENGTH);
    readSerialPort();
    // tools.dump_byte_array(inbuffer, SM30_REGULAR_PACKET_LENGTH);
    // Serial.println();
    if (inbuffer[8] == 1)
    {
        Serial << "Err in deleting all templates" << endl;
        return false;
    }
    Serial << "All templates deleted" << endl;
    return true;
}

#endif