Dashboard.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                                                         */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "Dashboard.h"
#include "DriverStation.h"
#include "Synchronized.h"
#include "WPIErrors.h"
#include <strLib.h>

const INT32 Dashboard::kMaxDashboardDataSize;

Dashboard::Dashboard(SEM_ID statusDataSem)
        : m_userStatusData (NULL)
        , m_userStatusDataSize (0)
        , m_localBuffer (NULL)
        , m_localPrintBuffer (NULL)
        , m_packPtr (NULL)
        , m_printSemaphore (0)
        , m_statusDataSemaphore (statusDataSem)
{
        m_userStatusData = new char[kMaxDashboardDataSize];
        m_localBuffer = new char[kMaxDashboardDataSize];
        m_localPrintBuffer = new char[kMaxDashboardDataSize * 2];
        m_localPrintBuffer[0] = 0;
        m_packPtr = m_localBuffer;
        m_printSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
}

Dashboard::~Dashboard()
{
        semDelete(m_printSemaphore);
        m_packPtr = NULL;
        delete [] m_localPrintBuffer;
        m_localPrintBuffer = NULL;
        delete [] m_localBuffer;
        m_localBuffer = NULL;
        delete [] m_userStatusData;
        m_userStatusData = NULL;
}

void Dashboard::AddI8(INT8 value)
{
        if (!ValidateAdd(sizeof(INT8))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kI8);
}

void Dashboard::AddI16(INT16 value)
{
        if (!ValidateAdd(sizeof(INT16))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kI16);
}

void Dashboard::AddI32(INT32 value)
{
        if (!ValidateAdd(sizeof(INT32))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kI32);
}

void Dashboard::AddU8(UINT8 value)
{
        if (!ValidateAdd(sizeof(UINT8))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kU8);
}

void Dashboard::AddU16(UINT16 value)
{
        if (!ValidateAdd(sizeof(UINT16))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kU16);
}

void Dashboard::AddU32(UINT32 value)
{
        if (!ValidateAdd(sizeof(UINT32))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kU32);
}

void Dashboard::AddFloat(float value)
{
        if (!ValidateAdd(sizeof(float))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kFloat);
}

void Dashboard::AddDouble(double value)
{
        if (!ValidateAdd(sizeof(double))) return;
        memcpy(m_packPtr, (char*)&value, sizeof(value));
        m_packPtr += sizeof(value);
        AddedElement(kDouble);
}

void Dashboard::AddBoolean(bool value)
{
        if (!ValidateAdd(sizeof(char))) return;
        *m_packPtr = value ? 1 : 0;
        m_packPtr += sizeof(char);      
        AddedElement(kBoolean);
}

void Dashboard::AddString(char* value)
{
        AddString(value, strlen(value));
}

void Dashboard::AddString(char* value, INT32 length)
{
        if (!ValidateAdd(length + sizeof(length))) return;
        memcpy(m_packPtr, (char*)&length, sizeof(length));
        m_packPtr += sizeof(length);
        memcpy(m_packPtr, (char*)&value, length);
        m_packPtr += length;
        AddedElement(kString);
}

void Dashboard::AddArray()
{
        if (!ValidateAdd(sizeof(INT32))) return;
        m_complexTypeStack.push(kArray);
        m_arrayElementCount.push_back(0);
        m_arraySizePtr.push_back((INT32*)m_packPtr);
        m_packPtr += sizeof(INT32);
}

void Dashboard::FinalizeArray()
{
        if (m_complexTypeStack.top() != kArray)
        {
                wpi_setWPIError(MismatchedComplexTypeClose);
                return;
        }
        m_complexTypeStack.pop();
        *(m_arraySizePtr.back()) = m_arrayElementCount.back();
        m_arraySizePtr.pop_back();
        if (m_arrayElementCount.back() != 0)
        {
                m_expectedArrayElementType.pop_back();
        }
        m_arrayElementCount.pop_back();
        AddedElement(kOther);
}

void Dashboard::AddCluster()
{
        m_complexTypeStack.push(kCluster);
}

void Dashboard::FinalizeCluster()
{
        if (m_complexTypeStack.top() != kCluster)
        {
                wpi_setWPIError(MismatchedComplexTypeClose);
                return;
        }
        m_complexTypeStack.pop();
        AddedElement(kOther);
}

void Dashboard::Printf(const char *writeFmt, ...)
{
        va_list args;
        INT32 size;

        // Check if the buffer has already been used for packing.
        if (m_packPtr != m_localBuffer)
        {
                wpi_setWPIError(DashboardDataCollision);
                return;
        }
        va_start (args, writeFmt);
        {
                Synchronized sync(m_printSemaphore);
                vsprintf(m_localPrintBuffer + strlen(m_localPrintBuffer), writeFmt, args);
                size = strlen(m_localPrintBuffer);
        }
        if (size > kMaxDashboardDataSize)
        {
                wpi_setWPIError(DashboardDataOverflow);
        }

        va_end (args);
}

INT32 Dashboard::Finalize()
{
        if (!m_complexTypeStack.empty())
        {
                wpi_setWPIError(MismatchedComplexTypeClose);
                return 0;
        }

        Synchronized sync(m_statusDataSemaphore);

        // Sequence number
        DriverStation::GetInstance()->IncrementUpdateNumber();

        // Packed Dashboard Data
        m_userStatusDataSize = m_packPtr - m_localBuffer;
        memcpy(m_userStatusData, m_localBuffer, m_userStatusDataSize);
        m_packPtr = m_localBuffer;

        return m_userStatusDataSize;
}

void Dashboard::GetStatusBuffer(char **userStatusData, INT32* userStatusDataSize)
{
        // User printed strings
        if (m_localPrintBuffer[0] != 0)
        {
                // Sequence number
                DriverStation::GetInstance()->IncrementUpdateNumber();

                INT32 printSize;
                Synchronized syncPrint(m_printSemaphore);
                printSize = strlen(m_localPrintBuffer);
                m_userStatusDataSize = printSize;
                memcpy(m_userStatusData, m_localPrintBuffer, m_userStatusDataSize);
                m_localPrintBuffer[0] = 0;
        }

        *userStatusData = m_userStatusData;
        *userStatusDataSize = m_userStatusDataSize;
}

bool Dashboard::ValidateAdd(INT32 size)
{
        if ((m_packPtr - m_localBuffer) + size > kMaxDashboardDataSize)
        {
                wpi_setWPIError(DashboardDataOverflow);
                return false;
        }
        // Make sure printf is not being used at the same time.
        if (m_localPrintBuffer[0] != 0)
        {
                wpi_setWPIError(DashboardDataCollision);
                return false;
        }
        return true;
}

void Dashboard::AddedElement(Type type)
{
        if(IsArrayRoot())
        {
                if (m_arrayElementCount.back() == 0)
                {
                        m_expectedArrayElementType.push_back(type);
                }
                else
                {
                        if (type != m_expectedArrayElementType.back())
                        {
                                wpi_setWPIError(InconsistentArrayValueAdded);
                        }
                }
                m_arrayElementCount.back() = m_arrayElementCount.back() + 1;
        }
}

bool Dashboard::IsArrayRoot()
{
        return !m_complexTypeStack.empty() && m_complexTypeStack.top() == kArray;
}