void fwav_stran_xyz(fwav fw,float *b1,int dx,int dy,int dz)
{
int dx0,dy0,dz0;
if (dx<=DX && dy<=DY && dz<=DZ) return;
dx0=dx; dy0=dy; dz0=dz;
if (dx>DX) dx=HALF(dx); if (dy>DY) dy=HALF(dy); if (dz>DZ) dz=HALF(dz);
fwav_stran_xyz(fw,b1,dx,dy,dz);
if (dz0>DZ) fwav_stran_z(fw,b1,dx ,dy ,dz0);
if (dy0>DY) fwav_stran_y(fw,b1,dx ,dy0,dz0);
if (dx0>DX) fwav_stran_x(fw,b1,dx0,dy0,dz0);
}
void fwav_stran_x(fwav fw,float *b1,int dx,int dy,int dz)
{
int j,dx2,dxv,y,z,d;
float a0,a2,v0,v1;
float *ba,*bu,*bv;
dx2=HALF(dx); d=dz*dy*dx2; dxv=dx>>1;
bu=fw->bt; ba=b1; for (j=d;j--;) *bu++ = *ba++;
for (z=0;z<dz;z++) {
for (y=0;y<dy;y++) {
ba=fw->bt+(z*dy+y)*dx2; bu=fw->burow;
for (j=dx2;j--;) *bu++ = *ba++;
ba=b1+(d+(z*dy+y)*dxv); bv=fw->bvrow;
for (j=dxv;j--;) *bv++ = *ba++;
ba=b1+(z*dy+y)*dx; bu=fw->burow; bv=fw->bvrow;
v0=0.0; v1= *bv++;
*ba++ =a2=(*bu++)-0.25*(v0+v1);
for (j=dxv-1;j--;) {
a0=a2; v0=v1;
v1= *bv++; a2=(*bu++)-0.25*(v0+v1);
*ba++ =0.5*(a0+a2)+v0;
*ba++ =a2;
}
if (dx&1) {
a0=a2; v0=v1;
v1=0.0; a2=(*bu++)-0.25*(v0+v1);
*ba++ =0.5*(a0+a2)+v0;
*ba++ =a2;
}else { *ba++ =0.5*(a2+a2)+v1; }
}
}
}
void fwav_atran_y(fwav fw,float *b0,int dx,int dy,int dz)
{
int j,dy2,y,z;
float *ba0,*ba1,*ba2,*bb,v1,td0,td1;
td0=fw->td0; td1=fw->td1;
dy2=HALF(dy); bb=fw->bt;
for (z=0;z<dz;z++) {
for (y=0;y+1<dy;y+=2) {
ba0=b0+(z*dy+y)*dx;
ba1=ba0+dx;
ba2=(y+2<dy?ba1+dx:ba0);
for (j=dx;j--;)
{ v1=(*ba1++)-0.5*((*ba0++)+(*ba2++)); THRESH(v1) *bb++ =v1; }
}
}
bb=b0;
for (z=0;z<dz;z++) {
for (y=0;y<dy;y+=2) {
ba1=b0+(z*dy+y)*dx;
ba2=(y+1<dy?fw->bt+(z*(dy>>1)+(y>>1))*dx:fw->zero);
ba0=(y?fw->bt+(z*(dy>>1)+(y>>1)-1)*dx:fw->zero);
for (j=dx;j--;) *bb++ =(*ba1++)+0.25*((*ba0++)+(*ba2++));
}
}
j=dx*dy*dz-(bb-b0); ba0=fw->bt; while (j--) *bb++ = *ba0++;
}
void fwav_stran_y(fwav fw,float *b1,int dx,int dy,int dz)
{
int j,dy2,y,z,d,dyv,y2;
float *ba0,*ba1,*ba2,*bb;
dy2=HALF(dy); d=dx*dy2*dz; dyv=dy>>1;
bb=fw->bt; ba1=b1; for (j=dz*dy*dx;j--;) *bb++ = *ba1++;
for (z=0;z<dz;z++) {
for (y=0;y<dy;y+=2) {
bb=b1+(z*dy+y)*dx;
y2=y>>1;
ba1=fw->bt+(z*dy2+y2)*dx;
ba2=(y2>=dyv?fw->zero:fw->bt+(d+(z*dyv+y2)*dx));
ba0=(y?fw->bt+(d+(z*dyv+y2-1)*dx):fw->zero);
for (j=dx;j--;) *bb++ =(*ba1++)-0.25*((*ba0++)+(*ba2++));
}
}
for (z=0;z<dz;z++) {
for (y=0;y+1<dy;y+=2) {
bb=fw->bt+(d+(z*dyv+(y>>1))*dx);
ba0=b1+(z*dy+y)*dx;
ba1=ba0+dx;
ba2=(y+2<dy?ba1+dx:ba0);
for (j=dx;j--;) *ba1++ =(*bb++)+0.5*((*ba0++)+(*ba2++));
}
}
}
void fwav_stran_z(fwav fw,float *b1,int dx,int dy,int dz)
{
int j,dz2,y,z,dd,d,dzv,z2;
float *ba0,*ba1,*ba2,*bb;
dz2=HALF(dz); dd=dx*dy; d=dx*dy*dz2; dzv=dz>>1;
bb=fw->bt; ba1=b1; for (j=dz*dy*dx;j--;) *bb++ = *ba1++;
for (z=0;z<dz;z+=2) {
for (y=0;y<dy;y++) {
z2=z>>1;
bb=b1+(z*dy+y)*dx;
ba1=fw->bt+(z2*dy+y)*dx;
ba2=(z2>=dzv?fw->zero:fw->bt+(d+(z2*dy+y)*dx));
ba0=(z?fw->bt+(d+((z2-1)*dy+y)*dx):fw->zero);
for (j=dx;j--;) *bb++ =(*ba1++)-0.25*((*ba0++)+(*ba2++));
}
}
for (z=0;z+1<dz;z+=2) {
for (y=0;y<dy;y++) {
bb=fw->bt+(d+((z>>1)*dy+y)*dx);
ba0=b1+(z*dy+y)*dx;
ba1=ba0+dd;
ba2=(z+2<dz?ba1+dd:ba0);
for (j=dx;j--;) *ba1++ =(*bb++)+0.5*((*ba0++)+(*ba2++));
}
}
}
void CBarShader::FillRect(CDC *dc, LPRECT rectSpan, float fRed, float fGreen,
float fBlue, bool bFlat) {
if(bFlat) {
COLORREF color = RGB((int)(fRed + .5f), (int)(fGreen + .5f), (int)(fBlue + .5f));
dc->FillRect(rectSpan, &CBrush(color));
} else {
if (m_Modifiers == NULL || (m_used3dlevel!=thePrefs.Get3DDepth() && !m_bIsPreview) )
BuildModifiers();
RECT rect = *rectSpan;
int iTop = rect.top;
int iBot = rect.bottom;
int iMax = HALF(m_iHeight);
for(int i = 0; i < iMax; i++) {
CBrush cbNew(RGB((int)(fRed * m_Modifiers[i] + .5f), (int)(fGreen * m_Modifiers[i] + .5f), (int)(fBlue * m_Modifiers[i] + .5f)));
rect.top = iTop + i;
rect.bottom = iTop + i + 1;
dc->FillRect(&rect, &cbNew);
rect.top = iBot - i - 1;
rect.bottom = iBot - i;
dc->FillRect(&rect, &cbNew);
}
}
}
void fwav_atran_z(fwav fw,float *b0,int dx,int dy,int dz)
{
int j,dz2,y,z,dd,d;
float *ba0,*ba1,*ba2,*bb,v1,td0,td1;
td0=fw->td0; td1=fw->td1;
dz2=HALF(dz); dd=dx*dy; d=dx*dy*dz2;
bb=fw->bt;
for (z=0;z+1<dz;z+=2) {
for (y=0;y<dy;y++) {
ba0=b0+(z*dy+y)*dx;
ba1=ba0+dd;
ba2=(z+2<dz?ba1+dd:ba0);
for (j=dx;j--;)
{ v1=(*ba1++)-0.5*((*ba0++)+(*ba2++)); THRESH(v1) *bb++ =v1; }
}
}
bb=b0;
for (z=0;z<dz;z+=2) {
for (y=0;y<dy;y++) {
ba1=b0+(z*dy+y)*dx;
ba2=(z+1<dz?fw->bt+((z>>1)*dy+y)*dx:fw->zero);
ba0=(z?fw->bt+(((z>>1)-1)*dy+y)*dx:fw->zero);
for (j=dx;j--;) *bb++ =(*ba1++)+0.25*((*ba0++)+(*ba2++));
}
}
j=dx*dy*dz-(bb-b0); ba0=fw->bt; while (j--) *bb++ = *ba0++;
}
void fwav_atran(fwav fw,float *v)
{
int dx,dy,dz;
float *b1,*td;
fw->v=v;
dx=fw->xs; dy=fw->ys; dz=fw->zs; b1=fw->v;
#define TD { fw->td0= -td[0]; fw->td1=td[0]; td++; }
td=(fw->thresh?fw->thresh:fw->zero);
while (dx>DX || dy>DY || dz>DZ) {
TD if (dx>DX) { fwav_atran_x(fw,b1,dx,dy,dz); dx=HALF(dx); }
TD if (dy>DY) { fwav_atran_y(fw,b1,dx,dy,dz); dy=HALF(dy); }
TD if (dz>DZ) { fwav_atran_z(fw,b1,dx,dy,dz); dz=HALF(dz); }
}
}
void buttons_runTest() {
buttons_init(); // Initialize buttons
display_init(); // Initialize display, which sets Rotation = 1 by default
display_fillScreen(DISPLAY_BLACK); // blank the screen
display_setTextColor(DISPLAY_BLACK); // Change text color to black
display_setTextSize(BUTTONS_TEXT_SIZE); // Make text larger
// Set the values of the global variables
x_max = display_width();
y_max = display_height();
// Set the values of screen positions
x_fourth = FOURTH(x_max);
x_half = HALF(x_max);
x_three_fourths = THREE_FOURTHS(x_max);
y_fourth = FOURTH(y_max);
y_half = HALF(y_max);
y_three_fourths = THREE_FOURTHS(y_max);
// Do an initial read of button values
int32_t buttonValues = buttons_read();
// Until all 4 BTNS are pressed simultaneously, write info to the LCD
while (buttonValues != BUTTONS_ALL_BTNS_ON) {
// Draw the Rects/Text on the LCD corresponding to current buttons
buttons_write_LCD(buttonValues);
// Poll new value of buttons
buttonValues = buttons_read();
}
// After all buttons are pushed simultaneously, finish test.
display_fillScreen(DISPLAY_BLACK); // blank screen
display_setTextColor(DISPLAY_CYAN); // change text color
display_setCursor(0,0); // reset cursor to origin
display_println("Button Test Finished!"); // print that the test is complete
}
void CBarShader::FillRect(CDC& dc, LPCRECT rectSpan, COLORREF crColor)
{
if(!crColor)
dc.FillSolidRect(rectSpan, crColor);
else
{
if (m_pdblModifiers == NULL)
BuildModifiers();
double dblRed = GetRValue(crColor), dblGreen = GetGValue(crColor), dblBlue = GetBValue(crColor);
double dAdd, dMod;
if (m_used3dlevel > 5) //Cax2 aqua bar
{
dMod = 1.0 - .025 * (256 - m_used3dlevel); //variable central darkness - from 97.5% to 87.5% of the original colour...
dAdd = 255;
dblRed = dMod * dblRed - dAdd;
dblGreen = dMod * dblGreen - dAdd;
dblBlue = dMod * dblBlue - dAdd;
}
else
dAdd = 0;
RECT rect;
int iTop = rectSpan->top, iBot = rectSpan->bottom;
double *pdCurr = m_pdblModifiers, *pdLimit = pdCurr + HALF(m_iHeight);
rect.right = rectSpan->right;
rect.left = rectSpan->left;
for (; pdCurr < pdLimit; pdCurr++)
{
crColor = RGB( static_cast<int>(dAdd + dblRed * *pdCurr),
static_cast<int>(dAdd + dblGreen * *pdCurr),
static_cast<int>(dAdd + dblBlue * *pdCurr) );
rect.top = iTop++;
rect.bottom = iTop;
dc.FillSolidRect(&rect, crColor);
rect.bottom = iBot--;
rect.top = iBot;
// Fast way to fill, background color is already set inside previous FillSolidRect
dc.FillSolidRect(&rect, crColor);
}
}
}
void CBarShader::BuildModifiers()
{
if (m_pdblModifiers != NULL)
{
delete[] m_pdblModifiers;
m_pdblModifiers = NULL; // 'new' may throw an exception
}
static const double dDepths[5] = { 5.5, 4.0, 3.0, 2.50, 2.25 }; //aqua bar - smoother gradient jumps...
double depth = dDepths[((m_used3dlevel > 5) ? (256 - m_used3dlevel) : m_used3dlevel) - 1];
uint32_t dwCount = HALF(static_cast<uint32_t>(m_iHeight));
double piOverDepth = PI / depth;
double base = PI / 2 - piOverDepth;
double increment = piOverDepth / (dwCount - 1);
m_pdblModifiers = new double[dwCount];
for (uint32_t i = 0; i < dwCount; i++, base += increment)
m_pdblModifiers[i] = sin(base);
}
void CBarShader::BuildModifiers() {
delete[] m_Modifiers;
m_Modifiers = NULL; // 'new' may throw an exception
if (!m_bIsPreview)
m_used3dlevel=thePrefs.Get3DDepth();
// Barry - New property page slider to control depth of gradient
// Depth must be at least 2
// 2 gives greatest depth, the higher the value, the flatter the appearance
// m_Modifiers[count-1] will always be 1, m_Modifiers[0] depends on the value of depth
int depth = (7-m_used3dlevel);
int count = HALF(m_iHeight);
double piOverDepth = M_PI/depth;
double base = piOverDepth * ((depth / 2.0) - 1);
double increment = piOverDepth / (count - 1);
m_Modifiers = new float[count];
for (int i = 0; i < count; i++)
m_Modifiers[i] = (float)(sin(base + i * increment));
}
void fwav_atran_x(fwav fw,float *ba0,int dx,int dy,int dz)
{
int j,dx2,y,z;
float a0,a1,a2,v0,v1;
float *ba,*bu,*bv,td0,td1;
td0=fw->td0; td1=fw->td1;
dx2=HALF(dx); bu=ba0; ba=ba0; bv=fw->bt;
for (z=0;z<dz;z++) {
for (y=0;y<dy;y++) {
v1=0.0; a2= *ba++;
for (j=dx2-1;j--;) {
a0=a2; v0=v1; a1= *ba++; a2= *ba++;
v1=a1-0.5*(a0+a2); THRESH(v1) *bv++ =v1;
*bu++ =a0+0.25*(v0+v1);
}
v0=v1;
if (dx&1) v1=0.0;
else { a1= *ba++; v1=a1-0.5*(a2+a2); THRESH(v1) *bv++ =v1; }
*bu++ =a2+0.25*(v0+v1);
}
}
j=bv-fw->bt; bv=fw->bt; while (j--) *bu++ = *bv++;
}
int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
HWND msgwnd = CreateWindow(L"STATIC", L"Shiftfocus window", 0, 0, 0, 0, 0, HWND_MESSAGE, 0, GetModuleHandle(NULL), 0);
# define MAKE(x) (RegisterHotKey(msgwnd, x, MOD_CONTROL | MOD_WIN, x) == 0)
if ((msgwnd == NULL) || MAKE(VK_LEFT)
|| MAKE(VK_RIGHT)
|| MAKE(VK_UP)
|| MAKE(VK_DOWN))
{
MessageBox(msgwnd, L"Unable to register hotkeys, exiting.", L"Error", MB_ICONERROR | MB_OK);
DestroyWindow(msgwnd);
return 1;
}
MSG msg;
BOOL ret;
while (0 != (ret = GetMessage(&msg, msgwnd, 0, 0))) {
if (-1 == ret)
return 1;
switch (msg.message) {
case WM_DESTROY:
PostQuitMessage(0);
break;
case WM_HOTKEY: {
RECT loc = {};
HWND fg = GetForegroundWindow();
HWND desktop = getDesktopWindow();
GetWindowRect(fg, &loc);
POINT p = {};
for (
int distance = 10;
MonitorFromPoint(p, MONITOR_DEFAULTTONULL);
distance += 10) {
switch (msg.wParam)
{
# define HALF(x,y) (((y)-(x))/2+(x))
case VK_LEFT:
p.x=loc.left-distance;
p.y=HALF(loc.top,loc.bottom);
break;
case VK_RIGHT:
p.x=loc.right+distance;
p.y=HALF(loc.top,loc.bottom);
break;
case VK_UP:
p.y=loc.top-distance;
p.x=HALF(loc.left,loc.right);
break;
case VK_DOWN:
p.y=loc.bottom+distance;
p.x=HALF(loc.left,loc.right);
break;
default:
MessageBeep(MB_ICONERROR);
}
HWND n = ChildWindowFromPoint(GetDesktopWindow(), p);
if (NULL == n)
MessageBeep(MB_ICONASTERISK);
else
{
if (desktop == n)
continue;
if (!SetForegroundWindow(n))
MessageBeep(MB_ICONERROR);
break;
}
}
} break;
}
TranslateMessage(&msg);
DispatchMessage(&msg);
}
return msg.wParam;
}
ldcomplex
cpowl(ldcomplex z, ldcomplex w) {
ldcomplex ans;
long double x, y, u, v, t, c, s, r;
long double t1, t2, t3, t4, x1, x2, y1, y2, u1, v1, b[4], w1, w2;
int ix, iy, hx, hy, hv, hu, iu, iv, i, j, k;
x = LD_RE(z);
y = LD_IM(z);
u = LD_RE(w);
v = LD_IM(w);
hx = HI_XWORD(x);
hy = HI_XWORD(y);
hu = HI_XWORD(u);
hv = HI_XWORD(v);
ix = hx & 0x7fffffff;
iy = hy & 0x7fffffff;
iu = hu & 0x7fffffff;
iv = hv & 0x7fffffff;
j = 0;
if (v == zero) { /* z**(real) */
if (u == one) { /* (anything) ** 1 is itself */
LD_RE(ans) = x;
LD_IM(ans) = y;
} else if (u == zero) { /* (anything) ** 0 is 1 */
LD_RE(ans) = one;
LD_IM(ans) = zero;
} else if (y == zero) { /* real ** real */
LD_IM(ans) = zero;
if (hx < 0 && ix < hiinf && iu < hiinf) {
/* -x ** u is exp(i*pi*u)*pow(x,u) */
r = powl(-x, u);
sincospil(u, &s, &c);
LD_RE(ans) = (c == zero)? c: c * r;
LD_IM(ans) = (s == zero)? s: s * r;
} else
LD_RE(ans) = powl(x, u);
} else if (x == zero || ix >= hiinf || iy >= hiinf) {
if (isnanl(x) || isnanl(y) || isnanl(u))
LD_RE(ans) = LD_IM(ans) = x + y + u;
else {
if (x == zero)
r = fabsl(y);
else
r = fabsl(x) + fabsl(y);
t = atan2pil(y, x);
sincospil(t * u, &s, &c);
LD_RE(ans) = (c == zero)? c: c * r;
LD_IM(ans) = (s == zero)? s: s * r;
}
} else if (fabsl(x) == fabsl(y)) { /* |x| = |y| */
if (hx >= 0) {
t = (hy >= 0)? 0.25L : -0.25L;
sincospil(t * u, &s, &c);
} else if ((LAST(u) & 3) == 0) {
t = (hy >= 0)? 0.75L : -0.75L;
sincospil(t * u, &s, &c);
} else {
r = (hy >= 0)? u : -u;
t = -0.25L * r;
w1 = r + t;
w2 = t - (w1 - r);
sincospil(w1, &t1, &t2);
sincospil(w2, &t3, &t4);
s = t1 * t4 + t3 * t2;
c = t2 * t4 - t1 * t3;
}
if (ix < 0x3ffe0000) /* |x| < 1/2 */
r = powl(fabsl(x + x), u) * exp2l(-0.5L * u);
else if (ix >= 0x3fff0000 || iu < 0x400cfff8)
/* |x| >= 1 or |u| < 16383 */
r = powl(fabsl(x), u) * exp2l(0.5L * u);
else /* special treatment */
j = 2;
if (j == 0) {
LD_RE(ans) = (c == zero)? c: c * r;
LD_IM(ans) = (s == zero)? s: s * r;
}
} else
j = 1;
if (j == 0)
return (ans);
}
if (iu >= hiinf || iv >= hiinf || ix >= hiinf || iy >= hiinf) {
/*
* non-zero imag part(s) with inf component(s) yields NaN
*/
t = fabsl(x) + fabsl(y) + fabsl(u) + fabsl(v);
LD_RE(ans) = LD_IM(ans) = t - t;
} else {
k = 0; /* no scaling */
if (iu > 0x7ffe0000 || iv > 0x7ffe0000) {
u *= 1.52587890625000000000e-05L;
v *= 1.52587890625000000000e-05L;
k = 1; /* scale u and v by 2**-16 */
}
/*
* Use similated higher precision arithmetic to compute:
* r = u * log(hypot(x, y)) - v * atan2(y, x)
* q = u * atan2(y, x) + v * log(hypot(x, y))
*/
t1 = __k_clog_rl(x, y, &t2);
t3 = __k_atan2l(y, x, &t4);
x1 = t1; HALF(x1);
y1 = t3; HALF(y1);
u1 = u; HALF(u1);
v1 = v; HALF(v1);
x2 = t2 - (x1 - t1); /* log(hypot(x,y)) = x1 + x2 */
y2 = t4 - (y1 - t3); /* atan2(y,x) = y1 + y2 */
/* compute q = u * atan2(y, x) + v * log(hypot(x, y)) */
if (j != 2) {
b[0] = u1 * y1;
b[1] = (u - u1) * y1 + u * y2;
if (j == 1) { /* v = 0 */
w1 = b[0] + b[1];
w2 = b[1] - (w1 - b[0]);
} else {
b[2] = v1 * x1;
b[3] = (v - v1) * x1 + v * x2;
w1 = sum4fpl(b, &w2);
}
sincosl(w1, &t1, &t2);
sincosl(w2, &t3, &t4);
s = t1 * t4 + t3 * t2;
c = t2 * t4 - t1 * t3;
if (k == 1) /* square j times */
for (i = 0; i < 10; i++) {
t1 = s * c;
c = (c + s) * (c - s);
s = t1 + t1;
}
}
/* compute r = u * (t1, t2) - v * (t3, t4) */
b[0] = u1 * x1;
b[1] = (u - u1) * x1 + u * x2;
if (j == 1) { /* v = 0 */
w1 = b[0] + b[1];
w2 = b[1] - (w1 - b[0]);
} else {
b[2] = -v1 * y1;
b[3] = (v1 - v) * y1 - v * y2;
w1 = sum4fpl(b, &w2);
}
/* scale back unless w1 is large enough to cause exception */
if (k != 0 && fabsl(w1) < 20000.0L) {
w1 *= 65536.0L; w2 *= 65536.0L;
}
hx = HI_XWORD(w1);
ix = hx & 0x7fffffff;
/* compute exp(w1 + w2) */
k = 0;
if (ix < 0x3f8c0000) /* exp(tiny < 2**-115) = 1 */
r = one;
else if (ix >= 0x400c6760) /* overflow/underflow */
r = (hx < 0)? tiny * tiny : huge * huge;
else { /* compute exp(w1 + w2) */
k = (int) (invln2 * w1 + ((hx >= 0)? 0.5L : -0.5L));
t1 = (long double) k;
t2 = w1 - t1 * ln2hil;
t3 = w2 - t1 * ln2lol;
r = expl(t2 + t3);
}
if (c != zero) c *= r;
if (s != zero) s *= r;
if (k != 0) {
c = scalbnl(c, k);
s = scalbnl(s, k);
}
LD_RE(ans) = c;
LD_IM(ans) = s;
}
return (ans);
}
void changeMotion()
{
//------------------------------------------------------------------------------
// change motion based on current cat state
// Author: Lingxiao Zheng
// Date: 09/29/2015
//------------------------------------------------------------------------------
switch (Car_State)
{
case 0: // pause 0.5s
case 2:
case 4:
case 6:
case 8:
case 10:
if (resetTimerA0)
changeTimerA0Interval(TA0_INTERVAL_0_5s);
stop();
break;
case 1: // move forward until BLK_LN
if (!startADC)
startADC=TRUE;
move_forward();
break;
case 3: // move reverse until BLK_LN
move_reverse();
// recording time
if (!isRecording)
startRecording();
if (!startADC)
{
five_msec_sleep(SLEEP_1s/2);
startADC=TRUE;
}
break;
case 5: // go forward for half of time going reverse
if (resetTimerA0)
changeTimerA0Interval(TA0_INTERVAL_0_1s*HALF(getRecording()));
move_forward();
break;
case 7: // spin clockwise
if (resetTimerA0)
changeTimerA0Interval(TA0_INTERVAL_3s);
spin_clockwise();
break;
case 9: // spin counter clockwise
if (resetTimerA0)
changeTimerA0Interval(TA0_INTERVAL_5s);
spin_counterclock();
break;
default: // done, disable all ISR
TA0CTL |= MC__STOP;
ADC10IE &= ~ADC10IE0;
while(ALWAYS)
LPM4;
}
}
void
profiler(struct monprof *mpp)
{
struct pdata *current, *base;
int nmem;
if(prof_Enabled == 0) {
return;
}
if(mpp->type & MONPROF_FUNCLOG) {
nmem = prof_FuncTot;
base = prof_FuncTbl;
while(nmem) {
current = &base[HALF(nmem)];
if(mpp->pc < current->data) {
nmem = HALF(nmem);
} else if(mpp->pc > current->data) {
if(mpp->pc < (current+1)->data) {
current->pcount++;
goto tidlog;
} else {
base = current + 1;
nmem = (HALF(nmem)) - (nmem ? 0 : 1);
}
} else {
current->pcount++;
goto tidlog;
}
}
prof_BadSymCnt++;
}
tidlog:
if(mpp->type & MONPROF_TIDLOG) {
/* First see if the tid is already in the table. If it is,
* increment the pcount. If it isn't add it to the table.
*/
nmem = prof_TidTally;
base = prof_TidTbl;
while(nmem) {
current = &base[HALF(nmem)];
if(mpp->tid < current->data) {
nmem = HALF(nmem);
} else if(mpp->tid > current->data) {
base = current + 1;
nmem = (HALF(nmem)) - (nmem ? 0 : 1);
} else {
current->pcount++;
goto pclog;
}
}
/* Since we got here, the tid must not be in the table, so
* do an insertion into the table. Items are in the table in
* ascending order.
*/
if(prof_TidTally == 0) {
prof_TidTbl->pcount = 1;
prof_TidTbl->data = mpp->tid;
prof_TidTally++;
} else if(prof_TidTally >= prof_TidTot) {
prof_TidOverflow++;
} else {
current = prof_TidTbl + prof_TidTally - 1;
while(current >= prof_TidTbl) {
if(mpp->tid > current->data) {
current++;
current->pcount = 1;
current->data = mpp->tid;
break;
} else {
*(current+1) = *current;
if(current == prof_TidTbl) {
current->pcount = 1;
current->data = mpp->tid;
break;
}
}
current--;
}
prof_TidTally++;
}
}
pclog:
if(mpp->type & MONPROF_PCLOG) {
ulong offset;
if((mpp->pc > prof_PcTxtEnd) || (mpp->pc < prof_PcTxtBase)) {
prof_PcOORCnt++;
} else {
offset = mpp->pc - prof_PcDelta;
switch(prof_PcWidth) {
case 2:
(*(ushort *)offset)++;
break;
case 4:
(*(ulong *)offset)++;
break;
}
}
}
prof_CallCnt++;
return;
}
void fdct_int32( short *const In )
{
short *pIn;
int i;
pIn = In;
for(i=8; i>0; --i)
{
int mm0, mm1, mm2, mm3, mm4, mm5, mm6, mm7, Spill;
// even
LOAD_BUTF(mm1,mm6, 1, 6, mm0, pIn);
LOAD_BUTF(mm2,mm5, 2, 5, mm0, pIn);
LOAD_BUTF(mm3,mm4, 3, 4, mm0, pIn);
LOAD_BUTF(mm0,mm7, 0, 7, Spill, pIn);
BUTF(mm1, mm2, Spill);
BUTF(mm0, mm3, Spill);
ROTATE(mm3, mm2, ROT6_C, ROT6_SmC, -ROT6_SpC, Spill, FIX-FPASS, HALF(FIX-FPASS));
pIn[2] = mm3;
pIn[6] = mm2;
BUTF(mm0, mm1, Spill);
pIn[0] = SHIFTL(mm0, FPASS);
pIn[4] = SHIFTL(mm1, FPASS);
// odd
mm3 = mm5 + mm7;
mm2 = mm4 + mm6;
ROTATE(mm2, mm3, ROT17_C, -ROT17_SpC, -ROT17_SmC, mm0, FIX-FPASS, HALF(FIX-FPASS));
ROTATE(mm4, mm7, -ROT37_C, ROT37_SpC, ROT37_SmC, mm0, FIX-FPASS, HALF(FIX-FPASS));
mm7 += mm3;
mm4 += mm2;
pIn[1] = mm7;
pIn[7] = mm4;
ROTATE(mm5, mm6, -ROT13_C, ROT13_SmC, ROT13_SpC, mm0, FIX-FPASS, HALF(FIX-FPASS));
mm5 += mm3;
mm6 += mm2;
pIn[3] = mm6;
pIn[5] = mm5;
pIn += 8;
}
pIn = In;
for(i=8; i>0; --i)
{
int mm0, mm1, mm2, mm3, mm4, mm5, mm6, mm7, Spill;
// even
LOAD_BUTF(mm1,mm6, 1*8, 6*8, mm0, pIn);
LOAD_BUTF(mm2,mm5, 2*8, 5*8, mm0, pIn);
BUTF(mm1, mm2, mm0);
LOAD_BUTF(mm3,mm4, 3*8, 4*8, mm0, pIn);
LOAD_BUTF(mm0,mm7, 0*8, 7*8, Spill, pIn);
BUTF(mm0, mm3, Spill);
ROTATE(mm3, mm2, ROT6_C, ROT6_SmC, -ROT6_SpC, Spill, 0, HALF(FIX+FPASS+3));
pIn[2*8] = (int16_t)SHIFTR(mm3,FIX+FPASS+3);
pIn[6*8] = (int16_t)SHIFTR(mm2,FIX+FPASS+3);
mm0 += HALF(FPASS+3) - 1;
BUTF(mm0, mm1, Spill);
pIn[0*8] = (int16_t)SHIFTR(mm0, FPASS+3);
pIn[4*8] = (int16_t)SHIFTR(mm1, FPASS+3);
// odd
mm3 = mm5 + mm7;
mm2 = mm4 + mm6;
ROTATE(mm2, mm3, ROT17_C, -ROT17_SpC, -ROT17_SmC, mm0, 0, HALF(FIX+FPASS+3));
ROTATE2(mm4, mm7, -ROT37_C, ROT37_SpC, ROT37_SmC, mm0);
mm7 += mm3;
mm4 += mm2;
pIn[7*8] = (int16_t)SHIFTR(mm4,FIX+FPASS+3);
pIn[1*8] = (int16_t)SHIFTR(mm7,FIX+FPASS+3);
ROTATE2(mm5, mm6, -ROT13_C, ROT13_SmC, ROT13_SpC, mm0);
mm5 += mm3;
mm6 += mm2;
pIn[5*8] = (int16_t)SHIFTR(mm5,FIX+FPASS+3);
pIn[3*8] = (int16_t)SHIFTR(mm6,FIX+FPASS+3);
pIn++;
}
}
