/***************************************************************
【函数功能】： 使能惯导模块的数据输出
【输    入】： bEnable：True使能输出；False失能输出
【输    出】： 执行结果：0正确；-1错误
【说    明】： ！经过实验验证！
***************************************************************/
int CGyro::Enable(bool bEnable)
{
	char eBuf[18] = {'@','*',1,1, 0,0,0,0, 0,0,0,0, 0,0,0,0, -2,'*'};
	char dBuf[18] = {'@','*',1,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, -1,'*'};

	if(!m_bPortOpened)
		return -1;
	//if (bEnable^m_bOutEnabled)	// 0,1或1,0才输出。不同才操作
	{
		if (bEnable)
		{
			sio_write(m_iPort, eBuf, 18);
			sleep(10);
			sio_write(m_iPort, eBuf, 18);
			m_bOutEnabled = true;
		} 
		else
		{
			sio_write(m_iPort, dBuf, 18);
			sleep(10);
			sio_write(m_iPort, dBuf, 18);
			m_bOutEnabled = false;
		}
	}
	return 0;
}

static ALuint sndio_proc(ALvoid *ptr)
{
    ALCdevice *device = ptr;
    sndio_data *data = device->ExtraData;
    ALsizei frameSize;
    size_t wrote;

    SetRTPriority();

    frameSize = FrameSizeFromDevFmt(device->FmtChans, device->FmtType);

    while(!data->killNow && device->Connected)
    {
        ALsizei len = data->data_size;
        ALubyte *WritePtr = data->mix_data;

        aluMixData(device, WritePtr, len/frameSize);
        while(len > 0 && !data->killNow)
        {
            wrote = sio_write(data->sndHandle, WritePtr, len);
            if(wrote == 0)
            {
                ERR("sio_write failed\n");
                aluHandleDisconnect(device);
                break;
            }

            len -= wrote;
            WritePtr += wrote;
        }
    }

    return 0;
}

/*
 * fluid_sndio_audio_run
 */
void*
fluid_sndio_audio_run2(void* d)
{
  fluid_sndio_audio_driver_t* dev = (fluid_sndio_audio_driver_t*) d;
  short* buffer = (short*) dev->buffer;
  float* left = dev->buffers[0];
  float* right = dev->buffers[1];
  int buffer_size = dev->buffer_size;
  int dither_index = 0;

  FLUID_LOG(FLUID_DBG, "Audio thread running");

  /* it's as simple as that: */
  while (dev->cont)
  {
    (*dev->callback)(dev->data, buffer_size, 0, NULL, 2, dev->buffers);

    fluid_synth_dither_s16 (&dither_index, buffer_size, left, right,
			    buffer, 0, 2, buffer, 1, 2);

    sio_write (dev->hdl, buffer, dev->buffer_byte_size);
  }

  FLUID_LOG(FLUID_DBG, "Audio thread finished");

  pthread_exit(NULL);

  return 0; /* not reached */
}

//---------------------------------------------------------------------------
bool __fastcall CSerialPort::WriteData(char *pBuffer)
{
        if(sio_write(m_nPortNo,pBuffer,strlen(pBuffer))<0)
        return false;

        return true;
}

static uint16_t wbsio_get_spibase(uint16_t port)
{
	uint8_t id;
	uint16_t flashport = 0;

	w836xx_ext_enter(port);
	id = sio_read(port, 0x20);
	if (id != 0xa0) {
		msg_perr("\nW83627 not found at 0x%x, id=0x%02x want=0xa0.\n", port, id);
		goto done;
	}

	if (0 == (sio_read(port, 0x24) & 2)) {
		msg_perr("\nW83627 found at 0x%x, but SPI pins are not enabled. (CR[0x24] bit 1=0)\n", port);
		goto done;
	}

	sio_write(port, 0x07, 0x06);
	if (0 == (sio_read(port, 0x30) & 1)) {
		msg_perr("\nW83627 found at 0x%x, but SPI is not enabled. (LDN6[0x30] bit 0=0)\n", port);
		goto done;
	}

	flashport = (sio_read(port, 0x62) << 8) | sio_read(port, 0x63);

done:
	w836xx_ext_leave(port);
	return flashport;
}

/* write as many blocks as is currently possible */
static void
sndio_update(int threaded)
{
	struct pollfd pfd;
	nfds_t nfds;
	int i, nblocks, nbytes;

	/* make sure counters have been updated */
	nfds = sio_pollfd(hdl, &pfd, POLLOUT);
	poll(&pfd, nfds, 0);
	if (!(sio_revents(hdl, &pfd) & POLLOUT))
		return;

	nblocks = (sndio_play_appbufsz - (sndio_playpos - sndio_realpos)) /
	    sndio_play_round;

	/* we got POLLOUT, so we can write something.  if we don't
	 * write anything, we could underrun.
	 */
	if (nblocks < 1)
		nblocks = 1;

	for (i = 0; i < nblocks; i++) {
		sio_write(hdl, sndio_play_bufdata, sndio_play_bufsize);
		sndio_playpos += sndio_play_round;
		if (sio_eof(hdl)) {
			/* print error message? */
			return;
		}
		_mix_some_samples((uintptr_t) sndio_play_bufdata, 0, sndio_signed);
	}
}

static u32_t
ppp_output_cb(ppp_pcb *pcb, u8_t *data, u32_t len, void *ctx)
{
  LWIP_UNUSED_ARG(pcb);
  LWIP_UNUSED_ARG(ctx);
  return sio_write(ppp_sio, data, len);
}

void AudioSndio::run( void )
{
	surroundSampleFrame * temp =
	    new surroundSampleFrame[mixer()->framesPerPeriod()];
	int_sample_t * outbuf =
	    new int_sample_t[mixer()->framesPerPeriod() * channels()];

	while( TRUE )
	{
		const fpp_t frames = getNextBuffer( temp );
		if( !frames )
		{
			break;
		}

		uint bytes = convertToS16( temp, frames,
		    mixer()->masterGain(), outbuf, FALSE );
		if( sio_write( m_hdl, outbuf, bytes ) != bytes )
		{
			break;
		}
	}

	delete[] temp;
	delete[] outbuf;
}

static gboolean
try_write (Stream *stream)
{
#define STEP 1024
  int avail, step, rendered, written, bpf;
  gboolean finish = FALSE;

  avail = swfdec_playback_stream_avail_update (stream);

  bpf = stream->par.bps * stream->par.pchan;

  while (avail > 0 && !finish) {
    gint16 data[2 * STEP];

    step = MIN (avail, STEP);
    rendered = swfdec_audio_render (stream->audio, data, stream->offset, step);
    if (rendered < step) {
      finish = TRUE;
    }
    written = sio_write (stream->hdl, data, rendered * bpf) / bpf;
    avail -= written;
    stream->offset += rendered;
    stream->f_written += written;
  }

  if (finish) {
    swfdec_playback_stream_remove_handlers (stream);
  }
  return TRUE;
#undef STEP
}

/***************************************************************
【函数功能】： 发送Set报文
【输    入】： 无
【输    出】： 执行结果：0正确；-1错误；-2发送报文失败；
【说    明】： 在timer中第6发送时刻被调用。直接通过串口发送。
***************************************************************/
int CSensor::SendSetFrame()
{
	if(!m_bPortOpened)
		return -1;
	if (m_pSendQueue->QueueNData() < 10)
	{
		m_pSendQueue->QueueFlush();
		return -1;
	}
	char buf[10];
	m_pSendQueue->QueueNRead(buf, 10);
	if (((BYTE)buf[0] != 0xAA) || ((BYTE)buf[1] != 0x55))
	{
		return -1;
	}
	/**********************************************/
	WaitForSingleObject(m_hMutex,1000);

	// 占用串口发送资源
	m_sCmd.send = (BYTE)buf[3];
	m_sCmd.stat = STAT_SENSOR_T2T;
	m_sCmd.addr = (BYTE)buf[4];
	m_sCmd.ack = CMD_IO_ACK;
	// 直接通过串口发送，保证时间精度
	int err = sio_write(m_iPort, buf, 10);		// 持续时间大约10us
	m_sCmd.stat = STAT_SENSOR_T2R;

	ReleaseMutex(m_hMutex);
	/**********************************************/
	if (err != 10)
		return -2;
	return 0;

}

static PaError
BlockingWriteStream(PaStream* stream, const void *data, unsigned long numFrames)
{
	PaSndioStream *s = (PaSndioStream *)stream;
	unsigned n, res;

	while (numFrames > 0) {
		n = s->par.round;
		if (n > numFrames)
			n = numFrames;
		PaUtil_SetOutputFrameCount(&s->bufproc, n);
		PaUtil_SetInterleavedOutputChannels(&s->bufproc, 0, s->wbuf, s->par.pchan);
		res = PaUtil_CopyOutput(&s->bufproc, &data, n);
		if (res != n) {
			DPR("BlockingWriteStream: copyOutput: %u != %u\n");
			return paUnanticipatedHostError;
		}
		res = sio_write(s->hdl, s->wbuf, n * s->par.pchan * s->par.bps);
		if (res == 0)
			return paUnanticipatedHostError;		
		s->wpos += n;
		numFrames -= n;
	}
	return paNoError;
}

static void
bufdump (struct xmp_context *ctx, int len)
{
	 void *buf;

	 if ((buf = xmp_smix_buffer (ctx)) != NULL)
		 sio_write (hdl, buf, len);
}

static int output_data(char *buf, int32 nbytes)
{
  if (!sio_write(sndio_ctx, buf, nbytes)) {
    ctl->cmsg(CMSG_WARNING, VERB_VERBOSE, "sio_write() failed");
    return -1;
  }
  return 0;
}

static int sndio_write(const char *buf, int cnt)
{
	size_t rc;

	rc = sio_write(hdl, buf, cnt);
	if (rc == 0)
		return -OP_ERROR_INTERNAL;

	return rc;
}

/***************************************************************
【函数功能】： 输入校正坐标数据
【输    入】： iFlag：校正哪个数据；fValue：数据值
【输    出】： 执行结果：0正确；-1错误。
【说    明】： ！经过实验验证！ 角度信息的单位必须是弧度
***************************************************************/
int CGyro::Reset(int iFlag, double fValue)
{
	if(!m_bPortOpened)
		return -1;
	UGyroFrame uFrame = {0};
	uFrame.st.head[0] = '@';
	uFrame.st.head[1] = '*';
	uFrame.st.cmd[0] = -1;
	
	if (iFlag > 7)
	{
		//AfxMessageBox("惯导：复位数据的flag错误！");
		return -1;
	}
	switch (iFlag)
	{
	case GYRO_RST_POSX:
		uFrame.st.posx = fValue;
		break;
	case GYRO_RST_POSY:
		uFrame.st.posy = fValue;
		break;
	case GYRO_RST_ANGLE:
		fValue -= M_PI/2.0f;
		while (fValue > MAX_GYRO_ANGLE)
		{
			fValue -= 2*M_PI;
		}
		while(fValue <= MIN_GYRO_ANGLE)
		{
			fValue += 2*M_PI;
		}
		fValue = fValue * (-1.0f); // 还原成顺正逆负
		if (fValue < 0)
		{
			fValue += 2*M_PI;
		}
		uFrame.st.angle = fValue;
		break;
	default:
		//AfxMessageBox("惯导：复位数据的flag错误！");
		return -1;
	}
	uFrame.st.cmd[1] = (BYTE)iFlag;
	char sum = 0;
	for (int i=0; i<14; i++)
	{
		sum += uFrame.buf[2+i];
	}
	uFrame.st.checksum = (BYTE)(-sum);
	uFrame.st.tail = '*';
	sio_write(m_iPort,uFrame.buf, 18);

	return 0;
}

// internal: handle message
//  return 1 to return to user, 0 to eat, othere to return this error
int _sio_handlemsg(std::string & msg) {
  
  // system handler
  if (msg.substr(0, 10)=="SYS-CPING\t") {
	std::string r2 =  msg.substr(10);
	if (sio_write(SIO_DATA, "SYS-CPONG\t%s", r2.c_str())==-1) {
	  return -1;
	};
	return 0;
  };


  int rv = 1; // return to user

  // message match handler
  int plen = 0;
  if (sio_matchpref(msg.c_str(), "SYS-SET\t", &plen)>0) {
	int evar = msg.find('\t', plen) + 1; // skip appname
	evar = msg.find('\t', evar) + 1; // skip varname
	evar = msg.find('\t', evar) + 1; // skip key
	if (evar<plen) evar=plen;
	std::string pref = msg.substr(plen, evar-plen-1);

	int mcount = 0;
	for (std::map<std::string,SIOVarRecord*>::iterator i=varlist.find(pref); i!=varlist.end(); i++) {
	  //printf("key = [%s], val=[0x%X]\n", i->first.c_str(), i->second);
	  if (i->first == pref) {
		SIOVarRecord * r = i->second;
		if (r) {
		  rv = r->parse_value_set(msg, evar);
		  mcount++;
		};
		//mcount += 100;
	  };
	  //mcount+=1000;
	};
	
	//printf("got sys-get to [%s] from [%s], matched %d\n", pref.c_str(), msg.c_str(), mcount);
  };

  sio_mmatch m;


  // other stuff
  for (std::list<SIOMessageHandler*>::iterator ll=matchlist.begin(); ll != matchlist.end(); ll++) {
	m.pnum = sio_matchpref(msg.c_str(), (*ll)->prefix, &m.plen);
	if (m.pnum > 0) {
	  m.message = msg;
	  int r = ((*ll)->func)(&m, (*ll)->userarg);
	  if (r < rv) rv = r; 
	};
  };

  return rv;
};

/**
 * Asynchronously prints a null-terminated string to the serial console.
 */
void sio_puts( char *buffer ) {
	char *temp = buffer;
	unsigned int len = 0;

	//poor man's strlen
	while( *buffer++ != '\0' ) {
		++len;
	}

	sio_write(temp, len);
}

LOCAL void actually_flush_buffer(void)
   {
   int l,i;

	if (idx)
		{
		total += idx * dsize;
		sio_write(hdl, buffer, dsize * idx);
		}
   idx = 0;
   }

/*
 * play given number of bytes until sio_write() blocks
 */
static int play(struct ao *ao, void **data, int samples, int flags)
{
    struct priv *p = ao->priv;
    int n;

    n = sio_write(p->hdl, data[0], samples * ao->sstride);
    p->delay += n;
    if (flags & AOPLAY_FINAL_CHUNK)
        reset(ao);
    return n / ao->sstride;
}

/*
 * play given number of samples until sio_write() blocks
 */
static int play(struct ao *ao, void **data, int samples, int flags)
{
    struct priv *p = ao->priv;
    int n;

    n = sio_write(p->hdl, data[0], samples * ao->sstride) / ao->sstride;
    p->delay += n;
    p->playing = true;
    /* on AOPLAY_FINAL_CHUNK, just let it underrun */
    return n;
}