#include "cambadge.h"
#include "globals.h"

// C5
#define CLK PMADDR & 8 ? 1 : 0
#define CLK_LOW PMADDR = PMADDR&~8
#define CLK_HIGH PMADDR = PMADDR | 8
// B4
#define OUT LATBbits.LATB4 ? 1 : 0
#define OUT_LOW LATBbits.LATB4 = 0
#define OUT_HIGH LATBbits.LATB4 = 1
// A2
#define IN PORTAbits.RA2 ? 1 : 0

#define DELAY_US 120

#define PACKET_SIZE 640u
#define GBP_MEM_SIZE 7680u

unsigned int GBSendByte(unsigned int b);

unsigned long GBSendPacket(unsigned int command, unsigned long size);

// Game Boy Printer Commands
#define GBC_INITIALIZE 0x01
#define GBC_PRINT 0x02
#define GBC_TRANSFER 0x04
#define GBC_REPORT 0x0F

char* gbprint(unsigned int action)
{
  // MAGIC CMD RLE SZLO SZHI DATA(640) CHKLO CHKHI STATUS STATUS
  // 88 33 01  00  00   00             01    00    00     00
    
  unsigned int init[] = {0x88, 0x33, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00};
  
  switch(action) {
     case act_name : return("PRINT_BOY");
     case act_help : return("Digital to paper yo");  
     case act_init : return(0); // run once at powerup
     case act_powerdown : return(0); // run before powerdown         
     case act_start : return(0); // called when app is selected
  }
 
 if (action!=act_poll) return(0);
  
 if(!tick) return(0);
  
 size_t init_length = sizeof(init)/sizeof(init[0]);
 unsigned int reply[] = {0x00, 0x00};
 unsigned int i = 0;
 while(i < init_length) {
     unsigned int byte = init[i];
     printf(cls whi top "Length %d / %d\n", i, init_length);
     printf(red " %#02x\n", byte);
     unsigned int j = 7;
     
     while(j>0) {
        CLK_LOW;
        delayus(DELAY_US/4);
        if (byte & (1<j)) {
            OUT_HIGH;
        } else {
            OUT_LOW;
        }
        delayus(DELAY_US/4);
        CLK_HIGH;
        delayus(DELAY_US/2);
        if (i == init_length-2) {
            reply[0] = reply[0] | (IN << j);
        } else if (i == init_length-1) {
            reply[1] = reply[1] | (IN << j);
        }
        j--;
     }
 }
 
 while (!(butpress & powerbut)) {
    printf(bot grn "Reply: %#02x %#02x", reply[0], reply[1]);
 }
 
 printf(grn " exit!");

 return("");
 
}