/* 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-class.h"


Turtle::Turtle(const char *hostname) {
  int rc ;
  socket_ = -1 ;
  team_ = 0 ;
  threshold_length_ = 2.0 ;
  rc = con(hostname) ;
}

Turtle::~Turtle() {
  discon() ;
}

void Turtle::discon(void)
{
#ifdef _WIN32
  if (socket_>=0) closesocket(socket_) ;
#else
  if (socket_>=0) close(socket_) ;
#endif
}

int Turtle::con(const char *hostname)
{
  struct sockaddr_in serveraddr;
  struct hostent *host;
  int rc ;
  
#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) ;
  }
  return rc ;
}

int Turtle::send_ttl_buf_and_receive_ack(void) 
{
  int len,n,k ;
  int eventcode, timecode ;
  char *ack[32] ;
  
  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++ ;
  }
  eventcode = (unsigned int) atoi(ack[k-1]) ; /* event code comes always last */
  timecode = (unsigned int) atoi(ack[k-2]) ; 
  if (eventcode!=0) check_event(eventcode,timecode) ;
#ifdef _WIN32
  Sleep(TSLEEP/1000) ;
#else
  usleep(TSLEEP) ;
#endif
  return n ;
}

void Turtle::check_event(int eventcode, int timecode)
{
  char msg_stack[BUFFLEN] ;
  strcpy(msg_stack,ttl_buf_) ;
  disable_event(eventcode) ;
  if (eventcode & EVENT_HIT_BY_BULLET) {
    hit_by_bullet(timecode) ;
  }
  if (eventcode & EVENT_RUN_INTO_TURTLE) {
    run_into_turtle(timecode);
  }
  if (eventcode & EVENT_RUN_INTO_DONUT) {
    run_into_donut(timecode);
  }
  if (eventcode & EVENT_RUN_INTO_STONE) {
    run_into_stone(timecode);
  }
  if (eventcode & EVENT_RUN_INTO_WALL) {
    run_into_wall(timecode);
  }
  if (eventcode & EVENT_FOUND_COIN) {
    found_coin(timecode);
  }
  if (eventcode & EVENT_DETECTED_BY_FINDER) {
    detected_by_finder(timecode);
  }
  if (eventcode & EVENT_DETECTED_BY_RADER) {
    detected_by_rader(timecode);
  }
  if (eventcode & EVENT_GOT_MESSAGE) {
    got_message(timecode);
  }
  enable_event(eventcode) ;
  strcpy(ttl_buf_,msg_stack) ;
}

void Turtle::hit_by_bullet(int timecode) { }
void Turtle::run_into_turtle(int timecode) { }
void Turtle::run_into_donut(int timecode) { }
void Turtle::run_into_stone(int timecode) { }
void Turtle::run_into_wall(int timecode) { }
void Turtle::found_coin(int timecode) { }
void Turtle::detected_by_finder(int timecode) { }
void Turtle::detected_by_rader(int timecode) { }
void Turtle::got_message(int timecode) { }

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

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

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

void Turtle::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++ ;
  }
}

int Turtle::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 ;
}

void Turtle::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 Turtle::home()
{
  ttl_send_command_("C H\n") ;
}

void Turtle::rst()
{
  ttl_send_command_("C\n") ;
}


void Turtle::cf()
{
  ttl_send_command_("C F\n") ;

}

void Turtle::clr()
{
  ttl_send_command_("C A\n") ;
}

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

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

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

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

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

void Turtle::east()
{
  ttl_send_command_("T E\n") ;
}

void Turtle::west()
{
  ttl_send_command_("T W\n") ;
}

void Turtle::north()
{
  ttl_send_command_("T N\n") ;
}

void Turtle::south()
{
  ttl_send_command_("T S\n") ;
}

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

void Turtle::pd()
{
  ttl_send_command_("P D\n") ;
}

void Turtle::pu()
{
  ttl_send_command_("P U\n") ;
}

void Turtle::fill()
{
  ttl_send_command_("L +\n") ;
}

void Turtle::brush()
{
  ttl_send_command_("L B\n") ;
}

void Turtle::line()
{
  ttl_send_command_("L -\n") ;
  ttl_send_command_("L N\n") ;
}

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

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

void Turtle::say(const char *something)
{
  int rc ;
  char str[BUFFLEN] ;
  strcpy(str,something) ;
  say(str) ;
}

void Turtle::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 Turtle::bcas(const char *msg)
{
  int rc ;
  char str[BUFFLEN] ;
  strcpy(str,msg) ;
  bcas(str) ;
}

void Turtle::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 Turtle::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 Turtle::tscanf(const char* format, ...)
{
  va_list arg ;
  char buf[BUFFLEN]={0} ;
  va_start(arg,format) ;
  vsscanf(ttl_buf_,format,arg) ;
  va_end(arg) ;
}
#endif

void Turtle::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 Turtle::fire()
{
  ttl_send_command_("X\n") ;
}

void Turtle::robot()
{
  ttl_send_command_("R\n") ;
}

void Turtle::doughnut()
{
  ttl_send_command_("D\n") ;
}

void Turtle::dropcoin()
{
  ttl_send_command_("O S\n") ;
}

void Turtle::pickcoin()
{
  ttl_send_command_("O R\n") ;
}

void Turtle::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 Turtle::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 Turtle::bmode()
{
  ttl_send_command_("! +\n") ;
}

void Turtle::gmode()
{
  ttl_send_command_("! -\n") ; 
}

void Turtle::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 Turtle::tm(int team)
{
  int rc ;
  sprintf(ttl_buf_,"I %d\n",team) ;
  rc = send_ttl_buf_and_receive_ack()  ;
}

int Turtle::q_id(void)
{
  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) ;
    return t ;
  }
  return -1 ;
}

struct t_position Turtle::q_pos(void)
{
  int rc ;
  float rx,ry ;
  char buf[16] ;
  struct t_position res ;
  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) ;
    res.x = rx ; res.y = ry ;
  }
  else { 
    res.x= 0 ; res.y = 0 ; 
  }
  return res ;
}

float Turtle::q_dir(void)
{
  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) ;
    return rd ;
  }
  else { 
    return 0 ; 
  }
}

bool Turtle::q_alive(void)
{
  int rc,stat=0 ;
  char buf[16] ;
  if (socket_ > 0) {
    int len,n ;
    sprintf(ttl_buf_,"Q A\n") ;
    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) stat=0 ;
      else stat=1 ;
    }
  }
  if (stat) return true ;
  else return false ;
}

int Turtle::q_score(void)
{
  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) ;
    return s ;
  }
  else { 
    return 0 ; 
  }
}

int Turtle::q_nt(void)
{
  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) ;
    return na ;
  }
  else { 
    return 0 ; 
  }
}

int Turtle::q_coin(void)
{
  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) ;
    return n ;
  }
  else { 
    return 0 ; 
  }
}

int Turtle::q_tm(void)
{
  return team_ ;
}

float Turtle::q_rader(void)
{
  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) ;
    team_ = team ;
    return angle ;
  }
  else { 
    team_ = 0 ;
    return 0 ; 
  }
}

int Turtle::q_finder(void)
{
  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) ;
    team_ = res2 ;
    return res1 ;

  }
  else { 
    team_ = 0 ;
    return 0 ;
  }
}

struct t_sonar Turtle::q_sonar(void)
{
  int rc,res1,res2,res3 ;
  char buf[16] ;
  struct t_sonar res ;
  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) ;
    if (res1==0) res.left = false ; else res.left = true ;
    if (res2==0) res.front = false ; else res.front = true ;
    if (res3==0) res.right = false ; else res.right = true ;
  }
  else {
    res.left = false ;
    res.front = false ;
    res.right = false ;
  }
  return res ;
}

int Turtle::q_bcas(char *msg)
{
  int rc, res=0 ;
  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) ;
  }
  return res ;
}

int Turtle::q_time(void)
{
  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) ;
    return res ;
  }
  else { 
    return 0 ;
  }
}

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

void Turtle::ttl_koch_curve(float length) {
  prev_angle_=0.0 ;
  koch_curve_(length,0.0) ;
}
void Turtle::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) ;
  }
}
