#include <Arduino.h>
#include <util/crc16.h>

#include "afsk_sinus.hpp"
#include "eepromedit.hpp"

#define DESTINATION "GPS   "
#define TXDELAY 32

#define EDITOR_EEPROM_BASE 42
char callsign[7] = { ' ', ' ', ' ', ' ', ' ', ' ', '\0' };
uint8_t editorPinMap[8] = { 10, 11, 4, 6, 7, 8, 9, 5 };

struct ax25 {
  uint8_t daddr[7];
  uint8_t saddr[7];
  uint8_t ctrl;
  uint8_t proto;
} __attribute__((packed)) frame;

enum AprsSymbol : uint8_t {
  NONE,
  AMBULANCE,
  BUS,
  FIRE_TRUCK,
  BICYCLE,
  YACHT,
  HELICOPTER,
  SMALL_AIRCRAFT,
  SHIP,
  CAR,
  MOTORCYCLE,
  BALLOON,
  JEEP,
  RECREATIONAL_VEHICLE,
  TRUCK,
  VAN
};


extern void sendOne(uint32_t *us, uint8_t *phaseShift);
extern void sendZero(uint32_t *us, uint8_t *phaseShift);
extern void setZero();

uint32_t m = 0;
uint8_t phaseShift = 0;
bool nrzi = true;
uint8_t oneCount = 0;

void sendNrziBit(bool bit, bool zeroStuff) {
  if (!bit) {
    nrzi = !nrzi;
    oneCount = 0;
  } else if (zeroStuff) {
    oneCount++;
  }
  if (nrzi) {
    sendOne(&m, &phaseShift);
  } else {
    sendZero(&m, &phaseShift);
  }
  if (oneCount >= 5) {
    sendNrziBit(0, true);
  }
}

void sendFlag(bool sync, size_t count) {
  if (sync) {
    phaseShift = 0;
    m = micros();
  }
  uint8_t flag = 0x7e;
  for (size_t n = 0; n < count; ++n) {
    for (uint8_t i = 0; i < 8; ++i) {
      sendNrziBit((flag >> i) & 1, false);
    }
  }
}

void sendAbort(bool sync) {
  if (sync) {
    phaseShift = 0;
    m = micros();
  }
  for (uint8_t i = 0; i < 16; ++i) {
    sendNrziBit(1, false);
  }
}

void sendBell202buf(uint8_t *buf, size_t len, bool sync) {
  if (sync) {
    phaseShift = 0;
    m = micros();
  }
  for (size_t n = 0; n < len; ++n) {
    for (uint8_t i = 0; i < 8; ++i) {
      sendNrziBit((buf[n] >> i) & 1, true);
    }
  }
}

void initFrame() {
  memset(&frame, 0, sizeof(frame));
  memcpy(&frame.daddr, DESTINATION, 6);
  memcpy(&frame.saddr, callsign, 6);
  for (uint8_t i = 0; i < 7; ++i) {
    frame.daddr[i] <<=1;
    frame.saddr[i] <<=1;
  }
  frame.daddr[6] = (NONE << 1) | 0xe0; // command bit + reserved bits (AX.25 3.12.2)
  frame.saddr[6] = (callsign[6] << 1) | 0xe1;  // command bit + reserved bits + end bit
  frame.ctrl = 0x03;
  frame.proto = 0xf0;
}


uint8_t mirrorByte(uint8_t byt) {
  return ((byt & 1) << 7) | ((byt & 2) << 5) | ((byt & 4) << 3) | ((byt & 8) << 1)
    | ((byt & 16) >> 1) | ((byt & 32) >> 3) | ((byt & 64) >> 5) | ((byt & 128) >> 7);
}

void setup() {
  initEditor(callsign, 7, EDITOR_EEPROM_BASE, editorPinMap);
  OUT_DDR = 0x3f;
  setZero();
  nrzi = true;
  initFrame();
  Serial.begin(9600);
#ifdef ALT
  phaseShift = 0;
  m = micros();
  while (true) {
    sendOne(&m, &phaseShift);
    sendZero(&m, &phaseShift);
  }
#endif
#ifdef ONE
  phaseShift = 0;
  m = micros();
  while (true) {
    sendOne(&m, &phaseShift);
  }
#endif
#ifdef ZERO
  phaseShift = 0;
  m = micros();
  while (true) {
    sendZero(&m, &phaseShift);
  }
#endif
}


void readNmeaGll() {
  uint8_t state = 0;
  char str[83];
  char *sptr;

  while (state != 2) {
    if (!Serial.available()) {
      continue;
    }
    int16_t byt = Serial.read();
    if (state == 0) {
      if (byt == '$') {
	state = 1;
	sptr = str;
	*(sptr++) = (char) byt;
      }
    } else if (state == 1) {
      *(sptr++) = (char) byt;
      if (byt == '\n') {
	state = 2;
	*sptr = 0;
	if (str[3] == 'R' && str[4] == 'M' && str[5] == 'C' && *(sptr-6) == 'A') {
	  state = 2;
	} else {
	  state = 0;
	}
	Serial.print(str);
	Serial.flush();
      }
    }
    if (sptr > str + sizeof(str)) {
      state = 0;
    }
  }
  // AX.25 3.8 - FCS is sent MSB first
  uint16_t fcs = 0xffff;
  for (size_t i = 0; i < sizeof(frame); ++i) {
    fcs = _crc_xmodem_update(fcs, mirrorByte(((uint8_t*) &frame)[i]));
  }
  for (char *c = str; c < sptr; ++c) {
    fcs = _crc_xmodem_update(fcs, mirrorByte(*c));
  }  
  fcs ^= 0xffff;
  fcs = (mirrorByte(fcs & 0xff) << 8) | mirrorByte(fcs >> 8);
  sendFlag(true, TXDELAY+1);
  sendBell202buf((uint8_t*) &frame, sizeof(frame), false);
  sendBell202buf((uint8_t*) str, sptr-str, false);
  sendBell202buf((uint8_t*) &fcs, 2, false);
  sendFlag(false, 1);
  setZero();
  nrzi = true;
}


void loop() {
  readNmeaGll();
  delay(10000);
}