#include "kameserial.h"
#include <errno.h>
#include <time.h>

#define SLEEP_TIME 0
#define TRY_TIMES 6
#define TIME_OUT 10
#define SERIAL_TIME 150000

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_init(JNIEnv* env,
                                       jobject thiz) {

    jint ret;
    
    // only first time.
    do_scenario(HOME_POSITON);

    return 0;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_homePosition(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(HOME_POSITON);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_walkToward2(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(WALK_TOWARD_2);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_walkToward4(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(WALK_TOWARD_4);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_walkToward8(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(WALK_TOWARD_8);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_walkToward16(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(WALK_TOWARD_16);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_walkToward32(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(WALK_TOWARD_32);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_walkBack4(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(WALK_BACK_4);


    return ret;

}


JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnRight1(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_RIGHT_1);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnRight2(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_RIGHT_2);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnRight4(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_RIGHT_4);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnRight6(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_RIGHT_6);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnRight8(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_RIGHT_8);


    return ret;

}


JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnLeft1(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_LEFT_1);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnLeft2(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_LEFT_2);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnLeft4(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_LEFT_4);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnLeft6(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_LEFT_6);


    return ret;

}

JNIEXPORT
jint
JNICALL
Java_cc_openframeworks_DroidBot_turnLeft8(JNIEnv* env,
                                       jobject thiz) {

    jint ret;

    ret = do_scenario(TURN_LEFT_8);


    return ret;

}





int do_scenario(enum eKAMEMOTION motion) {
    char init_code[1], senario[4], softsw[5], ack[1], ret_code, endsenario[32];
    int fd, len, i;
    struct termios oldtio, newtio;

    len = 0;
    i = 0;

    fd = open(SERIAL_PORT, O_RDWR);
    if(fd<0){
         __android_log_print(ANDROID_LOG_DEBUG, "serial","Open Error");
         return -1;
    }

    ioctl(fd, TCGETS, &oldtio);
    newtio = oldtio;
    /* init */
    newtio.c_iflag = 0;
    newtio.c_oflag = 0;
    newtio.c_cflag = 0;
    newtio.c_lflag = 0;
    newtio.c_line = 0;
    bzero(newtio.c_cc,sizeof(newtio.c_cc));

    newtio.c_iflag = IGNPAR;
    newtio.c_cflag = CS8 | CLOCAL | CREAD;
    newtio.c_lflag = 0;
    newtio.c_oflag = 0;
    newtio.c_cc[VTIME] = TIME_OUT;
    newtio.c_cc[VMIN] = 0;
    cfsetispeed(&newtio,BAUD_RATE);
    cfsetospeed(&newtio,BAUD_RATE);
    ioctl(fd, TCSETS, &newtio);



    for(i = 0; i < TRY_TIMES; i++) {
	   /* init */
		init_code[0] = 0x0D;

		__android_log_print(ANDROID_LOG_DEBUG, "serial","write init command.");
		len = write(fd, init_code, sizeof(init_code));
		if(len < 0) {
			 __android_log_print(ANDROID_LOG_DEBUG, "serial","Init Error:%d", errno);
			ioctl(fd, TCSETS, &oldtio);
			close(fd);
			return -1;
		}

		init_code[0] = 0x00;
		__android_log_print(ANDROID_LOG_DEBUG, "serial","read init command.");
		len = read(fd, init_code, sizeof(init_code));
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Open len:%d, code:%d", len, init_code[0]);
		if(len == 0) {
			 ioctl(fd, TCSETS, &oldtio);
			 close(fd);
			 return -1;
		} else if(len < 0) {
			 ioctl(fd, TCSETS, &oldtio);
			 close(fd);
			 return -1;
		}



		/* play scenario */
		senario[0] = 0xF4;
		// senario[1] = 0x07;
		senario[1] =  0x01;
		senario[2] = (char)motion;
		senario[3] = (senario[0]+senario[1]+senario[2]);
//		senario[3] = (senario[0]+senario[1]+senario[2]) & 0x7F;
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Play scenario buf0:%d", senario[0]);
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Play scenario buf1:%d", senario[1]);
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Play scenario buf2:%d", senario[2]);
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Play scenario buf3:%d", senario[3]);

		usleep(SLEEP_TIME + (i*0));
		len = write(fd, senario, sizeof(senario));
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Play scenario len:%d", len);
		if(len <= 0) {
			ioctl(fd, TCSETS, &oldtio);
			close(fd);
			return -1;
		}
		ack[0] = 0;
		len = read(fd, ack, sizeof(ack));
		__android_log_print(ANDROID_LOG_DEBUG, "serial","Open len:%d, ack:%d", len, ack[0]);
		if(len > 0) {
			break;
		}
    }
    if(ack[0] == 0) {
		ioctl(fd, TCSETS, &oldtio);
		close(fd);
		return -1;
    }

	/* End of scenario */
    for(i = 0; i < TRY_TIMES; i++) {
		len = read(fd, endsenario, sizeof(endsenario));
		__android_log_print(ANDROID_LOG_DEBUG, "serial","End of scenario len:%d", len);
		ret_code = 0;
		if(len > 0) {
			ret_code = endsenario[0];
		   __android_log_print(ANDROID_LOG_DEBUG, "serial","End of scenario return code:%02X", ret_code);
		   if(ret_code == 255) {
			   break;
		   }
		}
    }
    if(ret_code == 0) {
		ioctl(fd, TCSETS, &oldtio);
		close(fd);
		return -1;
    }

    ioctl(fd, TCSETS, &oldtio);
    close(fd);

    usleep(SERIAL_TIME);

    return 0;
}
