/*
 *****************************************************************************
 *                                                                           *
 *                 IMPINJ CONFIDENTIAL AND PROPRIETARY                       *
 *                                                                           *
 * This source code is the sole property of Impinj, Inc.  Reproduction or    *
 * utilization of this source code in whole or in part is forbidden without  *
 * the prior written consent of Impinj, Inc.                                 *
 *                                                                           *
 * (c) Copyright Impinj, Inc. 2009. All rights reserved.                     *
 *                                                                           *
 *****************************************************************************
 */

/*
 *****************************************************************************
 *
 * $Id: blockwrite.c 61529 2009-12-22 21:12:04Z dshaheen $
 * 
 * Description: 
 *   This sample application demonstrates how to configure the radio to use 
 *   only the first antenna, configure and select the Fixed Q singulation 
 *   algorithm, perform a tag access read and a tag access blockwrite.  The 
 *   Tag EPC data is read starting at offset 2 in the EPC memory bank (ie, 
 *   skipping the PC and CRC), for the number of words specified in the 
 *   command argument, then the data is inverted and written back to the tag
 *   EPC data using the tag access blockwrite.  The data is then read back
 *   from the tag EPC once again, to confirm the inverted data made it down
 *   to the tag.
 *   
 *   The functional flow is as follows:
 *     - Start up the RFID Reader Library 
 *     - Enumerate the attached radios
 *     - Open the first enumerated radio and
 *       - Retrieve the current antenna status and configuration for each 
 *         of the enabled antennas
 *       - Disable all but the first antenna port
 *       - Configure the first antenna port for tag access performance
 *       - Retrieve the current singulation algorithm and fixedQ singulation 
 *         algorithm settings
 *       - Configure the fixedQ algorithm settings for tag access performance
 *       - Select the fixdeQ singulation algorithm (not really required)
 *       - Read the EPC data starting at offset 2 in the EPC memory bank
 *         (ie, skipping the PC and CRC) for the number of words specified 
 *         on the command line
 *       - Invert the EPC data and write it back to the tag with blockwrite 
 *         access command
 *       - Read the EPC data back again and confirm the inverted data made it
 *         to the tag
 *       - Restore the antenna states, the first antenna configuration, the 
 *         fixedQ singulation algorithm settings, and current singulation 
 *         algorithm to their original values
 *     - Close the radio
 *     - Shut down the library
 *
 * Parameters:
 *   A single argument specifies the number of consecutive words to write
 *   to the tag for the blockwrite access operation.
 *
 * NOTE - for this sample to work, it is best if a single tag is present in
 *        the field of view of the radio.
 *
 *****************************************************************************
 */

#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include "rfid_library.h"
#include "sample_utility.h"

#define EPC_START_OFFSET    2
#define EPC_WORD_LENGTH     6

int main(
    int     argc,
    char**  argv
    )
{
    RFID_STATUS                             status;
    RFID_RADIO_ENUM*                        pEnum;
    RFID_RADIO_HANDLE                       handle;
    INT8U                                   readData[EPC_WORD_LENGTH * 2];
    INT16U                                  writeData[EPC_WORD_LENGTH];
    RFID_18K6C_READ_PARMS                   readParms;
    RFID_18K6C_BLOCK_WRITE_PARMS            blockwriteParms;
    CONTEXT_PARMS                           context;
    INT32U                                  index;
    INT32U                                  antenna;
    RFID_ANTENNA_PORT_CONFIG                antennaConfig;
    RFID_18K6C_SINGULATION_FIXEDQ_PARMS     singulationParms;
    RFID_18K6C_SINGULATION_ALGORITHM        originalSingulationAlgorithm;
    RFID_18K6C_SINGULATION_FIXEDQ_PARMS     originalFixedQSettings;
    RFID_ANTENNA_PORT_CONFIG                originalAntennaConfig;
    RFID_ANTENNA_PORT_STATUS                originalAntennaState[64]; 
    INT8U                                   maxOriginalAntennas = 0;
    INT16U                                  epcWord;
    int                                     tagConsecutiveWordLimit = 1;
    INT8U                                   maxAccessAPIRetries = 6;
    INT8U                                   accessAPIRetryCount = 0;

    if (argc != 2)
    {
        fprintf(
            stderr, 
            "Must specify consecutive words to write:  blockwrite <n>\n");
        goto EXIT;
    }

    tagConsecutiveWordLimit = atoi( argv[1]);

    /* Initialize the RFID library                                         */
    status = RFID_Startup(NULL, 0);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr, 
            "ERROR: RFID_Startup returned %d\n", 
            status);
        goto EXIT;
    }

    /* Create an initial structure for enumerating the radios.  We'll adjust  */
    /* it once we know how big to make it.                                    */
    pEnum = (RFID_RADIO_ENUM *) malloc(sizeof(RFID_RADIO_ENUM));
    if (NULL == pEnum)
    {
        fprintf(
            stderr, 
            "ERROR: Failed to allocate memory\n");
        goto SHUTDOWN_LIBRARY;
    }
    pEnum->length       =
        pEnum->totalLength  = sizeof(RFID_RADIO_ENUM);

    /* Enumerate the radios                                                   */
    while (RFID_ERROR_BUFFER_TOO_SMALL == 
        (status = RFID_RetrieveAttachedRadiosList(pEnum, 0)))
    {
        RFID_RADIO_ENUM* pNewEnum = 
            (RFID_RADIO_ENUM *) realloc(pEnum, pEnum->totalLength);
        if (NULL == pNewEnum)
        {
            fprintf(
                stderr, 
                "ERROR: Failed to allocate memory\n");
            goto CLEANUP_ENUM;
        }
        pEnum = pNewEnum;
    }
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_RetrieveAttachedRadiosList returned %d\n",
            status);
        goto CLEANUP_ENUM;
    }

    /* Open up the first radio - if more than one radio is attached, so be it */
    if (!pEnum->countRadios)
    {
        fprintf(
            stderr, 
            "ERROR: No radios attached to the system\n");
        goto CLEANUP_ENUM;
    }
    status = RFID_RadioOpen(pEnum->ppRadioInfo[0]->cookie, &handle, 0);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr, 
            "ERROR: RFID_RadioOpen returned %d\n", 
            status);
        goto CLEANUP_ENUM;
    }

    /* Make sure the first antenna is enabled and the rest are disabled      */
    for (antenna = 0; RFID_STATUS_OK == status; ++antenna)
    {
        /* save original state, so it can be restored at the end */
        status = RFID_AntennaPortGetStatus(
                        handle,
                        antenna,
                        &originalAntennaState[antenna]);
        if (RFID_STATUS_OK != status)
        {
            break;
        }

        status =  RFID_AntennaPortSetState(
                        handle,
                        antenna,
                        (antenna ? RFID_ANTENNA_PORT_STATE_DISABLED :  
                                   RFID_ANTENNA_PORT_STATE_ENABLED));
    }

    /* We broke out of loop after RFID_AntennaPortGetState/SetState returned a */
    /* non ok status.  We exepect an invalid parameter error when we set state */
    /* for an invalid antenna port, which will happen when the loop exhausts   */
    /* the set of valid antennas.  Any other error is unexpected and is        */
    /* reported.                                                               */
    if (RFID_ERROR_INVALID_PARAMETER != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_AntennaPortGetState/SetState returned %d\n",
            status);
        goto CLOSE_RADIO;
    }

    /* Store the numbe of antennas, for use when we restore the state at the   */
    /* end                                                                     */
    maxOriginalAntennas = antenna;

    /* Set up the first antenna for a single inventory cycle. Init the         */
    /* the antennaConfig structure with the current settings from the radio    */
    /* Retain the original antenna settings, so they can be restored when we   */
    /* are done                                                                */
    originalAntennaConfig.length = sizeof(RFID_ANTENNA_PORT_CONFIG);
    status = RFID_AntennaPortGetConfiguration(handle, 0, &originalAntennaConfig);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_AntennaPortGetConfiguration returned %d\n",
            status);
        goto CLOSE_RADIO;
    }
    antennaConfig  = originalAntennaConfig;
    antennaConfig.numberInventoryCycles = 1;
    status = RFID_AntennaPortSetConfiguration(handle, 0, &antennaConfig);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_AntennaPortSetConfiguration returned %d\n",
            status);
        goto CLOSE_RADIO;
    }

    /* First get the original fixedq singulation parameters and the current   */
    /* singulation algorithm selection, so they can be restored when we are   */
    /* done                                                                   */
    originalFixedQSettings.length = sizeof(RFID_18K6C_SINGULATION_FIXEDQ_PARMS);
    status = RFID_18K6CGetSingulationAlgorithmParameters(
                 handle,
                 RFID_18K6C_SINGULATION_ALGORITHM_FIXEDQ,
                 &originalFixedQSettings);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_18K6CGetSingulationAlgorithmParameters returned %d\n",
            status);
        goto CLOSE_RADIO;
    }
    status = RFID_18K6CGetCurrentSingulationAlgorithm(
                 handle,
                 &originalSingulationAlgorithm);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_18K6CGetCurrentSingulationAlgorithm returned %d\n",
            status);
        goto CLOSE_RADIO;
    }

    /* The MAC uses the fixed-Q singulation algorithm for tag read and write, */
    /* so we'll configure the algorithm so that it works well for a single    */
    /* tag and even though it is not necessary, set the singulation algorithm */
    /* to fixed Q.                                                            */
    singulationParms.length             = 
        sizeof(RFID_18K6C_SINGULATION_FIXEDQ_PARMS);
    singulationParms.qValue             = 0;
    singulationParms.retryCount         = 0;
    singulationParms.toggleTarget       = 0;
    singulationParms.repeatUntilNoTags  = 0;
    status = RFID_18K6CSetSingulationAlgorithmParameters(
                 handle,
                 RFID_18K6C_SINGULATION_ALGORITHM_FIXEDQ,
                 &singulationParms);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_18K6CSetSingulationAlgorithmParameters returned %d\n",
            status);
        goto CLOSE_RADIO;
    }
    status = RFID_18K6CSetCurrentSingulationAlgorithm(
                 handle,
                 RFID_18K6C_SINGULATION_ALGORITHM_FIXEDQ);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: RFID_18K6CSetCurrentSingulationAlgorithm returned %d\n",
            status);
        goto CLOSE_RADIO;
    }

    /* Setup tag access read params to read the requested number of words    */
    /* of the EPC.  We'll use this data to invert and blockwrite it back     */    
    context.succesfulAccessPackets  = 0;
    context.pReadData               = readData;
    readParms.length                = sizeof(readParms);
    readParms.readCmdParms.length   = sizeof(readParms.readCmdParms);
    readParms.readCmdParms.bank     = RFID_18K6C_MEMORY_BANK_EPC;
    readParms.readCmdParms.count    = (INT16U) tagConsecutiveWordLimit;
    readParms.readCmdParms.offset   = EPC_START_OFFSET;
    readParms.accessPassword        = 0;
    readParms.common.pCallback      = RfidTagAccessCallback;
    readParms.common.pCallbackCode  = NULL;
    readParms.common.tagStopCount   = 0;
    readParms.common.context        = &context;

    /* Keep attempting to read the EPC from the tag's EPC memory until it     */
    /* succeeds or until the tag-read function fails for some reason.         */
    accessAPIRetryCount = 0;
    while ((RFID_STATUS_OK == status) && 
           !context.succesfulAccessPackets &&
           (accessAPIRetryCount < maxAccessAPIRetries))
    {
        printf( "Attempting to read EPC\n\n");

        status = RFID_18K6CTagRead(handle, &readParms, 0);
        if (RFID_STATUS_OK != status)
        {
            fprintf(
                stderr,
                "ERROR: RFID_18K6CTagRead returned %d\n",
                status);
            goto CLOSE_RADIO;
        }
        RFID_MacClearError(handle);
        accessAPIRetryCount++;
    }

    if (!context.succesfulAccessPackets)
    {
        printf("Tag access read failed\n");
        goto RESTORE;
    }

    printf( "Read EPC Data=");

    /* Flip all of the EPC bits so we have something to write back that is    */
    /* different from the tag's current EPC.                                  */
    for (index = 0; index < (INT32U)tagConsecutiveWordLimit; ++index)
    {
        epcWord = (((INT16U) readData[index * 2]) << 8) | readData[(index * 2) + 1];
        printf( "%04X ", epcWord);
        writeData[index] = ~epcWord;
    }

    printf( "\n\n");

    context.succesfulAccessPackets               = 0;
    blockwriteParms.length                       = sizeof(blockwriteParms);
    blockwriteParms.blockWriteCmdParms.length    = sizeof(blockwriteParms.blockWriteCmdParms);
    blockwriteParms.blockWriteCmdParms.bank      = RFID_18K6C_MEMORY_BANK_EPC;
    blockwriteParms.blockWriteCmdParms.count     = (INT16U) tagConsecutiveWordLimit;
    blockwriteParms.blockWriteCmdParms.offset    = EPC_START_OFFSET;
    blockwriteParms.blockWriteCmdParms.pData     = writeData;
    blockwriteParms.accessPassword               = 0;
    blockwriteParms.common.pCallback             = RfidTagAccessCallback;
    blockwriteParms.common.pCallbackCode         = NULL;
    blockwriteParms.common.tagStopCount          = 0;
    blockwriteParms.common.context               = &context;

    /* Keep attempting to write the EPC to the tag's EPC memory until it           */
    /* succeeds or until the tag-blockwrite function fails for some reason.        */
    accessAPIRetryCount = 0;
    while ((RFID_STATUS_OK == status) && 
           !context.succesfulAccessPackets &&
           (accessAPIRetryCount < maxAccessAPIRetries))
    {
        printf( "Attempting to blockwrite EPC\n\n");

        status = RFID_18K6CTagBlockWrite(handle, &blockwriteParms, 0);
        if (RFID_STATUS_OK != status)
        {
            fprintf(
                stderr,
                "ERROR: RFID_18K6CTagBlockWrite returned %d\n",
                status);
            goto CLOSE_RADIO;
        }
        RFID_MacClearError(handle);
        accessAPIRetryCount++;
    }

    if (!context.succesfulAccessPackets)
    {
        printf("Tag access blockwrite failed\n");
        goto RESTORE;
    }

    context.succesfulAccessPackets = 0;

    /* Now read back the data we just wrote, so we can confirm it matches     */
    /* what we wrote.  Keep attempting to read the EPC from the tag's EPC     */
    /* memory until it succeeds or until the tag-read function fails for some */
    /* reason.                                                                */
    accessAPIRetryCount = 0;
    while ((RFID_STATUS_OK == status) && 
           !context.succesfulAccessPackets &&
           (accessAPIRetryCount < maxAccessAPIRetries))
    {
        status = RFID_18K6CTagRead(handle, &readParms, 0);
        if (RFID_STATUS_OK != status)
        {
            fprintf(
                stderr,
                "ERROR: RFID_18K6CTagRead returned %d\n",
                status);
            goto CLOSE_RADIO;
        }

        RFID_MacClearError(handle);
        accessAPIRetryCount++;
    }

    if (!context.succesfulAccessPackets)
    {
        printf("Tag access post blockwrite read failed\n");
        goto RESTORE;
    }

    printf( "Read EPC data (after being inverted)\n\n");
    printf( "Read EPC Data=");

    /* Verify that the EPC read back matches the EPC that was just written    */
    for (index = 0; index < (INT32U)tagConsecutiveWordLimit; ++index)
    {
        INT16U readWord = 
            (((INT16U) readData[index * 2]) << 8) | readData[(index * 2) +1];        

        printf( "%04X ", readWord);
        if (writeData[index] != readWord)
        {
            fprintf(
                stderr,
                "ERROR: EPC word %d (0x%.4x) doesn't match written (0x%.4x)\n",
                index + 1, readWord, writeData[index]);
            goto RESTORE;
        }
    }
RESTORE:

    printf( "\n\n");

    /* Restore original antenna port configuration for the first antenna*/
    status = RFID_AntennaPortSetConfiguration(handle, 0, &originalAntennaConfig);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: Restoring RFID_AntennaPortSetConfiguration returned %d\n",
            status);
        goto CLOSE_RADIO;
    }
    /* Restore original antenna state enables */
    for (antenna = 0; antenna < maxOriginalAntennas; antenna++)
    {
        status =  RFID_AntennaPortSetState(
                        handle,
                        antenna,
                        originalAntennaState[antenna].state);
    }
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: Restoring RFID_AntennaPortSetState returned %d\n",
            status);
        goto CLOSE_RADIO;
    }
    /* Restore original fixedQ algorithm settings */
    status = RFID_18K6CSetSingulationAlgorithmParameters(
                 handle,
                 RFID_18K6C_SINGULATION_ALGORITHM_FIXEDQ,
                 &originalFixedQSettings);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: Restoring RFID_18K6CSetSingulationAlgorithmParameters returned %d\n",
            status);
        goto CLOSE_RADIO;
    }
    /* Restore original current singulation algorithm selection */
    status = RFID_18K6CSetCurrentSingulationAlgorithm(
                 handle,
                 originalSingulationAlgorithm);
    if (RFID_STATUS_OK != status)
    {
        fprintf(
            stderr,
            "ERROR: Restoring RFID_18K6CSetCurrentSingulationAlgorithm returned %d\n",
            status);
        goto CLOSE_RADIO;
    }

CLOSE_RADIO:
    RFID_RadioClose(handle);

CLEANUP_ENUM:
    free(pEnum);

SHUTDOWN_LIBRARY:
    RFID_Shutdown();

EXIT:
    return 0;
} /* main */

