/*
 * Userspace program that communicates with the led_vga device driver
 * primarily through ioctls
 *
 * Stephen A. Edwards
 * Revised by Penrose
 *


//1.ball control when changing plane
//2.goal point
 * Columbia University
 */

#include <stdio.h>
#include "vga_led.h"
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include "usbkeyboard.h"
#include<math.h>

#define BUFFER_size 128
#define cubnum  5

struct cpnode{
    int plane_num;
    float x_node, y_node, z_node;
    float x_va[256];
    float y_va[256];
    float z_va[256];
}cp_node[cubnum*2][cubnum];


struct Ball{
    int plane_num;      // the plane the Ball stays in
    int bx, by, bz;
    int b_dict;
    int next_jump_p;
    int jump_x, jump_y, jump_z;
    int jump_Node;
    int prior_plane;
    int prior_direct;
    int next_is_edge;
    int edge_plane_num;
    int outedge;
    int ret_bx, ret_by, ret_bz;
    int ret_direct;
    int return_protect;
}balls[cubnum-1];


struct Plane{
    int connectpoint_num;
    int N_connect[cubnum-1];
    int P_x[cubnum-1];
    int P_y[cubnum-1];
    int P_z[cubnum-1];
    int edge_node[2];
    int edge_node_connect[2];
    int edge_node_jumpx[2];
    int edge_node_jumpy[2];
    int edge_node_jumpz[2];
    int edge_node_jump_plane[2];
    
}Plane_info[cubnum];



//
float D = 0;
float D2 = 0, D3;
float precision = 0.01;
int cubic[8];
float v_x,v_y,v_z;
float v_x1,v_y1,v_z1;
float v_x2,v_y2,v_z2;
double rotate_angle_x, rotate_angle_y;
int cubic_nodes_num[cubnum];
int record_location1x, record_location2x,record_location1y, record_location2y,record_location1z, record_location2z;
int seeSurface1, nonSeeSurface2, parallel;

//



//
 struct libusb_device_handle *keyboard;
 uint8_t endpoint_address;
 int sub0;
 int sub1;
 int sub2;
 int vga_led_fd;
 int x_angle_l;
 int y_angle_l;
 int x_angle_h;
 int y_angle_h;
 int rotation_rate = 0;
 int flag = 0;
 int ball_reach[cubnum-1];

 //end point info
  int x_ep;
  int y_ep;
  int z_ep;

 //trans cubics info
  int extend_d[cubnum];
  int x_s[cubnum];
  int x_e[cubnum];
  int y_s[cubnum];
  int y_e[cubnum];
  int z_s[cubnum];
  int z_e[cubnum];

///////////////////////Cong///////////////////////
  int extend_d1[] = {3,1,3,1,1};
  int x_s1[] = {-50, -40, 40, -20, -50};
  int x_e1[] = {-40, 20, 50, 50, 20};
  int y_s1[] = {-50, 40, 0, -20, -20};
  int y_e1[] = {-40, 50, 10, -10, -10};
  int z_s1[] = {-30, -50, -10, 40, 0};
  int z_e1[] = {50, -40, 30, 50, 10};

  int extend_d2[] = {3,3,3,3,3};
  int x_s2[] = {-50, -20, 10, 40, 40};
  int x_e2[] = {-40, -10, 20, 50, 50};
  int y_s2[] = {-50, 40, -50, 40, -50};
  int y_e2[] = {-40, 50, -40, 50, -40};
  int z_s2[] = {-10, -50, -50, -50, -10};
  int z_e2[] = {50, 10, 10, 10, 50};

  int extend_d3[] = {3,1,3,1,1};
  int x_s3[] = {-50, -40, 40, -40, -5};
  int x_e3[] = {-40, 40, 50, 40, 25};
  int y_s3[] = {30, 30, 30, 30, -5};
  int y_e3[] = {40, 40, 40, 40, 5};
  int z_s3[] = {-40, -50, -40, 40, -5};
  int z_e3[] = {40, -40, 40, 50, 5};

  int extend_d4[] = {3,1,3,1,3};
  int x_s4[] = {-50, -50, 10, -10, 40};
  int x_e4[] = {-40, 10, 20, 50, 50};
  int y_s4[] = {-50, 40, -50, 40, -50};
  int y_e4[] = {-40, 50, -40, 50, -40};
  int z_s4[] = {-10, -40, -50, 30, -10};
  int z_e4[] = {50, -30, 10, 40, 50};

  int extend_d5[] = {3,1,3,1,3};
  int x_s5[] = {-50, -30, 20, 20, 0};
  int x_e5[] = {-40, 30, 30, 80, 10};
  int y_s5[] = {-10, 40, 40, -20, -50};
  int y_e5[] = {0, 50, 50, -10, -40};
  int z_s5[] = {-50, 40, -30, 40, -10};
  int z_e5[] = {10, 50, 30, 50, 50};

  int extend_d6[] = {1,1,1,1,1};
  int x_s6[] = {-5, -5, -5, -5, -5};
  int x_e6[] = {5, 5, 5, 5, 5};
  int y_s6[] = {-5, -5, -5, -5, -5};
  int y_e6[] = {5, 5, 5, 5, 5};
  int z_s6[] = {-5, -5, -5, -5, -5};
  int z_e6[] = {5, 5, 5, 5, 5};
//////////////////////////////////////////////////

  int width = 5;

/*  modified by ly  */
 static unsigned char message[VGA_LED_DIGITS] =  
{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };



//ball control
void initialize_ball(){
    int bii,wi;
    for(bii=0;bii<cubnum-1;bii++){
        balls[bii].plane_num = bii;
        balls[bii].bx = x_s[bii] + 5;
        balls[bii].by = y_s[bii] + 5;
        balls[bii].bz = z_s[bii] + 5;
	balls[bii].b_dict = 1;
	balls[bii].next_jump_p = 0;  		//0 not exits
	balls[bii].prior_plane = -1;
	balls[bii].prior_direct = 1;
	balls[bii].next_is_edge = 0;
	balls[bii].outedge = 0;
	//balls[bii].return_protect = 0;
	
    } 


    for(wi = 0;wi<4;wi++){
	message[40+wi*3] = balls[wi].bx;
	message[41+wi*3] = balls[wi].by-10;
	message[42+wi*3] = balls[wi].bz;
    }
    write_segments(message);


}


///////////////////////////////////////////////////////Cong/////////////////////////////////////////////
void initialize_endPoint(){
	x_ep = (x_s[4] + x_e[4])/2;
  	y_ep = y_s[4];
  	z_ep = (z_s[4] + z_e[4])/2;

	message[7] = x_ep;
	message[8] = y_ep;
	message[9] = z_ep;

}
////////////////////////////////////////////////////////////////////////////////////////////////////////	

void initialize_plane(){
	int i;	
	for(i = 0;i<cubnum;i++){
		Plane_info[i].connectpoint_num = 0;
		Plane_info[i].edge_node_connect[0] = 0;		//low one
		Plane_info[i].edge_node_connect[1] = 0;		//high one

	}
}






void ball_moving(){

    int bi, wi;
    int kk, kk2;
    for (bi = 0; bi<cubnum-1; bi++) {

	if(balls[bi].bx==x_ep&&balls[bi].by==(y_ep+5)&&balls[bi].bz==z_ep) ball_reach[bi] = 1;
	if(ball_reach[bi] == 1) continue;	

        if ((balls[bi].bx>=(x_s[balls[bi].plane_num]+5)&&balls[bi].bx<=(x_e[balls[bi].plane_num]-5)&&balls[bi].by>=(y_s[balls[bi].plane_num]+5)&&
            balls[bi].by<=(y_e[balls[bi].plane_num]-5)&&balls[bi].bz>=(z_s[balls[bi].plane_num]+5)&&balls[bi].bz<=(z_e[balls[bi].plane_num]-5))||balls[bi].next_is_edge == 1){
        if (balls[bi].next_jump_p == 0) {
            switch (extend_d[balls[bi].plane_num]) {
                case 1:{
                    if (balls[bi].b_dict == 1) {
                        if (balls[bi].bx < x_e[balls[bi].plane_num] - 5) {
                            balls[bi].bx = balls[bi].bx + balls[bi].b_dict;
                        }
                        else{
                            balls[bi].b_dict = - balls[bi].b_dict;
			    calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
                        if (balls[bi].bx > x_s[balls[bi].plane_num] + 5) {
                            balls[bi].bx = balls[bi].bx + balls[bi].b_dict;
                        }
                        else{
                            balls[bi].b_dict = - balls[bi].b_dict;
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                case 2:{
                    if (balls[bi].b_dict == 1) {
                        if (balls[bi].by < y_e[balls[bi].plane_num] - 5) {
                            balls[bi].by = balls[bi].by + balls[bi].b_dict;
                        }
                        else{
                            balls[bi].b_dict = - balls[bi].b_dict;
			    calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
                        if (balls[bi].by > y_s[balls[bi].plane_num] + 5) {
                            balls[bi].by = balls[bi].by + balls[bi].b_dict;
                        }
                        else{
                            balls[bi].b_dict = - balls[bi].b_dict;
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                case 3:{
                    if (balls[bi].b_dict == 1) {
                        if (balls[bi].bz < z_e[balls[bi].plane_num] - 5) {
                            balls[bi].bz = balls[bi].bz + balls[bi].b_dict;
                        }
                        else{
                            balls[bi].b_dict = - balls[bi].b_dict;
		            calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
                        if (balls[bi].bz > z_s[balls[bi].plane_num] + 5) {
                            balls[bi].bz = balls[bi].bz + balls[bi].b_dict;
                        }
                        else{
                            balls[bi].b_dict = - balls[bi].b_dict;
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                default:
                    break;
            }

        }
	
        else if(balls[bi].next_jump_p == 1&&balls[bi].next_is_edge == 0){
            switch (extend_d[balls[bi].plane_num]) {
                case 1:{
                    if (balls[bi].b_dict == 1) {
                        if (balls[bi].bx < balls[bi].jump_x) {
                            balls[bi].bx = balls[bi].bx + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                            
                        }
                    }
                    else if(balls[bi].b_dict == -1){
                        if (balls[bi].bx > balls[bi].jump_x) {
                            balls[bi].bx = balls[bi].bx + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                case 2:{
                    if (balls[bi].b_dict == 1) {
                        if (balls[bi].by < balls[bi].jump_y) {
                            balls[bi].by = balls[bi].by + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
                        if (balls[bi].by > balls[bi].jump_y) {
                            balls[bi].by = balls[bi].by + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                case 3:{
                    if (balls[bi].b_dict == 1) {
                        if (balls[bi].bz < balls[bi].jump_z) {
                            balls[bi].bz = balls[bi].bz + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
                        if (balls[bi].bz > balls[bi].jump_z) {
                            balls[bi].bz = balls[bi].bz + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                default:
                    break;
            }
        }
	else if(balls[bi].next_jump_p == 1&&balls[bi].next_is_edge == 1){
			
			balls[bi].ret_bx = balls[bi].jump_x;
			balls[bi].ret_by = balls[bi].jump_y;
			balls[bi].ret_bz = balls[bi].jump_z;
			balls[bi].ret_direct = balls[bi].plane_num;
			

		switch (extend_d[balls[bi].plane_num]) {
		
                case 1:{
                    if (balls[bi].b_dict == 1) {
			if (balls[bi].bx >= x_e[balls[bi].plane_num] - 5)balls[bi].return_protect = 1;
			else balls[bi].return_protect = 0;
                        if (balls[bi].bx < x_e[balls[bi].plane_num] + 5) {
                            balls[bi].bx = balls[bi].bx + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
			if (balls[bi].bx <= x_s[balls[bi].plane_num] + 5)balls[bi].return_protect = 1;
			else balls[bi].return_protect = 0;
                        if (balls[bi].bx > x_s[balls[bi].plane_num] - 5) {
                            balls[bi].bx = balls[bi].bx + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                case 2:{
                    if (balls[bi].b_dict == 1) {
			if (balls[bi].by >= y_e[balls[bi].plane_num] - 5)balls[bi].return_protect = 1;
			else balls[bi].return_protect = 0;
                        if (balls[bi].by < y_e[balls[bi].plane_num] + 5) {
                            balls[bi].by = balls[bi].by + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
			if (balls[bi].by <= y_s[balls[bi].plane_num] + 5)balls[bi].return_protect = 1;
			else balls[bi].return_protect = 0;
                        if (balls[bi].by > y_s[balls[bi].plane_num] - 5) {
                            balls[bi].by = balls[bi].by + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                case 3:{
                    if (balls[bi].b_dict == 1) {
			if (balls[bi].bz >= z_e[balls[bi].plane_num] - 5)balls[bi].return_protect = 1;
			else balls[bi].return_protect = 0;
                        if (balls[bi].bz < z_e[balls[bi].plane_num] + 5) {
                            balls[bi].bz = balls[bi].bz + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
		            calculate_jump_point(bi);
                        }
                    }
                    else if(balls[bi].b_dict == -1){
			if (balls[bi].bz <= z_s[balls[bi].plane_num] + 5)balls[bi].return_protect = 1;
			else balls[bi].return_protect = 0;
                        if (balls[bi].bz > z_s[balls[bi].plane_num] - 5) {
                            balls[bi].bz = balls[bi].bz + balls[bi].b_dict;
                        }
                        else{
                            ball_changeplane(bi);
			    calculate_jump_point(bi);
                        }
                    }
                    break;
                }
                    
                default:
                    break;
            }
	}
    }


        else{
		////printf("try");
            switch (extend_d[balls[bi].plane_num]){
                case 1: {
		    if(balls[bi].b_dict==-1&&balls[bi].bx<x_s[balls[bi].plane_num]-5)balls[bi].b_dict=1;
		    if(balls[bi].b_dict==1&&balls[bi].bx>x_e[balls[bi].plane_num]+5)balls[bi].b_dict=-1;
                    balls[bi].bx += balls[bi].b_dict;
                    break;
                }
                case 2: {
 		    if(balls[bi].b_dict==-1&&balls[bi].by<y_s[balls[bi].plane_num]-5)balls[bi].b_dict=1;
		    if(balls[bi].b_dict==1&&balls[bi].by>y_e[balls[bi].plane_num]+5)balls[bi].b_dict=-1;
                    balls[bi].by += balls[bi].b_dict;
                    break;
                }
                case 3: {
		    if(balls[bi].b_dict==-1&&balls[bi].bz<z_s[balls[bi].plane_num]-5)balls[bi].b_dict=1;
		    if(balls[bi].b_dict==1&&balls[bi].bz>z_e[balls[bi].plane_num]+5)balls[bi].b_dict=-1;
                    balls[bi].bz += balls[bi].b_dict;
                    break;
                }
            }
        }
	
}
    	
	for(kk=0;kk<cubnum;kk++){
		for(kk2=kk+1;kk2<cubnum;kk2++)
		if(balls[kk].return_protect==0&&balls[kk2].return_protect==0)combine_ball(kk,kk2);
	}
// write message
	for(wi = 0;wi<4;wi++){
		message[40+wi*3] = balls[wi].bx;
		message[41+wi*3] = balls[wi].by-10;
		message[42+wi*3] = balls[wi].bz;
	}
	write_segments(message);
	////printf("x, y, z = %d, %d, %d\n", message[40], message[41], message[42]);
	////printf("x, y, z = %d, %d, %d\n", balls[0].bx, balls[0].by, balls[0].bz);
	////printf("dict  = %d\n", balls[0].b_dict);
	//sleep(1);
}


void ball_changeplane(int ballnum){
	
    if(balls[ballnum].next_is_edge == 0){
    balls[ballnum].next_jump_p = 0;
    balls[ballnum].prior_plane = balls[ballnum].plane_num;



    
//
    balls[ballnum].bx = (int)cp_node[balls[ballnum].jump_Node][0].x_node;
    balls[ballnum].by = (int)cp_node[balls[ballnum].jump_Node][0].y_node;
    balls[ballnum].bz = (int)cp_node[balls[ballnum].jump_Node][0].z_node;

	if(extend_d[cp_node[balls[ballnum].jump_Node][0].plane_num] == extend_d[balls[ballnum].plane_num]){
		
switch(extend_d[balls[ballnum].plane_num]){

			case 1:{if(((balls[ballnum].bx + balls[ballnum].prior_direct*5)!=(x_s[cp_node[balls[ballnum].jump_Node][0].plane_num]))&&((balls[ballnum].bx + balls[ballnum].prior_direct*5)!=(x_e[cp_node[balls[ballnum].jump_Node][0].plane_num])))balls[ballnum].prior_direct = -balls[ballnum].b_dict;printf("%d\n",balls[ballnum].prior_direct);break;}
			case 2:{if(((balls[ballnum].by + balls[ballnum].prior_direct*5)!=(y_s[cp_node[balls[ballnum].jump_Node][0].plane_num]))&&((balls[ballnum].by + balls[ballnum].prior_direct*5)!=(y_e[cp_node[balls[ballnum].jump_Node][0].plane_num])))balls[ballnum].prior_direct = -balls[ballnum].b_dict;printf("%d\n",balls[ballnum].prior_direct);break;}
			case 3:{if(((balls[ballnum].bz + balls[ballnum].prior_direct*5)!=(z_s[cp_node[balls[ballnum].jump_Node][0].plane_num]))&&((balls[ballnum].bz + balls[ballnum].prior_direct*5)!=(z_e[cp_node[balls[ballnum].jump_Node][0].plane_num])))balls[ballnum].prior_direct = -balls[ballnum].b_dict;printf("%d\n",balls[ballnum].prior_direct);break;}
		}	

	}
	else

    balls[ballnum].prior_direct = balls[ballnum].b_dict;

balls[ballnum].plane_num = cp_node[balls[ballnum].jump_Node][0].plane_num;
    if (balls[ballnum].jump_Node%2==0) {
        balls[ballnum].b_dict = 1;
    }
    else
        balls[ballnum].b_dict = -1;
    balls[ballnum].return_protect = 0;
    //calculate_jump_point(ballnum);
	////printf("new coor x = %d, y = %d, z = %d\n", balls[ballnum].bz,balls[ballnum].by,balls[ballnum].bz);
	////printf("new plane num = %d\n", balls[ballnum].plane_num);
    }
    else if(balls[ballnum].next_is_edge == 1){
	balls[ballnum].next_jump_p = 0;

        if(extend_d[balls[ballnum].edge_plane_num] == extend_d[balls[ballnum].plane_num])
	balls[ballnum].b_dict = -balls[ballnum].prior_direct;
	else{
	balls[ballnum].b_dict = balls[ballnum].prior_direct;
}
	balls[ballnum].prior_plane = balls[ballnum].plane_num;
	//balls[ballnum].prior_direct = balls[ballnum].b_dict;
	balls[ballnum].plane_num = balls[ballnum].edge_plane_num;
	balls[ballnum].bx = balls[ballnum].jump_x;
    	balls[ballnum].by = balls[ballnum].jump_y;
   	balls[ballnum].bz = balls[ballnum].jump_z;	
	balls[ballnum].next_is_edge = 0;
	balls[ballnum].return_protect = 0;

/*
	//printf("--------------------------\n");
//printf("planenum = %d, dict = %d\n", balls[0].plane_num, balls[0].b_dict);
//printf("bx = %d, by = %d, bz = %d\n", balls[0].bx, balls[0].by, balls[0].bz);
//printf("next jump point = %d, nodenum = %d\n", balls[0].next_jump_p, balls[0].jump_Node);
//printf("nie = %d, ep = %d\n", balls[0].next_is_edge, balls[0].edge_plane_num);
//printf("jumpx = %d, jumpy = %d, jumpz = %d\n", balls[0].jump_x, balls[0].jump_y, balls[0].jump_z);
*/


    }
}



// calculate the nearest jump point for ball
void calculate_jump_point(int ball_num){
    int i;
    int x_value, y_value, z_value;
    int first = 1;
    int diff;
    int diff_value1;
    int diff_value2;
    int pass = 0;
    diff = extend_d[balls[ball_num].plane_num];
    balls[ball_num].next_jump_p = 0;
    balls[ball_num].next_is_edge = 0;
    if (Plane_info[balls[ball_num].plane_num].connectpoint_num != 0){
        //balls[ball_num].next_jump_p = 1;
        for (i = 0; i<Plane_info[balls[ball_num].plane_num].connectpoint_num; i++){
            //if the jumping point is in front of balls direct
            if (((Plane_info[balls[ball_num].plane_num].P_x[i]-balls[ball_num].bx)/balls[ball_num].b_dict)>0||((Plane_info[balls[ball_num].plane_num].P_y[i]-balls[ball_num].by)/balls[ball_num].b_dict)>0||((Plane_info[balls[ball_num].plane_num].P_z[i]-balls[ball_num].bz)/balls[ball_num].b_dict)>0) {
	
			if(balls[ball_num].bx != Plane_info[balls[ball_num].plane_num].P_x[i]||balls[ball_num].by != Plane_info[balls[ball_num].plane_num].P_y[i]||balls[ball_num].bz != Plane_info[balls[ball_num].plane_num].P_z[i]){
                    balls[ball_num].next_jump_p = 1;
			//printf("kllll + %d\n",ball_num);
			}
                if (first==1) {
                    first = 0;
                    balls[ball_num].jump_Node = Plane_info[balls[ball_num].plane_num].N_connect[i];
                    balls[ball_num].jump_x =Plane_info[balls[ball_num].plane_num].P_x[i];
                    balls[ball_num].jump_y =Plane_info[balls[ball_num].plane_num].P_y[i];
                    balls[ball_num].jump_z =Plane_info[balls[ball_num].plane_num].P_z[i];
                    switch (diff) {
                        case 1:{
                            diff_value1 = Plane_info[balls[ball_num].plane_num].P_x[i]-balls[ball_num].bx;
                            break;
                        }
                        case 2:{
                            diff_value1 = Plane_info[balls[ball_num].plane_num].P_y[i]-balls[ball_num].by;
                            break;
                        }
                        case 3:{
                            diff_value1= Plane_info[balls[ball_num].plane_num].P_z[i]-balls[ball_num].bz;
                            break;
                        }
                        default:
                            break;
                    }
                    diff_value2 = diff_value1;
                }
                else{
                    switch (diff) {
                        case 1:{
                            diff_value1 = Plane_info[balls[ball_num].plane_num].P_x[i]-balls[ball_num].bx;
                            break;
                        }
                        case 2:{
                            diff_value1 = Plane_info[balls[ball_num].plane_num].P_y[i]-balls[ball_num].by;
                            break;
                        }
                        case 3:{
                            diff_value1 = Plane_info[balls[ball_num].plane_num].P_z[i]-balls[ball_num].bz;
                            break;
                        }
                        default:
                            break;
                    }
                    if (diff_value1<diff_value2) {
                        balls[ball_num].jump_Node = Plane_info[balls[ball_num].plane_num].N_connect[i];
                        balls[ball_num].jump_x =Plane_info[balls[ball_num].plane_num].P_x[i];
                        balls[ball_num].jump_y =Plane_info[balls[ball_num].plane_num].P_y[i];
                        balls[ball_num].jump_z =Plane_info[balls[ball_num].plane_num].P_z[i];
                    }
                }
            }
        }           
    }  
	if(balls[ball_num].next_jump_p == 0){
		if(Plane_info[balls[ball_num].plane_num].edge_node_connect[0]==1&&balls[ball_num].b_dict==-1){
			balls[ball_num].next_is_edge = 1;
			balls[ball_num].jump_x = Plane_info[balls[ball_num].plane_num].edge_node_jumpx[0];
			balls[ball_num].jump_y = Plane_info[balls[ball_num].plane_num].edge_node_jumpy[0];
			balls[ball_num].jump_z = Plane_info[balls[ball_num].plane_num].edge_node_jumpz[0];
			balls[ball_num].edge_plane_num = Plane_info[balls[ball_num].plane_num].edge_node_jump_plane[0];
			balls[ball_num].next_jump_p = 1;
		}
		else if(Plane_info[balls[ball_num].plane_num].edge_node_connect[1]==1&&balls[ball_num].b_dict==1){
			balls[ball_num].next_is_edge = 1;
			balls[ball_num].jump_x = Plane_info[balls[ball_num].plane_num].edge_node_jumpx[1];
			balls[ball_num].jump_y = Plane_info[balls[ball_num].plane_num].edge_node_jumpy[1];
			balls[ball_num].jump_z = Plane_info[balls[ball_num].plane_num].edge_node_jumpz[1];
			balls[ball_num].edge_plane_num = Plane_info[balls[ball_num].plane_num].edge_node_jump_plane[1];
			balls[ball_num].next_jump_p = 1;
		}
	} 

}


/* Read and print the segment values */
void print_segment_info() {
  vga_led_arg_t vla;
  int i;

  for (i = 0 ; i < VGA_LED_DIGITS ; i++) {
    vla.digit = i;
    if (ioctl(vga_led_fd, VGA_LED_READ_DIGIT, &vla)) {
      perror("ioctl(VGA_LED_READ_DIGIT) failed");
      return;
    }
    //printf("%02x ", vla.segments);
  }
  //printf("\n");
}

/* Write the contents of the array to the display */
void write_segments(const unsigned char segs[64]) /*  modified by ly  */
{
  vga_led_arg_t vla;
  int i;
  for (i = 0 ; i < VGA_LED_DIGITS ; i++) {
    vla.digit = i;
    vla.segments = segs[i];
    if (ioctl(vga_led_fd, VGA_LED_WRITE_DIGIT, &vla)) {
      perror("ioctl(VGA_LED_WRITE_DIGIT) failed");
      return;
    }
  }
}

int main()
{

/*  Variable defination  */
  int i;
  static const char filename[] = "/dev/vga_led";
  int operation;
  int err, col;
  struct usb_keyboard_packet packet;
  int transferred;
  char keystate[12];

  x_angle_l = 0;
  y_angle_l = 0;
  x_angle_h = 0;
  y_angle_h = 0;

  memcpy(&extend_d, &extend_d1, 20);
  memcpy(&x_s, &x_s1, 20);
  memcpy(&x_e, &x_e1, 20);
  memcpy(&y_s, &y_s1, 20);
  memcpy(&y_e, &y_e1, 20);
  memcpy(&z_s, &z_s1, 20);
  memcpy(&z_e, &z_e1, 20);

  vga_led_arg_t vla;

/*  Open the keyboard  */
  if ( (keyboard = openkeyboard(&endpoint_address)) == NULL ) {
    fprintf(stderr, "Did not find a keyboard\n");
    exit(1);
  }
  //printf("VGA LED Userspace program started\n");
  if ( (vga_led_fd = open(filename, O_RDWR)) == -1) {
    fprintf(stderr, "could not open %s\n", filename);
    return -1;
  }
  //printf("initial state: ");
  print_segment_info();
  write_segments(message);
  //printf("current state: ");
  print_segment_info();
  //printf("VGA LED Userspace program started\n");
  if ( (vga_led_fd = open(filename, O_RDWR)) == -1) {
    fprintf(stderr, "could not open %s\n", filename);
    return -1;
  }
  //printf("initial state: ");
  print_segment_info();
  write_segments(message);
  //printf("current state: ");
  print_segment_info();



change_level:   ////////////////////////////cong/////////////////////////////
printf("initialize\n");
  initialize();
  operation = 3;rotation_rate = 2;for(i=0;i<25600000;i++){if(i%100000 == 0)transfervalue	(operation);}flag = 1;
  operation = 1;rotation_rate = 2;for(i=0;i<25600000;i++){if(i%100000 == 0)transfervalue  (operation);}flag = 1;
  transfervalue(5);
/* Look for and handle keypresses */
  for (;;) {
    
    if(ball_reach[3] == 1&&ball_reach[2] == 1&&ball_reach[1] == 1&&ball_reach[0] == 1) message[6] = 0x01;
	//printf("ball_reach == %d\n", message[6]);

    ball_moving();
    operation = 0;
    libusb_interrupt_transfer(keyboard, endpoint_address,
			      (unsigned char *) &packet, sizeof(packet),
			      &transferred, 0);
    if (transferred == sizeof(packet)) {
      //sprintf(keystate, "%02x %02x %02x %02x %02x %02x", packet.modifiers, packet.keycode[0], packet.keycode[1], 		packet.keycode[2], packet.keycode[3], packet.keycode[4]);
	//printf("%s\n",keystate);

		if(packet.keycode[3] == 0x06&&packet.keycode[4] == 0x00){operation = 1;rotation_rate = 1;}
		else if(packet.keycode[3] == 0x02&&packet.keycode[4] == 0x00){operation = 2;rotation_rate = 1;}
		else if(packet.keycode[3] == 0x00&&packet.keycode[4] == 0x00){operation = 3;rotation_rate = 1;}
		else if(packet.keycode[3] == 0x04&&packet.keycode[4] == 0x00){operation = 4;rotation_rate = 1;}
		else if(packet.keycode[3] == 0x16&&packet.keycode[4] == 0x00){operation = 1;rotation_rate = 4;}
		else if(packet.keycode[3] == 0x12&&packet.keycode[4] == 0x00){operation = 2;rotation_rate = 4;}
		else if(packet.keycode[3] == 0x10&&packet.keycode[4] == 0x00){operation = 3;rotation_rate = 4;}
		else if(packet.keycode[3] == 0x14&&packet.keycode[4] == 0x00){operation = 4;rotation_rate = 4;}
		else if(packet.keycode[3] == 0x4f&&packet.keycode[4] == 0x00){operation = 5;}
		else if(packet.keycode[4] == 0x04&&packet.keycode[3] == 0x0f&&flag == 0)
				{operation = 1;rotation_rate = 2;for(i=0;i<5120000;i++){if(i%10000 == 0)transfervalue	(operation);}flag = 1;}
		else if(packet.keycode[4] == 0x08&&packet.keycode[3] == 0x0f&&flag == 0)
				{operation = 2;rotation_rate = 2;for(i=0;i<5120000;i++){if(i%10000 == 0)transfervalue(operation);}flag = 1;}
////////////////////////////////////////////////////////Cong////////////////////////////////////////////////////////
		else if(packet.keycode[3] == 0x1f&&packet.keycode[4] == 0x0c){operation = 6;}
		else if(packet.keycode[3] == 0x2f&&packet.keycode[4] == 0x0c){operation = 7;}
		else if(packet.keycode[3] == 0x4f&&packet.keycode[4] == 0x0c){operation = 8;}
		else if(packet.keycode[3] == 0x8f&&packet.keycode[4] == 0x0c){operation = 9;}
		else if(packet.keycode[3] == 0x0f&&packet.keycode[4] == 0x0d){operation = 10;}
		else if(packet.keycode[3] == 0x0f&&packet.keycode[4] == 0x0e){operation = 11;}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	
		if(packet.keycode[3] == 0x0f&&packet.keycode[4] == 0x00)flag=0;

		//if(packet.keycode[3] == 0x1f)
		//if(packet.keycode[3] == 0x2f)	
		//if(packet.keycode[4] == 0x01) 
    }
	
/////////////////////////////////////////////////////////Cong///////////////////////////////////////////////////////
	if(operation == 6||operation == 7||operation == 8||operation == 9||operation ==10||operation ==11){
	switch(operation)
	{
		case 6:
			{
				memcpy(&extend_d, &extend_d1, 20);
				memcpy(&x_s, &x_s1, 20);
				memcpy(&x_e, &x_e1, 20);
				memcpy(&y_s, &y_s1, 20);
				memcpy(&y_e, &y_e1, 20);
				memcpy(&z_s, &z_s1, 20);
				memcpy(&z_e, &z_e1, 20);
				break;
			}
		case 7:
			{
				memcpy(&extend_d, &extend_d2, 20);
				memcpy(&x_s, &x_s2, 20);
				memcpy(&x_e, &x_e2, 20);
				memcpy(&y_s, &y_s2, 20);
				memcpy(&y_e, &y_e2, 20);
				memcpy(&z_s, &z_s2, 20);
				memcpy(&z_e, &z_e2, 20);
				break;
			}
		case 8:
			{
				memcpy(&extend_d, &extend_d3, 20);
				memcpy(&x_s, &x_s3, 20);
				memcpy(&x_e, &x_e3, 20);
				memcpy(&y_s, &y_s3, 20);
				memcpy(&y_e, &y_e3, 20);
				memcpy(&z_s, &z_s3, 20);
				memcpy(&z_e, &z_e3, 20);
				break;
			}
		case 9:
			{
				memcpy(&extend_d, &extend_d4, 20);
				memcpy(&x_s, &x_s4, 20);
				memcpy(&x_e, &x_e4, 20);
				memcpy(&y_s, &y_s4, 20);
				memcpy(&y_e, &y_e4, 20);
				memcpy(&z_s, &z_s4, 20);
				memcpy(&z_e, &z_e4, 20);
				break;
			}
		case 10:
			{
				memcpy(&extend_d, &extend_d5, 20);
				memcpy(&x_s, &x_s5, 20);
				memcpy(&x_e, &x_e5, 20);
				memcpy(&y_s, &y_s5, 20);
				memcpy(&y_e, &y_e5, 20);
				memcpy(&z_s, &z_s5, 20);
				memcpy(&z_e, &z_e5, 20);
				break;
			}
		case 11:
			{

				memcpy(&extend_d, &extend_d6, 20);
				memcpy(&x_s, &x_s6, 20);
				memcpy(&x_e, &x_e6, 20);
				memcpy(&y_s, &y_s6, 20);
				memcpy(&y_e, &y_e6, 20);
				memcpy(&z_s, &z_s6, 20);
				memcpy(&z_e, &z_e6, 20);
  				initialize();
  				operation = 3;rotation_rate = 2;for(i=0;i<25600000;i++){if(i%100000 == 0)transfervalue	(operation);}flag = 1;
				operation = 1;rotation_rate = 2;for(i=0;i<25600000;i++){if(i%100000 == 0)transfervalue  (operation);}flag = 1;
				custom_design(packet,transferred);
				break;
			}
		break;	
	}
	goto change_level;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	if(operation == 1||operation == 2||operation == 3||operation == 4||operation ==5){
	transfervalue(operation);
	calculate_pass();
	////printf("cubic = %d %d %d %d %d \n", cubic[0], cubic[1], cubic[2], cubic[3], cubic[4]);
	message[5] =  (2*(2*(2*(2*(0 + cubic[4]) + cubic[3]) + cubic[2]) + cubic[1]) + cubic[0]);
	write_segments(message);
	////printf("message = %d \n", message[5]);
	}
  }
  
  //printf("VGA LED Userspace program terminating\n");
  return 0;
}

/////////////////////////////////////////////////////////Cong//////////////////////////////////////////////////////////
void custom_design(struct usb_keyboard_packet packet, int transferred){
	int x, y, z;
	int flag = 0;
	int length;
	int orientation;
	int i;
	for(i = 0; i<5; i++){
		x=0; 
		y=0; 
		z=0;
		length = 5;
		orientation = 0;
		wait:	
    			libusb_interrupt_transfer(keyboard, endpoint_address,
			      (unsigned char *) &packet, sizeof(packet),
			      &transferred, 0);		
			if(!(packet.keycode[3] == 0x0f&&packet.keycode[4] == 0x00)) goto wait;

		while(packet.keycode[3] != 0x0f||packet.keycode[4] != 0x02){		
    			
			generate_cubic();
		
			if(packet.keycode[3] == 0x0f&&packet.keycode[4] == 0x00)flag=0;

			else if(packet.keycode[4] == 0x04&&packet.keycode[3] == 0x0f&&flag == 0){orientation = 0;}
			else if(packet.keycode[4] == 0x08&&packet.keycode[3] == 0x0f&&flag == 0){orientation = 1;}
	
			else if(packet.keycode[3] == 0x1f&&packet.keycode[4] == 0x00&&flag == 0){
				if(length != 125) length = length + 5;
				flag = 1;
			}
			else if(packet.keycode[3] == 0x4f&&packet.keycode[4] == 0x00&&flag == 0){
				if(length != 5) length = length - 5;
				flag = 1;
			}

			else if(packet.keycode[3] == 0x00&&packet.keycode[4] == 0x00&&flag == 0){
				if(y != -100) y = y - 10;
				flag = 1;
			}
			else if(packet.keycode[3] == 0x04&&packet.keycode[4] == 0x00&&flag == 0){
				if(y != 100) y = y + 10;
				flag = 1;
			}
			else if(packet.keycode[3] == 0x06&&packet.keycode[4] == 0x00&&flag == 0){
					if(orientation == 0&&x != -100) x = x - 10;
					else if(orientation == 1&&z != -100) z = z - 10;
				flag = 1;
			}
			else if(packet.keycode[3] == 0x02&&packet.keycode[4] == 0x00&&flag == 0){
					if(orientation == 0&&x != 100) x = x + 10;
					else if(orientation == 1&&z != 100) z = z + 10;
				flag = 1;
			}
			
	
			if(orientation == 0){
  				extend_d[i] = 3;
  				x_s[i] = x - width;
  				x_e[i] = x + width;
  				y_s[i] = y - width;
  				y_e[i] = y + width;
  				z_s[i] = z - length;
  				z_e[i] = z + length;
			} else{
  				extend_d[i] = 1;
  				x_s[i] = x - length;
  				x_e[i] = x + length;
  				y_s[i] = y - width;
  				y_e[i] = y + width;
  				z_s[i] = z - width;
  				z_e[i] = z + width;
			}
			
			initialize_endPoint();
			initialize_ball();

    			libusb_interrupt_transfer(keyboard, endpoint_address,
			      (unsigned char *) &packet, sizeof(packet),
			      &transferred, 0);	
		}
	}
}
/////////////////////////////////////////////////////////////////////////////////////
/*  function defined by lyh  */
void generate_cubic()
{
	int i,j,k;
	k = 10;
	for(i = 0; i<cubnum; i++){
		
		message[k] = x_s[i];	
		k++;	
		message[k] = x_e[i];
		k++;
		message[k] = y_s[i];
		k++;
		message[k] = y_e[i];
		k++;
		message[k] = z_s[i];
		k++;
		message[k] = z_e[i];
		k++;
	}
	write_segments(message);
}


void initialize(){

    ball_reach[0] = 0;
    ball_reach[1] = 0;
    ball_reach[2] = 0;
    ball_reach[3] = 0;

    message[0] = 0x00;
    message[1] = 0x00;
    message[2] = 0x00;
    message[3] = 0x00;
    message[5] = 0x00;
    message[6] = 0x00;
    x_angle_l = 0;
    y_angle_l = 0;
    x_angle_h = 0;
    y_angle_h = 0;

    generate_cubic();

    int i,j,k;
	
	for(i = 0;i<cubnum;i++){
		Plane_info[i].edge_node[0] = 2*i;
		Plane_info[i].edge_node[1] = 2*i + 1;	
	}

    initialize_ball();
    initialize_endPoint();
    //generate all nodes
    for (i = 0; i<cubnum; i++) {
        switch (extend_d[i]) {
            case 1:
                cubic_nodes_num[i] = ((x_e[i] - x_s[i])/10 - 2)*10 + 10;
                break;
            case 2:
                cubic_nodes_num[i] = ((y_e[i] - y_s[i])/10 - 2)*10 + 10;
                break;
            case 3:
                cubic_nodes_num[i] = ((z_e[i]- z_s[i])/10 - 2)*10 + 10;
                break;
            default:
                break;
        }
        
    }

//generate all comparing nodes, stored in structuren cp_node[cubnum*2][cubnum]
    for (i = 0; i<2*cubnum; i++) {	
        for (j = 0; j<cubnum; j++) {
            cp_node[i][j].plane_num = i/2;		
            switch (extend_d[i/2]) {
                case 1:{
                    if (i % 2 ==0)
                    	cp_node[i][j].x_node = x_s[i/2] - width;
                    else
                    	cp_node[i][j].x_node = x_e[i/2] + width;
                    cp_node[i][j].y_node = y_s[i/2] + width;
                    cp_node[i][j].z_node = z_s[i/2] + width;
                    break;
                }
                case 2:{
                    if (i % 2 ==0)
                        cp_node[i][j].y_node = y_s[i/2] - width;
                    else
                        cp_node[i][j].y_node = y_e[i/2] + width;           
                    cp_node[i][j].x_node = x_s[i/2] + width;
                    cp_node[i][j].z_node = z_s[i/2] + width;                    
                    break;
                }
                case 3:{
                    if (i % 2 ==0)
                        cp_node[i][j].z_node = z_s[i/2] - width;
                    else
                        cp_node[i][j].z_node = z_e[i/2] + width;                   
                    cp_node[i][j].y_node = y_s[i/2] + width;
                    cp_node[i][j].x_node = x_s[i/2] + width;                    
                    break;
                }
                default:
                    break;
                    
                    
            }

            if (cp_node[i][j].plane_num != j)
            for (k = 0; k<=cubic_nodes_num[j]; k++) {
                switch (extend_d[j]) {
                    case 1:{
                        cp_node[i][j].x_va[k] = x_s[j] + 5 + k - cp_node[i][j].x_node;
                        cp_node[i][j].y_va[k] = y_s[j] + 5 - cp_node[i][j].y_node;
                        cp_node[i][j].z_va[k] = z_s[j] + 5 - cp_node[i][j].z_node;
                        D = cp_node[i][j].x_va[k]*cp_node[i][j].x_va[k] + cp_node[i][j].y_va[k] *cp_node[i][j].y_va[k]
                        + cp_node[i][j].z_va[k]*cp_node[i][j].z_va[k];
                        D = sqrt(D);
                        cp_node[i][j].x_va[k] = cp_node[i][j].x_va[k]/D;
                        cp_node[i][j].y_va[k] = cp_node[i][j].y_va[k]/D;
                        cp_node[i][j].z_va[k] = cp_node[i][j].z_va[k]/D;
                        break;
                    }
                    case 2:{
                        cp_node[i][j].x_va[k] = x_s[j] + 5  - cp_node[i][j].x_node;
                        cp_node[i][j].y_va[k] = y_s[j] + 5 + k - cp_node[i][j].y_node;
                        cp_node[i][j].z_va[k] = z_s[j] + 5 - cp_node[i][j].z_node;
                        D = cp_node[i][j].x_va[k]*cp_node[i][j].x_va[k] + cp_node[i][j].y_va[k] *cp_node[i][j].y_va[k]
                        + cp_node[i][j].z_va[k]*cp_node[i][j].z_va[k];
                        D = sqrt(D);
                        cp_node[i][j].x_va[k] = cp_node[i][j].x_va[k]/D;
                        cp_node[i][j].y_va[k] = cp_node[i][j].y_va[k]/D;
                        cp_node[i][j].z_va[k] = cp_node[i][j].z_va[k]/D;
                        break;
                    }
                    case 3:{
                        cp_node[i][j].x_va[k] = x_s[j] + 5 - cp_node[i][j].x_node;
                        cp_node[i][j].y_va[k] = y_s[j] + 5 - cp_node[i][j].y_node;
                        cp_node[i][j].z_va[k] = z_s[j] + 5 + k - cp_node[i][j].z_node;
                        D = cp_node[i][j].x_va[k]*cp_node[i][j].x_va[k] + cp_node[i][j].y_va[k] *cp_node[i][j].y_va[k]
                        + cp_node[i][j].z_va[k]*cp_node[i][j].z_va[k];
                        D = sqrt(D);
                        cp_node[i][j].x_va[k] = cp_node[i][j].x_va[k]/D;
                        cp_node[i][j].y_va[k] = cp_node[i][j].y_va[k]/D;
                        cp_node[i][j].z_va[k] = cp_node[i][j].z_va[k]/D;
                        break;
                    }
                        
                    default:
                        break;
                }            
            }
        }      
    }
}



void calculate_pass(){
    int ci, cj, ck;
    int bi;
    rotate_angle_x = 2*3.14159*(x_angle_l + x_angle_h*256)/4096;
    rotate_angle_y = 2*3.14159*(y_angle_l + y_angle_h*256)/4096;
	
    initialize_plane();
    v_x = 0.0;
    v_y = 0.0;
    v_z = 1.0;

	
	
    v_x1 = v_x;
    v_y1 = cos(rotate_angle_y)*v_y + sin(rotate_angle_y)*v_z;
    v_z1 = -sin(rotate_angle_y)*v_y + cos(rotate_angle_y)*v_z;
    
    v_x2 = -sin(rotate_angle_x)*v_z1 + cos(rotate_angle_x)*v_x1;
    v_y2 = v_y1;
    v_z2 = cos(rotate_angle_x)*v_z1 + sin(rotate_angle_x)*v_x1;	 

	
    for (cj = 0; cj<cubnum; cj++) {
        cubic[cj] = 0;
    }
    for(ci=0;ci<2*cubnum;ci++)
        for (cj=0; cj<cubnum; cj++) {
            if ((cubic[cj]!=1)||(cubic[cp_node[ci][cj].plane_num] != 1))
            for (ck = 0; ck<=cubic_nodes_num[cj]; ck++) {
                if (cubic[cj]==1&&cubic[cp_node[ci][cj].plane_num] == 1)break;
                if (cp_node[ci][cj].plane_num!=(cj)) {

D2 = sqrt((v_x2-cp_node[ci][cj].x_va[ck])*(v_x2-cp_node[ci][cj].x_va[ck]) + (v_y2-cp_node[ci][cj].y_va[ck])*(v_y2-cp_node[ci][cj].y_va[ck]) + (v_z2-cp_node[ci][cj].z_va[ck])*(v_z2-cp_node[ci][cj].z_va[ck]))<sqrt((v_x2+cp_node[ci][cj].x_va[ck])*(v_x2+cp_node[ci][cj].x_va[ck]) + (v_y2+cp_node[ci][cj].y_va[ck])*(v_y2+cp_node[ci][cj].y_va[ck]) + (v_z2+cp_node[ci][cj].z_va[ck])*(v_z2+cp_node[ci][cj].z_va[ck]))?sqrt((v_x2-cp_node[ci][cj].x_va[ck])*(v_x2-cp_node[ci][cj].x_va[ck]) + (v_y2-cp_node[ci][cj].y_va[ck])*(v_y2-cp_node[ci][cj].y_va[ck]) + (v_z2-cp_node[ci][cj].z_va[ck])*(v_z2-cp_node[ci][cj].z_va[ck])):sqrt((v_x2+cp_node[ci][cj].x_va[ck])*(v_x2+cp_node[ci][cj].x_va[ck]) + (v_y2+cp_node[ci][cj].y_va[ck])*(v_y2+cp_node[ci][cj].y_va[ck]) + (v_z2+cp_node[ci][cj].z_va[ck])*(v_z2+cp_node[ci][cj].z_va[ck])); 
		
		switch(extend_d[ci/2]){
			case 1:{
				D3 = (v_x2-1)*(v_x2-1) + (v_y2-0)*(v_y2-0) + (v_z2-0)*(v_z2-0);
				break;
			}
			case 2:{
				D3 = (v_x2-0)*(v_x2-0) + (v_y2-1)*(v_y2-1) + (v_z2-0)*(v_z2-0);
				break;
			}
			case 3:{
				D3 = (v_x2-0)*(v_x2-0) + (v_y2-0)*(v_y2-0) + (v_z2-1)*(v_z2-1);
				break;
			}
		}

			seeSurface1 = (ci%2 ? D3 > 2 : D3 < 2) && (sqrt((v_x2-cp_node[ci][cj].x_va[ck])*(v_x2-cp_node[ci][cj].x_va[ck]) + (v_y2-cp_node[ci][cj].y_va[ck])*(v_y2-cp_node[ci][cj].y_va[ck]) + (v_z2-cp_node[ci][cj].z_va[ck])*(v_z2-cp_node[ci][cj].z_va[ck]))<sqrt((v_x2+cp_node[ci][cj].x_va[ck])*(v_x2+cp_node[ci][cj].x_va[ck]) + (v_y2+cp_node[ci][cj].y_va[ck])*(v_y2+cp_node[ci][cj].y_va[ck]) + (v_z2+cp_node[ci][cj].z_va[ck])*(v_z2+cp_node[ci][cj].z_va[ck])));
			nonSeeSurface2 = !(ci%2 ? D3 > 2 : D3 < 2) && (sqrt((v_x2-cp_node[ci][cj].x_va[ck])*(v_x2-cp_node[ci][cj].x_va[ck]) + (v_y2-cp_node[ci][cj].y_va[ck])*(v_y2-cp_node[ci][cj].y_va[ck]) + (v_z2-cp_node[ci][cj].z_va[ck])*(v_z2-cp_node[ci][cj].z_va[ck]))>sqrt((v_x2+cp_node[ci][cj].x_va[ck])*(v_x2+cp_node[ci][cj].x_va[ck]) + (v_y2+cp_node[ci][cj].y_va[ck])*(v_y2+cp_node[ci][cj].y_va[ck]) + (v_z2+cp_node[ci][cj].z_va[ck])*(v_z2+cp_node[ci][cj].z_va[ck])));
			parallel = (extend_d[ci/2] == extend_d[cj]) && (sqrt((v_x2-cp_node[ci][cj].x_va[ck])*(v_x2-cp_node[ci][cj].x_va[ck]) + (v_y2-cp_node[ci][cj].y_va[ck])*(v_y2-cp_node[ci][cj].y_va[ck]) + (v_z2-cp_node[ci][cj].z_va[ck])*(v_z2-cp_node[ci][cj].z_va[ck]))>sqrt((v_x2+cp_node[ci][cj].x_va[ck])*(v_x2+cp_node[ci][cj].x_va[ck]) + (v_y2+cp_node[ci][cj].y_va[ck])*(v_y2+cp_node[ci][cj].y_va[ck]) + (v_z2+cp_node[ci][cj].z_va[ck])*(v_z2+cp_node[ci][cj].z_va[ck])));

                    if (D2<precision && !seeSurface1 && !nonSeeSurface2 && !parallel) {  ////////////////to be continue/////////////////
			//D3 = D2;
	
                        cubic[cp_node[ci][cj].plane_num] = 1;
                        cubic[cj] = 1;
			record_location1x = (int)cp_node[ci][cj].x_node;
			record_location1y = (int)cp_node[ci][cj].y_node;
			record_location1z = (int)cp_node[ci][cj].z_node;

			switch(extend_d[cj]){
				case 1:{
					record_location2x = (int)x_s[cj] + 5 +ck;
					record_location2y = (int)y_s[cj] + 5;
					record_location2z = (int)z_s[cj] + 5;
					break;
				}
				case 2:{
					record_location2x = (int)x_s[cj] + 5;
					record_location2y = (int)y_s[cj] + 5 +ck;
					record_location2z = (int)z_s[cj] + 5;
					break;
				}
				case 3:{
					record_location2x = (int)x_s[cj] + 5;
					record_location2y = (int)y_s[cj] + 5;
					record_location2z = (int)z_s[cj] + 5 +ck;
					break;
				}
			}	
			
			Plane_info[ci/2].edge_node_connect[ci%2] = 1;
			Plane_info[ci/2].edge_node_jumpx[ci%2] = record_location2x;
			Plane_info[ci/2].edge_node_jumpy[ci%2] = record_location2y;	
			Plane_info[ci/2].edge_node_jumpz[ci%2] = record_location2z;
			Plane_info[ci/2].edge_node_jump_plane[ci%2] = cj;

			
			Plane_info[cj].N_connect[Plane_info[cj].connectpoint_num] = ci;
			Plane_info[cj].P_x[Plane_info[cj].connectpoint_num] = record_location2x;
			Plane_info[cj].P_y[Plane_info[cj].connectpoint_num] = record_location2y;	
			Plane_info[cj].P_z[Plane_info[cj].connectpoint_num] = record_location2z;	
			Plane_info[cj].connectpoint_num++;	
                    }
            	}
            }
        }	

	for(bi = 0; bi<cubnum-1;bi++)calculate_jump_point(bi);
}










void transfervalue(int op)
{
	int p;
	for(p =0;p<cubnum - 1; p++){
		if(balls[p].return_protect == 1){
			balls[p].bx = balls[p].ret_bx;
			balls[p].by = balls[p].ret_by;
			balls[p].bz = balls[p].ret_bz;
			balls[p].plane_num = balls[p].ret_direct;
			balls[p].return_protect = 0;
		}
	}
	switch(op)
	{
		case 1: 
			{	
				if(x_angle_l>=256 - rotation_rate){
					x_angle_l = x_angle_l + rotation_rate - 256;
					message[0] = message[0] + rotation_rate;
					if(x_angle_h == 15)
					{
						x_angle_h = 0;	
						message[1] = 0x00;				
					}
					else	
					{
						x_angle_h++;
						message[1] = message[1]+1;
					}
				}
				else
				{
					x_angle_l+=rotation_rate;
					message[0] = message[0]+rotation_rate;
				}
				break;
			}
		case 2: 
			{
				if(x_angle_l<=rotation_rate - 1){
					x_angle_l = x_angle_l + 256 - rotation_rate;
					message[0] = message[0] - rotation_rate;
					if(x_angle_h == 0)
					{
						x_angle_h = 15;	
						message[1] = 0x0F;				
					}
					else	
					{
						x_angle_h--;
						message[1] = message[1]-1;
					}
				}
				else
				{
					x_angle_l-=rotation_rate;
					message[0] = message[0]-rotation_rate;
				}
				break;
			}

		case 3: 
			{	

				if(y_angle_l>=256 - rotation_rate){
					y_angle_l = y_angle_l + rotation_rate - 256;
					message[2] = message[2] + rotation_rate;
					if(y_angle_h == 15)
					{
						y_angle_h = 0;	
						message[3] = 0x00;				
					}
					else	
					{
						y_angle_h++;
						message[3] = message[3]+1;
					}
				}
				else
				{
					y_angle_l+=rotation_rate;
					message[2] = message[2]+rotation_rate;
				}
				break;
			}
		case 4: 
			{
				if(y_angle_l<=rotation_rate - 1){
					y_angle_l = y_angle_l + 256 - rotation_rate;
					message[2] = message[2] - rotation_rate;
					if(y_angle_h == 0)
					{
						y_angle_h = 15;	
						message[3] = 0x0F;				
					}
					else	
					{
						y_angle_h--;
						message[3] = message[3]-1;
					}
				}
				else
				{
					y_angle_l-=rotation_rate;
					message[2] = message[2]-rotation_rate;
				}
				break;
			}
		
		case 5:
			{
  				ball_reach[0] = 0;
    				ball_reach[1] = 0;
    				ball_reach[2] = 0;
    				ball_reach[3] = 0;
				initialize_ball();
				break;
			}
		break;	
	}
	write_segments(message);
	if(message[4] = 0x01)message[4] = 0x00;
  return NULL;
}




void combine_ball(int i, int j){

			if(abs(balls[i].bx - balls[j].bx)<=1&&abs(balls[i].by - balls[j].by)<=1&&abs(balls[i].bz - balls[j].bz)<=1)
			{
				
				balls[j].b_dict = balls[i].b_dict;
				balls[j].next_jump_p = balls[i].next_jump_p;	
				balls[j].jump_x = balls[i].jump_x;
				balls[j].jump_y = balls[i].jump_y;
				balls[j].jump_z = balls[i].jump_z;
				balls[j].jump_Node = balls[i].jump_Node;
				balls[j].prior_plane = balls[i].prior_plane;
				balls[j].prior_direct = balls[i].prior_direct;	
				balls[j].next_is_edge = balls[i].next_is_edge;
				balls[j].edge_plane_num = balls[i].edge_plane_num;
				balls[j].ret_bx = balls[i].ret_bx;
				balls[j].ret_by = balls[i].ret_by;
				balls[j].ret_bz = balls[i].ret_bz;
				balls[j].return_protect = balls[i].return_protect;
				balls[j].ret_direct = balls[i].ret_direct;	
			}


}
