#include "ams20x4LCD.hpp"
#include "amswirelib.hpp"

const static unsigned long rwdelay = 1000;

lcd20x4::lcd20x4()
{
  int I;
  pin_RS = 0;
  pin_RW = 0;
  pin_E = 0;
  for(I=0;I<8;I++) pin_DB[I] = 0;
}

lcd20x4::~lcd20x4()
{
  int I;
  pin_RS = 0;
  pin_RW = 0;
  pin_E = 0;
  for(I=0;I<8;I++) pin_DB[I] = 0;
}

void lcd20x4::setup(uint8_t RSpin, uint8_t RWpin, uint8_t Epin, const uint8_t* DBpins)
{
  int I;
  pin_RS = RSpin;
  pin_RW = RWpin;
  pin_E = Epin;
  set_pinmode(pin_RS,1);
  set_pinmode(pin_RW,1);
  set_pinmode(pin_E,1);

  for(I=0;I<8;I++)
  {
    pin_DB[I] = DBpins[I];
    set_pinmode(pin_DB[I],1);
  }

  cursx = 0;
  cursy = 0;
  
  return;
}

void lcd20x4::loop()
{
  //nothing really
}

uint8_t lcd20x4::read_op(uint8_t b, uint8_t RS)
{
  uint8_t ret = 0;
  uint8_t bt = 0;
  int I;
  for(I=0;I<8;I++)
  {
    set_pinmode(pin_DB[I],0);
  }

  write_pin(pin_E,1);
  write_pin(pin_RW,1);
  write_pin(pin_RS,RS);
  delayMicroseconds(rwdelay);

  for(I=0;I<8;I++)
  {
    bt = read_pin(pin_DB[I]);
    ret = ret + bt<<I;
  }

  delayMicroseconds(rwdelay);
  write_pin(pin_E,0);

  for(I=0;I<8;I++)
  {
    set_pinmode(pin_DB[I],1);
  }
  delayMicroseconds(rwdelay);
  return ret;
}
void lcd20x4::write_op(uint8_t b, uint8_t RS)
{
  int I;

  write_pin(pin_E,1);
  write_pin(pin_RW,0);
  write_pin(pin_RS,RS);
  delayMicroseconds(rwdelay);

  for(I=0;I<8;I++)
  {
    write_pin(pin_DB[I],(b & (1<<I))>>I);
  }

  delayMicroseconds(rwdelay);
  write_pin(pin_E,0);
  delayMicroseconds(rwdelay);
}

void lcd20x4::display_clear()
{
  write_op(0b00000001,0);
}

void lcd20x4::cursor_home()
{
  write_op(0b00000010,0);
  cursx = 0; cursy = 0;
}

void lcd20x4::entrymode(bool ID, bool S)
{
  write_op((1<<2) | (ID<<1) | S,0);
}

void lcd20x4::display_toggle(bool display, bool cursor, bool charblink)
{
  write_op((1<<3) | (display<<2) | (cursor<<1) | charblink, 0);
}

void lcd20x4::cursor_shift(bool screen_cursor, bool left_right)
{
  write_op((1<<4) | (screen_cursor<<3) | (left_right<<2),0);
}

//void lcd20x4::function_set(uint8_t q)
//{
//  write_op((1<<5) | (q & 0b00001111) , 0);
//}

  //DL: data length: 0 - 4-bit, 1 - 8-bit
  //N: display lines: 0 - 1-line-mode 1 - 2-line-mode
  //F: font: 0 - 5x8 1 - 5x10
void lcd20x4::function_set(bool DL,bool N,bool F)
{
  write_op((1<<5)|(DL<<4)|(N<<3)|(F<<2),0);
  return;
}

void lcd20x4::CGRAM_set(uint8_t Acg_6bits)
{
  write_op((1<<6)|(Acg_6bits & 0b00111111),0);
}

void lcd20x4::DDRAM_set(uint8_t Add_7bits)
{
  write_op((1<<7)|(Add_7bits & 0b01111111),0);
}

void lcd20x4::setmydefs()
{
  write_pin(pin_E,1);
  delay(50);
  write_op(0x30,0); //wake up 0x30 0b00110000
  delay(50);
  write_op(0x30,0); //wake up
  delay(50);
  write_op(0x30,0); //wake up
  delay(50);
  //write_op(0x38,0); //function set 8-bit, 2 lines 0b00111000
  function_set(1,1,1);
  delay(50);
  //write_op(0x10,0); //set cursor? (looks like a function set to me?)
  entrymode(1,0);
  delay(50);
    //write_op(0x0c,0); //0x0c 0b00001100
  display_toggle(1,0,0);
    //write_op(0x0c,0); //0x06 0b00000110
  entrymode(1,0);



//  display_clear();
//  cursor_home();
//  entrymode(1,0);
//  display_toggle(1,0,0);
//  DDRAM_set(0);
}

void lcd20x4::set_pos(uint8_t x, uint8_t y)
{
  uint8_t b;
  switch(y)
  {
    case 0: b = 0x00; break;
    case 1: b = 0x40; break;
    case 2: b = 0x14; break;
    case 3: b = 0x54; break;
    default: b = 0x00;
  }
  b += (x%20);
  DDRAM_set(b);
}

void lcd20x4::write_char(char b, uint8_t x, uint8_t y)
{
  set_pos(x,y);
  write_op((uint8_t) b, 1);
}

void lcd20x4::write_char(char b)
{
  //may have to do translation of ASCII code
  //01000001: A on the chart, 41h on ASCII chart - should be good
  //write_op((uint8_t) b,1);

  write_char(b,cursx,cursy);
  cursx = cursx+1;
  if(cursx>=20) {cursy = (cursy+1)%4; cursx = 0;}
}

void lcd20x4::newline()
{
  cursx = 0;
  cursy = (cursy+1)%4;
}

void lcd20x4::write_chars(const char *bytes)
{
  int I;
  for(I=0;I<80;I++)
  {
    if(bytes[I]=='\0')
    {
      break;
    }
    else if(bytes[I] == '\n')
    {
      newline();
    }
    else
    {
      write_char(bytes[I]);
    }
  }
  return;
}