/* a simple interface to turtle field      */
/* for JOHO KISO                           */
/* Yoshinori Hayakawa (CITE, Tohoku Univ.) */
/* 08-JUN-2013 version                     */

#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>

#ifdef _WIN32
 #include <Windows.h>
 #define _USE_MATH_DEFINES
#else
 #include <sys/socket.h>
 #include <sys/types.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
 #include <netdb.h>
 #include <unistd.h>
#endif

#include <math.h>

#include "turtle-def.h"

#define BUFFLEN 512
#define MAX_MSG 128
#define TSLEEP  2000
#define TTLPORT 2222

static char ttl_buf_[BUFFLEN] ;

static int socket_ = -1 ;
static int team_ = 0 ;

static char ttl_ack_[BUFFLEN] ;
static char *ack_[16] ;
static unsigned int ttl_eventcode_ ;
static unsigned int ttl_timecode_ ;
static void (* ttl_hit_by_bullet_handler_)(unsigned int) ;
static void (* ttl_run_into_turtle_handler_)(unsigned int) ;
static void (* ttl_run_into_donut_handler_)(unsigned int) ;
static void (* ttl_run_into_stone_handler_)(unsigned int) ;
static void (* ttl_run_into_wall_handler_)(unsigned int) ;
static void (* ttl_found_coin_handler_)(unsigned int) ;
static void (* ttl_detected_by_finder_handler_)(unsigned int) ;
static void (* ttl_detected_by_rader_handler_)(unsigned int) ;
static void (* ttl_got_message_handler_)(unsigned int) ;

static void ttl_call_event_handler_(unsigned int eventcode, unsigned int timecode) ;


/* 
   Notes:
   Students, generally speaking, it is definitely bad idea to describe
   functions in header files. This is only for the purpose to
   reduce your trouble in lessons.
*/

void DISCON()
{
  int i ;
#ifdef _WIN32
  if (socket_>=0) closesocket(socket_) ;
#else
  if (socket_>=0) close(socket_) ;
#endif
}

int CON(char *hostname)
{
  struct sockaddr_in serveraddr;
  struct hostent *host;
  int rc,i ;
#ifdef _WIN32
  WSADATA wsaData;
  WSAStartup(MAKEWORD(2,0), &wsaData);
#endif

  socket_ = socket(AF_INET, SOCK_STREAM, 0);

  memset((char *) &serveraddr, 0, sizeof(serveraddr));
  serveraddr.sin_family = AF_INET;
  serveraddr.sin_port = htons(TTLPORT);

  if (isdigit(hostname[0])) {
    serveraddr.sin_addr.s_addr = inet_addr(hostname);
  }
  else {
    host = gethostbyname(hostname) ;
    if (host == NULL) {
      fprintf(stderr,"no such host: %s\n",hostname) ;
      return -1 ;
    }
    else {
      memcpy(&serveraddr.sin_addr,host->h_addr, host->h_length) ;
    }
  }

  rc = connect(socket_, (struct sockaddr *)&serveraddr, sizeof(serveraddr));
  if (rc==-1) {
    fprintf(stderr,"cannot connect to %s\n",hostname) ;
    fprintf(stderr,"it seems turtle field is not ready...\n") ;
    exit(0) ;
  }
  ttl_hit_by_bullet_handler_=NULL ;
  ttl_run_into_turtle_handler_=NULL ;
  ttl_run_into_donut_handler_=NULL ;
  ttl_run_into_stone_handler_=NULL ;
  ttl_run_into_wall_handler_=NULL ;
  ttl_found_coin_handler_=NULL ;
  ttl_detected_by_finder_handler_=NULL ;
  ttl_detected_by_rader_handler_=NULL ;
  ttl_got_message_handler_=NULL ;
  ttl_eventcode_ = 0 ;
  ttl_timecode_ = 0 ;
  return rc ;
}

static int send_ttl_buf_and_receive_ack(void) 
{
  int len,n,k ;
  if (socket_ < 0) return 0 ;
  len = strlen(ttl_buf_) ;
  n = send(socket_, ttl_buf_, len,0) ;
  if (n>0) {
    memset(ttl_buf_, 0, sizeof(ttl_buf_));
    n = recv(socket_, ttl_buf_, sizeof(ttl_buf_),0);
    if (n<=0) {
      fprintf(stderr,"I'm dead.. bye !!\n") ;
      exit(0) ;
    }
    ttl_buf_[n]='\0' ;
    strcpy(ttl_ack_, ttl_buf_) ;
    ack_[0] = strtok(ttl_ack_," ") ;
    k=1 ;
    while ((ack_[k]=strtok(NULL," "))!=NULL) {
      k++ ;
    }
    ttl_eventcode_ = (unsigned int) atoi(ack_[k-1]) ; /* event code comes always last */
    ttl_timecode_ = (unsigned int) atoi(ack_[k-2]) ; 
    if (ttl_eventcode_!=0) ttl_call_event_handler_(ttl_eventcode_,ttl_timecode_) ;
  }
#ifdef _WIN32
  Sleep(TSLEEP/1000) ;
#else
  usleep(TSLEEP) ;
#endif
  return n ;
}

static void ttl_send_command_(const char *cmd)
{
  int rc ;
  strcpy(ttl_buf_,cmd) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void TTL_DISABLE_EVENT(unsigned int eventcode)
{
  int rc ;
  sprintf(ttl_buf_,"Y - %u\n",eventcode) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void TTL_ENABLE_EVENT(unsigned int eventcode)
{
  int rc ;
  sprintf(ttl_buf_,"Y + %u\n",eventcode) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

static void ttl_call_event_handler_(unsigned int eventcode, unsigned int timecode) {
  char msg_stack[BUFFLEN] ;
  strcpy(msg_stack,ttl_buf_) ;
  TTL_DISABLE_EVENT(eventcode) ;
  if (eventcode & EVENT_HIT_BY_BULLET) {
    if (ttl_hit_by_bullet_handler_!=NULL) {
      ttl_hit_by_bullet_handler_(timecode) ;
    }
  }
  if (eventcode & EVENT_RUN_INTO_TURTLE) {
    if (ttl_run_into_turtle_handler_!=NULL) {
      ttl_run_into_turtle_handler_(timecode);
    }
  }
  if (eventcode & EVENT_RUN_INTO_DONUT) {
    if (ttl_run_into_donut_handler_!=NULL) {
      ttl_run_into_donut_handler_(timecode);
    }
  }
  if (eventcode & EVENT_RUN_INTO_STONE) {
    if (ttl_run_into_stone_handler_!=NULL) {
      ttl_run_into_stone_handler_(timecode);
    }
  }
  if (eventcode & EVENT_RUN_INTO_WALL) {
    if (ttl_run_into_wall_handler_!=NULL) {
      ttl_run_into_wall_handler_(timecode);
    }
  }
  if (eventcode & EVENT_FOUND_COIN) {
    if (ttl_found_coin_handler_!=NULL) {
      ttl_found_coin_handler_(timecode);
    }
  }
  if (eventcode & EVENT_DETECTED_BY_FINDER) {
    if (ttl_detected_by_finder_handler_!=NULL) {
      ttl_detected_by_finder_handler_(timecode);
    }
  }
  if (eventcode & EVENT_DETECTED_BY_RADER) {
    if (ttl_detected_by_rader_handler_!=NULL) {
      ttl_detected_by_rader_handler_(timecode);
    }
  }
  if (eventcode & EVENT_GOT_MESSAGE) {
    if (ttl_got_message_handler_!=NULL) {
      ttl_got_message_handler_(timecode);
    }
  }
  TTL_ENABLE_EVENT(eventcode) ;      
  strcpy(ttl_buf_,msg_stack) ;
}

void SET_HIT_BY_BULLET_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_hit_by_bullet_handler_ = callbackfunc ;
}

void SET_RUN_INTO_TURTLE_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_run_into_turtle_handler_ = callbackfunc ;
}

void SET_RUN_INTO_DONUT_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_run_into_donut_handler_ = callbackfunc ;
}

void SET_RUN_INTO_STONE_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_run_into_stone_handler_ = callbackfunc ;
}

void SET_RUN_INTO_WALL_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_run_into_wall_handler_ = callbackfunc ;

}

void SET_FOUND_COIN_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_found_coin_handler_ = callbackfunc ;
}

void SET_DETECTED_BY_FINDER_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_detected_by_finder_handler_ = callbackfunc ;
}

void SET_DETECTED_BY_RADER_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_detected_by_rader_handler_ = callbackfunc ;
}

void SET_GOT_MESSAGE_HANDLER(void (*callbackfunc)(unsigned int)) {
  ttl_got_message_handler_ = callbackfunc ;
}

static void ttl_convert_str2hex(char *str, char *hex)
{
  int cnt=0 ;
  char buf[3] ;
  printf("%s\n",str) ;
  hex[0]='\0';
  while (*str != '\0' && cnt++<MAX_MSG) {
    sprintf(buf,"%02x",*str) ;
    strcat(hex,buf) ;
    str++ ;
  }
}

static int ttl_char2int(char c)
{
  int r=0 ;
  if (c >= '0' && c<= '9') r = c - '0' ;
  else if (c>='a' && c<='f') r = c - 'a' + 10 ;
  else if (c>='A' && c<='F') r = c - 'A' + 10 ;
  return r ;
}

static void ttl_convert_hex2str(char *hex, char *str)
{
  int cnt,c0,c1 ;
  char buf[2] ;
  str[0]='\0' ;
  while (*hex != '\0' && cnt++<MAX_MSG) {
    c1 = ttl_char2int(*hex) ; hex++ ;
    c0 = ttl_char2int(*hex) ; hex++ ;
    buf[0] = (unsigned char)(c1*16+c0) ; buf[1]='\0' ;
    strcat(str,buf) ;
  }
}


void HOME()
{
  ttl_send_command_("C H\n") ;
}

void RST()
{
  ttl_send_command_("C\n") ;
}


void CF()
{
  ttl_send_command_("C F\n") ;

}

void CLR()
{
  ttl_send_command_("C A\n") ;
}

void FD(float step)
{
  int rc ;
  sprintf(ttl_buf_,"F %f\n",step) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void JUMP(float x, float y)
{
  int rc ;
  sprintf(ttl_buf_,"J %f %f\n",x,y) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void BK(float step)
{
  int rc ;
  sprintf(ttl_buf_,"B %f\n",step) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void RT(float angle)
{
  int rc ;
  sprintf(ttl_buf_,"T R %f\n",angle) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void LT(float angle)
{
  int rc ;
  sprintf(ttl_buf_,"T L %f\n",angle) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void EAST()
{
  ttl_send_command_("T E\n") ;
}

void WEST()
{
  ttl_send_command_("T W\n") ;
}

void NORTH()
{
  ttl_send_command_("T N\n") ;
}

void SOUTH()
{
  ttl_send_command_("T S\n") ;
}

void LW(float width)
{
  int rc ;
  sprintf(ttl_buf_,"W %f\n",width) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void PD()
{
  ttl_send_command_("P D\n") ;
}

void PU()
{
  ttl_send_command_("P U\n") ;
}

void FILL()
{
  ttl_send_command_("L +\n") ;
}

void BRUSH()
{
  ttl_send_command_("L B\n") ;
}

void LINE()
{
  ttl_send_command_("L N\n") ;
  ttl_send_command_("L -\n") ;
}

void IDLE(float u)
{
#ifdef _WIN32
  Sleep((DWORD)(u*1000)) ;
#else
  usleep(u*1000000.0) ;
#endif
}

void RF(float range)
{
  int rc ;
  sprintf(ttl_buf_,"E %f\n",range) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void SAY(char *something)
{
  int rc,cnt=0 ;
  char hex[MAX_MSG*2+1] ;
  ttl_convert_str2hex(something,hex) ;
  sprintf(ttl_buf_,"S %s\n",hex) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void BCAS(char *msg)
{
  int rc ;
  char hex[MAX_MSG*2+1] ;
  ttl_convert_str2hex(msg,hex) ;
  sprintf(ttl_buf_,"U %s\n",hex) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void TPRINTF(const char* format, ...)
{
  va_list arg ;
  char buf[BUFFLEN]={0} ;
  va_start(arg,format) ;
  vsprintf(buf,format,arg) ;
  va_end(arg) ;
  SAY(buf) ;
}

#ifndef _WIN32
void TSCANF(const char* format, ...)
{
  va_list arg ;
  char buf[BUFFLEN]={0} ;
  va_start(arg,format) ;
  vsscanf(ttl_buf_,format,arg) ;
  va_end(arg) ;
}
#endif

static void ttl_send_packet(char *msg)
{
  int rc ;
  if (strlen(msg)>=BUFFLEN) fprintf(stderr,"message is too long\n") ;
  sprintf(ttl_buf_,"%s\n",msg) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void FIRE()
{
  ttl_send_command_("X\n") ;
}

void ROBOT()
{
  ttl_send_command_("R\n") ;
}

void DONUT()
{
  ttl_send_command_("D\n") ;
}

void COIN()
{
  ttl_send_command_("O X\n") ;
}

void BORROWCOIN()
{
  ttl_send_command_("O B\n") ;
}

void DROPCOIN()
{
  ttl_send_command_("O S\n") ;
}

void PICKCOIN()
{
  ttl_send_command_("O R\n") ;
}

void COL(float r, float g, float b)
{
  int rc ;
  sprintf(ttl_buf_,"K %f %f %f\n",r,g,b) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void BGC(float r, float g, float b)
{
  int rc ;
  sprintf(ttl_buf_,"H %f %f %f\n",r,g,b) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void BMODE()
{
  ttl_send_command_("! +\n") ;
}

void GMODE()
{
  ttl_send_command_("! -\n") ; 
}

void NM(char *s)
{
  int rc ;
  char hex[MAX_MSG*2+1] ;
  ttl_convert_str2hex(s,hex) ;
  sprintf(ttl_buf_,"N %s\n",hex) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void TM(int team)
{
  int rc ;
  sprintf(ttl_buf_,"I %d\n",team) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

void Q_ID(int *tid)
{
  int rc,t ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q M\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %s",&t,buf) ;
    *tid = t ;
  }
  else *tid = -1 ;
}

void Q_POS(float *x, float *y)
{
  int rc ;
  float rx,ry ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q P\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%f %f %s",&rx,&ry,buf) ;
    *x = rx ; *y = ry ;
  }
  else { 
    *x= 0 ; *y = 0 ; 
  }
}

void Q_DIR(float *dir)
{
  int rc ;
  float rd ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q D\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%f %s",&rd,buf) ;
    *dir = rd ;
  }
  else { 
    *dir= 0 ; 
  }
}

void Q_ALIVE(int *yn)
{
  int rc,stat=0 ;
  char buf[16] ;
  if (socket_ > 0) {
    int len,n ;
    sprintf(ttl_buf_,"Q A\n") ;
    len = strlen(ttl_buf_) ;
    n = write(socket_, ttl_buf_, len) ;
    if (n>0) {
      memset(ttl_buf_, 0, sizeof(ttl_buf_));
      n = read(socket_, ttl_buf_, sizeof(ttl_buf_));
      if (n<=0) stat=0 ;
      else stat=1 ;
    }
  }
  *yn = stat ;
}

void Q_SCORE(int *score)
{
  int rc,s ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q S\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %s",&s,buf) ;
    *score = s ;
  }
  else { 
    *score= 0 ; 
  }
}

void Q_NT(int *nt)
{
  int rc,na ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q N\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %s",&na,buf) ;
    *nt = na ;
  }
  else { 
    *nt = 0 ; 
  }
}

void Q_COIN(int *nc)
{
  int rc,n ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q C\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %s",&n,buf) ;
    *nc = n ;
  }
  else { 
    *nc = 0 ; 
  }
}

void Q_MYCOIN(int *nmyc)
{
  int rc,n ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q O\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %s",&n,buf) ;
    *nmyc = n ;
  }
  else { 
    *nmyc = 0 ; 
  }
}

void Q_TM(int *team)
{
  *team = team_ ;
}

void Q_RADER(float *dir)
{
  int rc,team ;
  float angle ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q R\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%f %d %s",&angle,&team,buf) ;
    *dir = angle ;
    team_ = team ;
  }
  else { 
    *dir= 0 ; 
    team_ = 0 ;
  }
}

void Q_FINDER(int *yn)
{
  int rc, res1, res2 ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q F\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %d %s",&res1,&res2,buf) ;
    *yn = res1 ;
    team_ = res2 ;
  }
  else { 
    *yn = 0 ;
    team_ = 0 ;
  }
}

void Q_SONAR(int *left, int *front, int *right)
{
  int rc,res1,res2,res3 ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q W\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %d %d %s",&res1,&res2,&res3,buf) ;
    *left = res1 ;
    *front = res2 ;
    *right = res3 ;
  }
  else {
    *left = 0 ;
    *front = 0 ;
    *right = 0 ;
  }
}

void Q_BCAS(int *etime, char *msg)
{
  int rc, res ;
  char buf[MAX_MSG*2+1],rest[BUFFLEN] ;
  sprintf(ttl_buf_,"Q B\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d%s%[^\n]",&res,buf,rest) ;
    ttl_convert_hex2str(buf,msg) ;
    *etime = res ;
  }
}

void Q_TIME(int *tim)
{
  int rc, res ;
  char buf[16] ;
  sprintf(ttl_buf_,"Q T\n") ;
  rc = send_ttl_buf_and_receive_ack()  ;
  if (rc>0) {
    sscanf(ttl_buf_,"%d %s",&res,buf) ;
    *tim = res ;
  }
  else { 
    *tim = 0 ;
  }
}

/* 
   <<Fun stuff>>
   try to call, for example,             
   ttl_koch_curve(2.0, 200.0) ;
*/

#include <math.h>

static float threshold_length_ = 2.0 ;
static float prev_angle_ ;
static void koch_curve_(float, float) ;
void ttl_koch_curve(float length) {
  prev_angle_=0.0 ;
  koch_curve_(length,0.0) ;
}
static void koch_curve_(float length, float angle)
{
  if (length < threshold_length_) {
    FD(length) ;
    LT( 180.0*(angle-prev_angle_)/M_PI ) ;
    prev_angle_ = angle ;
  }
  else {
    koch_curve_(length/3, angle);
    koch_curve_(length/3, angle+M_PI/3) ;
    koch_curve_(length/3, angle-M_PI/3) ;
    koch_curve_(length/3, angle) ;
  }
}
