#include "serialstream.h"
#include "../utils/string.h"

SerialStream::SerialStream(ComPort port, BaudRate baud_rate, DataBits data_bits,
                           StopBits stop_bits, Parity parity)
    :Serial(port, baud_rate, data_bits, stop_bits, parity) {}

void SerialStream::flush() {
  print(buffer, strlen(buffer));
}

// https://stackoverflow.com/questions/3440726/what-is-the-proper-way-of-implementing-a-good-itoa-function
// Yet, another good itoa implementation
// returns: the length of the number string
int itoa(int value, char *sp, int radix)
{
    char tmp[16];// be careful with the length of the buffer
    char *tp = tmp;
    int i;
    unsigned v;

    int sign = (radix == 10 && value < 0);    
    if (sign)
        v = -value;
    else
        v = (unsigned)value;

    while (v || tp == tmp)
    {
        i = v % radix;
        v /= radix;
        if (i < 10)
          *tp++ = i+'0';
        else
          *tp++ = i + 'a' - 10;
    }

    int len = tp - tmp;

    if (sign) 
    {
        *sp++ = '-';
        len++;
    }

    while (tp > tmp)
        *sp++ = *--tp;

    return len;
}

void SerialStream::setForeground(Color c) {
    write(0x1b);
    write('[');
    write('3');
    write(c + 0x30);
    write('m');
}

void SerialStream::setBackground(Color c) { 
    write(0x1b);
    write('[');
    write('4');
    write(c + 0x30);
    write('m');
}

void SerialStream::setAttribute(Attrib a) {
    write(0x1b);
    write('[');
    write(a + 0x30);
    write('m');
}

void SerialStream::reset() {
    write(0x1b);
    write('c');
}

void SerialStream::setPos(int x, int y) {
    char out[] = {0x1b, '[', 0, 0, ';', 0, 0, 'H', 0};
    //*this << 0x1b;
    //*this << '[';
    //*this << dec << x;
    //*this << ';' << y << 'H' << endl;
    //flush();
    itoa(x, &out[2], 10);
    itoa(y, &out[5], 10);
    print( out , strlen(out));
}

void SerialStream::print(char* str, int length) {
    for(int i=0; i < length; i++)
        write(str[i]);
}