//#include "ams_swserial.hpp"
#include "amswirelib.hpp"
#include "amsgenlib.hpp"
#include "ams20x4LCD.hpp"

#include "amsswcomm.hpp"

#include <stdio.h>

lcd20x4 lcd;
amsswcomm comm;

char buff1[128];

unsigned long loopcount;

 void setup()
 {
     const uint8_t q[8] = {9,10,21,20,19,18,17,16};
     lcd.setup(6,7,8,q);
     lcd.setmydefs();
     //initialize
     lcd.display_clear();
     lcd.cursor_home();
     lcd.write_chars("INIT");
     //set up comm as a servant receiver
     //C1 - D2 - pin 2 - using as CLK
     //C2 - D3 - pin 3 - using as sendpin
     //C3 - D4 - pin 4 - using as recvpin
     //C4 - D5 - pin 5
     // divisor = 5 ticks
     comm.init(0,2,3,4,5);
 }

 void loop()
 {
     comm.update_step();
     if(comm.has_message())
     {
         comm.recv_message(buff1,128);
         buff1[127] = 0;
         comm.send_message("ACK",4);
         lcd.display_clear();
         lcd.cursor_home();
         lcd.write_chars(buff1);
     }
 }


//void setup()
//{
//    set_pinmode(2,1);
//    set_pinmode(3,1);
//    set_pinmode(4,1);
//    loopcount = 0;
//}

//// Test pins
//void loop()
//{
//    delay(100);
//    write_pin(2, loopcount%2);
//    write_pin(3, (loopcount/2)%2);
//    write_pin(4, (loopcount/4)%2);
//    
//
//    loopcount = loopcount + 1;
//    loopcount = loopcount % 16;
//}