/*****************************************************************
 METP0000_lib Arduino library
 Structure based on standard Arduino LiquidCrystal.h library
 Much of the Print routine code is based on code developed and
   maintanied by MCUdude
   https://github.com/MCUdude/KTMS1201
   Based on Tronixstruff's work
     tronixstuff.com/2013/03/11/arduino-and-ktm-s1201-lcd-modules
     (Jeff Albertson, Robert W. Mech, John Boxall)
 Also incorporating font/code ideas from all over   
    
 Conversion to supporting METP0000 module by Tony Wills
   2018-11-24 v 1.00 initial version
   2018-11-27 v 1.30 print.h version based on LiquidCrystal.h
   2018-11-29 v 1.40 back to using custom print routines
   2018-12-01 v 1.50 new character set
*****************************************************************/

#include "METP0000_lib.h"
#include "Arduino.h"

#if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
  #else
  #include "WProgram.h"
  #endif


// Constructor with busy pin
METP0000::METP0000(uint8_t NSCK, uint8_t SI, uint8_t BUSY, uint8_t CS)
{
  init(NSCK, SI, BUSY, CS);
}


// Constructor without busy pin
METP0000::METP0000(uint8_t NSCK, uint8_t SI, uint8_t CS)
{
  init(NSCK, SI, 255, CS);
}


void METP0000::init(uint8_t NSCK, uint8_t SI, uint8_t BUSY, uint8_t CS)
{
  // Store parameters
  _SCK = NSCK; //data clock
  _SI = SI; //serial data
  _BUSY = BUSY; //LCD busy
  _CS = CS; //LCD select
  
  // Set inputs and outputs
  pinMode(_SCK, OUTPUT);
  pinMode(_SI, OUTPUT);
  if (_BUSY != 255)
    pinMode(_BUSY, INPUT);
  pinMode(_CS, OUTPUT);
}


void METP0000::begin(uint8_t cols, uint8_t rows, uint8_t charsize)  //parameters just to maintain compatibility
{
  // Initialise LCD
  digitalWrite(_SCK,LOW);
  digitalWrite(_SI, LOW);
  digitalWrite(_CS, LOW); //deselect LCD
  
  command(_Mode); //initialise LCD bias, timing etc
  command(_Sync); //delay screen update to sync with frame rate
  delay(2);
  command(_NoFlash); //disable flashing
  delay(2);
  command(_DispOn); //turn on display
  delay(1);
  command(_NoDecode); //not using character decoders as custom wired
  command(_ClearDsp); //clear LCD, home cursor
  delay(2);
}


/********** high level commands, for the user! */

// Clear the whole LCD
void METP0000::clear(uint8_t start, uint8_t stop)
{
  //expanded form - can be called with optional parameters
  if(start == 0 && stop >= 5) {
    // Clear whole display
    command(_ClearDsp);
    delay(2);
    setCursor(0); //home cursor
    _bufferPtr = 0; //initialise scroll buffer pointer
    // Turn off all under-bars
    for (int i=0; i<5; i++ ) _bar[i] = false;
  } else {
    // Clear a section
    uint8_t currentCursorPos = _cursorPos; //store the current cursor position
    setCursor(start);
    String blankSpace = "";
    // Fill string with spaces
    for(uint8_t i = 0; i < (stop - start + 1); i++)
      blankSpace += ' ';
    print(blankSpace);
    setCursor(currentCursorPos);
    //TODO: should we perhaps set cursor to start of cleared region?
  }
}


// Move the cursor to the start of the display
void METP0000::home()
{
  setCursor(0);
}


// Store cursor position
void METP0000::setCursor(uint8_t cursorPos, uint8_t row) //row is ignored, only here for compatibility
{
  //TODO: what is the best way to handle the cursor being positioned beyond the screen edge
  //should we perhaps ignore it instead?
  //Doesn't actually move the LCD cursor directly
  if(cursorPos > 5)
    _cursorPos = 5;
  else
    _cursorPos = cursorPos;
}


// Turn the display on/off
void METP0000::display() {
  command(_DispOn);
}

void METP0000::noDisplay() {
  command(_DispOff);
}


// Turns the underline cursor on/off
void METP0000::noCursor() {
//TODO: null rountine for now
}


void METP0000::cursor() {
//TODO: null rountine for now
}


// Turn on and off the blinking cursor
void METP0000::noBlink() {
//TODO: null rountine for now
}


void METP0000::blink() {
//TODO: null rountine for now
}


// These commands scroll the display without changing the RAM
// Scroll display left without re-writing display buffer
void METP0000::scrollDisplayLeft(void) {
  //TODO: quick implimentation, not sure that this is the best way to do this
  _bufferPtr++;
  if (_bufferPtr>5) _bufferPtr = 0;
  uint8_t currentCursorPos = _cursorPos; //store the current cursor position
  uint8_t i;
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+0); //position cursor
  // Write string to LCD
  i = _bufferPtr;
  while (i < 6) {
    writeChar(_textString[i]);
    i++;
  }
  i = 0;
  while (i < _bufferPtr) {
    if (_wrapBuf)
      writeChar(_textString[i]);
    else
      writeChar(0x0000);
    i++;
  }
  digitalWrite(_CS, LOW); //deselect LCD to display data  
  setCursor(currentCursorPos); //update our cursor pointer
}


// Scroll display right without re-writing display buffer
void METP0000::scrollDisplayRight(void) {
  //TODO: quick implimentation, not sure that this is the best way to do this
  if (_bufferPtr==0) _bufferPtr = 6;
  _bufferPtr--;
  uint8_t currentCursorPos = _cursorPos; //store the current cursor position
  uint8_t i;
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+0); //position cursor
  // Write string to LCD
  i = _bufferPtr;
  while (i < 6) {
    if (_wrapBuf)
      writeChar(_textString[i]);
    else
      writeChar(0x0000);
    i++;
  }
  i = 0;
  while (i < _bufferPtr) {
    writeChar(_textString[i]);
    i++;
  }
  digitalWrite(_CS, LOW); //deselect LCD to display data  
  setCursor(currentCursorPos); //update our cursor pointer
}


// This is for text that flows Left to Right
void METP0000::leftToRight(void) {
//TODO: replace with code for METP0000
//_displaymode |= LCD_ENTRYLEFT;
//command(LCD_ENTRYMODESET | _displaymode);
}


// This is for text that flows Right to Left
void METP0000::rightToLeft(void) {
//TODO: replace with code for METP0000
//_displaymode &= ~LCD_ENTRYLEFT;
//command(LCD_ENTRYMODESET | _displaymode);
}


// This will 'right justify' text from the cursor
void METP0000::autoscroll(void) {
//TODO: replace with code for METP0000
//_displaymode |= LCD_ENTRYSHIFTINCREMENT;
//command(LCD_ENTRYMODESET | _displaymode);
}


// This will 'left justify' text from the cursor
void METP0000::noAutoscroll(void) {
//TODO: replace with code for METP0000
//_displaymode &= ~LCD_ENTRYSHIFTINCREMENT;
//command(LCD_ENTRYMODESET | _displaymode);
}


// Define custom characters
void METP0000::createChar(uint8_t location, uint8_t charmap[]) {
//TODO: replace with code for METP0000
//Allows us to fill the first 8 CGRAM locations
//with custom characters
//location &= 0x7; //we only have 8 locations 0-7
//command(LCD_SETCGRAMADDR | (location << 3));
//for (int i=0; i<8; i++) {
//  write(charmap[i]);
//}
}


// Set all segments flashing that have their flash bit set
void METP0000::flash(bool speed)
{	
  // Flash fast
  if(speed == true)
    command(_FFlash);
  // Flash slow	
  else
    command(_SFlash);
}


// Turn off flashing
void METP0000::noFlash()
{
  command(_NoFlash);
}


// Set all segements so they will flash/blink when command given
void METP0000::setFlash()
{
//TODO: In the future we might want to be able to specify individual
// digits, or cursor underscore to flash/blink
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  // Enable all segments for flashing
  for (int i=0; i<32; i++) {
    send(_WFlshMem + 0x07);
    wait(); //wait for the LCD to finish
  }
  digitalWrite(_CS, LOW); //deselect LCD to display data
}


// Clear the flash/blink data from all segments
void METP0000::unSetFlash()
{
  command(_ClearFlsh);
}


// Set a set of segments on a particular digit
void METP0000::setSegment(uint16_t seg)
{
  //seg is a 4 nybble word,
  //the first nybble is the character location 0 - 6 (includes phantom 7th character)
  //the next 3 nybbles represent the segment pattern to turn on
  int chr;
  chr = seg >> 12;
  //TODO: validate chr is 0 to 6
  uint8_t currentCursorPos = _cursorPos; //store the current cursor position
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+chr*3);
  send(_OrDspMem + ((seg >> 8) & 0x007));
  wait();
  send(_OrDspMem + ((seg >> 4) & 0x007));
  wait();
  send(_OrDspMem + ((seg >> 0) & 0x007));
  wait();
  digitalWrite(_CS, LOW); //deselect LCD to display data
  wait();
  setCursor(currentCursorPos);
}


// Clear a set of segments on a particular digit
void METP0000::clearSegment(uint16_t seg)
{
  //seg is a 4 nybble word,
  //the first nybble is the character location 0 - 6 (includes phantom 7th character)
  //the next 3 nybbles represent the segment pattern to turn off
  int chr;
  chr = seg >> 12;
  //TODO: validate chr is 0 to 6
  uint8_t currentCursorPos = _cursorPos; //store the current cursor position
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+chr*3);
  send(_AndDspMem + ((~seg >> 8) & 0x007));
  wait();
  send(_AndDspMem + ((~seg >> 4) & 0x007));
  wait();
  send(_AndDspMem + ((~seg >> 0) & 0x007));
  wait();
  digitalWrite(_CS, LOW); //deselect LCD to display data
  wait();
  setCursor(currentCursorPos);
}


// Set under-bar n
void METP0000::setBar(uint8_t bar)
{
  //TODO: validate bar is 0 to 4
  _bar[bar] = true;
  setSegment(barArray[bar]);
}


// Reset under-bar n
void METP0000::unSetBar(uint8_t bar)
{
  //TODO: validate bar is 0 to 4
  _bar[bar] = false;
  clearSegment(barArray[bar]);
}


// Print String data type
void METP0000::print(String str)
{
  //TODO: this is basically the same as the print array function, one could call the other after converting the parameter
  uint8_t arrayLength = str.length();
  if (arrayLength>80) arrayLength=80;
  //NB at the moment the output buffer is oversized, with a mind to future scrolling capability
  for(uint8_t i = 0; i < 7; i++) _textString[i]=0x0000; //ensure really is blank

  //TODO: this can be done the same way as in the print array function
  if( str[0] == '.' ) str = ' ' + str; //can't have rightmost character displayed as dot
  for(uint8_t i = 0; i < str.length(); i++) {
    if((str[i] == '.') && (str[i+1] != '.')) {
      str.remove(i, 1);
      _textString[i] = _SetDp;  //string will have a decimal point preceding this digit
    }
  }
  uint8_t strLength = str.length();
  if((strLength + _cursorPos) > 6) strLength = 6 - _cursorPos;
  //uint8_t p = 6 - strLength - _cursorPos;
  uint8_t p = _cursorPos;

  // Assemble TextString with 12bit hex values of characters
  for(uint8_t i = 0; i < strLength; i++)
    _textString[i] |= sevenSegHex[(uint8_t)str[i]-32]; 
  //NB at the moment the character definitions start at char 32 (space)
    
  // Restore left and right under-bar elements
  //TODO: this can be more elegant   
  if (_bar[0])
    _textString[0] |= (barArray[0] & 0x777);
  else
    _textString[0] &= ~(barArray[0] & 0x777);
  if (_bar[4])
    _textString[5] |= (barArray[4] & 0x777);
  else
    _textString[5] &= ~(barArray[4] & 0x777);
  
  // Write out string
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+p*3); //position cursor
    // Write string to LCD
  for(uint8_t i = 0; i < strLength; i++)
    writeChar(_textString[i]);
  digitalWrite(_CS, LOW); //deselect LCD to display data  
  setCursor(p+strLength); //update our cursor pointer
} 


// Print null terminated array of characters
void METP0000::print(const char* inputArray)
{
  uint8_t arrayLength = 0; //can't use sizeof to determine function parameter array length
  uint8_t i = 0; //input string index
  uint8_t j = 0; //output string index
  
  for(uint8_t i = 0; i < 7; i++) _textString[i]=0x0000; //ensure really is blank
  
  while(inputArray[i] > 0) i++; //sizeof() workaround
  arrayLength = i;
  
  // Assemble TextString with 12bit hex values of characters
  i = 0;j=0;
  if (inputArray[0]=='.')
    j++; //leave first output char blank
  while ((i < arrayLength) && (j < (6 - _cursorPos))) {
    if((inputArray[i] == '.') && (inputArray[i+1] != '.'))
      _textString[j] = _SetDp; //next digit will have preceding decimal point
    else {
      _textString[j] |= sevenSegHex[(uint8_t)inputArray[i]-32];
      //NB at the moment the character definitions start at char 32 (space)
      j++;
    }
    i++;
  }
  
  // Restore left and right under-bar elements
  //TODO: this can be more elegant
  if (_bar[0])
    _textString[0] |= (barArray[0] & 0x777);
  else
    _textString[0] &= ~(barArray[0] & 0x777);
  if (_bar[4])
    _textString[5] |= (barArray[4] & 0x777);
  else
    _textString[5] &= ~(barArray[4] & 0x777);
  
  // Write out string
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr + _cursorPos*3); //position cursor
  // Write string to LCD
  //for(uint8_t i = 0; i < arrayLength; i++)
  for(i = 0; i < j; i++)
    writeChar(_textString[i]);
  digitalWrite(_CS, LOW); //deselect LCD to display data   
  setCursor(_cursorPos + arrayLength); //update our cursor pointer
}


void METP0000::print(char character)
{
  //TODO: need to restore left and right under-bar elements as above
  uint8_t p = _cursorPos;
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+p*3); //position cursor
  writeChar(sevenSegHex[(uint8_t)character-32]); //write 12bit character
  //NB at the moment the character definitions start at char 32 (space)
  digitalWrite(_CS, LOW); //deselect LCD to display data  
  setCursor(_cursorPos+1);
}

//TODO: I haven't actually analysed what these print functions do
void METP0000::print(uint32_t n, uint8_t base)
{
  if(base > 1)
    printNumber(n, base);
}


void METP0000::print(int32_t n, uint8_t base)
{
  if(base > 1) {
    if(n < 0) {
      n = -n;
      printNumber(n, DEC, true);
    } else 
      printNumber(n, base); 
  }
}


void METP0000::print(uint16_t n, uint8_t base)
{
  if(base > 1)
    printNumber((uint32_t)n, base);
}


void METP0000::print(int16_t n, uint8_t base)
{
  if(base > 1) {
    if(n < 0) {
      n = -n;
      printNumber((int32_t)n, DEC, true);
    } else 
      printNumber((int16_t)n, base); 
  }
}


void METP0000::print(double n, uint8_t digits)
{
  printFloat(n, digits);
}


/*********** mid level commands, for sending data/cmds */

// Send a byte to the display as a command
// (NB we no longer have a corresponding data send function)
void METP0000::command(uint8_t cmd)
{
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(cmd);
  wait(); //wait for the LCD to finish
  digitalWrite(_CS, LOW); //deselect LCD to display data
}


//TODO: No longer needed function?
//size_t METP0000::write(uint8_t character)
uint8_t METP0000::write(uint8_t character)
{
  digitalWrite(_CS, HIGH); //select LCD
  wait(); //wait for the LCD to finish
  send(_LoadPtr+_cursorPos*3); //position cursor
  writeChar(sevenSegHex[(uint8_t)character-32]); //write 12bit character
  digitalWrite(_CS, LOW); //deselect LCD to display data
  setCursor(_cursorPos+1);
  return 1; //assume success
}


/************ low level data pushing commands **********/

//TODO: I haven't actually analysed what these print functions do
void METP0000::printNumber(uint32_t number, uint8_t base, bool table)
{
  //TODO: Might want to check based <= 36, or unpredictable output
  //NB no idea why 'table' is used to signify positive/negative
  char buf[8 * sizeof(long) + 1 + table]; //Assumes 8-bit chars plus zero byte
  char *str = &buf[sizeof(buf) - 1];

  *str = '\0';
  
  do {
    char c = number % base;
    number /= base;
    
    *--str = c < 10 ? c + '0' : c + 'A' - 10;
  } while(number);

  // Add minus if negative number
  if(table == true)
    *--str = '-';
  print(str);
}


void METP0000::printFloat(double number, uint8_t digits) 
{ 
  
  String dblValue = "";
  if(isnan(number)) {
    dblValue += "nan";
    print(dblValue);
    return;
  }
  if(isinf(number)) {
    dblValue += "inf";
    print(dblValue);
    return;
  }
  if(number > 4294967040.0) {
    dblValue += "ovf";
    print(dblValue);
    return;
  }
  if(number <-4294967040.0) {
    dblValue += "ovf";
    print(dblValue);
    return;
  }
  
  // Handle negative numbers
  if(number < 0.0) {
     dblValue += "-";
     number = -number;
  }

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for(uint8_t i = 0; i < digits; i++)
    rounding /= 10.0;
  
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  dblValue += (String)int_part;
  //dblValue += String(int_part);

  // Print the decimal point, but only if there are digits beyond
  if(digits > 0)
    dblValue += ".";

  // Extract digits from the remainder one at a time
  while(digits-- > 0)
  {
    remainder *= 10.0;
    int toPrint = int(remainder);
    dblValue += (String)toPrint;
    //dblValue += String(toPrint);
    remainder -= toPrint; 
  } 
  print(dblValue);
}


// Serial output 8 bits, MSB first
void METP0000::send(uint8_t character)
{
  for(uint8_t i = 0; i < 8; i++) 
  {
    digitalWrite(_SI, !(character & (1 << (7-i)))); //invert data due to hardware inverter buffers
                                                    //NB the parameter of digitalWrite is a
                                                    //boolean true/false so don't just negate data
    // Make clock cycle
    digitalWrite(_SCK,HIGH);
    waitClk();
    digitalWrite(_SCK, LOW); 
    waitClk();
  }
  wait();
}


// Write a nine-bit character (stored in a sixteen bit word) using load-immediate commands
void METP0000::writeChar(uint16_t _dta)
{
  wait();
  send(_WriteMem + ((_dta >> 8) & 0x007));
  wait();
  send(_WriteMem + ((_dta >> 4) & 0x007));
  wait();
  send(_WriteMem + ((_dta >> 0) & 0x007));
  wait();
}


// Delay long enough for most commands to complete, or use Busy pin if available
void METP0000::wait()
{
  if(_BUSY == 255)
    delay(busyDelay);
  else
    while(digitalRead(_BUSY) == 0);
}


// Serial clock period
void METP0000::waitClk() {
  delayMicroseconds(15);
}