i2c& i2c::setup(uint16_t bus)
{
switch (bus)
{
case 0: return setup0();
case 1: return setup1();
case 2: return setup2();
default: return setup0();
}
}
void exec_loop(void)
{
#if 0
uint16_t i = 0;
while(1)
{
while(!g_run)
chirpService();
setup0();
while(g_run)
{
loop0();
i++;
if (i%50==0)
{
_DBD16(i); _DBG("\n");
}
}
// set variable to indicate we've stopped
g_running = 0;
}
#endif
#if 1
while(1)
{
while(!g_run)
chirpService();
setup0();
while(g_run)
{
loop0();
chirpService();
}
// set variable to indicate we've stopped
g_running = 0;
}
#endif
}
void ba81NormalQuad::setup(double Qwidth, int Qpoints, double *means,
Eigen::MatrixXd &priCov, Eigen::VectorXd &sVar)
{
quadGridSize = Qpoints;
numSpecific = sVar.rows() * sVar.cols();
primaryDims = priCov.rows();
maxDims = primaryDims + (numSpecific? 1 : 0);
maxAbilities = primaryDims + numSpecific;
if (maxAbilities == 0) {
setup0();
return;
}
totalQuadPoints = 1;
for (int dx=0; dx < maxDims; dx++) {
totalQuadPoints *= quadGridSize;
}
if (int(Qpoint.size()) != quadGridSize) {
Qpoint.clear();
Qpoint.reserve(quadGridSize);
double qgs = quadGridSize-1;
for (int px=0; px < quadGridSize; ++px) {
Qpoint.push_back(px * 2 * Qwidth / qgs - Qwidth);
}
}
std::vector<double> tmpWherePrep(totalQuadPoints * maxDims);
Eigen::VectorXi quad(maxDims);
for (int qx=0; qx < totalQuadPoints; qx++) {
double *wh = tmpWherePrep.data() + qx * maxDims;
decodeLocation(qx, maxDims, quad.data());
pointToWhere(quad.data(), wh, maxDims);
}
totalPrimaryPoints = totalQuadPoints;
weightTableSize = totalQuadPoints;
if (numSpecific) {
totalPrimaryPoints /= quadGridSize;
speQarea.resize(quadGridSize * numSpecific);
weightTableSize *= numSpecific;
}
std::vector<double> tmpPriQarea;
tmpPriQarea.reserve(totalPrimaryPoints);
{
Eigen::VectorXd where(primaryDims);
for (int qx=0; qx < totalPrimaryPoints; qx++) {
decodeLocation(qx, primaryDims, quad.data());
pointToWhere(quad.data(), where.data(), primaryDims);
double den = exp(dmvnorm(primaryDims, where.data(), means, priCov.data()));
tmpPriQarea.push_back(den);
}
}
std::vector<int> priOrder;
priOrder.reserve(totalPrimaryPoints);
for (int qx=0; qx < totalPrimaryPoints; qx++) {
priOrder.push_back(qx);
}
if (0) {
sortAreaHelper priCmp(tmpPriQarea);
std::sort(priOrder.begin(), priOrder.end(), priCmp);
}
priQarea.clear();
priQarea.reserve(totalPrimaryPoints);
double totalArea = 0;
for (int qx=0; qx < totalPrimaryPoints; qx++) {
double den = tmpPriQarea[priOrder[qx]];
priQarea.push_back(den);
//double prevTotalArea = totalArea;
totalArea += den;
// if (totalArea == prevTotalArea) {
// mxLog("%.4g / %.4g = %.4g", den, totalArea, den / totalArea);
// }
}
for (int qx=0; qx < totalPrimaryPoints; qx++) {
priQarea[qx] *= One;
priQarea[qx] /= totalArea;
//mxLog("%.5g,", priQarea[qx]);
}
for (int sgroup=0; sgroup < numSpecific; sgroup++) {
totalArea = 0;
double mean = means[primaryDims + sgroup];
double var = sVar(sgroup);
for (int qx=0; qx < quadGridSize; qx++) {
double den = exp(dmvnorm(1, &Qpoint[qx], &mean, &var));
speQarea[sIndex(sgroup, qx)] = den;
totalArea += den;
}
for (int qx=0; qx < quadGridSize; qx++) {
speQarea[sIndex(sgroup, qx)] /= totalArea;
}
}
//pda(speQarea.data(), numSpecific, quadGridSize);
for (int sx=0; sx < int(speQarea.size()); ++sx) {
speQarea[sx] *= One;
}
//pda(speQarea.data(), numSpecific, quadGridSize);
wherePrep.clear();
wherePrep.reserve(totalQuadPoints * maxDims);
if (numSpecific == 0) {
for (int qx=0; qx < totalPrimaryPoints; qx++) {
int sortq = priOrder[qx] * maxDims;
for (int dx=0; dx < maxDims; ++dx) {
wherePrep.push_back(tmpWherePrep[sortq + dx]);
}
}
} else {
for (int qx=0; qx < totalPrimaryPoints; ++qx) {
int sortq = priOrder[qx] * quadGridSize;
for (int sx=0; sx < quadGridSize; ++sx) {
int base = (sortq + sx) * maxDims;
for (int dx=0; dx < maxDims; ++dx) {
wherePrep.push_back(tmpWherePrep[base + dx]);
}
}
}
}
// recompute whereGram because the order might have changed
whereGram.resize(triangleLoc1(maxDims), totalQuadPoints);
for (int qx=0; qx < totalQuadPoints; qx++) {
double *wh = wherePrep.data() + qx * maxDims;
gramProduct(wh, maxDims, &whereGram.coeffRef(0, qx));
}
}
void
setconfig(Device *d, int n)
{
setup0(d, RH2D|Rstandard|Rdevice, SET_CONFIGURATION, n, 0, 0);
d->state = Configured;
}
