static int cholesky_unpivoted(double *_rr,double _tol,int _n){
double akk;
int k;
/*We derive the tolerance from \cite{High90}.
Higham reported that akk*50*DBL_EPSILON was always sufficient in his
numerical experiments on matrices up to 50x50.
We use the empirical bound of 10 on ||W||_2 observed when pivoting, even
though we do no pivoting here, so this is optimistic.
@INPROCEEDINGS{High90,
author="Nicholas J. Higham",
title="Chapter 9: Analysis of the {Cholesky} Decomposition of a
Semi-Definite Matrix",
editor="Maurice G. Cox and Sven J. Hammarling",
booktitle="Reliable Numerical Computation",
publisher="Oxford University Press",
pages="161--185",
year=1990
}*/
akk=0;
for(k=0;k<_n;k++)if(_rr[UT_IDX(k,k,_n)]>akk)akk=_rr[UT_IDX(k,k,_n)];
_tol*=40*_n*(_n+1)*akk;
for(k=0;k<_n&&_rr[UT_IDX(k,k,_n)]>_tol;k++){
ch_update(_rr,sqrt(_rr[UT_IDX(k,k,_n)]),k,_n);
}
return k;
}
int cholesky(double *_rr,int *_pivot,double _tol,int _n){
double akk;
int pi;
int j;
int k;
if(_pivot==NULL)return cholesky_unpivoted(_rr,_tol,_n);
/*Find the first pivot element.*/
akk=0;
pi=-1;
for(j=0;j<_n;j++)if(_rr[UT_IDX(j,j,_n)]>akk){
pi=j;
akk=_rr[UT_IDX(j,j,_n)];
}
_tol*=40*_n*(_n+1)*akk;
/*Initialize the pivot list.*/
for(k=0;k<_n;k++)_pivot[k]=k;
for(k=0;pi>=0;k++){
if(pi!=k)ch_pivot(_rr,NULL,_pivot,k,pi,_n);
ch_update(_rr,sqrt(akk),k,_n);
/*Find the next pivot element.*/
akk=_tol;
pi=-1;
for(j=k+1;j<_n;j++)if(_rr[UT_IDX(j,j,_n)]>akk){
akk=_rr[UT_IDX(j,j,_n)];
pi=j;
}
}
return k;
}
// trace_add?
// var8, vara, varc, vare, var10
void trace_add(u16 op, FUNC *table, u8 *log_data, u16 table_offset, u16 result)
{
AGI_EVENT *temp4 = 0;
LOGIC *logic_orig = 0; // orig logic_cur
u8 *msg;
//table += op<<2;
table += op;
push_row_col();
text_attrib_push();
text_colour(0, 0xF);
trace_scroll();
if (logic_called != 0)
{
logic_called = 0;
agi_printf("==========================");
trace_scroll();
}
logic_orig = logic_cur;
if ((trace_logic==0) || ((logic_cur=logic_list_find(trace_logic)) == 0))
{
agi_printf("%d: cmd.%d", logic_orig->num, op);
}
else
{
if (op == 0)
agi_printf("%d: %s", logic_orig->num, "return");
else
{
msg = logic_msg(op + table_offset);
if (msg != 0)
agi_printf("%d: %s", logic_orig->num, logic_msg(op + table_offset));
else
{
if (result != 0xFFFF)
agi_printf("%d: eval.%d", logic_orig->num, op);
else
agi_printf("%d: cmd.%d", logic_orig->num, op);
}
}
}
logic_cur = logic_orig;
// print function name?
trace_var_print(table, log_data);
if (result != 0xFFFF)
{
goto_row_col(trace_bottom, trace_right-2);
if (result == 0)
agi_printf(" :%c", 'F');
else
agi_printf(" :%c", 'T');
}
ch_update();
while (trace_state != 0)
{
temp4 = event_read();
if (temp4 != 0)
if (temp4->type == 1)
break;
}
if (temp4 != 0)
if (temp4->data == '+')
trace_state = 2;
pop_row_col();
text_attrib_pop();
ch_update();
}
// return 0-(size-1)
// return -1 if canceled.
// list comprises of pointers to strings.. they will be cut to fit into screen
// only scrolls up and down
// first pointer = heading for listbox..
// size refers to the number of strings OTHER than the header
// init is the first item to point at.
int list_box(u8 **list, int size, int init)
{
AGISIZE list_size;
TPOS list_pos;
int item_cur;
int item_top;
AGI_EVENT *state_event;
// what can i do?
if (list == 0)
return -1;
text_attrib_push();
push_row_col();
text_colour(0, 0x0F);
item_cur = init;
if (item_cur >= size)
item_cur = size-1;
if (item_cur < 0)
item_cur = 0;
// draw message box
message_box_draw(list[0], LISTBOX_H, LISTBOX_W, 1);
// get size of remaining msgbox
list_size.h = msgstate.tpos_edge.row - msgstate.tpos.row - msgstate.printed_height;
list_size.w = msgstate.tsize.w;
if (size > list_size.h)
{
list_size.w -= 2;
}
if (size < list_size.h)
list_size.h = size;
list_pos.row = msgstate.tpos.row + msgstate.printed_height + 1;
list_pos.col = msgstate.tpos.col;
if (size <= list_size.h)
item_top = 0;
else
{
item_top = item_cur - ((list_size.h-1) / 2);
if (item_top < 0)
item_top = 0;
if (item_top > (size-list_size.h))
item_top = size-list_size.h;
}
list_print(list+1+item_top, &list_pos, &list_size,
(size>list_size.h), (item_top > 0), ((item_top + list_size.h) < size));
list_box_draw_arrow(list_pos.row + item_cur - item_top);
// later on:
// letters.. sort / left/right/pgup/pgdn/home/end/mouse/
for(;;)
{
int force;
ch_update();
state_event = event_wait();
//joy_butt_map(state_event);
switch (state_event->type)
{
case 1:
switch (state_event->data)
{
case 0xD:
pop_row_col();
text_attrib_pop();
return item_cur;
case 0x1B:
pop_row_col();
text_attrib_pop();
return -1;
}
break;
case 2:
list_box_draw_blank(list_pos.row + item_cur - item_top);
force = 0;
switch(state_event->data)
{
case 1:
item_cur--;
break;
case 5:
item_cur++;
break;
case 4:
item_cur = item_top + list_size.h-1;
force = 4;
break;
case 2:
item_cur = item_top;
force = 2;
break;
default:
;
}
if (item_cur < 0)
item_cur = 0;
else if (item_cur >= size)
item_cur = size-1;
if ((item_cur > (item_top+list_size.h-1)) || (force==4))// down below
{
item_top = item_cur - (list_size.h/4);
if (item_top < 0)
item_top = 0;
if ((item_top + list_size.h) > size)
item_top = size - list_size.h;
list_print(list+1+item_top, &list_pos, &list_size,
(size>list_size.h), (item_top > 0), ((item_top + list_size.h) < size));
}
else if ((item_cur < item_top) || (force==2)) // up above
{
item_top = item_cur - (list_size.h*3)/ 4;
if (item_top < 0)
item_top = 0;
if ((item_top + list_size.h) > size)
item_top = size - list_size.h;
list_print(list+1+item_top, &list_pos, &list_size,
(size>list_size.h), (item_top > 0), ((item_top + list_size.h) < size));
}
list_box_draw_arrow(list_pos.row + item_cur - item_top);
break;
}
}
pop_row_col();
text_attrib_pop();
return -1;
}
void s_opcodeCheck( uint8_t * pdu, uint8_t len )
{
uint8_t* attValue = NULL;
uint8_t valueLen_w;
uint16_t instantP[1] = {0};
uint16_t instantChm[1] = {0};
//memset(l2cap_pdu,0,sizeof(l2cap_pdu));
memcpy(l2cap_pdu, pdu, len);
dataHdrP = l2cap_pdu[0]; ///data header 8bit
llid = l2cap_pdu[0] & 0x03; ///get LLID
//printf("**pdu_type[%d]**", pdu_type);
if(llid == 0x01){ //data
//// server_notify(dataHdrP);
if(counter == 3){
s_llVersion( l2cap_pdu, dataHdrP);
counter++;
}else if(counter > 30){
if(i < 20){
ser_pdu_n[i] +=1;
i++;
}else{
i = 0;
}
ser_notify_handle(dataHdrP+1, attOpcode, SEC_CHAR_HD, ser_pdu_n,20 );
counter++;
}else if(counter >200){
counter = 7;
counter++;
}else{
att_empPk(dataHdrP);
counter++;
}
/*
if(counter >200){
counter = 4;
}
counter++;
if(counter == 3){
s_llVersion( l2cap_pdu, dataHdrP);
}else{
att_empPk(dataHdrP);
}*/
}else if(llid == 0x03){
//att_empPk(dataHdrP);
//llCoOpcode = l2cap_pdu[2]; /// ll control opcode
llCoOpcode = l2cap_pdu[3]; /// ll control opcode
///ll control pdu
switch(llCoOpcode){
case LL_VERSION :
//if(ver_count == 1){
// s_llVersion( l2cap_pdu, dataHdrP);
//}else{
// att_empPk( dataHdrP );
// ver_count = 1;
//}
///empty packet
att_empPk( dataHdrP );
break;
case LL_TERMINATE_IND :
att_empPk( dataHdrP );
//// s_llTer(l2cap_pdu, dataHdrP);
break;
case LL_CONN_UPDATE_REQ :
att_empPk( dataHdrP );
//update connection parameter to master
instantP[0] = l2cap_pdu[12] + (l2cap_pdu[13] << 8);
//// connPaInstant(instantP);
//// connUpdatePtr(l2cap_pdu, l2cap_len[0]);
//printf(".recv LL_CONN_UPDATE_REQ.");
break;
case LL_CH_MAP_REQ :
//att_empPk( dataHdrP );
//update connection parameter to master
ch_update(l2cap_pdu, l2cap_pdu[9] + (l2cap_pdu[10] << 8));// ch + instant
att_empPk( dataHdrP );
//// instantChm[0] = l2cap_pdu[6] + (l2cap_pdu[7] << 8);
//// connPaInstant(instantChm);
//// connUpdateChm(l2cap_pdu, l2cap_len[0]);
//printf(".recv LL_CH_MAP_REQ.");
break;
case LL_Fea_REQ :
//att_empPk( dataHdrP );
s_llFea(l2cap_pdu, dataHdrP);
break;
default:
att_empPk( dataHdrP );
break;
}
}else if(llid == 0x02){ //att
// if(l2cap_len[0] < 9){
//printf("**Len Error[%d]**", l2cap_len[0]);
// }
//att_empPk(dataHdrP);
attOpcode = l2cap_pdu[7]; ///att opcode
switch(attOpcode){
case ATT_RD_BY_TYPE_REQ :
//att_incl.start_hdl = l2cap_pdu[7] + (l2cap_pdu[8] << 8);
att_incl.start_hdl = l2cap_pdu[8] + (l2cap_pdu[9] << 8);
att_incl.end_hdl = l2cap_pdu[10] + (l2cap_pdu[11] << 8);
att_incl.uuid = l2cap_pdu[12] + (l2cap_pdu[13] << 8);
att_server_rdByType(dataHdrP, attOpcode, att_incl.start_hdl, att_incl.end_hdl, att_incl.uuid );
break;
case ATT_RD_BY_GROUP_TYPE_REQ :
att_incl.start_hdl = l2cap_pdu[8] + (l2cap_pdu[9] << 8);
att_incl.end_hdl = l2cap_pdu[10] + (l2cap_pdu[11] << 8);
att_incl.uuid = l2cap_pdu[12] + (l2cap_pdu[13] << 8);
att_server_rdByGrType( dataHdrP, attOpcode, att_incl.start_hdl, att_incl.end_hdl, att_incl.uuid );
break;
case ATT_EXCHANGE_MTU_REQ :
att_exMtuReq( dataHdrP, attOpcode, clientMtu);
break;
case ATT_FIND_IN_REQ :
att_server_findIn( dataHdrP, attOpcode, att_incl.start_hdl, att_incl.end_hdl );
break;
case ATT_RD_REQ :
attHandle = l2cap_pdu[8] + (l2cap_pdu[9] << 8);
server_rd_rsp( attOpcode, attHandle, dataHdrP);
break;
case ATT_WR_REQ :
valueLen_w = l2cap_pdu[2] + (l2cap_pdu[3] << 8) - 3;//l2caplen - opcode(1) - attHandle(2)
attHandle = l2cap_pdu[8] + (l2cap_pdu[9] << 8);
attValue = &l2cap_pdu[10];
//// ser_write_rsp( dataHdrP, attOpcode, attHandle, attValue, valueLen_w);
break;
case ATT_WR_CMD :
valueLen_w = l2cap_pdu[2] + (l2cap_pdu[3] << 8) - 3;//l2caplen - opcode(1) - attHandle(2)
attHandle = l2cap_pdu[8] + (l2cap_pdu[9] << 8);
attValue = &l2cap_pdu[10];
ser_write_no_rsp( dataHdrP, attOpcode, attHandle, attValue, valueLen_w);
break;
default:
//// att_empPk(dataHdrP);
break;
}
} else { //0
///att_empPk(0x01);
//printf("*WID[%d,%d]*", l2cap_pdu[0], l2cap_len[0]);
}
}
