#include <Arduino.h>
#include "HamShield.h"
#include "SimpleFIFO.h"
#include <util/atomic.h>

#define PHASE_BIT 8
#define PHASE_INC 1

#define PHASE_MAX (SAMPLEPERBIT * PHASE_BIT)
#define PHASE_THRES (PHASE_MAX / 2)

#define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01)
#define EDGE_FOUND(bitline)            BIT_DIFFER((bitline), (bitline) >> 1)

#define PPOOL_SIZE 2

#define AFSK_SPACE 2200
#define AFSK_MARK  1200

// Timers
volatile unsigned long lastTx = 0;
volatile unsigned long lastTxEnd = 0;
volatile unsigned long lastRx = 0;

#define T_BIT ((unsigned int)(9600/1200))

void AFSK::Encoder::process() {
  // Check what clock pulse we're on
  if(bitClock == 0) { // We are onto our next bit timing  
    // We're on the start of a byte position, so fetch one
    if(bitPosition == 0) {
      if(preamble) { // Still in preamble
        currentByte = HDLC_PREAMBLE;
        --preamble; // Decrement by one
      } else {
        if(!packet) { // We aren't on a packet, grab one
          // Unless we already sent enough
          if(maxTx-- == 0) {
            stop();
            lastTxEnd = millis();
            return;
          }
          packet = pBuf.getPacket();
          if(!packet) { // There actually weren't any
            stop(); // Stop transmitting and return
            lastTxEnd = millis();
            return;
          }
        lastTx = millis();
        currentBytePos = 0;
      }
    
      // We ran out of actual data, provide an HDLC frame (idle)
      if(currentBytePos++ == packet->len) {
        pBuf.freePacket(packet);
        packet = pBuf.getPacket(); // Get the next, if any
        currentBytePos = 0;
        currentByte = HDLC_FRAME;
        hdlc = true;
      } else {
        // Grab the next byte
        currentByte = packet->getByte(); //[currentBytePos++];
        if(currentByte == HDLC_ESCAPE) {
          currentByte = packet->getByte(); //[currentBytePos++];
          hdlc = true;
        } else {
          hdlc = false;
        }
      }
    }
  }

  // Pickup the last bit
  currentBit = currentByte & 0x1;    

  if(lastZero == 5) {
    currentBit = 0; // Force a 0 bit output
  } else {
    currentByte >>= 1; // Bit shift it right, for the next round
    ++bitPosition; // Note our increase in position
  }

  // To handle NRZI 5 bit stuffing, count the bits
  if(!currentBit || hdlc)
    lastZero = 0;
  else
    ++lastZero;

  // NRZI and AFSK uses toggling 0s, "no change" on 1
  // So, if not a 1, toggle to the opposite tone
  if(!currentBit)
    currentTone = !currentTone;
  }

  // Advance the bitclock here, to let first bit be sent early 
  if(++bitClock == T_BIT)
    bitClock = 0;

  if(currentTone == 0) {
    dds->setFrequency(AFSK_SPACE);
  } else {
    dds->setFrequency(AFSK_MARK);
  }
}

bool AFSK::Encoder::start() {
  if(!done || sending) {
    return false;
  }
  
  if(randomWait > millis()) {
    return false;
  }
  
  // First real byte is a frame
  currentBit = 0;
  lastZero = 0;
  bitPosition = 0;
  bitClock = 0;
  preamble = 23; // 6.7ms each, 23 = 153ms
  done = false;
  hdlc = true;
  packet = 0x0; // No initial packet, find in the ISR
  currentBytePos = 0;
  maxTx = 3;
  sending = true;
  dds->setFrequency(0);
  dds->on();
  return true;
}

void AFSK::Encoder::stop() {
  randomWait = 0;
  sending = false;
  done = true;
  dds->off();
}

AFSK::Decoder::Decoder() {
  // Initialize the sampler delay line (phase shift)
  for(unsigned char i = 0; i < SAMPLEPERBIT/2; i++)
  delay_fifo.enqueue(0);
}

bool AFSK::HDLCDecode::hdlcParse(bool bit, SimpleFIFO<uint8_t,HAMSHIELD_AFSK_RX_FIFO_LEN> *fifo) {
  bool ret = true;
  
  demod_bits <<= 1;
  demod_bits |= bit ? 1 : 0;
  
  // Flag
  if(demod_bits == HDLC_FRAME) {
    fifo->enqueue(HDLC_FRAME);
    rxstart = true;
    currchar = 0;
    bit_idx = 0;
    return ret;
  }
  
  // Reset
  if((demod_bits & HDLC_RESET) == HDLC_RESET) {
    rxstart = false;
    lastRx = millis();
    return ret;
  }
  if(!rxstart) {
    return ret;
  }
  
  // Stuffed?
  if((demod_bits & 0x3f) == 0x3e)
    return ret;
  
  if(demod_bits & 0x01)
    currchar |= 0x80;
  
  if(++bit_idx >= 8) {
    if(currchar == HDLC_FRAME ||
      currchar == HDLC_RESET ||
      currchar == HDLC_ESCAPE) {
        fifo->enqueue(HDLC_ESCAPE);
      }
    fifo->enqueue(currchar & 0xff);
    currchar = 0;
    bit_idx = 0;
  } else {
    currchar >>= 1;
  }  

  return ret;
}
  
// Handle the A/D converter interrupt (hopefully quickly :)
void AFSK::Decoder::process(int8_t curr_sample) {
  // Run the same through the phase multiplier and butterworth filter
  iir_x[0] = iir_x[1];
  iir_x[1] = ((int8_t)delay_fifo.dequeue() * curr_sample) >> 2;
  iir_y[0] = iir_y[1];
  iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0]>>3) + (iir_y[0]>>5);
  
  // Shift the bit into place based on the output of the discriminator
  sampled_bits <<= 1;
  sampled_bits |= (iir_y[1] > 0) ? 1 : 0;
  
  // Place this ADC sample into the delay line
  delay_fifo.enqueue(curr_sample);
  
  // If we found a 0/1 transition, adjust phases to track
  if(EDGE_FOUND(sampled_bits)) {
    if(curr_phase < PHASE_THRES)
      curr_phase += PHASE_INC;
    else
      curr_phase -= PHASE_INC;
  }
  
  // Move ahead in phase
  curr_phase += PHASE_BIT;
  
  // If we've gone over the phase maximum, we should now have some data
  if(curr_phase >= PHASE_MAX) {
    curr_phase %= PHASE_MAX;
    found_bits <<= 1;
    
    // If we have 3 bits or more set, it's a positive bit
    register uint8_t bits = sampled_bits & 0x07;
    if(bits == 0x07 || bits == 0x06 || bits == 0x05 || bits == 0x03) {
      found_bits |= 1;
    }
    
    hdlc.hdlcParse(!EDGE_FOUND(found_bits), &rx_fifo); // Process it
  }
}
  
// This routine uses a pre-allocated Packet structure
// to save on the memory requirements of the stream data
bool AFSK::Decoder::read() {
  bool retVal = false;
  if(!currentPacket) { // We failed a prior memory allocation
    currentPacket = pBuf.makePacket(PACKET_MAX_LEN);
    if(!currentPacket) // Still nothing
      return false;
  }
  // While we have AFSK receive FIFO bytes...
  while(rx_fifo.count()) {
    // Grab the character
    char c = rx_fifo.dequeue();
    
    bool escaped = false;
    if(c == HDLC_ESCAPE) { // We received an escaped byte, mark it
      escaped = true;
      currentPacket->append(HDLC_ESCAPE); // Append without FCS
      c = rx_fifo.dequeue(); // Reset to the next character
    }
    
    // Append all the bytes
    // This will include unescaped HDLC_FRAME bytes
    //if(c == HDLC_FRAME && !escaped)
    //currentPacket->append(c); // Framing bytes don't get FCS updates
    //else
    if(c != HDLC_FRAME)
      currentPacket->appendFCS(c); // Escaped characters and all else go into FCS
    
    if(currentPacket->len > PACKET_MAX_LEN) {
      // We've now gone too far and picked up far too many bytes
      // Cancel this frame, start back at the beginning
      currentPacket->clear();
      continue;
    }
    
    // We have a frame boundary, if it isn't escaped
    // If it's escaped, it was part of the data stream
    if(c == HDLC_FRAME && !escaped) {
      if(!currentPacket->len) {
        currentPacket->clear(); // There wasn't any data, restart stream
        continue;
      } else {
        // We have some bytes in stream, check it meets minimum payload length
        // Min payload is 1 (flag) + 14 (addressing) + 2 (control/PID) + 1 (flag)
        if(currentPacket->len >= 16) {          
          // We should end up here with a valid FCS due to the appendFCS
          if(currentPacket->crcOK()) { // Magic number for the CRC check passing
            // Valid frame, so, let's filter for control + PID
            // Maximum search distance is 71 bytes to end of the address fields
            // Skip the HDLC frame start
            bool filtered = false;
            for(unsigned char i = 0; i < (currentPacket->len<70?currentPacket->len:71); ++i) {
              if((currentPacket->getByte() & 0x1) == 0x1) { // Found a byte with LSB set
                // which marks the final address payload
                // next two bytes should be the control/PID
                if(currentPacket->getByte() == 0x03 && currentPacket->getByte() == 0xf0) {
                  filtered = true;
                  break; // Found it
                }
              }
            }
            
            if(!filtered) {
              // Frame wasn't one we care about, discard
              currentPacket->clear();
              continue;
            }
            
            // It's all done and formatted, ready to go
            currentPacket->ready = 1;
            if(!pBuf.putPacket(currentPacket)) // Put it in the receive FIFO
              pBuf.freePacket(currentPacket); // Out of FIFO space, so toss it
            
            // Allocate a new one of maximum length
            currentPacket = pBuf.makePacket(PACKET_MAX_LEN);
            retVal = true;
          }
        }
      }
      // Restart the stream
      currentPacket->clear();
    }
  }
  return retVal; // This is true if we parsed a packet in this flow
}
  
void AFSK::Decoder::start() {
  // Do this in start to allocate our first packet
  currentPacket = pBuf.makePacket(PACKET_MAX_LEN);
/*  ASSR &= ~(_BV(EXCLK) | _BV(AS2));

  // Do non-inverting PWM on pin OC2B (arduino pin 3) (p.159).
  // OC2A (arduino pin 11) stays in normal port operation:
  // COM2B1=1, COM2B0=0, COM2A1=0, COM2A0=0
  // Mode 1 - Phase correct PWM
  TCCR2A = (TCCR2A | _BV(COM2B1)) & ~(_BV(COM2B0) | _BV(COM2A1) | _BV(COM2A0)) |
           _BV(WGM21) | _BV(WGM20);
  // No prescaler (p.162)
  TCCR2B = (TCCR2B & ~(_BV(CS22) | _BV(CS21))) | _BV(CS20) | _BV(WGM22);
  
  OCR2A = pow(2,COMPARE_BITS)-1;
  OCR2B = 0;
  // Configure the ADC and Timer1 to trigger automatic interrupts
  TCCR1A = 0;
  TCCR1B = _BV(CS11) | _BV(WGM13) | _BV(WGM12);
  ICR1 = ((F_CPU / 8) / REFCLK) - 1;
  ADMUX = _BV(REFS0) | _BV(ADLAR) | 0; // Channel 0, shift result left (ADCH used)
  DDRC &= ~_BV(0);
  PORTC &= ~_BV(0);
  DIDR0 |= _BV(0);
  ADCSRB = _BV(ADTS2) | _BV(ADTS1) | _BV(ADTS0);
  ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIE) | _BV(ADPS2); // | _BV(ADPS0);  */
}
  
AFSK::PacketBuffer::PacketBuffer() {
  nextPacketIn = 0;
  nextPacketOut = 0;
  inBuffer = 0;
  for(unsigned char i = 0; i < PACKET_BUFFER_SIZE; ++i) {
    packets[i] = 0x0;
  }
}

unsigned char AFSK::PacketBuffer::readyCount() volatile {
  unsigned char i;
  unsigned int cnt = 0;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    for(i = 0; i < PACKET_BUFFER_SIZE; ++i) {
      if(packets[i] && packets[i]->ready)
        ++cnt;
    }
  }  
  return cnt;
}
  
// Return NULL on empty packet buffers
AFSK::Packet *AFSK::PacketBuffer::getPacket() volatile {
  unsigned char i = 0;
  AFSK::Packet *p = NULL;
  
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if(inBuffer == 0) {
      return 0x0;
    }
    
    do {
      p = packets[nextPacketOut];
      if(p) {
        packets[nextPacketOut] = 0x0;
        --inBuffer;
      }
      nextPacketOut = ++nextPacketOut % PACKET_BUFFER_SIZE;
      ++i;
    } while(!p && i<PACKET_BUFFER_SIZE);
    
    // Return whatever we found, if anything
  }  
  return p;
}
  
//void Packet::init(uint8_t *buf, unsigned int dlen, bool freeData) {
void AFSK::Packet::init(unsigned short dlen) {
  //data = (unsigned char *)buf;
  ready = 0;
  freeData = 1; //freeData;
  type = PACKET_STATIC;
  len = 0; // We had a length, but don't put it here.
  maxLen = dlen; // Put it here instead
  dataPtr = (uint8_t *)malloc(dlen+16);
  dataPos = dataPtr;
  readPos = dataPtr;
  fcs = 0xffff;
}
  
// Allocate a new packet with a data buffer as set
AFSK::Packet *AFSK::PacketBuffer::makePacket(unsigned short dlen) {
  AFSK::Packet *p;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    //Packet *p = findPooledPacket();
    p = new Packet(); //(Packet *)malloc(sizeof(Packet));
    if(p) // If allocated
      p->init(dlen);
  }
  return p; // Passes through a null on failure.
}
  
// Free a packet struct, mainly convenience
void AFSK::PacketBuffer::freePacket(Packet *p) {
  if(!p)
  return;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    p->free();
    /*unsigned char i;
    for(i = 0; i < PPOOL_SIZE; ++i)
    if(p == &(pPool[i]))
    break;
    if(i < PPOOL_SIZE)
    pStatus &= ~(1<<i);*/
    delete p;
  }  
}
  
// Put a packet onto the buffer array
bool AFSK::PacketBuffer::putPacket(Packet *p) volatile {
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if(inBuffer >= PACKET_BUFFER_SIZE) {
      return false;
    }
    packets[nextPacketIn] = p;
    nextPacketIn = ++nextPacketIn % PACKET_BUFFER_SIZE;
    ++inBuffer;
  }  
  return true;
}
  
// Print a single byte to the data array
size_t AFSK::Packet::write(uint8_t c) {
  return (appendFCS(c)?1:0);
}
  
size_t AFSK::Packet::write(const uint8_t *ptr, size_t len) {
  size_t i;
  for(i = 0; i < len; ++i)
    if(!appendFCS(ptr[i]))
      break;
  return i;
}
  
// Determine what we want to do on this ADC tick.
void AFSK::timer() {
  if(encoder.isSending())
    encoder.process();
  decoder.process(ADCH - 128);
}

void AFSK::start(DDS *dds) {
  afskEnabled = true;
  encoder.setDDS(dds);
  decoder.start();
}