//
//  bt_sco.c
//
//  Created by hark on 2014/06/13.
//  Copyright (c) 2014 hark. All rights reserved.
//
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <unistd.h>
#include <time.h>
#include <sys/select.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <pthread.h>
#include <samplerate.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <algorithm>

#include "bt_const.h"
#include "bt_extern.h"
#include "bt_printf.h"
#include "bt_sco.h"
#include "bt_rfcomm.h"

#include "lockobject.h"
#include "buffer.h"

#define NELEM(arg) (sizeof(arg) / sizeof(arg[0]))

// 16KHz -> 8KHz ダウンサンプリング
#define ENABLE_DOWNSAMPLING

// 1パケット送るごとに間隔を開けてタイミングを取る(HCI)
#define ENABLE_SOFTWARE_WAIT
//#undef ENABLE_SOFTWARE_WAIT

// Voice path to PCM
#define ENABLE_VOICE_PATH_TO_PCM

static pthread_mutex_t sco_send_mutex;
static int sock_data[2];

static int fpga_fd;

enum event {
  SCO_RAW_DATA_ARRIVE = 0,
};

static void *send_loop(void *param)
{

  int sock = (long int)param;

  debug_printf("**** incoming sco connection %d\n", sock);
  
  struct sco_options so;
  socklen_t so_size = sizeof(so);
  getsockopt(sock, SOL_SCO, SCO_OPTIONS, &so, &so_size);
  const uint16_t max_packet_size = so.mtu;
  //const long wait_nsec = ((long long)so.mtu / 2) * 1000000000 / 8000;
  const long wait_nsec = ((int64_t)so.mtu / 2) * 1000000000 / 8000;

  debug_printf("**** mtu %d %ld(nsec)\n", so.mtu, wait_nsec);

  lockobject lock(&sco_send_mutex);

  char *buf = (char *)malloc(max_packet_size);

  fd_set rfds;
  
  struct rawdata rdata;
  
  FD_ZERO(&rfds);
  
  FD_SET(sock, &rfds);
  FD_SET(sock_data[1], &rfds);
  
  const int watch_sock = std::max(sock, sock_data[1]);
  
  while (select(watch_sock + 1, &rfds, NULL, NULL, NULL) > 0) {
    
    if (FD_ISSET(sock_data[1], &rfds)) {
      
      unsigned int event;
      ssize_t remain = sizeof(event);
      char *ptr = reinterpret_cast<char *>(&event);

      while(remain > 0) {
	ssize_t rsize = read(sock_data[1], ptr, remain);
	if (rsize > 0) {
	  remain -= rsize;
	  ptr += rsize;
	} else {
	  perror("read");
	  debug_printf("socketpair ?\n");
	  return((void *)-1);
	}
      }


      //debug_printf("event = %u\n", event);
      
      if (event == SCO_RAW_DATA_ARRIVE) {
	
	buf_read(&rdata);

#ifdef ENABLE_DOWNSAMPLING
	if (bt_resampling) {

	  // 16K->8Kにダウンサンプリング

	  typedef short int int16;
	  float data_in[sizeof(rdata.data) / sizeof(int16)];
	  float data_out[(sizeof(rdata.data) / sizeof(int16)) / 2];
	  
	  int i;
	  int16 *ptr = (int16 *)rdata.data;
	  const long input_frames = rdata.num_bytes / sizeof(int16);

	  src_short_to_float_array(ptr, data_in, input_frames);
	  
	  SRC_DATA src;
	  src.data_in = data_in;
	  src.data_out = data_out;
	  src.input_frames = input_frames;
	  src.output_frames = NELEM(data_out);
	  src.input_frames_used = 0;
	  src.output_frames_gen = 0;
	  src.end_of_input = 0;
	  src.src_ratio = 8000.0 / 16000;

	  const int channels = 1;

	  if (src_simple(&src, SRC_SINC_BEST_QUALITY, channels) != 0) {
	    perror("src_simple");
	    return((void *)-1);
	  }

	  src_float_to_short_array(data_out, ptr, src.output_frames_gen);

	  rdata.num_bytes = src.output_frames_gen * sizeof(int16);

#if 0

	  static int once = 0;
	  if (once == 0) {
	    FILE *fp = fopen("output000-8k.raw", "wb");
	    if (fp == NULL) {
	      perror("file?");
	      exit(1);
	    }
	    fwrite(ptr, sizeof(int16), src.output_frames_gen, fp);
	  
	    fclose(fp);

	    once = 1;

	  }
#endif

	}
#endif

	if (bt_wait_voice_recognition) {
	  // 音声認識ON
	  rfcomm_voice_recognition_activate();
	  // OKが返っていても、適当な時間待たないとSiriが受け取れない	
	  debug_printf("sleep 3sec.\n");
	  sleep(3);
	}

	debug_printf("send wavdata %d\n", rdata.num_bytes);

#ifndef ENABLE_VOICE_PATH_TO_PCM
	//
	// HCI
	//
	int pos = 0;
	int remain = rdata.num_bytes;

	fd_set wfds;
	
	FD_ZERO(&wfds);
	FD_SET(sock, &wfds);
	
#ifdef ENABLE_SOFTWARE_WAIT
	struct timespec now;
	clock_gettime(CLOCK_MONOTONIC, &now);
#endif
	while (select(sock + 1, NULL, &wfds, NULL, NULL) > 0) {
	  
	  const int sendsize = std::min(remain, static_cast<int>(max_packet_size));
	  ssize_t sent = 0;
	  if ((sent = send(sock, &rdata.data[pos], sendsize, 0)) <= 0) {
	    goto Err;
	  } else {
	    remain -= sent;
	    pos += sent;
	    if (remain <= 0) {
	      break;
	    }
	  }
	  
	  FD_ZERO(&wfds);
	  FD_SET(sock, &wfds);
	  
#ifdef ENABLE_SOFTWARE_WAIT
	  // selectで書いていいことになっているはずだが、待たないと潰す
	  struct timespec tm;
	  
#if 1
	  tm = now;
	  tm.tv_nsec += wait_nsec;
	  if (tm.tv_nsec >= 10000000000) {
	    tm.tv_nsec -= 10000000000;
	    tm.tv_sec++;
	  }
	  
	  if (clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &tm, NULL) != 0) {
	    //perror("clock_nanosleep");
	    //debug_printf("nanosleep error?\n");
	  }
	  
#else
	  
	  struct timespec remain;
	  tm.tv_sec = 0;
	  tm.tv_nsec = wait_nsec;
	  
	  int ret = 0;
	  while((ret = clock_nanosleep(CLOCK_MONOTONIC, 0, &tm, &remain)) != 0) {
	    debug_printf("nanosleep error? %d\n", ret);
	    tm = remain;
	  }
#endif
	  clock_gettime(CLOCK_MONOTONIC, &now);
	  
#endif
	  //usleep(4000);
	  //usleep(3000);
	  
	}

#else  //  ENABLE_VOICE_PATH_TO_PCM
	//
	// PCM
	//

	// rdata.data に入っている rdata.num_bytes 分が生データ

	int pos = 0;
	int remain = rdata.num_bytes;

	fd_set wfds;
	
	FD_ZERO(&wfds);
	FD_SET(fpga_fd, &wfds);
	
	while (select(fpga_fd + 1, NULL, &wfds, NULL, NULL) > 0) {
	  
	  const int sendsize = remain;
	  ssize_t sent = 0;
	  if ((sent = write(fpga_fd, &rdata.data[pos], sendsize)) <= 0) {
	    goto Err;
	  } else {
	    remain -= sent;
	    pos += sent;
	    if (remain <= 0) {
	      break;
	    }
	  }
	  
	  FD_ZERO(&wfds);
	  FD_SET(fpga_fd, &wfds);

	}



#endif	//  ENABLE_VOICE_PATH_TO_PCM

	if (bt_wait_voice_recognition) {
	  // 送信が終わって音声認識をしている時間待つ必要がある
	  // 終了が通知されることはない
	  // とりあえず固定値
	  debug_printf("sleep 5sec.\n");
	  sleep(5);
	}
	
      } else {
	// unknown
	debug_printf("unknown event %u", event);
      }
      
    }
    
    if (FD_ISSET(sock, &rfds)) {
      
      ssize_t readsize = recv(sock, buf, max_packet_size, 0);
      if (readsize <= 0) {
	// sco接続きれた?
	debug_printf("**** sco disconnect?\n");
	break;
      }
    }


    FD_ZERO(&rfds);
    
    FD_SET(sock, &rfds);
    FD_SET(sock_data[1], &rfds);
    
  }

Err:

  close(sock);

  free(buf);
  
  debug_printf("**** sco send loop %d exit\n", sock);
  
  return(NULL);
}

static void *send_loop_sco(void *param)
{

  int sock = (long int)param;

  debug_printf("**** send_loop_sco\n");
  debug_printf("**** incoming sco connection %d\n", sock);
  
  struct sco_options so;
  socklen_t so_size = sizeof(so);
  getsockopt(sock, SOL_SCO, SCO_OPTIONS, &so, &so_size);
  const uint16_t max_packet_size = so.mtu;
  //const long wait_nsec = ((long long)so.mtu / 2) * 1000000000 / 8000;
  const long wait_nsec = ((int64_t)so.mtu / 2) * 1000000000 / 8000;

  debug_printf("**** mtu %d %ld(nsec)\n", so.mtu, wait_nsec);

  lockobject lock(&sco_send_mutex);

  char *buf = (char *)malloc(max_packet_size);

  fd_set rfds;
  
  struct rawdata rdata;
  
  FD_ZERO(&rfds);
  
  FD_SET(sock, &rfds);
  FD_SET(sock_data[1], &rfds);
  
  const int watch_sock = std::max(sock, sock_data[1]);
  
  while (select(watch_sock + 1, &rfds, NULL, NULL, NULL) > 0) {
    
    if (FD_ISSET(sock_data[1], &rfds)) {
      
      unsigned int event;
      ssize_t remain = sizeof(event);
      char *ptr = reinterpret_cast<char *>(&event);

      while(remain > 0) {
	ssize_t rsize = read(sock_data[1], ptr, remain);
	if (rsize > 0) {
	  remain -= rsize;
	  ptr += rsize;
	} else {
	  perror("read");
	  debug_printf("socketpair ?\n");
	  return((void *)-1);
	}
      }


      //debug_printf("event = %u\n", event);
      
      if (event == SCO_RAW_DATA_ARRIVE) {
      } else {
	// unknown
	debug_printf("unknown event %u", event);
      }
      
    }
    
    if (FD_ISSET(sock, &rfds)) {
      
      ssize_t readsize = recv(sock, buf, max_packet_size, 0);
      if (readsize <= 0) {
	// sco接続きれた?
	debug_printf("**** sco disconnect?\n");
	break;
      }
    }


    FD_ZERO(&rfds);
    
    FD_SET(sock, &rfds);
    FD_SET(sock_data[1], &rfds);
    
  }

Err:

  close(sock);

  free(buf);
  
  debug_printf("**** sco send loop %d exit\n", sock);
  
  return(NULL);
}


static void *send_loop_data(void *param)
{

  debug_printf("**** send_loop_data start\n");
  
  struct rawdata rdata;
  
  while (1) {
    
    buf_read(&rdata);
    
#ifdef ENABLE_DOWNSAMPLING
    if (bt_resampling) {
      
      // 16K->8Kにダウンサンプリング
      
      typedef short int int16;
      float data_in[sizeof(rdata.data) / sizeof(int16)];
      float data_out[(sizeof(rdata.data) / sizeof(int16)) / 2];
      
      int i;
      int16 *ptr = (int16 *)rdata.data;
      const long input_frames = rdata.num_bytes / sizeof(int16);
      
      src_short_to_float_array(ptr, data_in, input_frames);
      
      SRC_DATA src;
      src.data_in = data_in;
      src.data_out = data_out;
      src.input_frames = input_frames;
      src.output_frames = NELEM(data_out);
      src.input_frames_used = 0;
      src.output_frames_gen = 0;
      src.end_of_input = 0;
      src.src_ratio = 8000.0 / 16000;
      
      const int channels = 1;
      
      if (src_simple(&src, SRC_SINC_BEST_QUALITY, channels) != 0) {
	perror("src_simple");
	return((void *)-1);
      }
      
      src_float_to_short_array(data_out, ptr, src.output_frames_gen);
      
      rdata.num_bytes = src.output_frames_gen * sizeof(int16);
    }
#endif

#ifndef ENABLE_VOICE_PATH_TO_PCM

#else  //  ENABLE_VOICE_PATH_TO_PCM
    //
    // PCM
    //
    
    // rdata.data に入っている rdata.num_bytes 分が生データ
    
    int pos = 0;
    int remain = rdata.num_bytes;
    
    fd_set wfds;
    
    FD_ZERO(&wfds);
    FD_SET(fpga_fd, &wfds);
    
    while (select(fpga_fd + 1, NULL, &wfds, NULL, NULL) > 0) {
      
      const int sendsize = remain;
      ssize_t sent = 0;
      if ((sent = write(fpga_fd, &rdata.data[pos], sendsize)) <= 0) {
	goto Err;
      } else {
	remain -= sent;
	pos += sent;
	if (remain <= 0) {
	  break;
	}
      }
      
      FD_ZERO(&wfds);
      FD_SET(fpga_fd, &wfds);
      
    }
#endif	//  ENABLE_VOICE_PATH_TO_PCM
    
  }

Err:

  debug_printf("**** send_loop_data exit\n");
  
  return(NULL);
}


//
// TODO:ため込まない版
//
#define DEBUG_FILE_OUT
#undef DEBUG_FILE_OUT

static void *send_loop_data_async(void *param)
{

  debug_printf("**** send_loop_data_async start\n");
  struct rawdata_v rdata;

#ifdef ENABLE_DOWNSAMPLING

  int total = 0;

  const int channels = 1;
  SRC_STATE *source_state = NULL;
  int error = 0;

  typedef short int int16;

  std::vector<float> data_in;
  std::vector<float> data_out;

  if (::bt_resampling) {

    data_in.resize(::bt_send_size_byte / sizeof(int16));
    data_out.resize(std::max((::bt_send_size_byte / sizeof(int16)), 143u)); // 143フレーム分抱え込んでいて最後にその分吐き出される

    source_state = ::src_new(SRC_SINC_BEST_QUALITY, channels, &error);

  }

#endif

#ifdef DEBUG_FILE_OUT
  FILE *fp = NULL;
#endif
  
  while (1) {
    
    dbuf_read(&rdata);

#ifdef ENABLE_DOWNSAMPLING

    if (::bt_resampling) {
      
      if (rdata.seq_no == 0) {
	src_reset(source_state);
      }

      // 16K->8Kにダウンサンプリング

      //debug_printf("num_bytes %d\n", rdata.num_bytes);

      const long input_frames = rdata.num_bytes / sizeof(int16);
      
      int i;
      int16 *ptr = (int16 *)&rdata.data[0];
      
      ::src_short_to_float_array(ptr, &data_in[0], input_frames);
      
      SRC_DATA src;
      src.data_in = &data_in[0];
      src.data_out = &data_out[0];
      src.input_frames = input_frames;
      src.output_frames = data_out.size();
      src.input_frames_used = 0;
      src.output_frames_gen = 0;
      src.end_of_input = (rdata.isLast ? 1 : 0);
      src.src_ratio = 8000.0 / 16000;
      
      if (::src_process(source_state, &src) != 0) {
	perror("src_process");
	return((void *)-1);
      }
      
      ::src_float_to_short_array(&data_out[0], ptr, src.output_frames_gen);
      
      rdata.num_bytes = src.output_frames_gen * sizeof(int16);

      //if (rdata.isLast) {
      //debug_printf("last %d\n", rdata.num_bytes);
      //}

    }
#endif
    
#ifndef ENABLE_VOICE_PATH_TO_PCM

#else  //  ENABLE_VOICE_PATH_TO_PCM
    //
    // PCM
    //
    
    // rdata.data に入っている rdata.num_bytes 分が生データ

#ifdef DEBUG_FILE_OUT
    if (rdata.seq_no == 0) {

      fp = fopen("/root/resample.raw", "wb");
      if (fp == NULL) {
	perror("file?");
	exit(1);
      }

    }
#endif
    
    if (::bt_resampling || ((!::bt_resampling) && (!rdata.isLast))) {

      int pos = 0;
      int remain = rdata.num_bytes;
      
      fd_set wfds;
      
      FD_ZERO(&wfds);
      FD_SET(fpga_fd, &wfds);
      
      while (select(fpga_fd + 1, NULL, &wfds, NULL, NULL) > 0) {
	
	const int sendsize = remain;
	ssize_t sent = 0;
	if ((sent = write(fpga_fd, &rdata.data[pos], sendsize)) <= 0) {
	  goto Err;
	} else {
	  remain -= sent;
	  pos += sent;
	  if (remain <= 0) {
	    break;
	  }
	}
	
	FD_ZERO(&wfds);
	FD_SET(fpga_fd, &wfds);
	
      }
      
      //struct timespec res;
      //::clock_gettime(CLOCK_MONOTONIC, &res);
      //debug_printf("sec %d msec %d\n", res.tv_sec, res.tv_nsec / 1000000);
      
      //debug_printf("dev rdata.num_bytes %d\n", rdata.num_bytes);

      total += rdata.num_bytes;

    }

#ifdef DEBUG_FILE_OUT
    if (fp != NULL) {
      if (::bt_resampling || ((!::bt_resampling) && (!rdata.isLast))) {
	//debug_printf("file rdata.num_bytes %d\n", rdata.num_bytes);
	size_t writesize = fwrite(&rdata.data[0], 1, rdata.num_bytes, fp);
      }
    }
    
    if ((fp != NULL) && rdata.isLast) {
      //debug_printf("%d close\n", total);
      fclose(fp);
      fp = NULL;
    }
#endif
    
#endif	//  ENABLE_VOICE_PATH_TO_PCM
    
  }

Err:

  if (::bt_resampling) {
    if (source_state != NULL) {
      ::src_delete(source_state);
    }
  }

  debug_printf("**** send_loop_data_async exit\n");
  
  return(NULL);
}



static int check_vs(void)
{

  int adapter_id = 0;
  int sock = 0;
  adapter_id = hci_get_route(NULL);
  if (adapter_id < 0) {
    perror("adapter_id");
    return(-1);
  }
  sock = hci_open_dev(adapter_id);
  if (sock < 0) {
    perror("open socket");
    return(-1);
  }
  
  {
    uint16_t vs = 0;
    int ret = 0;
    ret = hci_read_voice_setting(sock, &vs, 1000);
    if (ret < 0) {
      perror("voice setting");
      hci_close_dev(sock);
      return(-1);
    }
    
    debug_printf("vs = 0x%02x\n", (int)vs);
    
    ret = hci_write_voice_setting(sock, vs, 1000);
    if (ret < 0) {
      perror("voice setting");
      hci_close_dev(sock);
      return(-1);
    }
    
  }
  
  hci_close_dev(sock);
  
  return(0);
}

static void *server_sco_loop(void *param)
{

  static const bdaddr_t bdaddr_any = {0, 0, 0, 0, 0, 0};
  
  //	int sock = *(int *)param;
  fd_set rfds;
  
  int sock = 0;
  struct sockaddr_sco addr;
  
  addr.sco_family = AF_BLUETOOTH;
  addr.sco_bdaddr = bdaddr_any;
  
  // alloc a sco socket
  sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
  if (sock < 0) {
    perror("alloc a sco socket");
    return((void *)-1);
  }
  
  if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0) {
    perror("bind a sco socket");
    close(sock);
    return((void *)-1);
  }
  
  listen(sock, 1);
  
  FD_ZERO(&rfds);
  FD_SET(sock, &rfds);
  
  debug_printf("**** wait for sco accept\n");
  
  while(select(sock + 1, &rfds, NULL, NULL, NULL) > 0) {

    check_vs();

    int client_sock = 0;
    struct sockaddr_sco addr;
    socklen_t addrlen = sizeof(addr);

    client_sock = accept(sock, (struct sockaddr *)&addr, &addrlen);

    //debug_printf("**** sco accepted %d!!!\n", client_sock);

    char pszremote[32];
    ba2str(&addr.sco_bdaddr, pszremote);
    
    debug_printf("**** sco connection from: %s\n", pszremote);


    {
      pthread_t thread;
      pthread_attr_t attr;
      
      pthread_attr_init(&attr);
      pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
      
      if(bt_async_send){
	if (pthread_create(&thread, &attr, send_loop_sco, (void *)(long int)client_sock) != 0) {
	  perror("send loop create");
	  close(sock);
	  pthread_attr_destroy(&attr);
	  return((void *)-1);
	}
      }
      else{
	if (pthread_create(&thread, &attr, send_loop, (void *)(long int)client_sock) != 0) {
	  perror("send loop create");
	  close(sock);
	  pthread_attr_destroy(&attr);
	  return((void *)-1);
	}
      }
      
      pthread_attr_destroy(&attr);
    }
    
    FD_ZERO(&rfds);
    FD_SET(sock, &rfds);
    
    debug_printf("wait for sco accept\n");
    
  }
  
  
  close(sock);
  return(NULL);
}


int tell_data_arrived(void)
{

  const unsigned int data = SCO_RAW_DATA_ARRIVE;
  return(write(sock_data[0], &data, sizeof(data)));

}

int make_sco_server(void)
{
  
  pthread_t thread;
  pthread_attr_t attr;
  
  pthread_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
  
  //debug_printf("sock %d\n", sock);
  
  if (pthread_create(&thread, &attr, server_sco_loop, NULL) != 0) {
    perror("server loop create");
    pthread_attr_destroy(&attr);
    return(-1);
  }

  if (bt_async_send){
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    
    if (pthread_create(&thread, &attr, send_loop_data, NULL) != 0) {
      perror("send loop data create");
      pthread_attr_destroy(&attr);
      return(-1);
    }
  }

  //
  // TODO:ため込まない版
  //
  pthread_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
  if (pthread_create(&thread, &attr, send_loop_data_async, NULL) != 0) {
    perror("server loop create");
    pthread_attr_destroy(&attr);
    return(-1);
  }
  
  pthread_attr_destroy(&attr);

  return(0);
}

int init_sco(void)
{

  if (socketpair(AF_UNIX, SOCK_STREAM, 0, sock_data) == -1 ) {
    perror("socketpair");
    return(-1);
  }

#if 0
  int rcvbuf = 0;
  int sndbuf = 0;
  socklen_t optlen;

  optlen = sizeof(rcvbuf);

  if (getsockopt(sock_data[0], SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) != 0) {
    perror("getsockopt");
    return(-1);
  }
  debug_printf("socketpair rcvbuf %d\n", rcvbuf);

  optlen = sizeof(sndbuf);

  if (getsockopt(sock_data[0], SOL_SOCKET, SO_SNDBUF, &sndbuf, &optlen) != 0) {
    perror("getsockopt");
    return(-1);
  }
  debug_printf("socketpair sndbuf %d\n", sndbuf);
#endif

  //debug_printf("%d %d\n", sock_data[0], sock_data[1]);

  int ret = ::pthread_mutex_init(&sco_send_mutex, NULL);
  if (ret != 0) {
    return(ret);
  }


  fpga_fd = open("/dev/kt", O_RDWR, 0666);
  if(fpga_fd < 0) {
    printf("/dev/kt open error.\n");
    return -1;
  }

  return(0);
}
