#include <cpu_prog_model.h>
#include <stdio.h>

#define FALSE  0
#define TRUE   1


typedef struct
{
   short  iob_type,
          slot,
          drive,
          volume,
          track,
          sector,
          dvc    [2],
          buffer [2],
          dummy  [2],
          command,
          error,
          act_volume,
          prev_slot,
          prev_drive;
}  iob;

hex_dump( array)
unsigned short array [];
{
   int  loop_1,
        loop_2;

   for (loop_1 = 0;loop_1 < 16;loop_1 ++)
   {
      printf("$%04x- ", loop_1 * 16);
      for (loop_2 = 0;loop_2 < 16;loop_2 ++)
         printf("%02x ", array [loop_1 * 16 + loop_2]);

      printf("  '");
      for (loop_2 = 0;loop_2 < 16;loop_2 ++)
      {
         char tkn;

         tkn = array [loop_1 * 16 + loop_2];
         if ( ((tkn & 0x7F) < 0x20) || ((tkn & 0x7f) == 0x7f) )
            tkn = '.';

         printf("%c", tkn);
      }
      printf("'\n");
   }
   printf("\n");
}

show_rwts_info( rwts_info)
iob *rwts_info;
{

   printf("IOB Type  %02x\n",  rwts_info->iob_type);
   printf("Slot      %02x\n",  rwts_info->slot);
   printf("Drive      %x\n",   rwts_info->drive);
   printf("Volume    %02x\n",  rwts_info->volume);
   printf("Track     %02d\n",  rwts_info->track);
   printf("Sector     %x\n",   rwts_info->sector);
   printf("DVC     %04x \n",  (rwts_info->dvc [1] << 8) | rwts_info->dvc [1]);
   printf("Buffer  %04x \n",  (rwts_info->buffer [1] << 8) |
                               rwts_info->buffer [0]         );
   printf("Command    %x\n",   rwts_info->command);
}

short rwts( code)
PM *code;
{
   iob             * rwts_info;
   char              temp_buffer [0x104];
   short             loop;
   unsigned short  * buffer;

   rwts_info = (iob *) &(code->Me [(code->Ac << 8) | code->Yr]);
   if (code->Tr)
      show_rwts_info( rwts_info);


/*
code->Mo = Mode_SLOW;
code->Qu = TRUE;
return;
*/

   rwts_info->error      = 0x00;
   rwts_info->act_volume = rwts_info->volume;
   rwts_info->prev_slot  = rwts_info->slot;
   rwts_info->prev_drive = rwts_info->drive;

   buffer = (unsigned short *) &(code->Me [(rwts_info->buffer [1] << 8) |
                                            rwts_info->buffer [0]         ]);

   switch (rwts_info->command)
   {
      case 0x00:
printf("Open drive and move the head\n");
         break;

      case 0x01:
         if (disk_open( _READ, rwts_info->slot, rwts_info->drive,
                                                rwts_info->volume) )
         {
            printf("Can't open the disk\n");
            code->Mo = Mode_SLOW;
            code->Qu = TRUE;
            return;
         }

         disk_seek( rwts_info->track, rwts_info->sector, 0x00);
         disk_read( temp_buffer, 0x104);
         disk_close();
         rwts_info->error = 0x00;

         for (loop = 0;loop <= 0xff;loop ++)
            buffer [loop] = temp_buffer [4 + loop];

         if (code->Tr)
         {
            printf("Read from\n");
            printf("Track $%02x  Sector $%02x\n", temp_buffer [1],
                                                  temp_buffer [2]);
            hex_dump( buffer);
         }
         break;

      case 0x02:
         if (disk_open( _MODIFY, rwts_info->slot, rwts_info->drive,
                                                  rwts_info->volume) )
         {
            printf("Can't open the disk\n");
            code->Mo = Mode_SLOW;
            code->Qu = TRUE;
            return;
         }

         for (loop = 0;loop <= 0xff;loop ++)
            temp_buffer [4 + loop] = buffer [loop];

         temp_buffer [0] = rwts_info->volume;
         temp_buffer [1] = rwts_info->track;
         temp_buffer [2] = rwts_info->sector;
         temp_buffer [3] = 0x55;  /* Dummy */

         if (code->Tr)
         {
            printf("Write on\n");
            printf("Track $%02x  Sector $%02x\n", temp_buffer [1],
                                                  temp_buffer [2]);
            hex_dump( buffer);
         }

         disk_seek( rwts_info->track, rwts_info->sector, 0x00);
         disk_write( temp_buffer, 0x104);
         disk_close();

         rwts_info->error = 0x00;
         break;

      case 0x04:
         if (rwts_info->drive == 0x02)
         {
            format_ext_drive( rwts_info->slot);
            break;
         }

         if (disk_open( _WRITE, rwts_info->slot, rwts_info->drive,
                                                 rwts_info->volume) )
         {
            printf("Can't open the disk\n");
            code->Mo = Mode_SLOW;
            code->Qu = TRUE;
            return;
         }

         for (rwts_info->track = 0;rwts_info->track < 35;rwts_info->track ++)
         {
            for (rwts_info->sector = 0;rwts_info->sector < 16;
                                                        rwts_info->sector ++)
            {
               temp_buffer [0] = rwts_info->volume;
               temp_buffer [1] = rwts_info->track;
               temp_buffer [2] = rwts_info->sector;
               temp_buffer [3] = 0x55;  /* Dummy */

               for (loop = 0;loop <= 0xff;loop ++)
                  temp_buffer [4 + loop] = 0x00;

               disk_seek( rwts_info->track, rwts_info->sector, 0x00);
               disk_write( temp_buffer, 0x104);
            }
         }

         if (code->Tr)
         {
            printf("Format_disk on\n");
            printf("Track $%02x  Sector $%02x\n", temp_buffer [1],
                                                  temp_buffer [2]);
            hex_dump( buffer);
         }

         disk_close();
         rwts_info->error = 0x00;

         if (code->Br = FALSE)
         {
            printf("Break is false\n");
            code->Mo = Mode_SLOW;
            code->Er = TRUE;
            code->Qu = TRUE;
            return;
         }
         break;
   }

   code->Ca = FALSE;   /* Done wery well */
   RTS( code);
}