/* tej - changed return type to void as nothing returned */
static void InitMFS(void)
{
    FAR_PTR LOL;
    FAR_PTR SDA;
    unsigned char _osmajor;
    unsigned char _osminor;
    state_t preg;

    LOL = GetListOfLists();
    SDA = GetSDAPointer();

    /* now get the DOS version */
    pre_msdos();
    HI(ax) = 0x30;
    call_msdos();
    _osmajor = LO(ax);
    _osminor = HI(ax);
    post_msdos();

    /* get DOS version into CX */
    preg.ecx = _osmajor | (_osminor <<8);

    preg.edx = FP_OFF16(LOL);
    preg.es = FP_SEG16(LOL);
    preg.esi = FP_OFF16(SDA);
    preg.ds = FP_SEG16(SDA);
    preg.ebx = 0x500;
    mfs_helper(&preg);
}

/** \brief Software SPI send.
 * 
 * Software SPI implementation to communicate with
 * RFM12B module.
 * 
 * \param cmd	uint16_t to send to RFM12B
 *
 */
unsigned int rf12_send_command(unsigned int cmd) 
{
	unsigned char i;
	unsigned int recv;
	recv = 0;
	LO(SCK);
	LO(CS);
	for(i=0; i<16; i++) 
	{
		if(cmd&0x8000) 
		{
			HI(SDI); 
		}
		else
		{ 
			LO(SDI);
		}
		
		HI(SCK);
		recv = recv << 1;
		if( RF_PIN&(1<<SDO) ) 
		{
			recv|=0x0001;
		}
		
		LO(SCK);
		cmd = cmd << 1;
	}
	HI(CS);
	return recv;
}

static int make_period_modifier(struct iforce* iforce,
	struct resource* mod_chunk, int no_alloc,
	__s16 magnitude, __s16 offset, u16 period, u16 phase)
{
	unsigned char data[7];

	period = TIME_SCALE(period);

	if (!no_alloc) {
		mutex_lock(&iforce->mem_mutex);
		if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0c,
			iforce->device_memory.start, iforce->device_memory.end, 2L,
			NULL, NULL)) {
			mutex_unlock(&iforce->mem_mutex);
			return -ENOSPC;
		}
		mutex_unlock(&iforce->mem_mutex);
	}

	data[0] = LO(mod_chunk->start);
	data[1] = HI(mod_chunk->start);

	data[2] = HIFIX80(magnitude);
	data[3] = HIFIX80(offset);
	data[4] = HI(phase);

	data[5] = LO(period);
	data[6] = HI(period);

	iforce_send_packet(iforce, FF_CMD_PERIOD, data);

	return 0;
}

static void
mpdigmul(mpdigit a, mpdigit b, mpdigit *p)
{
	mpdigit x, ah, al, bh, bl, p1, p2, p3, p4;
	int carry;

	/* half digits */
	ah = HI(a);
	al = LO(a);
	bh = HI(b);
	bl = LO(b);

	/* partial products */
	p1 = ah*bl;
	p2 = bh*al;
	p3 = bl*al;
	p4 = ah*bh;

	/* p = ((p1+p2)<<(Dbits/2)) + (p4<<Dbits) + p3 */
	carry = 0;
	x = p1<<(Dbits/2);
	p3 += x;
	if(p3 < x)
		carry++;
	x = p2<<(Dbits/2);
	p3 += x;
	if(p3 < x)
		carry++;
	p4 += carry + HI(p1) + HI(p2);	/* can't carry out of the high digit */
	p[0] = p3;
	p[1] = p4;
}

int stlink2_swim_read_range(programmer_t *pgm, stm8_device_t *device, unsigned char *buffer, unsigned int start, unsigned int length) {
	stlink2_init_session(pgm);

	int i;
	for(i = 0; i < length; i += STLK_READ_BUFFER_SIZE) {
		// Determining current block start & size (relative to 0x8000)
		int block_start = start + i;
		int block_size = length - i;
		if(block_size > STLK_READ_BUFFER_SIZE) {
			block_size = STLK_READ_BUFFER_SIZE;
		}

		// Sending USB packet
		stlink2_cmd(pgm, 0xf40b, 6,
				HI(block_size), LO(block_size),
				0x00, 0x00, 
				HI(block_start), LO(block_start));
		TRY(128, (stlink2_get_status(pgm) & 0xffff) == 0);

		// Seems like we've got some bytes from stlink, downloading them
		stlink2_cmd(pgm, 0xf40c, 0);
		msg_recv(pgm, &(buffer[i]), block_size);
	}

	return(length);
}

void eeprom_init(void)
{ char j, sofs, i;
  char *ptr;
  unsigned short checksum;

  eeprom[0]= 0x00;
  eeprom[1]= 0x00;
  eeprom[2]= LO(dev_descript.idVendor);
  eeprom[3]= HI(dev_descript.idVendor);
  eeprom[4]= LO(dev_descript.idProduct);
  eeprom[5]= HI(dev_descript.idProduct);
  eeprom[6]= LO(dev_descript.bcdDevice);
  eeprom[7]= HI(dev_descript.bcdDevice);
  eeprom[8]= config_descript.bmAttributes;
  eeprom[9] = config_descript.MaxPower;
  eeprom[10] = 0x1C;
  eeprom[11] = 0x00;
  eeprom[12] = LO(dev_descript.bcdUSB);
  eeprom[13] = HI(dev_descript.bcdUSB);
  eeprom[14] = sofs = 0x86+14;
  
  sofs += (eeprom[15] = LO(ManufacturerDscr[0]));
  eeprom[16]= sofs;
  
  sofs += (eeprom[17]=ProductDscr[0]);
  eeprom[18]= sofs;
  
  eeprom[19]= LO(SerialNumDscr[0]);
  i=20;

  for(j=0, ptr = (char*) &(ManufacturerDscr);j<LO(ManufacturerDscr[0]);j++) 
    eeprom[i++]=ptr[j];

  for(j=0, ptr = (char*) &(ProductDscr);j<LO(ProductDscr[0]);j++) 
    eeprom[i++]=ptr[j];

  for(j=0, ptr = (char*) &(SerialNumDscr);j<LO(SerialNumDscr[0]);j++) 
    eeprom[i++]=ptr[j];

  for(j=0, ptr = (char*) & dev_descript.iManufacturer;j<4;j++)
    eeprom[i++]=ptr[j]; 

  while(i<126)
    eeprom[i++]=0;

  checksum = 0xAAAA;

  for(i=0;i<63;i++)
  { checksum^=((word*)eeprom)[i];
   
    if(checksum&0x8000)
      checksum=(checksum<<1)|1;
    else 
      checksum=(checksum<<1);
   }
    
   ((word*)eeprom)[63]=checksum;
 }

int stlink2_write_word(programmer_t *pgm, unsigned int word, unsigned int start) {
	unsigned char buf[4], start2[2];
	pack_int16(start, start2);
	stlink2_cmd(pgm, 0xf40a, 8,
			0x00, 0x02,
			0x00, 0x00,
			HI(start), LO(start),
			HI(word), LO(word));
	usleep(2000);
	return(stlink2_get_status(pgm)); // Should be '1'
}

/** \brief Setup the AVR ports used by the Module
 *
 * Set up the correct data direction and state for the
 * ports used by the RFM12B as specifed in rfm12b.h
 */
void rf12_portSetup()
{
	RF_PORT = 0x0;
	HI(CS);
	HI(SDI);
	LO(SCK);
	RF_DDR = (1<<CS) | (1<<SDI) | (1<<SCK);
	HI(NIRQ);
	HI(CS);
	LO(SDI);
	HI(SDO);
}

INT16U rtu_getAnalogInput(INT8U addr,INT8U *buf,INT16U start_address,INT16U lenth)
{
unsigned char tmp[256],tmp_lenth;
    tmp[0] = addr;
    tmp[1] = READ_AI;
    tmp[2] = HI(start_address);
    tmp[3] = LOW(start_address);
    tmp[4] = HI(lenth);
    tmp[5] = LOW(lenth);
    tmp_lenth = 6;
    construct_rtu_frm ( buf,tmp,tmp_lenth);
    return 8;
}

void usart_hex(unsigned char c){
	#define HI(b)	((b>>4) & 0xf)
	#define LO(b)	(b & 0xf)
	#define HE(b)	(((b & 0x7)-1) | 0x40)
	
	if (HI(c)>9)
		usart_putchar(HE(HI(c)));
	else
		usart_putchar(0x30 | HI(c));
	if (LO(c)>9)
		usart_putchar(HE(LO(c)));
	else
		usart_putchar(0x30 | LO(c));
}

INT16U rtu_setHoldingRegister(INT8U addr,INT8U *buf, INT16U start_address,INT16U value )
{
    unsigned char tmp[256], tmp_lenth;
    
    tmp[0] = addr;
    tmp[1] = SET_HLD_REG;
    tmp[2] = HI(start_address);
    tmp[3] = LOW(start_address);
    tmp[4] = HI(value);
    tmp[5] = LOW(value);    
    tmp_lenth = 6;
    construct_rtu_frm ( buf, tmp, tmp_lenth);
    return 8 ;
}

u08 i2c_eep_WriteByte(u08 SAddr,u16 Addr, u08 Byte, IIC_F WhatDo)
{

if (i2c_Do & i2c_Busy) return 0;

i2c_index = 0;
i2c_ByteCount = 3;

i2c_SlaveAddress = SAddr;


i2c_Buffer[0] = HI(Addr);
i2c_Buffer[1] = LO(Addr);
i2c_Buffer[2] = Byte;

i2c_Do = i2c_sawp;

MasterOutFunc = WhatDo;
ErrorOutFunc = WhatDo;

TWCR = 1<<TWSTA|0<<TWSTO|1<<TWINT|0<<TWEA|1<<TWEN|1<<TWIE;

i2c_Do |= i2c_Busy;

return 1;
}

u08 i2c_eep_ReadByte(u08 SAddr, u16 Addr, u08 ByteNumber, IIC_F WhatDo)
{
if (i2c_Do & i2c_Busy) return 0;

i2c_index = 0;
i2c_ByteCount = ByteNumber;

i2c_SlaveAddress = SAddr;

i2c_PageAddress[0] = HI(Addr);
i2c_PageAddress[1] = LO(Addr);

i2c_PageAddrIndex = 0;
i2c_PageAddrCount = 2;

i2c_Do = i2c_sawsarp;

MasterOutFunc = WhatDo;
ErrorOutFunc = WhatDo;

TWCR = 1<<TWSTA|0<<TWSTO|1<<TWINT|0<<TWEA|1<<TWEN|1<<TWIE;

i2c_Do |= i2c_Busy;

return 1;
}

void xms_control(void)
{
  int is_umb_fn = 0;

 /* First do the UMB functions */
 switch (HI(ax)) {
  case XMS_ALLOCATE_UMB:
    {
      int size = LWORD(edx) << 4;
      unsigned int addr = umb_allocate(size);
      is_umb_fn = 1;
      Debug0((dbg_fd, "Allocate UMB memory: %#x\n", size));
      if (addr == 0) {
        int avail=umb_query();

	Debug0((dbg_fd, "Allocate UMB Failure\n"));
	XMS_RET(avail ? 0xb0 : 0xb1);
	LWORD(edx) = avail >> 4;
      }
      else {
	Debug0((dbg_fd, "Allocate UMB Success\n"));
	LWORD(eax) = 1;
	LWORD(ebx) = addr >> 4;
	LWORD(edx) = size >> 4;
      }
      Debug0((dbg_fd, "umb_allocated: %#x0:%#x0\n",
	      (unsigned) LWORD(ebx),
	      (unsigned) LWORD(edx)));
      /* retval = UNCHANGED; */
      break;
    }

/**
 * @brief BTRecordFile::readRecordFromDiskTest
 * @param pDisk
 * @param pRecordID
 * Hace la lectura de un registro en la RAM
 */
void BTRecordFile::readRecordFromDiskTest(Disk pDisk, unsigned short pRecordID)
{
    const char *padre = pDisk.read(this->_metadataPtr->getFirstRecordPos(), 7);
    const char *hizq = pDisk.read(this->_metadataPtr->getFirstRecordPos() + 8, 7);
    const char *hder = pDisk.read(this->_metadataPtr->getFirstRecordPos() + 16, 7);
    std::string father(padre);          // obtiene el padre
    std::string HI(hizq);               // obtiene el hijo izq
    std::string HD(hder);               // obtiene el hijo der
    unsigned short _sizeCounter = this->_metadataPtr->getFirstRecordPos() + 24;       // inicio de la data
    DLL<IRecordDataType*> *tmp1 = _metadataPtr->getRecordStruct();
    DLLNode<IRecordDataType*> *tmp = tmp1->getHeadPtr();
    const char *data;

    cout << "Binario " << father << " " << HI << " " << HD << endl;
    std::string P = _conversion->binaryToDecimal(father);
    std::string PHI = _conversion->binaryToDecimal(HI);
    std::string PHD = _conversion->binaryToDecimal(HD);
    cout << P << " " << PHI << " " << PHD << " ";
    while (tmp != nullptr) {
        data = (dynamic_cast<RecordDataType<char>*>(tmp->getData()))->getDataPtr();
        const char *DATO = pDisk.read(_sizeCounter, 7);
        std::string DATOSTR(DATO);       // obtiene el padre
        _sizeCounter += 8;
        cout << sortUserDataFromDisk(DATOSTR, *data) << endl;
        tmp = tmp->getNextPtr();
    }
}

//-----------------------------------------------------------------------------
void SetSingleRegister(void)//установить значение одного регистра
{
	unsigned char i=0;
	unsigned int CRC=0x0;
	unsigned int addr;//адрес регистра
	unsigned int num;//значение регистра

	//-----------установка нужного регистра------------
	addr=(((unsigned int)RecieveBuf[2])|((unsigned int)RecieveBuf[3]<<8));
	num=(((unsigned int)RecieveBuf[4]<<8)|((unsigned int)RecieveBuf[5]));

	if(addr>=REG_ADDR_MIN && addr<=REG_ADDR_MAX)
	{
			controller_reg[addr]=num;

			for (i=0;i<(buf_count-CRC_LEN);i++)
			{
				TransferBuf[i]=RecieveBuf[i];		
			}
			buf_len=buf_count;

			CRC=CRC16(&TransferBuf,buf_len-CRC_LEN);

			TransferBuf[buf_len-2]=HI(CRC);
			TransferBuf[buf_len-1]=LO(CRC);	
	}
	else
	{
		//обработка ошибки
		MD_STATE=MD_ERR_HANDLING;
	}
	//-------------------------------------------------	
}

void SQuIDS::Derive(double at){
  t=at;
  PreDerive(at);
  for(unsigned int ei = 0; ei < nx; ei++){
    // Density matrix
    for(unsigned int i = 0; i < nrhos; i++){
      // Coherent interaction
      if(CoherentRhoTerms)
        dstate[ei].rho[i] = iCommutator(estate[ei].rho[i],HI(ei,i,t));
      else
        dstate[ei].rho[i].SetAllComponents(0.);

      // Non coherent interaction
      if(NonCoherentRhoTerms)
        dstate[ei].rho[i] -= ACommutator(GammaRho(ei,i,t),estate[ei].rho[i]);
      // Other possible interaction, for example involving the Scalars or non linear terms in rho.
      if(OtherRhoTerms)
        dstate[ei].rho[i] += InteractionsRho(ei,i,t);
    }
    //Scalars
    for(unsigned int is=0;is<nscalars;is++){
      dstate[ei].scalar[is]=0.;
      if(GammaScalarTerms)
        dstate[ei].scalar[is] += -estate[ei].scalar[is]*GammaScalar(ei,is,t);
      if(OtherScalarTerms)
        dstate[ei].scalar[is] += InteractionsScalar(ei,is,t);
    }
  }
}

static int make_magnitude_modifier(struct iforce* iforce,
	struct resource* mod_chunk, int no_alloc, __s16 level)
{
	unsigned char data[3];

	if (!no_alloc) {
		mutex_lock(&iforce->mem_mutex);
		if (allocate_resource(&(iforce->device_memory), mod_chunk, 2,
			iforce->device_memory.start, iforce->device_memory.end, 2L,
			NULL, NULL)) {
			mutex_unlock(&iforce->mem_mutex);
			return -ENOSPC;
		}
		mutex_unlock(&iforce->mem_mutex);
	}

	data[0] = LO(mod_chunk->start);
	data[1] = HI(mod_chunk->start);
	data[2] = HIFIX80(level);

	iforce_send_packet(iforce, FF_CMD_MAGNITUDE, data);

	iforce_dump_packet("magnitude: ", FF_CMD_MAGNITUDE, data);
	return 0;
}

inline void InitAll(void)
{

//InitUSART
UBRRL = LO(bauddivider);
UBRRH = HI(bauddivider);
UCSRA = 0;
UCSRB = 1<<RXEN|1<<TXEN|0<<RXCIE|0<<TXCIE|0<<UDRIE;
UCSRC = 1<<URSEL|1<<UCSZ0|1<<UCSZ1;

//InitPort
PORTA = 0x00;
DDRA = 0xFF;


//Init ADC


//Init PWM


//Init Interrupts


//Init Timers

}

int dosc_interface(void)
{
    switch (HI(ax)) {
    case DOSC_NOTIFY: {  /* install check and notify */
        running_DosC = LWORD(ebx);
        LWORD(eax) = (Bit16u)DOSEMU_VERSION_CODE;
        LWORD(edx) = DOSEMU_VERSION_CODE >> 16;
        c_printf("Booted DosC kernel build %d\n", running_DosC);
        break;
    }
    case DOSC_RUNNING: {
        LWORD(eax) = 0;
        LWORD(ebx) = running_DosC;
        break;
    }
    case DOSC_GENERIC: {
        Bit16u *ssp = SEG_ADR((Bit16u *), ss, sp);
        int mode = ssp[1];
        switch (mode) {
        case E6_INT2f11:
            return pass_to_int2f(ssp);
        }
    }
    }
    return 1;
}