#include "debug.h"

#if defined(__WATCOMC__)
#pragma aux PUSH_DISABLE =	\
	"pushf",		\
	"cli"
#pragma aux POP_FLAGS =		\
	"popf"
#else
#define PUSH_DISABLE()       	\
        asm {                   \
            pushf;              \
            cli;                \
        }
#define POP_FLAGS()          	\
        asm {                   \
            popf;               \
        }
#endif

#define CR 0x0d
#define LF 0x0a
#define LEADING_ZEROES          0x8000
#define SIGNIFICANT_FIELD       0x0007
char    hextab[]  = "0123456789ABCDEF";

static char debug_text[256U];
static unsigned short debug_index = 0;

static char FAR * DecWordToASCII(char FAR *StrPtr, short wDecVal, short Option)
{
    int fNonZero=0;
    short Digit;
    unsigned short Power=10000;

    while (Power) {
	Digit=0;
	while (wDecVal >= (short)Power) {
	    Digit++;
	    wDecVal-=Power;
	}
	if (Digit) fNonZero=1;
	if (Digit ||
	     fNonZero ||
	     (Option & LEADING_ZEROES) ||
	     ((Power==1) && (fNonZero==0)))
		*StrPtr++=(char)('0'+Digit);
	Power = Power / 10;
    } // end while
    return (StrPtr);
}

static char FAR * DecLongToASCII(char FAR *StrPtr, long lDecVal,short Option)
{
   int  fNonZero=0;
   long Digit;
   unsigned long Power=1000000000;                      // 1 billion

   while (Power) {
      Digit=0;                                                                        // Digit=lDecVal/Power
      while (lDecVal >= (long)Power) {
         Digit++;
         lDecVal-=Power;
      }

      if (Digit) fNonZero=1;

      if (Digit ||
          fNonZero ||
          (Option & LEADING_ZEROES) ||
          ((Power==1) && (fNonZero==0)))
	     *StrPtr++=(char)('0'+Digit);

      Power = Power / 10;
   }
   return (StrPtr);
}

static char FAR * HexWordToASCII(char FAR *StrPtr, unsigned short wHexVal, short Option)
{
   int fNonZero=0;
   unsigned short Digit;
   unsigned short ShiftVal=12;
   unsigned short Power=0xF000;

   while (Power) {
      Digit=(wHexVal & Power)>>ShiftVal;
      if (Digit) fNonZero=1;

      if (Digit ||
          fNonZero ||
          (Option & LEADING_ZEROES) ||
          ((Power==0x0F) && (fNonZero==0)))
         //*StrPtr++=(char)('0'+Digit);
	     *StrPtr++=hextab[Digit];

      Power>>=4;
      ShiftVal-=4;
   } // end while

   return (StrPtr);
}

static char FAR * HexLongToASCII(char FAR *StrPtr, long wHexVal, short Option)
{
   int  fNonZero=0;
   long Digit;
   long ShiftVal=28;
   unsigned long Power=0xF0000000;

   while (Power) {
      Digit=(wHexVal & Power)>>ShiftVal;
      if (Digit) fNonZero=1;

      if (Digit ||
          fNonZero ||
          (Option & LEADING_ZEROES) ||
          ((Power==0x0F) && (fNonZero==0)))
	      *StrPtr++=hextab[Digit];

      Power >>= 4;
      ShiftVal-=4;
   }

   return (StrPtr);
}

#if defined(__WATCOMC__)
static char FAR *disp_start=(char *)0xb0000;
#else
static char FAR *disp_start=(char FAR *)0xb0000000;
#endif
static unsigned short disp_offset=0;

void dcls(void)
{
    int i;

    PUSH_DISABLE();
    for (i=0; i <= 160*24; i += 2) {
	*(disp_start+i) = ' ';
    }
    disp_offset = 0;
    POP_FLAGS();
}

void myputs(char FAR *s)
{
	int i, j;
	char c;

#if 0
	PUSH_DISABLE();
	while (*s) {
	    	if (debug_index < 59000U) {
		    debug_text[debug_index++] = *s;
		}
		if (disp_offset >= 160*24) {
		    for (j=160; j < 160*24; j+=2) {
			*(disp_start+j-160) = *(disp_start+j);
		    }
		    disp_offset = 160*23;
		    for (j=disp_offset; j < (160*24); j+=2) {
			*(disp_start+j) = ' ';
		    }
		}
		if (*s >= 32) {
		    *(disp_start+disp_offset) = *s;
		    disp_offset = disp_offset + 2;
		}
		switch (*s) {
		    case '\n':
			disp_offset = disp_offset + 160;
			if (disp_offset < 160*24) {
			    for (j=disp_offset;
				j < (((disp_offset/160)+1)*160); j+=2) {
				*(disp_start+j) = ' ';
			    }
			}
		    case '\r':
			disp_offset = disp_offset / 160;
			disp_offset = disp_offset * 160;
			break;
		}
		s++;
	}
	POP_FLAGS();
#endif
	while (*s) {
		c = *s++;
		asm mov dl, c
		asm mov ah, 02h
		asm int 21h
		if (c == '\n') {
			asm mov dl, 13
			asm mov ah, 02h
			asm int 21h
		}
	}
}

static char BuildString[1024];

void dprintf(char FAR *DbgStr , ...)
{
   char FAR *BuildPtr=BuildString;
   char FAR *pStr=(char FAR *) DbgStr;
   char FAR *SubStr;

   union {
         void    FAR *VoidPtr;
         short    FAR *WordPtr;
         long   FAR *LongPtr;
         long FAR *StringPtr;
   } Parm;
   short wBuildOption;

   PUSH_DISABLE();

   Parm.VoidPtr=(void FAR *) &DbgStr;
   Parm.StringPtr++;                            // skip size of string pointer

   while (*pStr) {
	 // don't overflow target
	 if (BuildPtr >= (char FAR *) &BuildString[1024-2])
	    break;

	 switch (*pStr) {
	    case '%':
	       wBuildOption=0;
	       pStr++;
	       if (*pStr=='0') {
		  wBuildOption|=LEADING_ZEROES;
		  pStr++;
	       }
	       if (*pStr=='u')                     // always unsigned
		  pStr++;

	       switch(*pStr) {
		  case 'x':
		     BuildPtr=HexWordToASCII(BuildPtr, *Parm.WordPtr++,wBuildOption);
		     pStr++;
		     continue;

		  case 'd':
		     BuildPtr=DecWordToASCII(BuildPtr, *Parm.WordPtr++,wBuildOption);
		     pStr++;
		     continue;
		  case 's':
		     SubStr=(char FAR *)*Parm.StringPtr;
		     if (*SubStr) {
			 while ((*BuildPtr++ = *SubStr++) != 0);
			 BuildPtr--;                      // remove the \0
		     }
		     Parm.StringPtr++;
		     pStr++;
		     continue;
		  case 'l':
		     pStr++;
		     switch (*pStr) {
			 case 'x':
			 BuildPtr=HexLongToASCII(BuildPtr, *Parm.LongPtr++,wBuildOption);
			 pStr++;
			 continue;
     
			 case 'd':
			    BuildPtr=DecLongToASCII(BuildPtr, *Parm.LongPtr++,wBuildOption);
			    pStr++;
			    continue;
		     } // end switch
		     continue;                        // dunno what he wants
	       } // end switch
	       break;
 
	 case '\\':
	    pStr++;
	    switch (*pStr)
	       {
	       case 'n':
	       *BuildPtr++=LF;
	       pStr++;
	       continue;
 
	       case 'r':
	       *BuildPtr++=CR;
	       pStr++;
	       continue;

	       case 0:
	       continue;
	       } // end switch
 
	    break;
	} // end switch
 
	*BuildPtr++=*pStr++;
     } // end while
 
     *BuildPtr=0;                                 // cauterize the string
     myputs(BuildString);         // print to comm port
     POP_FLAGS();
}
