static el_status_t do_case(el_case_t type)
{
int i;
int end;
int count;
char *p;
do_forward(CSstay);
if (old_point != rl_point) {
if ((count = rl_point - old_point) < 0)
count = -count;
rl_point = old_point;
if ((end = rl_point + count) > rl_end)
end = rl_end;
for (i = rl_point, p = &rl_line_buffer[i]; rl_point < end; p++) {
if ((type == TOupper) || (type == TOcapitalize && rl_point == i)) {
if (islower(*p))
*p = toupper(*p);
} else if (isupper(*p)) {
*p = tolower(*p);
}
right(CSmove);
}
}
return CSstay;
}
static void searching_ball(Info * info) {
info->sched_var.count ++;
//mylogfd(SCHFD, "count = %d\n", info->sched_var.count);
if (IsDoorSeen(info)) {
if (getDoorMidx(info) != 0 && getDoorMidx(info) < info->sched_var.door_dis) {
info->sched_var.door_dis = getDoorMidx(info);
info->sched_var.door_count = info->sched_var.count;
}
}
if (getGreenSum(info) > info->sched_var.max_green_sum) {
info->sched_var.max_green_sum = getGreenSum(info);
info->sched_var.max_green_count = info->sched_var.count;
}
if (IsBallSeen(info)) {
mylogfd(SCHFD, "[sche]search_ball->forwarding_ball\n");
info->last_cmd = CMD_FORWARD;
info->state = FORWARDING_BALL;
mylogfd(SCHFD,"[sche]left %d %d %d %d, right %d %d %d %d\n", info->video_info->is_ball_seen, info->video_info->is_ball_acc, info->video_info->ball_acc_pos.x, info->video_info->ball_acc_pos.y,
info->video_info2->is_ball_seen, info->video_info2->is_ball_acc, info->video_info2->ball_acc_pos.x, info->video_info2->ball_acc_pos.y);
info->destP.x = gety(info);
info->destP.y = - getx(info);
if (!info->video_info->is_ball_acc && !info->video_info2->is_ball_acc) {
info->destP.x = 100;
info->destP.y = 0;
}
do_forward(info, DEFAULT_A);
return;
}
if (info->sched_var.count >= 90 || checkDone()) {
mylogfd(SCHFD, "[sche]searching_ball->change_place checkdone\n");
info->state = CHANGE_PLACE;
do_search_ball(info, info->sched_var.search_dir);
return;
}
}
void forward(size_t n, size_t b, cube_p<complex>&& f) override
{
ZI_ASSERT(n<nodes::size());
if ( fwd_accumulators_[n]->add(b,std::move(f)) )
{
do_forward(n);
}
}
void forward(size_t n, cube_p<real>&& f, int idx)
{
ZI_ASSERT(n<nodes::size());
if ( fwd_accumulators_[n]->add(std::move(f),idx) )
{
do_forward(n);
}
}
void forward(size_t n, size_t b,
ccube_p<complex> const & f,
ccube_p<complex> const & w ) override
{
ZI_ASSERT(n<nodes::size());
if ( fwd_accumulators_[n]->add(b,f,w) )
{
do_forward(n);
}
}
static void shoot (Info * info) {
// mylogfd(SCHFD, "[sche]shoot %d\n", info->video_info->door_pos.y);
info->destP.x = 300;
// info->destP.y = - getDoorMidx(info) + 100 * (- getDoorMidx(info)) / getDoorMidy(info);
//mylogfd(SCHFD, "[sche shoot]left eye %d %d %d, right eye %d %d %d\n", info->video_info->is_door_seen, info->video_info->door_pos.x, info->video_info->door_pos.y,
// info->video_info2->is_door_seen, info->video_info2->door_pos.x, info->video_info2->door_pos.y);
do_forward(info, MAX_A);
info->state = SHOOTING;
}
static void search_ball(Info * info) {
//info->destP.x = gety(info);
//info->destP.y = getx(info)+50;
//do_forward(info,DEFAULT_A);
/*if (is_start == 0 && POSITI !=0) {
run_rota(45,POSITI);
run_forward(50,200);
//run_rotaR(25);
//run_forward(40,200);
is_start = 1;
}*/ //--- by hanlei
if (IsBallCatch(info)) {
info->state = BALL_CATCHED;
info->sched_var.waitlost = 3;
info->sched_var.doorseen = 0;
mylogfd(SCHFD, "[sche]State changing:SEARCH_BALL->ball_catched\n");
if (!IsDoorSeen(info)) do_rota_with_ball(info, POSITI*360, 1);
return;
}
if (IsMiddle(info)) {
if (info->sched_var.waitforward -- == 0) {
info->sched_var.waitforward = SCHED_waitforward_TIME;
info->last_cmd = CMD_FORWARD;
info->state = FORWARDING_BALL;
info->destP.x = gety(info);
info->destP.y = - getx(info);
mylogfd(SCHFD, "[sche]SEARCH_BALL->forwarding_ball\n");
do_forward(info, DEFAULT_A);
return;
}
} else {
info->sched_var.count = 0;
info->sched_var.door_count = 0;
info->sched_var.door_dis = 10000;
info->sched_var.max_green_sum = 0;
info->sched_var.max_green_count = 0;
if (IsBallAllSeen(info)) info->sched_var.wait = 0;
else if (IsBallSeen(info)) info->sched_var.wait = 1;
else info->sched_var.wait = 3;
int dir ;
if (START_FLAG == 0){
START_FLAG = 1;
dir = POSITI;
}
dir = info->sched_var.search_dir; // search_ball -- redwalker
if (IsBallSeen(info) && IsRight(info)) dir = -1;
if (IsBallSeen(info) && IsLeft(info)) dir = 1;
info->state = SEARCHING_BALL;
mylogfd(SCHFD, "[sche]SEARCH_BALL->search_ball\n");
info->last_cmd = CMD_SEARCH_BALL;
info->sched_var.search_dir = dir;
do_search_ball(info, dir);
}
}
static void change_place(Info * info) {
mylogfd(SCHFD, "green count = %d\n", info->sched_var.max_green_count);
if (info->sched_var.max_green_count-- == 0) {
info->state = CHANGING_PLACE;
info->destP.x = 100;
info->destP.y = 0;
if (info->sched_var.door_dis > 250) info->destP.x = 50;
do_forward(info, DEFAULT_A);
return;
}
}
void forward(size_t n,
ccube_p<real> const & f,
ccube_p<real> const & w,
vec3i const & stride) override
{
ZI_ASSERT(n<nodes::size());
if ( fwd_accumulators_[n]->add(f,w,stride) )
{
do_forward(n);
}
}
static void finding_dir(Info * info) {
//mylogfd(SCHFD, "[sche]finding_dir:%d %d\n", IsEdgeValid(info), getEdgeX(info));
if (IsEdgeValid(info) && (!IsOutSide(info) || getEdgeX(info) > 100)) {
info->destP.x = 50 + Random(200);
if (info->destP.x > getEdgeX(info) - 10) info->destP.x = getEdgeX(info) - 10;
info->destP.y = 0;
info->state = CHANGING_PLACE;
do_stop(info);
do_forward(info, MAX_A);
mylogfd(SCHFD, "[sche]finding_dir->changing_place");
}
}
static el_status_t fd_kill_word(void)
{
int i;
do_forward(CSstay);
if (old_point != rl_point) {
i = rl_point - old_point;
rl_point = old_point;
return delete_string(i);
}
return CSstay;
}
static void catching_ball(Info* info) {
if (IsBallCatch(info)) {
mylogfd(SCHFD, "[sche]State changing:catching_ball->ball_catched\n");
info->state = BALL_CATCHED;
info->sched_var.doorseen = 0;
info->sched_var.waitlost = 3;
do_stop(info);
if (!IsDoorSeen(info)) do_rota_with_ball(info, -POSITI*360, 1); // get in a paramiter -- redwalker
}
else if (!IsBallAllSeen(info)) {
mylogfd(SCHFD, "[sche]catching_ball->SEARCH_BALL\n");
info->state = SEARCH_BALL;
do_stop(info);
}
else if (!IsBallVeryNear(info))
{
mylogfd(SCHFD, "[sche]catching_ball->forwarding_ball\n");
info->state = FORWARDING_BALL;
info->destP.x = gety(info);
info->destP.y = - getx(info);
do_forward(info, DEFAULT_A);
}
else if (checkDone()) {
mylogfd(SCHFD,"[sche]checkDone catching_ball \n");
info->last_cmd = CMD_CATCH_BALL;
info->destP.x = gety(info);
info->destP.y = - getx(info);
do_catch_ball(info);
}
if (IsMiddle(info)) {
do_adjust(-1);
}
if (IsLeft(info)) {
do_adjust(1);
}
if (IsRight(info)) {
do_adjust(0);
}
}
static void face_door(Info * info) {
if (!IsBallCatch(info)) {
mylogfd(SCHFD, "[sche]lose ball\n");
info->state = SEARCH_BALL;
do_stop(info);
return;
}
if (getDoorRightx(info) * getDoorMidx(info) < 0) {
int cx = 0;
int x1 = getDoorMidx(info);
int y1 = getDoorMidy(info);
int x2 = getDoorRightx(info);
int y2 = getDoorRighty(info);
int cy = abs(x1) * y1 / (abs(x1) + abs(x2)) + abs(x2) * y2 / (abs(x1) + abs(x2));
int d1 = Distance(getDoorRightx(info), getDoorRighty(info), getRobotX(info), getRobotY(info));
int d2 = Distance(getRobotX(info), getRobotY(info), cx, cy);
int d = Distance(x2, y2, cx, cy);
mylogfd(SCHFD, "right x1 = %d y1 = %d x2 = %d y2 = %d cy = %d d1 = %d d2 = %d d = %d %d\n", x1, y1, x2, y2, cy, d1, d2, d, getRobotX(info));
mylogfd(SCHFD, "robot=%d door=%d\n", getRobotX(info), getDoorMidx(info));
if (!IsRobotSeen(info) || getRobotX(info) <= getDoorMidx(info)) {
// do_stop(info);
info->state = SHOOTING;
info->destP.x = cy + 100;
info->destP.y = - getDoorRightx(info) + 5;
do_forward(info, MAX_A);
return;
}
}
if (getDoorLeftx(info) * getDoorMidx(info) < 0) {
int cx = 0;
int x1 = getDoorMidx(info);
int y1 = getDoorMidy(info);
int x2 = getDoorLeftx(info);
int y2 = getDoorLefty(info);
int cy = abs(x1) * y1 / (abs(x1) + abs(x2)) + abs(x2) * y2 / (abs(x1) + abs(x2));
int d1 = Distance(getDoorLeftx(info), getDoorLefty(info), getRobotX(info), getRobotY(info));
int d2 = Distance(getRobotX(info), getRobotY(info), cx, cy);
int d = Distance(x2, y2, cx, cy);
mylogfd(SCHFD, "left:x1 = %d y1 = %d x2 = %d y2 = %d cy = %d d1 = %d d2 = %d d = %d %d\n", x1, y1, x2, y2, cy, d1, d2, d, getRobotX(info));
mylogfd(SCHFD, "robot=%d door = %d\n", getRobotX(info), getDoorMidx(info));
if (!IsRobotSeen(info) || getRobotX(info) >= getDoorMidx(info)) {
info->state = SHOOTING;
//do_stop(info);
info->state = SHOOTING;
info->destP.x = cy + 100;
info->destP.y = - getDoorLeftx(info) - 5;
do_forward(info, MAX_A); // adjust while shooting -- redwalker
if (!IsBallCatch(info)) {
mylogfd(SCHFD, "[sche]lose ball\n");
info->state = SEARCH_BALL;
do_stop(info);
return;
}
if (IsDoorMiddle(info)) {
do_adjust(-1);
}
if (IsDoorLeft(info)) {
do_adjust(1);
}
if (IsDoorRight(info)) {
do_adjust(0);
}
return;
}
}
if (checkDone()) {
mylogfd(SCHFD,"[sche]checkDone face_door\n");
info->sched_var.search_door_dir = -info->sched_var.search_door_dir;
do_rota_with_ball(info, 90 * info->sched_var.search_door_dir, 1);
return;
}
}
static el_status_t fd_word(void)
{
return do_forward(CSmove);
}
int service_vsap(Connection *Conn)
{ CStr(request,1024);
CStr(reqver,128);
const char *req;
int svsock,shared,clsock,rcode;
CStr(myport,256);
CStr(sockname,MaxHostNameLen);
CStr(peername,MaxHostNameLen);
int wcc,rcc;
CStr(com,1024);
CStr(arg,1024);
const char *argp;
CStr(opt,32);
const char *op;
int timeout;
int AuthOk;
FILE *authout;
minit_vsapsv();
if( ToS <= 0 || FromS <= 0 ){
/*
If the DST_HOST is not local
connect to the master and simple_relay...
*/
}
if( !isMYSELF(DFLT_HOST) ){
daemonlog("E","VSAP relaying to %s:%d\n",DFLT_HOST,DFLT_PORT);
if( ToS < 0 )
connect_to_serv(Conn,FromC,ToC,0);
relay_svcl(Conn,FromC,ToC,FromS,ToS);
close(ToS);
return 0;
}
/*
timeout = 300;
*/
timeout = IO_TIMEOUT;
shared = 0;
myport[0] = 0;
SvSockN = 0;
ClSockN = 0;
clsock = -1;
svsock = -1;
reqver[0] = 0;
authout = TMPFILE("VSAP-AUTH");
if( doAUTH(Conn,NULL,authout,"vsap","-",0,CVStr("user-xxxx:pass-xxxx"),CVStr("host-xxxx"),NULL,NULL) == EOF ){
AuthOk = 0;
}else AuthOk = -1;
if( ImMaster ){
sprintf(myport,"%s:%d",DST_HOST,DST_PORT);
}else
for(;;){
if( DDI_fgetsFromCbuf(Conn,AVStr(request),sizeof(request),NULL) == 0 )
{ int closed = 0;
for(;;){
if( PollIn(FromC,1*1000) != 0 )
break;
closed |= checkCloseOnTimeout(1);
if( 0 <= clsock && !IsAlive(clsock) ){
daemonlog("E","## disconnected by peer\n");
SockPrintf(ToC,"%s %d %s.\r\n",VER,NO_GENERIC_BYE,"disconnected by peer");
close(clsock);del_clsock(clsock);
goto EXIT;
}
}
if( (rcc = RecvLine(FromC,request,sizeof(request))) <= 0 )
break;
}
daemonlog("D","CLIENT-SAYS: %s",request);
daemonlog("E","CLIENT-SAYS: %s",request);
req = request;
if( strncmp(req,"VSAP/",5) == 0 )
req = wordScan(req,reqver);
argp = wordScan(req,com);
arg[0] = 0;
lineScan(argp,arg);
if( strcasecmp(com,"AUTH") == 0 ){
CStr(ahost,MaxHostNameLen);
ahost[0] = 0;
if( doAUTH(Conn,NULL,authout,"vsap","-",0,AVStr(arg),AVStr(ahost),NULL,NULL) == EOF ){
}else{
AuthOk = 1;
SockPrintf(ToC,"%s %d OK\r\n",VER,OK_GENERIC);
continue;
}
}
if( AuthOk == 0 ){
SockPrintf(ToC,"%s %d forbidden\r\n",VER,NO_PERMISSION);
sv1log("WITH AUTHORIZER, but NO AUTH from client\n");
break;
}
if( strcasecmp(com,"ECHO") == 0 ){
CStr(stime,64);
StrftimeLocal(AVStr(stime),sizeof(stime),TIMEFORM_HTTPD,time(0),0);
SockPrintf(ToC,"%s %d [%s] %s\r\n",VER,OK_GENERIC,
stime,arg);
}else
if( strcasecmp(com,"CONNECT") == 0 ){
strcpy(myport,arg);
if( !vsap_permit(Conn,myport) )
break;
clsock = do_connect(svsock,myport,ToC);
if( clsock < 0 )
break;
}else
if( strcasecmp(com,"BIND") == 0 ){
CStr(opts,1024);
opts[0] = 0;
Xsscanf(arg,"%s %[^\r\n]",AVStr(myport),AVStr(opts));
if( !vsap_permit(Conn,myport) )
break;
svsock = do_bind(-1,AVStr(myport),opts,&shared,AVStr(sockname),ToC);
if( svsock < 0 )
break;
}else
if( strcasecmp(com,"LISTEN") == 0 ){
int nlisten = atoi(arg);
Socket1("VSAP",svsock,NULL,NULL,NULL,VStrANYPORT,ANYPORT,nlisten,NULL,0);
SockPrintf(ToC,"%s %d listen ok.\r\n",VER,OK_LISTEN);
}else
if( strcasecmp(com,"ACCEPT") == 0 ){
int priority;
if( Conn->cl_count <= 1 )
priority = 0;
else priority = 1;
clsock = do_accept(myport,arg,shared,priority,FromC,ToC);
if( !shared )
svsock = -1;
if( clsock < 0 ){
wcc = SockPrintf(ToC,"%s %d accept fail\r\n",
VER,NO_ACCEPT);
break;
}
add_clsock(clsock);
if( myport[0] == '/' ){
strcpy(sockname,myport);
strcpy(peername,myport);
}else getpairName(clsock,AVStr(sockname),AVStr(peername));
wcc = SockPrintf(ToC,"%s %d %d %s %s accepted.\r\n",
VER,OK_ACCEPT,ClSockN,sockname,peername);
}else
if( strcasecmp(com,"QUIT") == 0 ){
SockPrintf(ToC,"%s %d bye.\r\n",VER,OK_BYE);
break;
}else
if( strcasecmp(com,"FORWARD") == 0 ){
do_forward(Conn,myport,arg,shared,svsock,0,FromC,ToC);
}else
if( strcasecmp(com,"RELAY") == 0 ){
/*
-t timeout
*/
for( op = arg; *op == '-'; ){
if( strneq(op,"-t=",3) ){
int to;
op = numscanX(op+3,AVStr(opt),sizeof(opt));
to = atoi(opt);
if( to < timeout )
timeout = to;
}else{
break;
}
}
tcp_relay2(timeout*1000,FromC,clsock,clsock,ToC);
/*
set_linger(clsock,10);
*/
set_linger(clsock,LIN_TIMEOUT);
close(clsock);del_clsock(clsock);
break;
}else
if( strcasecmp(com,"PROXY") == 0 ){
ToS = FromS = ToC;
ToC = FromC = clsock;
daemonlog("E","##### VSAP switch protocol to '%s'\n",arg);
if( strcmp(arg,"http")==0 )
service_http(Conn);
else
if( strcmp(arg,"ftp")==0 )
service_ftp(Conn);
set_linger(clsock,10);
close(clsock);del_clsock(clsock);
break;
}else
{
/*
wcc = write(clsock,req,rcc);
tcp_relay2(timeout*1000,FromC,clsock,clsock,ToC);
*/
SockPrintf(ToC,"%s %d %s",VER,NO_GENERIC,request);
}
}
EXIT:
fclose(authout);
return 0;
}
