/* read a sequence of MPEG TS packets (to fit into the given buffer)
*/
static ssize_t
read_ts_file( int fd, char* data, const size_t len, FILE* log )
{
const size_t pkt_len = ((len - 1) / TS_SEG_LEN) * TS_SEG_LEN;
off_t k = 0;
u_int bad_frg = 0;
ssize_t n = -1;
assert( (fd > 0) && data && len && log);
assert( !buf_overrun( data, len, 0, pkt_len, log ) );
n = read_buf( fd, data, pkt_len, log );
if( n <= 0 ) return n;
if( 0 != sizecheck( "Bad TS packet stream",
pkt_len, n, log, __func__ ) )
return -1;
/* make sure we've read TS records, not random data
*/
for( k = 0; k < (off_t)pkt_len; k += TS_SEG_LEN ) {
if( -1 == ts_sigcheck( data[k], k, (u_long)pkt_len,
log, __func__ ) ) {
++bad_frg;
}
}
return (bad_frg ? -1 : n);
}
/* determine type of stream saved in file
*/
upxfmt_t
get_fstream_type( int fd, FILE* log )
{
ssize_t n = 0;
off_t offset = 0, where = 0;
upxfmt_t dtype = UPXDT_UNKNOWN;
char* data = NULL;
/* read in enough data to contain extended header
* and beginning of payload segment */
size_t len = TS_SEG_LEN + RTP_XTHDRLEN;
assert( (fd > 0) && log );
if( NULL == (data = malloc( len )) ) {
mperror( log, errno, "%s: malloc", __func__ );
return UPXDT_UNKNOWN;
}
do {
/* check if it is a MPEG TS stream
*/
n = read( fd, data, len );
if( 0 != sizecheck( "Not enough space for stream data",
len, n, log, __func__ ) ) break;
offset += n;
dtype = get_mstream_type( data, len, log );
if( UPXDT_UNKNOWN == dtype ) {
TRACE( (void)tmfprintf( log, "%s: file type is not recognized\n",
__func__ ) );
dtype = UPXDT_UNKNOWN;
break;
}
} while(0);
if( NULL != data ) free( data );
if( n <= 0 ) {
mperror( log, errno, "%s", __func__ );
return UPXDT_UNKNOWN;
}
where = lseek( fd, (-1) * offset, SEEK_CUR );
if( -1 == where ) {
mperror( log, errno, "%s: lseek", __func__ );
return UPXDT_UNKNOWN;
}
/*
TRACE( (void)tmfprintf( log, "%s: stream type = [%d]=[%s]\n",
__func__, (int)dtype, fmt2str(dtype) ) );
*/
return dtype;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
if (nrhs != 1 || nlhs != 0) mexErrMsgTxt("usage: resetInitialization(obj)");
int narg = 0;
sizecheck(prhs[narg], 1, 1);
QPReactiveRecoveryPlan *plan = (QPReactiveRecoveryPlan*) getDrakeMexPointer(mxGetPropertySafe(prhs[narg], "mex_ptr"));
++narg;
plan->resetInitialization();
}
void matlabToCArrayOfArrays(const mxArray *source, const int idx, const char *fieldname, double *destination) {
// Matlab arrays come in as column-major data. To represent a matrix in C++, as we do in our LCM messages, we need an array of arrays. But that convention results in a row-major storage, so we have to be careful about how we copy data in.
const mxArray *field = myGetField(source, idx, fieldname);
if (!mxIsDouble(field)) {
std::cout << fieldname << std::endl;
mexErrMsgTxt("Numeric data must be passed in as doubles\n");
}
sizecheck(field, M, N);
Map<Matrix<double, M, N>>A(mxGetPr(field));
// C is row-major, matlab is column-major
Matrix<double, N, M> A_t = A.transpose();
memcpy(destination, A_t.data(), sizeof(double)*M*N);
return;
}
int generateH2value(int new_sock, int password, int R)
{
int result = 0;
char r[255];
char pass[3];
sprintf(r, "%d", R);
sprintf(pass, "%d", password);
if (sizecheck(pass, r) == 1)
{
//Size does not reach limit. Proceed to concat
strcpy(H2_notsigned, pass);
strcat(H2_notsigned, r);
struct tcpquery val = signed_withSHA(H2_notsigned);
strncpy(H2, val.command, sizeof(H2));
result = 1;
}
return result;
}
// Fixed step Backward Euler ODE solver
void back_euler(ode_workspace *odews)
{
// Declare and initialise additional workspace variables
double *beta, *w, *x; // arrays
workspace *W;
W = odews->W;
int ny = W->nu;
beta = zerosv(ny);
w = zerosv(ny);
x = zerosv(ny);
double t = odews->t0;
double tnext;
// Initial newton_matrix computation
newton_matrix(odews);
int jac_needed = 0; // flag to say that Jacobian is current
int converged = 0;
write_data(W, t, odews->y); // Write initial data to file
write_flow(W, t, W->q, W->q0);
write_pressure(W, t, W->p, W->p0);
MPI_Barrier(MPI_COMM_WORLD);
// Timestep loop
for (int i = 0; t < odews->tf; i++)
{
// Perform a Jacobian update if necessary. This is in sync across
// all processors
if (jac_needed)
{
jacupdate(W, t, odews->y);
jac_needed = 0;
newton_matrix(odews);
}
// Copy values from previous completed timestep
dcopy(ny, odews->y, beta);
dcopy(ny, odews->y, w);
tnext = t + odews->dt;
// Indicate that we haven't converged yet
W->flag[W->rank] = 0;
converged = 0;
// Newton loop
for (int k = 0; k < odews->maxits; k++)
{
evaluate(W, tnext, w, odews->f); // f = g(w)
// evaluate also exchanges convergence information. If everyone
// has converged, then we can stop. Everyone likewise gets the
// request for Jacobian update
if (all(W->flag, W->n_procs))
{
converged = 1;
if (k > odews->nconv)
{
jac_needed = 1;
}
break; // w contains the correct value
}
// Form x = w - beta - dt g (our fcn value for Newton)
dcopy(ny, w, x);
daxpy(ny, -1, beta, x);
daxpy(ny, -odews->dt, odews->f, x);
// TEST x[0] = radius etc. - w is the state var value, x is passed on to Newton
// for (int la = 0; la < 27; la++)
// {
// printf("iteration %d, state variable %2d - x: %e w: %e\n", k, la, x[la], w[la] );
// }
W->flag[W->rank] = sizecheck(x, ny, odews->ftol); // function value size check
lusoln(odews, x); // solve (x is now increment)
W->flag[W->rank] |= sizecheck(x, ny, odews->ytol); // increment size check
daxpy(ny, -1, x, w); // update w with new value
}
if (!converged)
{
printf("Newton iteration failed to converge\n");
exit(1);
}
t = tnext;
dcopy(ny, w, odews->y); // update y values
if (fmod(t, odews->dtwrite) < odews->dt)
{
write_data(W, t, odews->y);
write_flow(W, t, W->q, W->q0);
write_pressure(W, t, W->p, W->p0);
if (W->rank == 0)
{
printf("time: %e \n",t);
}
}
}
free(w);
free(x);
free(beta);
}
/* read an RTP packet
*/
static ssize_t
read_rtp_file( int fd, char* data, const size_t len, FILE* log )
{
ssize_t nrd = -1, offset = 0;
size_t hdrlen = 0, rdlen = 0;
u_int frg = 0;
int rtp_end = 0, rc = 0;
off_t where = 0;
assert( (fd > 0) && data && len && log);
assert( !buf_overrun( data, len, 0, RTP_HDR_SIZE, log ) );
nrd = read_buf( fd, data, RTP_HDR_SIZE, log );
if( nrd <= 0 ) return nrd;
offset += nrd;
if( -1 == sizecheck( "Bad RTP header", RTP_HDR_SIZE, nrd,
log, __func__ ) )
return -1;
if( 0 == RTP_verify( data, nrd, log ) )
return -1;
if( -1 == (rc = RTP_hdrlen( data, nrd, &hdrlen, log )) )
return -1;
/* if there is an extended header, read it in */
if( ENOMEM == rc ) {
assert( !buf_overrun( data, len, offset,
RTP_XTHDRLEN - RTP_HDR_SIZE, log ) );
nrd = read_buf( fd, data + offset,
RTP_XTHDRLEN - RTP_HDR_SIZE, log );
if( (nrd <= 0) ||
(-1 == sizecheck("Bad RTP x-header",
RTP_XTHDRLEN - RTP_HDR_SIZE, nrd,
log, __func__ )) ) {
return -1;
}
if( 0 == nrd ) return nrd;
offset += nrd;
rc = RTP_hdrlen( data, offset, &hdrlen, log );
if( 0 != rc ) {
TRACE( (void)tmfprintf( log, "%s: bad RTP header - quitting\n",
__func__ ) );
return -1;
}
/*
TRACE( (void)tmfprintf( log, "%s: RTP x-header length=[%lu]\n",
__func__, (u_long)hdrlen ) );
*/
if( (size_t)offset > hdrlen ) {
/* read more than needed: step back */
where = lseek( fd, (-1)*((size_t)offset - hdrlen), SEEK_CUR );
if( -1 == where ) {
mperror( log, errno, "%s: lseek", __func__ );
return -1;
}
offset -= ((size_t)offset - hdrlen);
assert( (size_t)offset == hdrlen );
/*
TRACE( (void)tmfprintf( log, "%s: back to fpos=[0x%X], "
"offset=[%ld]\n", __func__, (u_int)where, (long)offset ) );
*/
}
else if( hdrlen > (size_t)offset ) {
/* read remainder of the header in */
assert( !buf_overrun( data, len, offset,
(hdrlen - (size_t)offset), log ) );
nrd = read_buf( fd, data + offset,
(hdrlen - (size_t)offset), log );
if( nrd <= 0 ||
(-1 == sizecheck("Bad RTP x-header tail",
(hdrlen - (size_t)offset), nrd,
log, __func__ )) ) {
return -1;
}
if( 0 == nrd ) return nrd;
offset += nrd;
assert( (size_t)offset == hdrlen );
}
} /* read extended header */
/* read TS records until there is another RTP header or EOF */
for( frg = 0; (ssize_t)len > offset; ++frg ) {
rdlen = ( (len - offset) < TS_SEG_LEN
? (len - offset)
: TS_SEG_LEN );
/*
TRACE( (void)tmfprintf( log, "%s: reading [%lu] more bytes\n",
__func__, (u_long)rdlen ) );
*/
assert( !buf_overrun( data, len, offset, rdlen, log ) );
nrd = read_buf( fd, data + offset, rdlen, log );
if( nrd <= 0 ) break;
/* if it's an RTP header, roll back and return
*/
rtp_end = RTP_verify( data + offset, nrd, log );
if( 1 == rtp_end ) {
if( -1 == lseek( fd, (-1) * nrd, SEEK_CUR ) ) {
mperror( log, errno, "%s: lseek", __func__ );
return -1;
}
break;
}
/* check if it is a TS packet and it's of the right size
*/
if( (-1 == ts_sigcheck( data[offset], offset, (u_long)TS_SEG_LEN,
log, __func__ )) ||
(-1 == sizecheck( "Bad TS segment size", TS_SEG_LEN, nrd,
log, __func__ )) ) {
TRACE( hex_dump( "Data in question", data, offset + nrd, log ) );
return -1;
}
offset += nrd;
} /* for */
/* If it is not EOF and no RTP header for the next message is found,
* it is either our buffer is too small (to fit the whole message)
* or the stream is invalid
*/
if( !rtp_end && (0 != nrd) ) {
(void)tmfprintf( log, "%s: no RTP end after reading [%ld] bytes\n",
__func__, (long)offset );
return -1;
}
return (nrd < 0) ? nrd : offset;
}
