#include <ctype.h>
#include "CDC.h"
#include "helpers.h"

#define DELAY30                                 \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"

/* 1 + 2 + 1 + 30 + 30 + 5 = 69 */
#define READ_BIT                                \
    "clc\n\t"                                   \
    "sbic %0, 2\n\t" /* PD2 (RX) */             \
    "sec\n\t"                                   \
    "ror r18\n\t"                               \
    DELAY30                                     \
    DELAY30                                     \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"                                   \
    "nop\n\t"


volatile static uint8_t buf[256+4];

static void __attribute__ ((noinline)) receive_data2(void) {
    int i;
    for (i = 0; i < 256 + 4; i++) {
        buf[i] = 0xf0;
    }
}

static void __attribute__ ((noinline)) receive_data(void) {
    uint8_t sreg;
    
    sreg = SREG;
    cli();
    asm volatile(
        "push r18\n\t"
        "push r24\n\t"
        "push r25\n\t"
        "push r30\n\t"
        "push r31\n\t"
        "ldi r24, lo8(0)\n\t"
        "ldi r25, hi8(0)\n\t"
        
        ".wait:\n\t"
        "sbis %1, 2\n\t" /* PE2 (HWB) */
        "rjmp .button_pressed\n\t"
        "sbis %0, 2\n\t" /* PD2 (RX) */
        "rjmp .start_bit\n\t"
        "rjmp .wait\n\t"
        
        ".start_bit:\n\t"
        /* 1 + 2 + 30 + 4 + 2 + 30 + 30 = 99 (4 less than)*/
        DELAY30
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "sbic %0, 2\n\t" /* PD2 (RX) */
        "rjmp .wait\n\t"
        DELAY30
        DELAY30

        READ_BIT
        READ_BIT
        READ_BIT
        READ_BIT

        READ_BIT
        READ_BIT
        READ_BIT
        READ_BIT

        /* 2 + 30 + 2 = 34 */
        "sbic %0, 2\n\t" /* PD2 (RX) */
        "rjmp .stopok\n\t"
        DELAY30
        "rjmp .wait\n\t"
        
        ".stopok:\n\t"
        "movw r30, r24\n\t"
        "subi r30, lo8(-(buf))\n\t"
        "sbci r31, hi8(-(buf))\n\t"
        "st Z, r18\n\t"
        "subi r24, lo8(-(1))\n\t"
        "rjmp .wait\n\t"
        
        ".button_pressed:\n\t"
        "pop r31\n\t"
        "pop r30\n\t"
        "pop r25\n\t"
        "pop r24\n\t"
        "pop r18\n\t"
        :
        : /* 8-byte integer constant */ "M" (_SFR_IO_ADDR(PIND)),
          /* 8-byte integer constant */ "M" (_SFR_IO_ADDR(PINE))
        : "r18", "r30", "r31"
        );
    SREG = sreg;
}

static void handle_cmd(uint8_t cmd) {
    int i;
    
    put_byte(cmd);
    put_byte('=');
    switch (cmd) {
    case 'b':
        reboot();
        break;
    case 'w':
        receive_data();
        break;
    case 'p':
        put_byte('\r');
        put_byte('\n');
        for (i = 0; i < 256 + 4; i++) {
            print_bits(buf[i]);
            put_byte('\r');
            put_byte('\n');
        }
        break;
    case 'P':
        put_byte('\r');
        put_byte('\n');
        for (i = 0; i < 256; i++) {
            print_byte(buf[i]);
        }
        break;
    case 'z':
        for (i = 0; i < 256; i++) {
            buf[i] = 0xff;
        }
        break;
    case 'h':
        print_bits(((uint16_t)buf) >> 8);
        print_bits(((uint16_t)buf) & 0xff);
        break;
    }
    
    put_byte('\r');
    put_byte('\n');
}

TASK(CDC_Task) {
    if (USB_IsConnected) {
        Endpoint_SelectEndpoint(CDC_RX_EPNUM);
        if (Endpoint_ReadWriteAllowed()) {
            if (Endpoint_BytesInEndpoint()) {
                uint8_t b;
                
                b = Endpoint_Read_Byte();
                Endpoint_ClearCurrentBank();
                handle_cmd(b);
            }
        }
    }
}

void CDC_Task_Init(void) {
    DDRD &= ~_BV(PD2); // PD2 is input
    DDRE &= ~_BV(PE2); // PE2 (HWB) is input
    receive_data2();
}