#include "common.h"
#include "protocol.h"

#include <inttypes.h>
#include <stdbool.h>
#include <avr/io.h>
#include <avr/eeprom.h>
#include <stdio.h>

#include "eeprom.h"
#include "debug.h"

#define EEPROM_READY() while(EECR & (1 << EEWE));

void eeprom_parser(char *buf)
{
  uint16_t addr;
  uint16_t length;

  addr =   ((uint16_t)buf[2]) +
           ((uint16_t)buf[3] << 8);

  length = ((uint16_t)buf[4]) +
           ((uint16_t)buf[5] << 8);

  debugf("eeprom_parser: length: %u, addr: %u\n", length, addr);

  switch(buf[0])
  {
    case CMD_EEPROM_READ_BYTES:
      eeprom_read_bytes_usb(addr, length);
      break;

    case CMD_EEPROM_WRITE_BYTES:
      eeprom_write_bytes_usb(addr, &buf[6], length);
      break;

    default:
      answer[0] = buf[0];
      answer[1] = RSP_UNKOWN_CMD;
      answer[2] = 0;
      CommandAnswer(3);
  }
}


void eeprom_read_bytes_usb(uint16_t addr, uint16_t length)
{
  answer[0] = CMD_EEPROM_READ_BYTES;
  answer[1] = eeprom_read_bytes(addr, (char *)&answer[2], length);
  answer[2+length] = 0;

  debugf("read: %s\n", &answer[2]);

  CommandAnswer(3+length);
}

uint8_t eeprom_read_bytes(uint16_t addr, char * buf, uint16_t length)
{
  uint8_t i;

  if(addr > MAX_ADDR)
    return RSP_ERROR;

  // wait for eeprom
  EEPROM_READY();

  for(i = 0; i < length; i++)
  {

    buf[i] = eeprom_read_byte((uint8_t*) addr++); 
    // set addr
    //EEAR = addr++;

    //EECR |= (1 << EERE);

    //buf[i] = EEDR;
    //debug_SendHex(buf[i]);
  }

  return RSP_OK;
}

void eeprom_write_bytes_usb(uint16_t addr, char * buf, uint16_t length)
{
  answer[0] = CMD_EEPROM_WRITE_BYTES;
  answer[1] = eeprom_write_bytes(addr, buf, length);
  answer[2] = 0;

  debugf("written: %s\n", buf);
  
  CommandAnswer(3);
}

uint8_t eeprom_write_bytes(uint16_t addr, char * buf, uint16_t length)
{
  uint8_t i;

  if(addr > MAX_ADDR)
    return RSP_ERROR;

  // wait for eeprom
  EEPROM_READY();

  for(i = 0; i < length; i++)
  {
    eeprom_write_byte ((uint8_t*) addr++, buf[i]); 
    
    //EEAR = addr++;
    //EEDR = buf[i];

    //EECR |= (1 << EEMWE);
    //EECR |= (1 << EEWE);
  }

  eeprom_read_bytes(addr, buf, length);

  return RSP_OK;
}