#include "rfcomm.h"
#include "softuart.h"

#include <stdio.h>

#include <util/delay.h>


// Raw byte to nibble lookup table
// Meghan Desai (mpd25)
char rtn_lut[16] = {
  0b10001011, // 0 -> 8B
  0b10001101, // 1 -> 8D
  0b10010011, // 2 -> 93
  0b10010101, // 3 -> 95
  0b10010110, // 4 -> 96
  0b10011001, // 5 -> 99
  0b10011010, // 6 -> 9A
  0b10011100, // 7 -> 9C
  0b10100011, // 8 -> A3
  0b10100101, // 9 -> A5
  0b10100110, // A -> A6
  0b10101001, // B -> A9
  0b10101100, // C -> AC
  0b10110001, // D -> B1
  0b10110010, // E -> B2
  0b10110100  // F -> B4
};

// RF Payload Size + Pre-amble + Stop-byte
char rf_buffer[RF_FRAME_SIZE];
char rf_payload[RF_PAYLOAD_SIZE];
int rf_buffposition;
char rf_state;

void rf_init() {
  rf_state = RF_IDLE_S;
  rf_buffposition = 0;
  sw_uart_init();
}

/****
 * Receive message from RF circuit
 ****/
char rf_receive_msg() {
  // Check to see if RF channel is free
  if (rf_state != RF_IDLE_S) return 1;
  // Else, set RF up for receiving
  // Reset buff position
  rf_buffposition = 0;
  // Set state to receive
  rf_state = RF_RX_S;
  // Return OK
  sw_uart_receive_byte();
  return 0;
}

// Decode Payload
char i,j;
char rawCode1,rawCode2;
void rf_decode_pl() {
  for (j = 0; j < RF_PAYLOAD_SIZE;j++) {
    // Lower nibble raw
    rawCode1 = rf_buffer[RF_PREAMBLE_SIZE+(j<<1)];
    // Upper nibble raw
    rawCode2 = rf_buffer[RF_PREAMBLE_SIZE+(j<<1)+1];
    rf_payload[(int)j] = 0x00;
    for (i=0; i < 16; i++) {
      if (rtn_lut[(int)i] == rawCode1) rf_payload[(int)j] |= i;
      if (rtn_lut[(int)i] == rawCode2) rf_payload[(int)j] |= (i<<4);
    }
  }
}

/****
 * Check frame integrity on received frames
 ****/
char rf_check_error() {
  // Check sync bytes and stop byte
  return rf_buffposition == RF_FRAME_SIZE &&
    rf_buffer[0] == RF_SYNC_MARKER &&
    rf_buffer[1] == RF_START_MARKER &&
    rf_buffer[RF_FRAME_SIZE-1] == RF_STOP_MARKER;
}

char rf_send_msg(char buffer[]) {
  // If Busy, error
  if (rf_state != RF_IDLE_S) return 1;
  // If not busy, encapsulate into frame
  // Adjust receiver gain
  rf_buffer[0] = 0x00;
  // Add preamble
  for(i = 1; i <= RF_SYNC_SIZE; i++) {
    rf_buffer[(int)i] = RF_SYNC_MARKER;
  }
  // Add start byte
  for(i = RF_SYNC_SIZE+1; i <= RF_PREAMBLE_SIZE; i++) {
    rf_buffer[(int)i] = RF_START_MARKER; 
  }
  // Add payload
  for (i=0; i < RF_PAYLOAD_SIZE; i++) {
    // Lower nibble
    rf_buffer[(int)((i<<1)+(1+RF_PREAMBLE_SIZE))] = rtn_lut[(int)(buffer[(int)i]&0x0F)];
    // Upper nibble
    rf_buffer[(int)((i<<1)+1+(1+RF_PREAMBLE_SIZE))] = rtn_lut[(int)((buffer[(int)i]&0xF0)>>4)];
  }
  // Add Stop byte
  for (i=1; i <=RF_STOP_SIZE; i++) {
    rf_buffer[RF_FRAME_SIZE-i] = RF_SYNC_MARKER;
  }
  // Reset buffer position for transmission
  rf_buffposition = 0;
  // Set state to transmite
  rf_state = RF_TX_S;
  return 0;
}

// Poll sw_uart done transmitting/receiving

char rf_process_byte() {
  if (sw_uart_state == SWUART_IDLE_S) {
    switch(rf_state) {
    case RF_RX_S:
      if (sw_uart_rxdata == 0xAA && rf_buffposition <= RF_PREAMBLE_SIZE-2) {
        rf_buffposition = 0; // Hold position at beginning
      } else if (sw_uart_rxdata == 0xCA &&
                 rf_buffer[0] == 0xAA &&
                 rf_buffposition == 1) {
        rf_buffposition = RF_PREAMBLE_SIZE-1; // Next is data
      } else if (rf_buffposition < RF_PREAMBLE_SIZE) {
        rf_buffposition = 0; // Bad frame
        rf_buffer[0] = 0x00;
      }
      rf_buffer[rf_buffposition] = sw_uart_rxdata;
      rf_buffposition++;
      if (rf_buffposition == RF_FRAME_SIZE) {
	rf_state = RF_IDLE_S;
      } else {
	sw_uart_receive_byte();
      }
      break;
    case RF_TX_S:
      if (rf_buffposition == RF_FRAME_SIZE && sw_uart_state == SWUART_IDLE_S) {
	rf_state = RF_IDLE_S;
        return 0;
      }
      sw_uart_send_byte(rf_buffer[rf_buffposition]);
      rf_buffposition++;
      break;
    default:
      rf_buffposition = 0;
      rf_state = RF_IDLE_S;
    } // End switch
    return 0;
  } else return 1;
}

void rf_reset() {
  sw_uart_reset();
  rf_state = RF_IDLE_S;
  rf_buffposition = 0;
}