#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <util/twi.h>
#include "lcd.h"
#include <Robostix.h>

/* Globals */
static char twi_buf[LCD_WRITE_BUFFER_SIZE];
static volatile int twi_buf_index;
static volatile int twi_buf_len;

static volatile int lcd_mutex = 0;

static volatile twi_mode_t mode;

static volatile char twi_read_buf[LCD_READ_BUFFER_SIZE];
static volatile int twi_read_buf_index; //points to the next avail one if size > 0
static volatile int twi_read_buf_end; //points to the next open slot if size < LCD_READ_BUFFER_SIZE
static volatile int twi_read_buf_size; // number of elements in the buffer

/* Private functions variables*/

/* Prototypes */
static void addCharToReadBuf(char nextByte);

static inline int8_t lcd_lock(int8_t block) {

  while(1) {
    cli();
    if (lcd_mutex == 0)
      break;
    sei();

    if (block == LCD_NONBLOCK)
      return -1;
  }

  lcd_mutex = 1;

  sei();

  return (int8_t)0;
}

static inline void lcd_unlock(void) {
  lcd_mutex = 0;
}

/**
 * Called whenever the i2c interface is addressed for slave mode or
 * data transmission has begun or occuring
 **/
ISR(TWI_vect) {
  char nextByte;

  switch (TW_STATUS) { /* See what the condition code from the i2c was */
  case TW_REP_START:
  case TW_START: /* the start command or repeated start was transmitted and acknowledged */
    if (mode == WRITE)
      TWDR = LCD_WRITE_ADDRESS;
    else
      TWDR = LCD_READ_ADDRESS;

    TWCR = _BV(TWEN) | _BV(TWIE);
    break;

    /* slave acknowledged, so we can send data */
  case TW_MT_SLA_ACK:
  case TW_MT_DATA_ACK:
    if (twi_buf_index < twi_buf_len) {
      TWCR = _BV(TWEN) | _BV(TWIE);
      TWDR = twi_buf[twi_buf_index++];
    } else {
      // End of buffer - stop.
      TWCR = _BV(TWSTO) | _BV(TWEN) | _BV(TWIE);

      //unlock the lcd so something else can read/write
      lcd_unlock();
    }
    break;

    /* slave isn't ready or data write failed */
  case TW_MT_SLA_NACK:
  case TW_MT_DATA_NACK:
    //stop the transfer
    TWCR = _BV(TWSTO) | _BV(TWEN) | _BV(TWIE);

    lcd_unlock();
    break;

    /* Lost arbitration so try again */
  case TW_MT_ARB_LOST: // Also same as TW_MR_ARB_LOST
    TWCR = _BV(TWSTA) | _BV(TWEN) | _BV(TWIE);
    break;

    /* Slave acknowledge master receiver mode and will start sending data */
  case TW_MR_SLA_ACK:
    TWCR = _BV(TWEN) | _BV(TWIE);
    break;

    /* Data received from slave */
  case TW_MR_DATA_ACK:
    //this is commented out since TW_MR_SLA_ACK does not set TWEA
    // so it never gets here.
    /*
    nextByte = TWDR;
    TWCR = _BV(TWEN) | _BV(TWIE);
    if (nextByte != 0) {
      addCharToReadBuf(nextByte);
    }
    */
    break;

    /* Last bit of data from the slave has already been received */
  case TW_MR_DATA_NACK:
    nextByte = TWDR;
    TWCR = _BV(TWSTO) | _BV(TWEN) | _BV(TWIE);
    if (nextByte != 0) {
      addCharToReadBuf(nextByte);
    }

    //unlock the lcd so something else can read/write
    lcd_unlock();
    break;


    /* One of the other address modes of i2c such as Slave receive or slave transmit */
  default:
    TWCR = _BV(TWSTO) | _BV(TWEN) | _BV(TWIE);
    break;
  }

  // write a 1 to the TWINT bit to clear the interrupt
  TWCR |= _BV(TWINT);
}

//if buffer is full, char gets dropped
//this doesn't need to protect the buffer, because get_key disables interrupts when reading
//  from the buffer so this won't be run if get_key is running
static void addCharToReadBuf(char nextByte) {
  if (twi_read_buf_size >= LCD_READ_BUFFER_SIZE) {
    return; // buffer full
  }

  twi_read_buf[twi_read_buf_end] = nextByte;
  twi_read_buf_end++;
  if (twi_read_buf_end >= LCD_READ_BUFFER_SIZE) {
    twi_read_buf_end = 0;
  }
  twi_read_buf_size++;
}

void init_lcd(void) {
  /* Set the clock divisor to reach a value under 100kpbs */
  TWBR = 40;

  /* Set the prescaler value for the clock divisor of 4^x */
  TWSR = 0x01;

  /* Enable the i2c interface, this will cause the sda and scl pins to be
   * taken over by i2c */
  TWCR = _BV(TWEN) | _BV(TWEA);

  twi_read_buf_index = 0;
  twi_read_buf_end = 0;
  twi_read_buf_size = 0;

  /* Delay startup for gumstix initial boot */
  for (int32_t i = 0; i != 0; i++) __asm__ __volatile__("nop");

  /* Set the lcd to save key presses in buffer so we can read them */
  lcdnwrite_P(PSTR("\xFE\xA0\x00\xfe\x45"), 5);


  // Initialize font - Small Filled
  lcdnwrite_P(PSTR("\xFE\x31\x01"), 3);

  lcd_clear();
  lcd_set_led(LCD_LED_1, LCD_LED_OFF);
  lcd_set_led(LCD_LED_2, LCD_LED_OFF);
  lcd_set_led(LCD_LED_3, LCD_LED_OFF);
}

void lcd_write(char* str) {
  if (!str) {
    return;
  }

  lcdnwrite(str, strlen(str));
}

//uint8_t is 8 bits so the largest value it can hold
// is 255 and since the write buffer is defined
// in lcd.h as length 512, what someone tries to
// write will always fit inside the write buffer
void lcdnwrite(char* buf, uint8_t length) {
  if (!buf)
    return;

  //if something is reading, block until its done reading
  lcd_lock(LCD_BLOCK);

  //set the mode to WRITE so the correct address gets sent
  mode = WRITE;

  cli();
  memcpy(twi_buf, buf, length);
  twi_buf_index = 0;
  twi_buf_len = length;
  sei();

  TWCR = _BV(TWINT) | _BV(TWSTA)  | _BV(TWEN) | _BV(TWIE);
}

void lcd_write_P(const char *str) {
  if (!str)
    return;

  lcdnwrite_P(str, strlen_P(str));
}

//uint8_t is 8 bits so the largest value it can hold
// is 255 and since the write buffer is defined
// in lcd.h as length 512, what someone tries to
// write will always fit inside the write buffer
void lcdnwrite_P(const char *str, uint8_t length) {
  if (!str)
    return;

    //if something is reading, block until its done reading
    lcd_lock(LCD_BLOCK);

    //set the mode to WRITE so the correct address gets sent
    mode = WRITE;

    cli();
    memcpy_P(twi_buf, str, length);
    twi_buf_index = 0;
    twi_buf_len = length;
    sei();

    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN) | _BV(TWIE);
}

/**
 * @brief Polls the LCD for a key press from its buffer
 *
 * @param flags ?
 *
 * @return 0 on success, -1 on failure
 */
char lcd_poll_key(int8_t flags) {
  if (lcd_lock(LCD_NONBLOCK) < 0) {
    return -1;
  }
  mode = READ;

  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN) | _BV(TWIE);

  return 0;
}

/**
 * @brief reads a character from the interal buffer and gives it to the user
 *  If no character is available, blocks until one becomes available and turns
 *  the yellow LED on
 *
 * @return The next character.
 */
char lcd_get_key() {
  char nextChar;

  //spin wait until a character is pressed on the lcd
  // turn the YELLOW led on while spin waiting
  while (twi_read_buf_size == 0) {
  }

  //disable interrupts so something can be read from the buffer
  // since this program is not multithreaded and this is the only
  // place that reads from the twi_read_buffer, after twi_read_buffer_size
  // goes above 0, nothing else can grab that character except this.
  cli();

  //get the next character from the buffer
  nextChar = twi_read_buf[twi_read_buf_index];
  twi_read_buf_index++;
  //wrap around
  if (twi_read_buf_index >= LCD_READ_BUFFER_SIZE)
    twi_read_buf_index = 0;

  twi_read_buf_size--;

  //reenable interrupts
  sei();

  return nextChar;
}

void lcd_clear(void) {
  lcd_write_P(PSTR("\xFE\x58"));
}

void lcd_set_brightness(uint8_t level) {
  uint8_t cmd[3] = {0xFE, 0x98};
  cmd[2] = level;
  lcdnwrite((char *)cmd, 3);
}

void lcd_set_led(lcd_led_t led, lcd_led_color_t color) {
  char on[2];
  char num0;

  switch (led) {
  case LCD_LED_1:
    num0 = 1;
    break;
  case LCD_LED_2:
    num0 = 3;
    break;
  case LCD_LED_3:
    num0 = 5;
    break;
  default:
    return;
  }

  switch (color) {
  case LCD_LED_YELLOW:
    on[0] = '\x56';
    on[1] = '\x56';
    break;
  case LCD_LED_GREEN:
    on[0] = '\x57';
    on[1] = '\x56';
    break;
  case LCD_LED_RED:
    on[0] = '\x56';
    on[1] = '\x57';
    break;
  case LCD_LED_OFF:
    on[0] = '\x57';
    on[1] = '\x57';
    break;
  default:
    return;
  }

  char buf[3];
  buf[0] = '\xFE';
  buf[1] = on[0];
  buf[2] = num0;

  lcdnwrite(buf, 3);

  buf[0] = '\xFE';
  buf[1] = on[1];
  buf[2] = num0 + 1;

  lcdnwrite(buf, 3);
}

void lcd_puts(char* s) {
  lcdnwrite(s, strlen(s));
}

void lcd_reset_cursor() {
  lcdnwrite_P(PSTR("\xFE\x48"), 2);
}

/**
 * Draw a rectangle that is either white or black for its entirety.
 * The arguements are top left coordinates followed by bottom right of rect.
 * The color arguement is reduced to either white or black
 **/
void lcd_draw_rect(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2,
                   uint8_t color) {
  char buf[7];
  buf[0] = '\xFE';
  buf[1] = '\x72';
  buf[2] = color;
  buf[3] = x1;
  buf[4] = y1;
  buf[5] = x2;
  buf[6] = y2;

  lcdnwrite(buf, 7);
}

void lcd_draw_line(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2) {
  char buf[7];
  buf[0] = '\xFE';
  buf[1] = '\x6C';
  buf[2] = LCD_BLACK;
  buf[3] = x1;
  buf[4] = y1;
  buf[5] = x2;
  buf[6] = y2;

  lcdnwrite(buf, 7);
}

void lcd_set_color(uint8_t color) {
  char buf[4];

  buf[0] = '\xFE';
  buf[1] = '\x63';
  buf[2] = color;

  lcdnwrite(buf, 3);
}