//should be optimized
const complex_matrix_type make_new_a() const
{
auto A = make_a();
auto gs = make_gscale();
auto gy = make_gyvec();
auto const gd = make_gd(make_beam_vector());
auto const Gm = make_gm(gd);
auto gm = make_unique_beams(Gm);
array_type gxu = scale_multiply( gx, gs );
array_type gyu = scale_multiply( gy, gs );
array_type gu = array_type( gxu.norm(), gyu.norm(), 0 );
array_vector_type gxy( gd );
std::for_each( gxy.begin(), gxy.end(), [gu](array_type& a){ a[0]*=gu[1]; a[1]*=gu[0]; a[2]=0; } );
vector_type gxy2( gxy.size() );
feng::for_each( gxy2.begin(), gxy2.end(), gxy.begin(), [](value_type& v, const array_type& a){ v = a.norm(); v*=v; } );
feng::for_each( A.diag_begin(), A.diag_end(), gxy2.begin(), [](complex_type& c, const value_type& v){ c = complex_type(-v, 0); } );
std::cout << "\nA=\n" << A;
return A;
}
std::size_t cyclic_eigen_jacobi( const Matrix1& A, Matrix2& V, Otor o, std::size_t max_rot = 80, const T eps = T( 1.0e-10 ) )
{
typedef typename Matrix1::value_type value_type;
typedef typename Matrix1::size_type size_type;
auto const compare_func = [eps]( const value_type lhs, const value_type rhs ) { return std::abs(lhs-rhs) < eps; };
assert( A.row() == A.col() );
//assert( is_symmetric( A, compare_func ) );
size_type i = 0;
auto a = A;
auto const n = a.row();
auto const one = value_type( 1 );
auto const zero = value_type( 0 );
// @V = diag{1, 1, ..., 1}
V.resize( n, n );
V = zero;
std::fill( V.diag_begin(), V.diag_end(), one );
for ( ; i != max_rot; ++i )
{
if ( !(i&7) && eigen_jacobi_private::norm(a) == zero )
{
break;
}
for ( size_type p = 0; p != n; ++p )
for ( size_type q = p+1; q != n; ++q )
eigen_jacobi_private::rotate(a, V, p, q);
}
std::copy( a.diag_begin(), a.diag_end(), o );
return i*n*n;
}
bool is_orthogonal( const matrix<T,N,A>& m, F f )
{
if ( m.row() != m.col() ) return false;
auto mm = m.transpose() * m;
std::for_each( mm.diag_begin(), mm.diag_end(), [](T& v){ v -= T(1); } );
return std::all_of( mm.begin(), mm.end(), f );
}
typename Matrix::value_type norm( const Matrix& A )
{
typedef typename Matrix::value_type value_type;
//typedef typename Matrix::size_type size_type;
auto A_ = feng::abs(A);
std::fill( A_.diag_begin(), A_.diag_end(), value_type(0) ); //diag element set to 0
auto const max_elem = *( std::max_element( A_.cbegin(), A_.cend() )); //find the max element
if ( value_type(0) == max_elem ) return value_type(0); //return 0 if all elements are 0
A_ /= max_elem; //in case of round-off error
auto const sum = std::inner_product( A_.cbegin(), A_.cend(), A_.cbegin(), value_type(0) ); //\sum A_{i,j}^2
return std::sqrt(sum) * max_elem;
}
const array_matrix_type make_gm( const array_vector_type& gd ) const
{
auto const N = gd.size();
array_matrix_type gr( N, N );
array_matrix_type gc( N, N );
for ( size_type i = 0; i != N; ++i )
{
std::fill( gr.row_begin( i ), gr.row_end( i ), gd[i] );
std::fill( gc.col_begin( i ), gc.col_end( i ), gd[i] );
}
auto gm = gr - gc;
//std::copy( gd.begin(), gd.end(), gm.diag_begin() );
std::fill( gm.diag_begin(), gm.diag_end(), gm[0][1] );
return gm;
}
std::size_t eigen_jacobi( const Matrix1& A, Matrix2& V, Otor o, const T eps = T( 1.0e-10 ) )
{
typedef typename Matrix1::value_type value_type;
typedef typename Matrix1::size_type size_type;
assert( A.row() == A.col() );
//assert( is_symmetric( A ) );
auto a = A;
auto const n = a.row();
auto const one = value_type( 1 );
auto const zero = value_type( 0 );
// @V = diag{1, 1, ..., 1}
V.resize( n, n );
V = zero;
std::fill( V.diag_begin(), V.diag_end(), one );
#if 1
for ( size_type i = 0; i != size_type( -1 ); ++i )
{
// @find max non-diag value in A
size_type p = 0;
size_type q = 1;
value_type current_max = std::abs( a[p][q] );
for ( size_type ip = 0; ip != n; ++ip )
for ( size_type iq = ip + 1; iq != n; ++iq )
{
auto const tmp = std::abs( a[ip][iq] );
if ( current_max > tmp )
continue;
current_max = tmp;
p = ip;
q = iq;
}
// @if all non-diag value small, then break iteration with success
if ( current_max < eps )
{
std::copy( a.diag_begin(), a.diag_end(), o );
return i;
}
// a and V iterations
eigen_jacobi_private::rotate(a, V, p, q);
}//end for
#endif
// @just to kill warnings, should never reach here
return size_type( -1 );
}//eigen_jacobi
/*
* CLI, returns results as CMD_xxx (such as CMD_EXIT)
* If argc is supplied, then this is one shot cli, ie run the command
*/
static int
do_cli(const struct cmd_tbl_entry *cmd_tbl, const char *prompt, int argc, char **argv)
{
/* Build up argc/argv */
const struct cmd_tbl_entry *ctp;
int cmd_argc;
char *cmd_argv[20];
char *input = NULL;
int rv, done;
int i;
char promptbuf[PROMPTBUFSIZE]; /* Was 1024, who needs that long a prompt? (the part before user input up to '>') */
static char nullstr[2]={0,0};
#ifdef HAVE_LIBREADLINE
//set the current command level for command completion
current_cmd_level = cmd_tbl;
#endif
rv = 0, done = 0;
snprintf(promptbuf, PROMPTBUFSIZE, "%s> ", prompt);
while (!done) {
char *inptr, *s;
if (argc == 0) {
/* Get Input */
if (input)
free(input);
input = command_line_input(promptbuf);
if (!input) {
if (instream == stdin)
printf("\n");
break;
}
/* Parse it */
inptr = input;
if (*inptr == '@') //printing comment
{
printf("%s\n", inptr);
continue;
}
if (*inptr == '#') //non-printing comment
{
continue;
}
cmd_argc = 0;
while ( (s = strtok(inptr, " \t")) != NULL ) {
cmd_argv[cmd_argc] = s;
inptr = NULL;
if (cmd_argc == (ARRAY_SIZE(cmd_argv)-1))
break;
cmd_argc++;
}
cmd_argv[cmd_argc] = nullstr;
} else {
/* Use supplied argc */
cmd_argc = argc;
for (i=0; i<=argc; i++)
cmd_argv[i] = argv[i];
}
if (cmd_argc != 0) {
ctp = cmd_tbl;
while (ctp->command) {
if (strcasecmp(ctp->command, cmd_argv[0]) == 0) {
if (ctp->sub_cmd_tbl) {
log_command(1, cmd_argv);
snprintf(promptbuf, PROMPTBUFSIZE,"%s/%s",
prompt, ctp->command);
/* Sub menu */
rv = do_cli(ctp->sub_cmd_tbl,
promptbuf,
cmd_argc-1,
&cmd_argv[1]);
#ifdef HAVE_LIBREADLINE
//went out of the sub-menu, so update the command level for command completion
current_cmd_level = cmd_tbl;
#endif
if (rv==CMD_EXIT) //allow exiting prog. from a submenu
done=1;
snprintf(promptbuf, PROMPTBUFSIZE, "%s> ", prompt);
} else {
/* Found command */
log_command(cmd_argc, cmd_argv);
rv = ctp->routine(cmd_argc, cmd_argv);
switch (rv) {
case CMD_USAGE:
printf("Usage: %s\n", ctp->usage);
break;
case CMD_EXIT:
rv = CMD_EXIT;
done = 1;
break;
case CMD_UP:
rv = CMD_UP;
done = 1;
break;
}
}
break;
}
if (!done)
ctp++;
}
if (ctp->command == NULL) {
printf("Huh? Try \"help\"\n");
}
if (argc) {
/* Single command */
done = 1;
break;
}
}
if (done)
break;
} //while !done
if (input)
free(input);
if (rv == CMD_UP)
return CMD_OK;
if (rv == CMD_EXIT) {
char *disco="disconnect";
if (global_logfp != NULL)
cmd_stoplog(0, NULL);
if (global_state > STATE_IDLE) {
do_cli(diag_cmd_table, "", 1, &disco); //XXX should be called recursively in case there are >1 active L3 conns...
}
rv=diag_end();
if (rv)
fprintf(stderr, FLFMT "diag_end failed !?\n", FL);
rv = CMD_EXIT;
}
return rv;
}
//cmd_watch : this creates a diag_l3_conn
//TODO: "press any key to stop" ...
static int
cmd_watch(int argc, char **argv)
{
int rv;
struct diag_l2_conn *d_l2_conn;
struct diag_l3_conn *d_l3_conn=NULL;
struct diag_l0_device *dl0d;
int rawmode = 0;
int nodecode = 0;
int nol3 = 0;
if (argc > 1) {
if (strcasecmp(argv[1], "raw") == 0)
rawmode = 1;
else if (strcasecmp(argv[1], "nodecode") == 0)
nodecode = 1;
else if (strcasecmp(argv[1], "nol3") == 0)
nol3 = 1;
else {
printf("Don't understand \"%s\"\n", argv[1]);
return CMD_USAGE;
}
}
rv = diag_init();
if (rv != 0) {
fprintf(stderr, "diag_init failed\n");
diag_end();
return CMD_FAILED;
}
dl0d = diag_l2_open(l0_names[set_interface_idx].longname, set_subinterface, global_cfg.L1proto);
if (dl0d == NULL) {
rv = diag_geterr();
printf("Failed to open hardware interface, ");
if (rv == DIAG_ERR_PROTO_NOTSUPP)
printf("does not support requested L1 protocol\n");
else if (rv == DIAG_ERR_BADIFADAPTER)
printf("adapter probably not connected\n");
else
printf("%s\n",diag_errlookup(rv));
return CMD_FAILED;
}
if (rawmode) {
d_l2_conn = diag_l2_StartCommunications(dl0d, DIAG_L2_PROT_RAW,
0, global_cfg.speed,
global_cfg.tgt,
global_cfg.src);
} else {
d_l2_conn = diag_l2_StartCommunications(dl0d, global_cfg.L2proto,
DIAG_L2_TYPE_MONINIT, global_cfg.speed, global_cfg.tgt, global_cfg.src);
}
if (d_l2_conn == NULL) {
printf("Failed to connect to hardware in monitor mode\n");
diag_l2_close(dl0d);
return CMD_FAILED;
}
//here we have a valid d_l2_conn over dl0d.
if (rawmode == 0) {
/* Put the SAE J1979 stack on top of the ISO device */
if (nol3 == 0) {
d_l3_conn = diag_l3_start("SAEJ1979", d_l2_conn);
if (d_l3_conn == NULL) {
printf("Failed to enable SAEJ1979 mode\n");
diag_l2_StopCommunications(d_l2_conn);
diag_l2_close(dl0d);
return CMD_FAILED;
}
} else {
d_l3_conn = NULL;
}
printf("Waiting for data to be received\n");
while (1) {
if (d_l3_conn != NULL) {
rv = diag_l3_recv(d_l3_conn, 10000,
j1979_watch_rcv,
(nodecode) ? NULL:(void *)d_l3_conn);
} else {
rv = diag_l2_recv(d_l2_conn, 10000,
j1979_watch_rcv, NULL);
}
if (rv == 0)
continue;
if (rv == DIAG_ERR_TIMEOUT)
continue;
}
} else {
//rawmode !=0 here
/*
* And just read stuff, callback routine will print out the data
*/
printf("Waiting for data to be received\n");
while (1) {
rv = diag_l2_recv(d_l2_conn, 10000,
j1979_data_rcv, (void *)&_RQST_HANDLE_WATCH);
if (rv == 0)
continue;
if (rv == DIAG_ERR_TIMEOUT)
continue;
printf("recv returns %d\n", rv);
break;
}
}
if (d_l3_conn != NULL)
diag_l3_stop(d_l3_conn);
diag_l2_StopCommunications(d_l2_conn);
diag_l2_close(dl0d);
return CMD_OK;
}
