#include "BackgroundIR.h"
#include "CircularBuffer.h"
#include "RawIR.h"
#include "AardErrors.h"

#define CIRC_SIZE 120

DECLARE_CIRCULAR_BUFFER(MarkSpaceBuffer, unsigned int, CIRC_SIZE)

IMPLEMENT_CIRCULAR_BUFFER(MarkSpaceBuffer, unsigned int, CIRC_SIZE)

// Private routines an data for the BackgroundIR namespace

namespace BackgroundIR {
   void
   DisableSender();

   void
   DisableReceiver();

   void
   EnableSender();

   void
   EnableReceiver();

   void
   SendNextPulse();

   void
   SeeTimeExpire();

   void
   SeeIRChange();

   MarkSpaceBuffer ms_buf;
   byte            ms_state = (byte)BGIR_STOPPED;
   unsigned int    ms_status = BGIR_STATUS;
   bool            receiving_enabled = false;
   bool            receiving_disrupted = false;
   bool            sending_enabled = false;
   bool            sending_disrupted = false;
   int             last_ir_freq = 0;
   bool            receiver_normally_running = false;

   byte            prev_time_valid = 0;
   unsigned int    prev_time = 0;
   unsigned int    max_duration = 0;
};

void
BackgroundIR::InitializeIR() {
   // Utility code to initialize all the timers and wot-not used by this
   // library.

   // For both sending and receiving, I want timer1 running in "normal" mode
   // with no overflow interrupt and scaled to count at the rate of system
   // clock/64 (which, with a 16MHz clock, gives us the microseconds/4 units
   // we measure duration in).

   // Start with all timer1 interrupts turned off
   TIMSK1 = 0;

   // This sets all of COM1A1 COM1A0 COM1B1 COM1B0 WGM11 WGM10 to zero
   TCCR1A = 0;

   // This sets input capture noise canceler on, initializes ICES1 to zero
   // to start capture on falling edge, WGM13 WGM12 to zero, and CS1 to 3
   // (divide by 64).
   TCCR1B = _BV(ICNC1) | _BV(CS11) | _BV(CS10);

   // Set input pin mode
   pinMode(IR_RECIEVE_PIN, INPUT);
   digitalWrite(IR_RECIEVE_PIN, HIGH);

   // Now I want the send timer running in phase correct PWM mode at full speed

   // We don't want any send timer interrupts
   DISABLE_SEND_TIMER_INTERRUPTS;

   // Initialize the IR xmit pin to output mode and set it LOW
   pinMode(IR_XMIT_PIN, OUTPUT);
   digitalWrite(IR_XMIT_PIN, LOW);

   INITIALIZE_SEND_TIMER_PWM_MODE_AND_CLOCK_DIVIDER;

   // Initialize the PWM rate to 38kHz
   SetIRModulation(38);
}

BackgroundIR::IRState
BackgroundIR::GetState() {
   if (ms_state == (byte)BGIR_HAS_DATA) {
      if (ms_buf.IsEmpty()) {
         ms_state = (byte)BGIR_WAITING;
      }
   } else if (ms_state == (byte)BGIR_WAITING) {
      if (! ms_buf.IsEmpty()) {
         ms_state = (byte)BGIR_HAS_DATA;
      }
   }
   return (BackgroundIR::IRState)ms_state;
}

void
BackgroundIR::StartReceivingIR() {

   // Wait for any sending to finish before enabling receiver.

   while (GetState() == BGIR_SENDING) {
      delayMicroseconds(100);
   }
   if (ms_state != (byte)BGIR_STOPPED) {
      receiver_normally_running = true;
      return;
   }
   EnableReceiver();

   receiver_normally_running = true;
}

void
BackgroundIR::StopReceivingIR() {

   receiver_normally_running = false;

   if ((ms_state == (byte)BGIR_STOPPED) || (ms_state == (byte)BGIR_SENDING)) {
      return;
   }
   DisableReceiver();
}

bool
BackgroundIR::ReceiverNormallyRunning() {
   return receiver_normally_running;
}

unsigned int
BackgroundIR::GetData() {
   return ms_buf.Pop();
}

bool
BackgroundIR::SetIRModulation(int kHz) {

#if PORT_TARGET == PORT_ARDUINO_UNO_R3

   // We are running timer2 at full SYSCLOCK rate.
   unsigned long rate = SYSCLOCK;

   // phase correct mode counts up then down for a full cycle, so
   // we start with SYSCLOCK/2
   rate >>= 1;

   // now we divide that by the kHz rate
   rate /= (unsigned long)kHz;

   // and by 1000 (the k part of kHz)
   rate /= 1000ul;

   // Now this really better be a number that fits in the 1 byte OCR2A
   // register, because that is what we want to count up to and
   // down from in order to achieve the specified kHz signal on
   // the output pin.

   if ((rate > 3ul) && (rate < 255ul)) {

      // OK, this is a good number, set OCR2A to the computed rate
      // and OCR2B to 1/3 of that to get a 33% duty cycle (which seems
      // to make every IR receiver happy). Timer 2 runs this way continuously
      // and we just enable and disable the OC2B output pin to turn the
      // emitter on and off.

      byte brate = (byte)rate;
      OCR2A = brate;
      OCR2B = brate/3;
      last_ir_freq = kHz;
      return true;
   } else {

      // Sorry, can't set that rate (possibly I could if I changed the
      // clock prescale value, but I don't think any legit IR modulation
      // rates need that).

      return false;
   }

#elif PORT_TARGET == PORT_TEENSY_2

// PUT CODE HERE - look up what all this means, add proper error checking
// for range of values, etc.

   const uint16_t pwmval = SYSCLOCK / 2000 / (kHz);
   TCCR4A = (1<<PWM4A);
   TCCR4B = _BV(CS40);
   TCCR4C = 0;
   TCCR4D = (1<<WGM40);
   TCCR4E = 0;
   TC4H = pwmval >> 8;
   OCR4C = pwmval;
   TC4H = (pwmval / 3) >> 8;
   OCR4A = (pwmval / 3) & 255;

   return true;
#endif
}

int
BackgroundIR::GetIRModulation() {
   return last_ir_freq;
}

bool
BackgroundIR::PutDataOK() {

   // If we aren't sending, then calling PutData will switch to sending
   // mode, emptying the buffer, so the first call will definitely succeed.
   // If we are sending, then just make sure the buffer isn't full.

   return ((IRState)ms_state != BGIR_SENDING) || (! ms_buf.IsFull());
}

bool
BackgroundIR::PutData(unsigned int ms_data) {

   if ((IRState)ms_state != BGIR_SENDING) {
      EnableSender();
   }

   if (ms_buf.IsFull()) {
      return false;
   }
   ms_buf.Push(ms_data);
   return true;
}

int
BackgroundIR::GetBufferSize() {
   return CIRC_SIZE;
}

unsigned int
BackgroundIR::GetGlobalErrors() {
   bool rd = receiving_disrupted;
   bool sd = sending_disrupted;
   if (rd || sd) {
      byte save_sreg = SREG;
      cli();
      rd = receiving_disrupted;
      receiving_disrupted = false;
      sd = sending_disrupted;
      sending_disrupted = false;
      SREG = save_sreg;
   }
   unsigned int rval = 0;
   if (rd) rval |= GLOB_ERR_RECEIVING_DISRUPTED;
   if (sd) rval |= GLOB_ERR_SENDING_DISRUPTED;
   return rval;
}

void
BackgroundIR::DisableSender() {
   // Utility code to turn off all outputs and interrupt associated
   // with the IR sending code.

   if (! sending_enabled) {
      return;
   }

   sending_enabled = false;

   // Make sure the PWM output is not enabled
   DISABLE_SEND_TIMER_PWM_OUTPUT;

   // Make sure the output pin is LOW
   digitalWrite(IR_XMIT_PIN, LOW);

   // Make sure the timer 1 compare register A interrupt is not enabled.
   TIMSK1 &= ~_BV(OCIE1A);

   // If the state reflects a sending mode, change it to stopped and flush
   // any queued send data.

   if (ms_state == (byte)BGIR_SENDING) {
      ms_state = (byte)BGIR_STOPPED;
      if (! ms_buf.IsEmpty()) {
         sending_disrupted = true;
         ms_buf.MakeEmpty();
      }
   }

   if (receiver_normally_running && (! receiving_enabled)) {
      EnableReceiver();
   }
}

void
BackgroundIR::DisableReceiver() {
   // Utility code to turn off all inputs and interrupt associated with
   // the IR receiving code.

   if (! receiving_enabled) {
      return;
   }

   receiving_enabled = false;

   // Make sure input capture and compare register B interrupts are disabled
   TIMSK1 &= ~(_BV(ICIE1) | _BV(OCIE1B));

   // If state reflects a receiving mode, change it to stopped and
   // flush the circular buffer.

   if ((ms_state == (byte)BGIR_WAITING) || (ms_state == (byte)BGIR_HAS_DATA)) {
      ms_state = (byte)BGIR_STOPPED;
      if (! ms_buf.IsEmpty()) {
         receiving_disrupted = true;
         ms_buf.MakeEmpty();
      }
   }
}

void
BackgroundIR::EnableSender() {
   if (sending_enabled) {
      return;
   }

   // Cannot receive at the same time as we send.
   DisableReceiver();

   sending_enabled = true;

   // In order for sending to start happening, we need to get a timer1
   // compare register A interrupt, so find a time in the near future,
   // and schedule an interrupt to happen then (at which point we
   // hope to find some data in the output queue to extract and send
   // from the interrupt routine).

   byte save_sreg = SREG;
   cli();
   unsigned int goodtime = TCNT1;
   SREG = save_sreg;
   goodtime += 40u;     // 40 ticks should be enough time to get data in queue
   OCR1A = goodtime;
   prev_time = goodtime;

   // Clear any existing OCF1A interrupt flag and enable compare A interrupt
   TIFR1 = _BV(OCF1A);
   TIMSK1 |= _BV(OCIE1A);

   ms_state = (byte)BGIR_SENDING;
}

void
BackgroundIR::EnableReceiver() {
   if (receiving_enabled) {
      return;
   }

   receiving_enabled = true;

   // Cannot send while receiver is active.
   DisableSender();

   // Receiver has to wait for IR to show up, and the first IR data
   // (given that the IR receiver module has an active LOW output) will
   // be a trailing edge, so setup input capture to happen on trailing
   // edge and generate an interrupt.

   prev_time_valid = 0;
   max_duration = 0;
   ms_state = (byte)BGIR_WAITING;

   TCCR1B &= ~_BV(ICES1);
   TIFR1 = _BV(ICF1);
   TIMSK1 |= _BV(ICIE1);
}

ISR(TIMER1_COMPA_vect) {
   BackgroundIR::SendNextPulse();
}

void
BackgroundIR::SendNextPulse() {
   if (ms_buf.IsEmpty()) {

      // OK, we hit end of IR sequence, turn off sender, go back to
      // stopped state.

      DisableSender();
      return;
   }
   unsigned int ms_data = ms_buf.Pop();
   if (BGIR_IS_MARK(ms_data)) {
      ENABLE_SEND_TIMER_PWM_OUTPUT;
   } else {
      DISABLE_SEND_TIMER_PWM_OUTPUT;
   }
   ms_data = BGIR_DURATION(ms_data);
   prev_time += ms_data;
   OCR1A = prev_time;
}

ISR(TIMER1_CAPT_vect) {
   BackgroundIR::SeeIRChange();
}

void
BackgroundIR::SeeIRChange() {

   // First things to do are read capture register and toggle capture
   // edge.

   unsigned int captured_time = ICR1;
   TCCR1B ^= _BV(ICES1);
   byte cur_edge = TCCR1B & _BV(ICES1);

   if (! prev_time_valid) {

      // This was the leading edge of the whole IR sequence, nothing to
      // push yet, just record the time so we'll be able to compute
      // duration when we see the next edge.

      prev_time = captured_time;
      prev_time_valid = 1;
   } else {
      unsigned int delta = captured_time - prev_time;
      prev_time = captured_time;
      if (delta > 0x3ffful) {
         delta = 0x3ffful;
         ms_status |= BGIR_LOST_TIME;
      }
      if (cur_edge == 0) {

         // I get very confused trying to actually understand when I'm at a
         // MARK or SPACE, but experimentation definitely shows that the
         // cur_edge == 0 above really does mean I just measured the time
         // of a MARK pulse, so what I'm seeing now must be a SPACE.

         if (delta > max_duration) {

            // Keep track of longest duration MARK (usually the 1st one,
            // given the way most IR protocols work).

            max_duration = delta;
         }
         delta |= BGIR_MARK;

         // Since we just saw a MARK, we are now looking at a SPACE and
         // it may well be the SPACE that goes on forever at the end of
         // the IR sequence. Set OCR1B to capture time plus 2*max_duration
         // and say we want to get an interrupt on OCR1B compare equal
         // to timer. This will let us know the space is too long to be
         // a real space and we are at the end of the IR sequence.

         OCR1B = captured_time + (max_duration << 1);
         TIFR1 = _BV(OCF1B);
         TIMSK1 |= _BV(OCIE1B);
      } else {

         // If the above if case was MARK, then this must be a SPACE
         // I just measured.

         delta |= BGIR_SPACE;

         // Since we are currently receiving a MARK which is bound to end
         // with a SPACE, we don't need the OCR1B compare interrupt for this
         // edge.

         TIMSK1 &= ~_BV(OCIE1B);
      }
      if (ms_buf.IsFull()) {
         ms_status |= BGIR_LOST_DATA;
      } else {
         ms_buf.Push(delta);
      }
   }
}

ISR(TIMER1_COMPB_vect) {
   BackgroundIR::SeeTimeExpire();
}

void
BackgroundIR::SeeTimeExpire() {

   // This routine gets called when timer 1 overflows which tells us
   // we have seen the end of an IR sequence.

   ms_status |= BGIR_END;
   if (ms_buf.IsFull()) {

      // Yikes! The buffer is full just as we want to push on the final
      // status. Add the "we had to disable the interrupt" bit to the
      // status, and bump OCR1B a bit to tell the timer to come back here
      // again real soon. Meanwhile, turn off the IR receiver so we won't
      // have a new IR signal confusing us before we can report the end of
      // this one.

      if (! BGIR_IS_DISABLED(ms_status)) {
         ms_status |= BGIR_DISABLED;
         TIMSK1 &= ~_BV(ICIE1);
      }
      OCR1B += (max_duration >> 1);
   } else {
      ms_buf.Push(ms_status);
      TIMSK1 = 0;
      ms_status = BGIR_STATUS;
      prev_time_valid = 0;
      max_duration = 0;
      TCCR1A = 0;
      TCCR1B = _BV(ICNC1) | _BV(CS11) | _BV(CS10);
      if (ms_state != BGIR_STOPPED) {
         TIFR1 = _BV(ICF1);
         TIMSK1 = _BV(ICIE1);
      } else {
         TIMSK1 = 0;
      }
   }
}
