#include "pch.h"
#include <stdio.h>
#include <conio.h>
#include <math.h>
#include <string>

using namespace std;

//Mam sobie licznik, który zlicza zmiany pozycji enkodera
//Mam też sobie pozycję w której defaultowo znajduje się serwo
//Chcę napisać funkcję, która będzie porównywała dwa następujące po sobie stany licznika
//Jeżeli poprzednia_pozycja_enkodera < obecna_pozycja_enkodera,  tzn. że wał obrócił się zgodnie z ruchem wskazówek zegara
//I trzeba zwiększyć o 1 liczbę, która później będzie konwertowana do HEX i wysyłana w pakiecie do wysterowania serwa
//Jeżeli natomiast jest ">", to analogicznie trzeba tę liczbę zmniejszyć
//A jak jest "=" to chuuuuuuuj jej w duuuuupę staaaaaaryyy
//Problem: jak porównać dwie zmieniające się w czasie pozycje enkodera (czyli de facto dwie liczby z zakresu 0-81** (?))

uint16_t impuls_count_encoder_1 = 528; //zmienna globalna; impulsy zliczone przez enkoder
uint16_t previous_encoder_position = 0;

uint16_t dec2hex(uint16_t impuls_count_encoder_1) {

    int default_position = 0x1ff; //pozycja bazowa serwa (narazie 0x1ff, czyli 150*), później trzeba ją będzie podzielić na dwa bajty: Position_H i Position_L
    int position = default_position;

    uint16_t actual_encoder_position = impuls_count_encoder_1;

    uint16_t how_many = (abs((previous_encoder_position - actual_encoder_position) / 8)); //dzielone przez 8, bo enkoder chyba zlicza 4*2048 imp/obr, a więc 8 imp z enkodera = 1 imp serwa

    if (actual_encoder_position > previous_encoder_position)
        position += how_many;
    else if (actual_encoder_position < previous_encoder_position)
        position -= how_many;

    previous_encoder_position = actual_encoder_position;

// dopełnienie liczby HEX do formy czteroznakowej
    int value = 0;
    int n;
    value = position;
    char data[4];
    n = sprintf_s(data, "%x", value);

    if (n == 1) {
        data[3] = data[0];
        for (int i = 0; i < 3; i++)
            data[i] = '0';
    }
    else {
        for (int i = n - 1; i >= 0; i--) {
            data[i + 4 - n] = data[i];
            data[i] = '0';
        }
    }

    return 0;
}

int main() {

    dec2hex(impuls_count_encoder_1);

    return 0;
}