int BookReader::lookup(const Board &board, vector<book::DataEntry> &results) {
// fetch the index page
if (!is_open()) return -1;
int probe = (int)(board.hashCode() % hdr.num_index_pages);
// seek to index
book_file.seekg((std::ios::off_type)(sizeof(book::BookHeader)+probe*sizeof(book::IndexPage)), std::ios_base::beg);
if (book_file.fail()) return -1;
book::IndexPage index;
book_file.read((char*)&index,sizeof(book::IndexPage));
if (book_file.fail()) return -1;
// correct for endianness
index.next_free = swapEndian32((byte*)&index.next_free);
book::BookLocation loc(0,book::INVALID_INDEX);
for (unsigned i = 0; i < index.next_free; i++) {
// correct for endianness
uint64_t indexHashCode = (uint64_t)(swapEndian64((byte*)&index.index[i].hashCode));
if (indexHashCode == board.hashCode()) {
// correct for endianness
index.index[i].page = (uint16_t)(swapEndian16((byte*)&index.index[i].page));
index.index[i].index = (uint16_t)(swapEndian16((byte*)&index.index[i].index));
loc = index.index[i];
break;
}
}
if (!loc.isValid()) {
// no book moves found
return 0;
}
book_file.seekg(sizeof(book::BookHeader)+
hdr.num_index_pages*sizeof(book::IndexPage)+
loc.page*sizeof(book::DataPage));
if (book_file.fail()) return -1;
book::DataPage data;
book_file.read((char*)&data,sizeof(book::DataPage));
if (book_file.fail()) return -1;
while(loc.index != book::NO_NEXT) {
ASSERT(loc.index < book::DATA_PAGE_SIZE);
book::DataEntry &bookEntry = data.data[loc.index];
bookEntry.next = swapEndian16((byte*)&bookEntry.next);
bookEntry.weight = swapEndian16((byte*)&bookEntry.weight);
bookEntry.count = swapEndian32((byte*)&bookEntry.count);
results.push_back(bookEntry);
loc.index = bookEntry.next;
}
return (int)results.size();
}
int BookWriter::write(const char* pathName) {
ofstream book_file(pathName, ios::out | ios::trunc | ios::binary);
book::BookHeader header;
header.version = book::BOOK_VERSION;
uint16_t pages = (uint16_t)index_pages;
header.num_index_pages = (uint16_t)swapEndian16((byte*)&pages);
book_file.write((char*)&header, sizeof(book::BookHeader));
if (book_file.fail()) return -1;
book::IndexPage empty;
for (int i = 0; i < index_pages; i++) {
book::IndexPage *ip = (index[i] == NULL) ? &empty : index[i];
// correct for endianness before disk write
for (unsigned j = 0; j < ip->next_free; j++) {
ip->index[j].page = (uint16_t)swapEndian16((byte*)&ip->index[j].page);
ip->index[j].index = (uint16_t)swapEndian16((byte*)&ip->index[j].index);
ip->index[j].hashCode = (uint64_t)swapEndian64((byte*)&ip->index[j].hashCode);
}
ip->next_free = (uint32_t)swapEndian32((byte*)&ip->next_free);
book_file.write((char*)ip, sizeof(book::IndexPage));
if (book_file.fail()) return -1;
}
for (unsigned i = 0; i < data.size(); i++) {
book::DataPage *dp = data[i];
// correct for endianness before disk write
dp->free_list = (uint32_t)swapEndian32((byte*)&dp->free_list);
dp->num_free = (uint32_t)swapEndian32((byte*)&dp->num_free);
for (int j = 0; j < book::DATA_PAGE_SIZE; j++) {
dp->data[j].next = (uint16_t)swapEndian16((byte*)&dp->data[j].next);
dp->data[j].weight = (uint16_t)swapEndian16((byte*)&dp->data[j].weight);
dp->data[j].count = (uint32_t)swapEndian32((byte*)&dp->data[j].count);
}
book_file.write((char*)dp, sizeof(book::DataPage));
if (book_file.fail()) return -1;
}
book_file.close();
return 0;
}
void XsDataPacket_assignFromLegacyDataPacket(struct XsDataPacket* thisPtr, struct LegacyDataPacket const* legacy, int index)
{
assert(legacy);
if (!legacy->isXbusSystem())
index = 0;
thisPtr->clear();
thisPtr->setDeviceId(legacy->deviceId(index));
thisPtr->setTimeOfArrival(legacy->timeOfArrival());
thisPtr->setPacketId(legacy->largePacketCounter());
XsDataFormat format = legacy->dataFormat(index);
if (legacy->containsRawData(index))
thisPtr->setRawData(legacy->rawData(index));
if (legacy->rawTemperatureChannelCount(index) == 4)
{
XsUShortVector rawGyroTemperatures;
rawGyroTemperatures[0] = legacy->rawTemperature(index, 1);
rawGyroTemperatures[1] = legacy->rawTemperature(index, 2);
rawGyroTemperatures[2] = legacy->rawTemperature(index, 3);
thisPtr->setRawGyroscopeTemperatureData(rawGyroTemperatures);
}
if (legacy->containsCalibratedAcceleration(index))
thisPtr->setCalibratedAcceleration(legacy->calibratedAcceleration(index));
if (legacy->containsCalibratedGyroscopeData(index))
{
//\todo Fix legacy watermarking
// XsDataIdentifier di;
// switch (format.m_outputSettings & XOS_Dataformat_Mask) {
// case XOS_Dataformat_Float:
// di = XDI_SubFormatFloat;
// break;
// case XOS_Dataformat_Double:
// di = XDI_SubFormatDouble;
// break;
// case XOS_Dataformat_Fp1632:
// di = XDI_SubFormatFp1632;
// break;
// case XOS_Dataformat_F1220:
// di = XDI_SubFormatFp1220;
// break;
// default:
// di = XDI_None;
// break;
// }
// Special copy to preserve watermarking
//thisPtr->message().setDataShort(XDI_RateOfTurn | di, thisPtr->message().getDataSize());
//thisPtr->message().setDataByte(3*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize());
//thisPtr->itemCount()++;
//thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_calGyr), 3*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_RateOfTurn | di));
thisPtr->setCalibratedGyroscopeData(legacy->calibratedGyroscopeData(index));
}
if (legacy->containsCalibratedMagneticField(index))
thisPtr->setCalibratedMagneticField(legacy->calibratedMagneticField(index));
if (legacy->containsOrientationQuaternion(index))
thisPtr->setOrientationQuaternion(legacy->orientationQuaternion(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsOrientationEuler(index))
thisPtr->setOrientationEuler(legacy->orientationEuler(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsOrientationMatrix(index))
thisPtr->setOrientationMatrix(legacy->orientationMatrix(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsPositionLLA(index))
thisPtr->setPositionLLA(legacy->positionLLA(index));
if (legacy->containsVelocity(index))
thisPtr->setVelocity(legacy->velocity(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsStatus(index))
{
bool isDetailed = true;
uint32_t status = legacy->status(index, &isDetailed);
// For MTw's only, the status needs to be swapEndian32-ed
if (thisPtr->deviceId().isMtw1())
{
status = swapEndian32(status);
}
if (isDetailed)
{
thisPtr->setStatus(status);
}
else
{
thisPtr->setStatusByte((uint8_t) status);
}
}
if (legacy->containsGpsPvtData())
{
XsPressure tmp;
tmp.m_pressure = legacy->gpsPvtData(index).m_pressure * 2.0; // Convert to Pascal
tmp.m_pressureAge = legacy->gpsPvtData(index).m_pressureAge;
thisPtr->setPressure(tmp);
XsGpsPvtData pvt;
pvt = legacy->gpsPvtData(index);
thisPtr->setGpsPvtData(pvt);
}
if (legacy->containsUtcTime(index))
{
XsUtcTime time = legacy->utcTime(index);
thisPtr->setUtcTime(time);
}
if (legacy->containsSampleTimeFine(index))
{
uint32_t sampleTimeFine = legacy->sampleTimeFine(index);
thisPtr->setSampleTimeFine(sampleTimeFine);
}
// packet counter MUST go before anything that sets a frame range!
if (legacy->containsPacketCounter(index))
thisPtr->setPacketCounter(legacy->packetCounter(index));
if (legacy->containsMtwSdiData(index))
{
// not yet converted:
//uint8_t m_timeSync;
//XsVector3 m_currentBias;
MtwSdiData mtwsdi = legacy->mtwSdiData(index);
thisPtr->setDeviceId(mtwsdi.m_deviceId);
{
XsSdiData tmp(mtwsdi.orientationIncrement(), mtwsdi.velocityIncrement());
thisPtr->setSdiData(tmp);
//// Special copy to preserve watermarking
//thisPtr->message().setDataShort(XDI_DeltaQ | di, thisPtr->message().getDataSize());
//thisPtr->message().setDataByte(4*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize());
//thisPtr->itemCount()++;
//thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_wOrientationIncrement), 4*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaQ | di));
//thisPtr->message().setDataShort(XDI_DeltaV | di, thisPtr->message().getDataSize());
//thisPtr->message().setDataByte(3*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize());
//thisPtr->itemCount()++;
//thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_wVelocityIncrement), 3*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaV | di));
}
//if (mtwsdi.m_aidingData)
thisPtr->setCalibratedMagneticField(mtwsdi.m_magnetoMeter);
if (mtwsdi.m_barometer)
{
XsPressure tmp;
tmp.m_pressure = mtwsdi.m_barometer * 100.0; // convert from millibar to pascal
tmp.m_pressureAge = mtwsdi.m_barometer?0:255;
thisPtr->setPressure(tmp);
}
thisPtr->setFrameRange(XsRange((int) mtwsdi.m_firstFrameNumber, (int) mtwsdi.m_lastFrameNumber));
thisPtr->setRssi(mtwsdi.m_rssi);
}
if (legacy->containsTemperature(index))
thisPtr->setTemperature(legacy->temperature(index));
// enable this when you experience weird crashes in XsByteArray_destruct or XsDataPacket_destruct
//validatePacket(thisPtr);
}
void XsDataPacket_assignFromXsLegacyDataPacket(
struct XsDataPacket* thisPtr, struct LegacyDataPacket const* pack,
int index)
{
assert(pack);
InternalDataPacket* hacket = (InternalDataPacket*)thisPtr;
hacket->legacyMsg() = pack->message();
if (!pack->isXbusSystem()) index = 0;
hacket->message().clear();
if (pack->isXbusSystem())
hacket->message().setBusId(index + 1);
else
hacket->message().setBusId(XS_BID_MASTER);
hacket->message().setMessageId(XMID_MtData2);
// hacket->m_legacyMsg = pack->message();
hacket->deviceId() = pack->deviceId(index);
hacket->itemCount() = 1;
hacket->toa() = pack->timeOfArrival();
hacket->originalMessageLength() =
(uint16_t)pack->originalMessage().getDataSize();
hacket->packetId() = pack->largePacketCounter();
PacketInfo info = pack->packetInfo(index);
XsDataFormat format = pack->dataFormat(index);
XsDataIdentifier di;
switch (format.m_outputSettings & XOS_Dataformat_Mask)
{
case XOS_Dataformat_Float:
di = XDI_SubFormatFloat;
break;
case XOS_Dataformat_Double:
di = XDI_SubFormatDouble;
break;
case XOS_Dataformat_Fp1632:
di = XDI_SubFormatFp1632;
break;
case XOS_Dataformat_F1220:
di = XDI_SubFormatFp1220;
break;
default:
di = XDI_None;
break;
}
if (pack->containsRawData(index)) hacket->setRawData(pack->rawData(index));
if (pack->rawTemperatureChannelCount(index) == 4)
{
XsUShortVector rawGyroTemperatures;
rawGyroTemperatures[0] = pack->rawTemperature(index, 1);
rawGyroTemperatures[1] = pack->rawTemperature(index, 2);
rawGyroTemperatures[2] = pack->rawTemperature(index, 3);
hacket->setRawGyroscopeTemperatureData(rawGyroTemperatures);
}
if (pack->containsCalibratedAcceleration(index))
hacket->setCalibratedAcceleration(pack->calibratedAcceleration(index));
if (pack->containsCalibratedGyroscopeData(index))
{
// Special copy to preserve watermarking
hacket->message().setDataShort(
XDI_RateOfTurn | di, hacket->message().getDataSize());
hacket->message().setDataByte(
3 * hacket->getFPValueSize(di), hacket->message().getDataSize());
hacket->itemCount()++;
hacket->message().setDataBuffer(
hacket->legacyMsg().getDataBuffer(info.m_calGyr),
3 * hacket->getFPValueSize(di),
XsDataPacket_itemOffsetExact(thisPtr, XDI_RateOfTurn | di));
}
if (pack->containsCalibratedMagneticField(index))
hacket->setCalibratedMagneticField(
pack->calibratedMagneticField(index));
if (pack->containsOrientationQuaternion(index))
hacket->setOrientationQuaternion(
pack->orientationQuaternion(index),
(format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
: XDI_CoordSysNwu);
if (pack->containsOrientationEuler(index))
hacket->setOrientationEuler(
pack->orientationEuler(index),
(format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
: XDI_CoordSysNwu);
if (pack->containsOrientationMatrix(index))
hacket->setOrientationMatrix(
pack->orientationMatrix(index),
(format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
: XDI_CoordSysNwu);
if (pack->containsPositionLLA(index))
hacket->setPositionLLA(pack->positionLLA(index));
if (pack->containsVelocity(index))
hacket->setVelocity(
pack->velocity(index),
(format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed
: XDI_CoordSysNwu);
if (pack->containsStatus(index))
{
bool isDetailed = true;
uint32_t status = pack->status(index, &isDetailed);
// For MTw's only, the status needs to be swapEndian32-ed
if (hacket->deviceId().isMtw())
{
status = swapEndian32(status);
}
if (isDetailed)
{
hacket->setStatus(status);
}
else
{
hacket->setStatusByte((uint8_t)status);
}
}
if (pack->containsGpsPvtData())
{
XsPressure tmp;
tmp.m_pressure =
pack->gpsPvtData(index).m_pressure * 2.0; // Convert to Pascal
tmp.m_pressureAge = pack->gpsPvtData(index).m_pressureAge;
hacket->setPressure(tmp);
XsGpsPvtData pvt;
pvt = pack->gpsPvtData(index);
hacket->setGpsPvtData(pvt);
}
if (pack->containsUtcTime(index))
{
XsUtcTime time = pack->utcTime(index);
hacket->setUtcTime(time);
}
if (pack->containsMtwSdiData(index))
{
// not yet converted:
// uint8_t m_timeSync;
// XsVector3 m_currentBias;
MtwSdiData mtwsdi = pack->mtwSdiData(index);
hacket->deviceId() = mtwsdi.m_deviceId;
{
// Special copy to preserve watermarking
hacket->message().setDataShort(
XDI_DeltaQ | di, hacket->message().getDataSize());
hacket->message().setDataByte(
4 * hacket->getFPValueSize(di),
hacket->message().getDataSize());
hacket->itemCount()++;
hacket->message().setDataBuffer(
hacket->legacyMsg().getDataBuffer(info.m_wOrientationIncrement),
4 * hacket->getFPValueSize(di),
XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaQ | di));
hacket->message().setDataShort(
XDI_DeltaV | di, hacket->message().getDataSize());
hacket->message().setDataByte(
3 * hacket->getFPValueSize(di),
hacket->message().getDataSize());
hacket->itemCount()++;
hacket->message().setDataBuffer(
hacket->legacyMsg().getDataBuffer(info.m_wVelocityIncrement),
3 * hacket->getFPValueSize(di),
XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaV | di));
}
// if (mtwsdi.m_aidingData)
hacket->setCalibratedMagneticField(mtwsdi.m_magnetoMeter);
if (mtwsdi.m_barometer)
{
XsPressure tmp;
tmp.m_pressure =
mtwsdi.m_barometer * 100.0; // convert from millibar to pascal
tmp.m_pressureAge = mtwsdi.m_barometer ? 0 : 255;
hacket->setPressure(tmp);
}
hacket->setFrameRange(
XsRange(
(int)mtwsdi.m_firstFrameNumber, (int)mtwsdi.m_lastFrameNumber));
hacket->setRssi(mtwsdi.m_rssi);
}
if (pack->containsTemperature(index))
hacket->setTemperature(pack->temperature(index));
if (pack->containsPacketCounter(index))
hacket->setPacketCounter(pack->packetCounter(index));
// enable this when you experience weird crashes in XsByteArray_destruct or
// XsDataPacket_destruct
// validatePacket(thisPtr);
}
