#include <stdio.h>
#include <stdlib.h>

/* 
   Simple simulator for robot moving through an XMAX by YMAX grid.
   g[i][j] == 1 for walls, 0 for open space.
   rx,ry are the robots coordinates.
   There is a sensor in every non-wall square, that fires (value 1) 
   if it senses the robot, where the sensor error probabilities 
   are SENSOR_FALSE_POS and SENSOR_FALSE_NEG.
   After each step the robot decides whether or not to move with probability
   MOTION_PROB.  If it decides to move, then it moves with equality probability
   to any adjacent open square.
   This continues for TMAX steps.
   Data generated is a series of lines of the form:
        TIME_STEP  SENSOR_X_COORD  SENSOR_Y_COORD  SENSOR_READING
   The SENSOR_READING is 1 (fires) or 0 (does not fire) and is given for
   every sensor for every time slice.

   output: TRUTH is the file specifying robot's actual location
           DATA is the sensed data
*/


#define XMAX 5
#define YMAX 5
#define XINIT 0
#define YINIT 0
#define TMAX 400
#define SENSOR_FALSE_POS 10
#define SENSOR_FALSE_NEG 60
#define MOTION_PROB 30
#define SEED 123
#define TRUTH "truth.txt"
#define DATA "data.txt"

int g[XMAX][YMAX] = {
    {0, 1, 0, 0, 0},
    {0, 0, 0, 1, 0},
    {1, 0, 1, 1, 0},
    {1, 0, 0, 0, 0},
    {1, 0, 0, 0, 1}};

int rx = XINIT;
int ry = YINIT;

int main(int argc, char ** argv)
{
    int t, coin, sensed;
    int i,j;
    int nc;
    int choicex[4];
    int choicey[4];
    FILE * truth;
    FILE * data;

    srandom(SEED);
    truth = fopen(TRUTH, "w");
    data = fopen(DATA, "w");

    fprintf(data, "%i %i\n", XMAX, YMAX);
    for (i=0; i<XMAX; i++){
        for (j=0; j<YMAX; j++){
            fprintf(data, " %1i", g[i][j]);
        }
        fprintf(data, "\n");
    }
    fprintf(data, "%f %f %f\n", SENSOR_FALSE_POS/100.0, SENSOR_FALSE_NEG/100.0, MOTION_PROB/100.0);
    fprintf(data, "%i\n", TMAX);

    for (t=1; t<=TMAX; t++){

        fprintf(truth, "%i %i %i\n", t, rx, ry);

        for (i=0; i<XMAX; i++){
            for (j=0; j<YMAX; j++){

                coin = (random()>>7)%100;

                if (g[i][j]==0){
                    if (rx==i && ry==j) {
                        if (coin < (100-SENSOR_FALSE_NEG))
                            sensed = 1;
                        else sensed = 0;
                    }
                    else {
                        if (coin < SENSOR_FALSE_POS)
                            sensed = 1;
                        else sensed = 0;
                    }
                    fprintf(data, "%d %d %d %d\n", t, i, j, sensed);
                }
            }
        }
        /* move robot */
        coin = (random()>>5)%100;
        if (coin < MOTION_PROB){

            nc = 0;
            if (rx>0 && g[rx-1][ry]==0) {choicex[nc]=rx-1; choicey[nc]=ry; nc++;}
            if (ry>0 && g[rx][ry-1]==0) {choicex[nc]=rx; choicey[nc]=ry-1; nc++;}
            if (rx<XMAX-1 && g[rx+1][ry]==0) {choicex[nc]=rx+1; choicey[nc]=ry; nc++;}
            if (ry<YMAX-1 && g[rx][ry+1]==0) {choicex[nc]=rx; choicey[nc]=ry+1; nc++;}
            coin = (random()>>9)%nc;
            rx = choicex[coin];
            ry = choicey[coin];

        }
    }
    return(0);
}