#ifndef CAMA
#define CAMA

#include "SensorController.h"

HardwareSerial SensorSerial(2);

SensorController::SensorController()
{
}

/**
 * @brief CHECK SENSOR
 *
 */
void SensorController::camaTest()
{
    String cmd = getCommand(CMD_TEST_CONNECTION, 0);
    Serial.println("TEST");
    Serial.println(cmd);

    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    Serial.println();
}

/**
 * @brief FINGER DETECT
 *
 * @return true
 * @return false
 */
bool SensorController::detectFinger()
{
    String cmd = getCommand(CMD_FINGER_DETECT, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    // tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    // Serial << "Bad scanned finger:" << inbuffer[10] << endl;
    if (inbuffer[10] == 1)
        return true;
    return false;
}

/**
 * @brief Used templates
 *
 * @return uint16_t
 */
uint16_t SensorController::templatesCount()
{
    String cmd = getCommand(CMD_GET_ENROLL_COUNT, 4, 1, MAX_TEMPLATES);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    // tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    // Serial.println();
    return inbuffer[11] * 256 + inbuffer[10];
}

/**
 * @brief Merge template
 *
 * @param ram_buffer Where to store merged template
 * @param count 2 or 3 templates to be merged.
 * If 2 : Merge RamBuffer0 and RamBuffer1 .
 * If 3 : Merge RamBuffer0 and RamBuffer1 and RamBuffer2 .
 * @return true
 * @return false
 */
bool SensorController::mergeTemplate(uint16_t ram_buffer, uint16_t count)
{
    String cmd = getCommand(CMD_MERGE, 3, ram_buffer, count);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Problem generating" << endl;
        return false;
    }
    return true;
}

// send buffer to HOST - CMD_UP_CHAR
/*
Before use this command, first you must use one of following command:
CMD_GENERATE, CMD_DOWN_CHAR, CMD_LOAD_CHAR to save an template data to the Ram Buffer.
*/

bool SensorController::saveTemplate(uint16_t slot, String template_chars, uint16_t template_len)
{

    // 1. Send command  CMD_DOWN_CHAR
    //  55 AA 00 00 43 00 02 00 F4 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 39 02
    Serial << "save template" << endl;
    String cmd = getCommand(CMD_DOWN_CHAR, 2, 0x01F4, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Problem in CMD_DOWN_CHAR command:" << inbuffer[8] << endl;
        return false;
    }
    Serial.println();

    // 2. save template to ram 0 buffer command
    //  5A A5 00 00 43 00 00 43 00 F4 01 00 00 xx xx ...... no - get template data
    Serial << "Template:" << template_chars << endl;      // ok
    cmd = getData(CMD_DOWN_CHAR, 0x01F4, template_chars); // parameters depends sensor template size
    Serial << "Command:" << cmd << endl;                  // OK
    tools.convertHexStringToByteArray(template_buffer, TEMPLATE_DATA_PACKET_LENGTH, cmd);
    tools.dump_byte_array(template_buffer, TEMPLATE_DATA_PACKET_LENGTH);
    SensorSerial.write(template_buffer, TEMPLATE_DATA_PACKET_LENGTH);
    Serial.println();
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH); // TODO returned short message!
    tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    Serial.println();
    if (inbuffer[8] != 0)
    {
        Serial << "Problem in CMD_DOWN_CHAR data:" << inbuffer[8] << endl;
        return false;
    }

    // 2. Store template to sensor CMD_STORE_CHAR
    cmd = getCommand(CMD_STORE_CHAR, 4, slot, 0); // Buffer 0
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    Serial << "store " << endl;
    tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    Serial.println();
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    Serial.println();
    if (inbuffer[8] != 0)
    {
        Serial << "Problem in CMD_STORE_CHAR command:" << inbuffer[8] << endl;
        return false;
    }
    return true;
}

/**
 * @brief Delete template
 *
 * @param slot
 * @return true
 * @return false
 */
bool SensorController::deleteTemplate(uint16_t slot)
{
    String cmd = getCommand(CMD_DEL_CHAR, 4, slot, slot, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Delete " << slot << endl;
        return false;
    }
    return true;
}

/**
 * @brief Delete all templates
 *
 * @return true
 * @return false
 */
bool SensorController::deleteAllTemplates()
{
    String cmd = getCommand(CMD_DEL_CHAR, 4, 1, 0x07D0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    Serial.println();
    // for(uint8_t i=0;i<REGULAR_PACKET_LENGTH;i++)
    // {
    //     inbuffer[i]=0;
    // }
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    Serial.println();
    if (inbuffer[8] != 0)
    {
        Serial << "Deleting error:" << inbuffer[8] << endl;
        if (inbuffer[8] == 0x12)
        {
            Serial << "No enrolled templates" << endl;
        }
        return false;
    }
    Serial << "All templates deleted" << endl;
    return true;
}

/**
 * @brief Get image
 *
 * @return true
 * @return false
 */
bool SensorController::getImage()
{
    String cmd = getCommand(CMD_GET_IMAGE, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Bad scanned image" << endl;
        return false;
    }
    return true;
}
/**
 * @brief Generating template
 *
 * @param ram_buffer
 * @return true
 * @return false
 */
bool SensorController::generateTemplate(uint16_t ram_buffer)
{
    String cmd = getCommand(CMD_GENERATE, 2, ram_buffer, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Problem generating" << endl;
        return false;
    }
    return true;
}

/**
 * @brief Identify finger after detection
 *
 * @return uint16_t
 */
uint16_t SensorController::identify()
{
    // 1. Get image (Capture fingerprint image) CMD_GET_IMAGE
    String cmd = getCommand(CMD_GET_IMAGE, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Bad scanned finger:" << inbuffer[8] << endl;
        return 0;
    }

    // 2. Generate template CMD_GENERATE
    cmd = getCommand(CMD_GENERATE, 2, 0, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[8] != 0)
    {
        Serial << "Problem generating" << endl;
        return 0;
    }

    // 3. Search CMD_SEARCH
    cmd = getCommand(CMD_SEARCH, 6, 1, MAX_TEMPLATES, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    // tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    //  Serial.println();
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    if (inbuffer[6] != 5)
    {
        Serial << "Unknown user" << endl;
        // tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
        // Serial.println();
        return 0;
    }
    // 10, 11, 12 - response
    return inbuffer[11] * 256 + inbuffer[10];
}

/**
 * Command creation
 * @param cmd command
 * @param len length
 * @param slot_h slot MSB
 * @param slot_l slot LSB
 * @return
 */
String SensorController::getCommand(uint16_t cmd, uint16_t len)
{
    String command = "55AA";                             // PREFIX
    command += "00";                                     // SID
    command += "00";                                     // DID
    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 += "00000000000000000000000000000000";       // 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;
}

/**
 * Command creation
 * @param cmd command
 * @param len length
 * @param slot_h slot MSB
 * @param slot_l slot LSB
 * @return
 */
String SensorController::getCommand(uint16_t cmd, uint16_t len, uint16_t slot_h, uint16_t slot_l)
{
    String command = "55AA";                                // PREFIX
    command += "00";                                        // SID
    command += "00";                                        // DID
    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_h & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(slot_h >> 8));     // MSB
    command += tools.byteToHex((uint8_t)(slot_l & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(slot_l >> 8));     // MSB
    command += "000000000000000000000000";                  // 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;
}

/**
 * Command creation
 * @param cmd command
 * @param len length
 * @param slot_h slot MSB
 * @param slot_l slot LSB
 * @return
 */
String SensorController::getCommand(uint16_t cmd, uint16_t len, uint16_t slot_h, uint16_t slot_l, uint16_t buffer_id)
{
    String command = "55AA";                                   // PREFIX
    command += "00";                                           // SID
    command += "00";                                           // DID
    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)(buffer_id & 0x00ff)); // LSB
    command += tools.byteToHex((uint8_t)(buffer_id >> 8));     // MSB
    command += tools.byteToHex((uint8_t)(slot_h & 0x00ff));    // LSB
    command += tools.byteToHex((uint8_t)(slot_h >> 8));        // MSB
    command += tools.byteToHex((uint8_t)(slot_l & 0x00ff));    // LSB
    command += tools.byteToHex((uint8_t)(slot_l >> 8));        // MSB
    command += "00000000000000000000";                         // 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 Set duplication parameter
 *
 * @param duplication
 */
void SensorController::setDuplicationCheck(bool duplication)
{
    uint16_t param = 0x0002;
    if (duplication)
    {
        param += 0x0100;
    }
    String cmd = getCommand(CMD_SET_PARAM, 5, param, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    // tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    // Serial.println();
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    // tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    // Serial.println();
}

/**
 * @brief Set security parameter
 *
 * @param securityLevel
 */
void SensorController::setSecurityLevel(uint8_t securityLevel)
{
    uint16_t level = 0x0100 * securityLevel + 0x0001;
    String cmd = getCommand(CMD_SET_PARAM, 5, level, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    // tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    // Serial.println();
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    // tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    // Serial.println();
}

/**
 * @brief Set learn parameter
 *
 * @param learn
 */
void SensorController::setAutoLearn(bool learn)
{
    uint16_t autolearn = 0x0004;
    if (learn)
    {
        autolearn += 0x0100;
    }
    String cmd = getCommand(CMD_SET_PARAM, 5, autolearn, 0);
    tools.convertHexStringToByteArray(outbuffer, REGULAR_PACKET_LENGTH, cmd);
    // tools.dump_byte_array(outbuffer, REGULAR_PACKET_LENGTH);
    // Serial.println();
    SensorSerial.write(outbuffer, REGULAR_PACKET_LENGTH);
    SensorSerial.readBytes(inbuffer, REGULAR_PACKET_LENGTH);
    // tools.dump_byte_array(inbuffer, REGULAR_PACKET_LENGTH);
    // 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 SensorController::getData(uint16_t cmd, uint16_t len, String data)
{
    // data = "04105F00EAD2F5CFCA5949EE45C3F6D1D61A8DE36A53F80E092B85E0C66BF6A3C79A4FE5C29BF718C8AB3AC80E84F88C421B48EFA574878D04B945FFEE84868C04FB47F8EA1C878103F8C9F8E64C888483FC45F8BD4AF4454648F417EECC888204FD83F856BA890FC5977FFFDE54898104FCC5F80DD2E7EEC59643DFD60146DAC3B903F8000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000044626523F237738A2385542F433531F317211AF4FC52000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000005854";
    String command = "5AA5";                             // PREFIX
    command += "00";                                     // SID
    command += "00";                                     // DID
    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 += "00";                                     // buffer 0
    command += "00";                                     // buffer 0
    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;
}

#endif