void wait(float T)
{
//this will pause further execution until specified # of wheel turns.
int pk = PointsOnWheel * T;
for(int i;i<pk;i++)
Count1();
}
void Count2Blue()
{
int DT=0;
while(CTest() != 1)
{
Count1();
DT++;
LineCheck();
Move();
}
ShootTarget();
Return(DT);
}
void Return(int Steps)
{
int i;int tp;
for(i=0;i<Steps;i++)
{
Count1();
LineCheck();
tp=ILeftSpeed;
ILeftSpeed=IRightSpeed;
IRightSpeed=tp;
}
TurnRight();
//now (just now) does it return to Navigate.
}
status_t Fft(mcon::Matrix<double>& complex, const mcon::Vector<double>& timeSeries)
{
const size_t N = timeSeries.GetLength();
if (Count1(N) != 1)
{
return -ERROR_ILLEGAL;
}
bool status = complex.Resize(2, N);
if (false == status)
{
return -ERROR_CANNOT_ALLOCATE_MEMORY;
}
mcon::VectordBase& real = complex[0];
mcon::VectordBase& imag = complex[1];
const double df = 2.0 * g_Pi / N;
mcon::Vector<double> sinTable(N/2);
mcon::Vector<double> cosTable(N/2);
for (size_t i = 0; i < N / 2; ++i)
{
sinTable[i] = sin(df * i);
cosTable[i] = cos(df * i);
}
// Substitute beforehand
real = timeSeries;
imag = 0;
size_t innerLoop = N / 2;
int outerLoop = 1;
//DEBUG_LOG("N=%d\n", N);
// Decimation in frequency.
for ( ; 0 < innerLoop; innerLoop >>= 1, outerLoop <<= 1)
{
//DEBUG_LOG("inner=%d, outer=%d\n", innerLoop, outerLoop);
for ( int outer = 0; outer < outerLoop; ++outer )
{
const int& step = innerLoop;
const int ofs = outer * step * 2;
#if 0
for (size_t k = 0; k < innerLoop; ++k )
{
const double r1 = real[k+ofs];
const double i1 = imag[k+ofs];
const double r2 = real[k+step+ofs];
const double i2 = imag[k+step+ofs];
real[k+ofs] = r1 + r2;
imag[k+ofs] = i1 + i2;
const int idx = k * outerLoop;
//DEBUG_LOG("step=%d, ofs=%d, k=%d, idx=%d\n", step, ofs, k, idx);
real[k+step+ofs] = (r1 - r2) * cosTable[idx] + (i1 - i2) * sinTable[idx];
imag[k+step+ofs] = - (r1 - r2) * sinTable[idx] + (i1 - i2) * cosTable[idx];
}
#else
// Easier to look into.
void* _pReal = real;
void* _pImag = imag;
double* pRealL = reinterpret_cast<double*>(_pReal) + ofs;
double* pImagL = reinterpret_cast<double*>(_pImag) + ofs;
double* pRealH = reinterpret_cast<double*>(_pReal) + ofs + step;
double* pImagH = reinterpret_cast<double*>(_pImag) + ofs + step;
for (size_t k = 0; k < innerLoop; ++k )
{
const double r1 = pRealL[k];
const double i1 = pImagL[k];
const double r2 = pRealH[k];
const double i2 = pImagH[k];
const size_t idx = k * outerLoop;
pRealL[k] = r1 + r2;
pImagL[k] = i1 + i2;
pRealH[k] = (r1 - r2) * cosTable[idx] + (i1 - i2) * sinTable[idx];
pImagH[k] = - (r1 - r2) * sinTable[idx] + (i1 - i2) * cosTable[idx];
//DEBUG_LOG("step=%d, ofs=%d, k=%d, idx=%d\n", step, ofs, k, idx);
}
#endif
}
}
// Bit-reverse
const int width = Ilog2(N);
//DEBUG_LOG("width=%d\n", width);
for (size_t i = 0; i < N; ++i )
{
const size_t k = BitReverse(i, width);
if (k == i || k < i)
{
continue;
}
const double real_temp = real[k];
const double imag_temp = imag[k];
real[k] = real[i];
imag[k] = imag[i];
real[i] = real_temp;
imag[i] = imag_temp;
}
return NO_ERROR;
}
status_t Ifft(mcon::Vector<double>& timeSeries, const mcon::Matrix<double>& complex)
{
const size_t N = complex.GetColumnLength();
if (Count1(N) != 1)
{
return -ERROR_ILLEGAL;
}
mcon::Vector<double> tsPair;
mcon::Vector<double> sinTable;
mcon::Vector<double> cosTable;
if ( false == timeSeries.Resize(N)
|| false == tsPair.Resize(N)
|| false == sinTable.Resize(N)
|| false == cosTable.Resize(N) )
{
return -ERROR_CANNOT_ALLOCATE_MEMORY;
}
const double df = 2.0 * g_Pi / N;
for (size_t i = 0; i < N / 2; ++i)
{
sinTable[i] = sin(df * i);
cosTable[i] = cos(df * i);
}
// Substitute beforehand
timeSeries = complex[0];
tsPair = complex[1];
size_t innerLoop = N / 2;
size_t outerLoop = 1;
// Decimation in frequency.
for ( ; 0 < innerLoop; innerLoop >>= 1, outerLoop <<= 1)
{
//DEBUG_LOG("inner=%d, outer=%d\n", innerLoop, outerLoop);
for (size_t outer = 0; outer < outerLoop; ++outer )
{
const int& step = innerLoop;
const int ofs = outer * step * 2;
// Easier to look into.
void* _pTs = timeSeries;
void* _pTsPair = tsPair;
double* pTsL = reinterpret_cast<double*>(_pTs) + ofs;
double* pTsPairL = reinterpret_cast<double*>(_pTsPair) + ofs;
double* pTsH = reinterpret_cast<double*>(_pTs) + ofs + step;
double* pTsPairH = reinterpret_cast<double*>(_pTsPair) + ofs + step;
for (size_t k = 0; k < innerLoop; ++k )
{
const double r1 = pTsL[k];
const double i1 = pTsPairL[k];
const double r2 = pTsH[k];
const double i2 = pTsPairH[k];
const size_t idx = k * outerLoop;
pTsL[k] = r1 + r2;
pTsPairL[k] = i1 + i2;
pTsH[k] = (r1 - r2) * cosTable[idx] - (i1 - i2) * sinTable[idx];
pTsPairH[k] = (r1 - r2) * sinTable[idx] + (i1 - i2) * cosTable[idx];
//DEBUG_LOG("step=%d, ofs=%d, k=%d, idx=%d\n", step, ofs, k, idx);
}
}
}
// Bit-reverse
const int width = Ilog2(N);
//DEBUG_LOG("width=%d\n", width);
for (size_t i = 0; i < N; ++i )
{
const size_t k = BitReverse(i, width);
if (k < i)
{
continue;
}
const double ts_temp = timeSeries[k] / N;
timeSeries[k] = timeSeries[i] / N;
timeSeries[i] = ts_temp;
}
return NO_ERROR;
}
