#include "adapter_ard.h"
#include "HCSR04.h"

#include <ctime>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <iostream>

serial_c Serial;

HCSR04_c HCSR04;

struct timespec start, end;

int pinState[32];
double dists[2];

int main(int argc, char** argv){
  clock_gettime(CLOCK_MONOTONIC_RAW, &start);
  
  setup();
  int c = '0';
  char cmd_buff[128];
  int cmd_buff_ind = 0;
  
  while(1){
    while((c = getchar()) != '@'){
      if(c == '$')
        cmd_buff_ind = 0;
      if(c == ';'){
        cmd_buff[cmd_buff_ind] = '\0';
        char* end_ptr;
        int pin = strtol(&cmd_buff[1], &end_ptr, 10);
        int level = strtol(&end_ptr[1], &end_ptr, 10);
        //printf("pin %d at %d\n", pin, level);
        if(pin >= 0)
          pinState[pin] = level;
        else dists[-pin-1] = level/10.0;
        cmd_buff_ind = 0;
      }
      cmd_buff[cmd_buff_ind++] = c;
    }
    printf("$dt,%ld;", micros());
    loop();
    delay(10);
    printf("*");
    fflush(stdout);
  }
}

void delay(unsigned long milli){
  unsigned long end = millis() + milli;
  while(millis() <= end);
}

long millis(){
  clock_gettime(CLOCK_MONOTONIC_RAW, &end);
  uint64_t milli = (end.tv_sec - start.tv_sec) * 1000 + (end.tv_nsec - start.tv_nsec) / 1000000;
  
  return milli;
}

long micros(){
  clock_gettime(CLOCK_MONOTONIC_RAW, &end);
  uint64_t milli = (end.tv_sec - start.tv_sec) * 1000000 + (end.tv_nsec - start.tv_nsec) / 1000;
  
  return milli;
}

int abs(int in){
  return abs(in);
}

double min(double a, double b){
  return a<b?a:b;
}
double max(double a, double b){
  return a>b?a:b;
}

void pinMode(int pin, int mode){
  printf("$pm,%d,%d;",pin, mode);
}

int analogRead(int pin){
  return pinState[pin];
}

void digitalWrite(int pin, int level){
  printf("$dw,%d,%d;", pin, level);
}

void analogWrite(int pin, int dc){
  printf("$aw,%d,%d;", pin, dc);
}

void serial_c::begin(int br){
  printf("started serial at %d bps\n", br);
}

void serial_c::print(std::string txt){
  std::cout << "serial : " << txt;
}

void serial_c::println(std::string txt){
  this->print(txt);
  std::cout << std::endl;
}

void serial_c::print(int nb){
  printf("serial : %d", nb);
}

void serial_c::print(float nb){
  printf("serial : %f", nb);
}

void serial_c::println(float nb){
  this->print(nb);
  printf("\n");
}
void serial_c::print(double nb){
  printf("serial : %f", nb);
}

void serial_c::println(double nb){
  this->print(nb);
  printf("\n");
}

void serial_c::println(int nb){
  this->print(nb);
  printf("\n");
}

void HCSR04_c::begin(byte trig, byte* echos, byte ecnt){
  //printf("");
}

double* HCSR04_c::measureDistanceCm(){
  //printf("dists %f %f", dists[0], dists[1]);
  return dists;
}