#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <windows.h>
#define QUADRADO 10
#define robo_x 30
#define robo_y 30

void pensar(float x, float y, float angulo, float distancia_sensor);
void ocupados(int cateto_o, int cateto_a);
void livre(int x0, int y0, int x1, int y1);

// cada quadrado == 10 cm
int mapa[60][60] = {0}; // 6x6 metros

int main(void){
    pensar(robo_x, robo_y, 120.2,10.34);
    return 0;
}

void pensar(float x, float y, float alfa, float distancia_sensor){

    int largura, altura;
    largura = (int) (x/QUADRADO); // casting of float for int
    altura = (int) (y/QUADRADO); // casting of float for int

    int cateto_oposto, cateto_adjacente = 0;
    cateto_oposto = (int) (sin(alfa) * distancia_sensor)/QUADRADO; // altura == linhas
    cateto_adjacente = (int) (cos(alfa) * distancia_sensor)/QUADRADO; // largura == colunas

    ocupados(cateto_oposto, cateto_adjacente);
    livre(robo_x, robo_y, cateto_adjacente, cateto_oposto);

}

void ocupados(int cateto_o, int cateto_a){

    mapa[cateto_o][cateto_a]+=2;

    if(mapa[cateto_o][cateto_a] > 10)
        mapa[cateto_o][cateto_a]-=1;
}

// x0 e y0 == posição do robo
// x1 e x2 == posição do objeto
void livre(int x0, int y0, int x1, int y1){

    int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
    int dy = abs(y1-y0), sy = y0<y1 ? 1 : -1;
    int err = (dx>dy ? dx : -dy)/2, e2;

    for(;;){
        mapa[y0][x0]-=1;

            if(mapa[y0][x0] < -10)
                mapa[y0][x0] + 1;

        if (x0==x1 && y0==y1) break;
        e2 = err;
        if (e2 >-dx) { err -= dy; x0 += sx; }
        if (e2 < dy) { err += dx; y0 += sy; }
  }



}