/**
 * \par Function
 *    setPID
 * \par Description
 *    Set PID for Motor.
 * \param[in]
 *    p - P means Proportion.
 * \param[in]
 *    i - I means Integration.
 * \param[in]
 *    d - D means Differentiation.
 * \param[in]
 *    s - S means slot of Motor.
 * \par Output
 *    None
 * \par Return
 *    None
 * \par Others
 *    None
 */
void MeEncoderNew::setPID(float p,float i,float d,float s)
{
  cmdBuf[0] = _slot;
  cmdBuf[1] = CMD_SET_PID;
  memcpy(&cmdBuf[2],&p,4);
  memcpy(&cmdBuf[6],&i,4);
  memcpy(&cmdBuf[10],&d,4);
  memcpy(&cmdBuf[14],&s,4);
  sendCmd();
}

bool EasyVR::eraseCommand(int8_t group, int8_t index)
{
  sendCmd(CMD_ERASE_SD);
  sendGroup(group);
  sendArg(index);

  if (recv(DEF_TIMEOUT) == STS_SUCCESS)
    return true;
  return false;
}

/*! Start MUX diagnostic.
 *  \param param address Address of LTC6804-2 IC.
 */
void ltc6804_startMuxDiag(uint8_t address)
{
    // send dummy bit to wakeup '6820 and '6804,
    dummyIo(1);
    /* Note: We assume here that the operations before next SPI io will
     * take at least 2 * t_ready and therefore we don't explicitly
     * insert a delay here.
     */
    sendCmd(address, CMD_DIAGN);
}

void EasyVR::detectToken(int8_t bits, int8_t rejection, uint16_t timeout)
{
  sendCmd(CMD_RECV_SN);
  sendArg(bits);
  sendArg(rejection);
  if (timeout > 0)
    timeout = (timeout * 2 + 53)/ 55; // approx / 27.46 - err < 0.15%
  sendArg((timeout >> 5) & 0x1F);
  sendArg(timeout & 0x1F);
}

/*! Start ADC conversion of cell voltage and GPIO1, GPIO2
 *  \param address Address of LTC6804-2 IC.
 *  \param mode ADC mode (conversion speed).
 */
void ltc6804_startCellGpioVoltageConversion(uint8_t address,
        enum ltc6804_ADC_modes_e mode)
{
    // send dummy bit to wakeup '6820 and '6804,
    dummyIo(1);
    /* Note: We assume here that the operations before next SPI io will
     * take at least 2 * t_ready and therefore we don't explicitly
     * insert a delay here.
     */
    sendCmd(address, CMD_ADCVAX | mode);
}

long long int hokuyo::Laser::getHokuyoClockOffset(int reps, int timeout)
{
    std::vector<long long int> offset(reps);

    sendCmd("TM0",timeout);
    for (int i = 0; i < reps; i++)
    {
        long long int prestamp = timeHelper();
        sendCmd("TM1",timeout);
        long long int hokuyostamp = readTime();
        long long int poststamp = timeHelper();
        offset[i] = hokuyostamp - (prestamp + poststamp) / 2;
        //printf("%lli %lli %lli", hokuyostamp, prestamp, poststamp);
    }
    sendCmd("TM2",timeout);

    long long out = median(offset);

    return out;
}

void LinTVPlay::setVolume(int volume)
{
    if (volume < 0 || volume > 100) {
        return;
    }
    if (m_deviceType == LinTVDeviceInfo::File || m_deviceType == LinTVDeviceInfo::PVR) {
        sendCmd(QString("volume %1 1").arg(volume));
    } else {
        m_mixer->setVolume(volume);
    }
}

bool QtServiceBasePrivate::start()
{
    if (sendCmd(controller.serviceName(), "alive")) {
        // Already running
        return false;
    }
    // Could just call controller.start() here, but that would fail if
    // we're not installed. We do not want to strictly require installation.
    ::setenv("QTSERVICE_RUN", "1", 1);  // Tell the detached process it's it
    return QProcess::startDetached(filePath(), args.mid(1), "/");
}

bool cs5463spi::VChannDCGainCal(){
	cout << "ufff" << endl;
	bool bSuccess = false;
	int result;
	result = sendCmd(m_sysCalGain.V_CHAN_AC_GAIN);  // Inits V channel DC Gain Calibration
//TODO - check for gain registered being 1; use it in the if statement.
	if(result >= 0)
		bSuccess = true;

	return bSuccess;
}

void LTCPClientTask::checkConn()
{
	LRTime currentTime;
	if (_ten_min(currentTime))
	{
#ifdef _NULCMD
		Cmd::t_NullCmd tNullCmd;		
		sendCmd(&tNullCmd, sizeof(tNullCmd));
#endif
	}
}

bool EspDrv::ping(const char *host)
{
	LOGDEBUG(F("> ping"));

	int ret = sendCmd(F("AT+PING=\"%s\""), 8000, host);
	
	if (ret==TAG_OK)
		return true;

	return false;
}

void EspDrv::reset()
{
	LOGDEBUG(F("> reset"));

	sendCmd(F("AT+RST"));
	delay(3000);
	espEmptyBuf(false);  // empty dirty characters from the buffer

	// disable echo of commands
	sendCmd(F("ATE0"));

	// set station mode
	sendCmd(F("AT+CWMODE=1"));

	// set multiple connections mode
	sendCmd(F("AT+CIPMUX=1"));

	// enable DHCP
	sendCmd(F("AT+CWDHCP=1,1"));
}

/**
 *
 * IChannACGainCal() .. returns true on a successful gain setting to 1 and successful
 * calibration send.
 */
bool cs5463spi::IChannACGainCal() {
	bool bSuccess = false;
	int registerWrite, result;
	registerWrite = WriteRegister(m_cs5463NumList[i_gain].addr, 0x40, 0x00, 0x00);  //sets Gain to 1;
	result = sendCmd(m_sysCalGain.V_CHAN_AC_GAIN);  // Inits I channel AC Gain Calibration

	if(registerWrite >= 0 && result >= 0)
	  bSuccess = true;

	return bSuccess;
}

void hokuyo::Laser::reset ()
{
	if (!portOpen())
	    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
        laserFlush();
        try
        {
          sendCmd("TM2", 1000);
        }
        catch (hokuyo::Exception &e)
        {} // Ignore. If the laser was scanning TM2 would fail
        try
        {
          sendCmd("RS", 1000);
        }
        catch (hokuyo::Exception &e)
        {} // Ignore. If the command coincided with a scan we might get garbage.
        laserFlush();
        sendCmd("RS", 1000); // This one should just work.
}

void Thing1Class::setLed(LedCmd ledcmd)
{
	switch (ledcmd)
	{
	case GreenOn:
		sendCmd("led 1 1");
		break;
	case GreenOff:
		sendCmd("led 1 0");
		break;
	case RedOn:
		sendCmd("led 2 1");
		break;
	case RedOff:
		sendCmd("led 2 0");
		break;
	default:
		break;
	}
	waitFor(CRLF);
}

	void addToContainer ()
	{
		for (unsigned int index = 0; index < cmds.size();++index)
		{
			sendCmd(cmds[index].c_str(),cmds[index].size());
		}
		cmds.clear();
		if (GetEvents()->doReconnect)
		{
			(*(GetEvents()->doReconnect))(this);
		}
	}

NyLPC_TBool NyLPC_cSnicApi_configure(NyLPC_TcSnicApi_t* i_inst,const struct NyLPC_TIPv4Addr* i_addr,const struct NyLPC_TIPv4Addr* i_netmask,const struct NyLPC_TIPv4Addr* i_gateway)
{
	NyLPC_TUInt8 data[2+2+4*3];
	i_inst->last_cmd=UART_CMD_SID_SNIC_IP_CONFIG_REQ;
	i_inst->last_seq=NyLPC_cSnicNetIf_regieterObject(i_inst);
	data[0]=i_inst->last_cmd;
	data[1]=i_inst->last_seq;
	data[2]=0;//STA
	if(i_addr==NULL){
		data[3]=1;
		sendCmd(i_inst,data,4);
	}else{
		data[3]=0;
		memcpy(&data[4],&(i_addr->v),4);
		memcpy(&data[8],&(i_netmask->v),4);
		memcpy(&data[12],&(i_gateway->v),4);
		sendCmd(i_inst,data,16);
	}
	return i_inst->last_status==UART_CMD_RES_SNIC_SUCCESS;

}

bool EspDrv::startClient(const char* host, uint16_t port, uint8_t sock, uint8_t protMode)
{
	LOGDEBUG2(F("> startClient"), host, port);
	
	// TCP
	// AT+CIPSTART=<link ID>,"TCP",<remote IP>,<remote port>

	// UDP
	// AT+CIPSTART=<link ID>,"UDP",<remote IP>,<remote port>[,<UDP local port>,<UDP mode>]

	// for UDP we set a dummy remote port and UDP mode to 2
	// this allows to specify the target host/port in CIPSEND

	int ret;
	if (protMode==TCP_MODE)
		ret = sendCmd(F("AT+CIPSTART=%d,\"TCP\",\"%s\",%u"), 5000, sock, host, port);
	else
		ret = sendCmd(F("AT+CIPSTART=%d,\"UDP\",\"%s\",0,%u,2"), 5000, sock, host, port);

	return ret==TAG_OK;
}

static uint8_t readId(uint8_t which){
	datOut;
	PORTB&=~(1<<4);//set OE# to low
	PORTB|=(1<<3)|(1<<5);//set CE# and WE# to high
	PORTB|=(1<<4);//set OE# to high
	sendCmd(0x5555,0xAA);
	sendCmd(0x2AAA,0x55);
	sendCmd(0x5555,0x90);
	/* From http://ww1.microchip.com/downloads/en/DeviceDoc/25022B.pdf
	 * A command is written by asserting WE# low while keeping CE# low.
	 * The address bus is latched on the falling edge of WE# or CE#, whichever  occurs last.
	 * The data bus is latched on the rising edge of WE# or CE#, whichever occurs first*/
	NOP;
	NOP;
	NOP;
	PORTB&=~((1<<4)|(1<<3));//set OE# and CE# to low
	datIn;
	shift24(which);
	uint8_t idR=rdDat();
	PORTB|=(1<<3);//set CE# to HIGH
	//exit
	datOut;
	PORTB|=(1<<4);//set OE# to high
	sendCmd(0x5555,0xAA);
	sendCmd(0x2AAA,0x55);
	sendCmd(0x5555,0xF0);
	NOP;
	NOP;
	NOP;
	PORTB&=~(1<<3);//set CE# to low
	return idR;
}