/*
/*
 * our program
 */

#use explego.icb

#define GLOBAL_COLS 10
#define GLOBAL_ROWS 10
#define MAXIMUM_X 20.2
#define MAXIMUM_Y 12.1
#define INITIAL_ODDS 1.0
#define MAX_SONAR_READING 32
#define BASE_SONAR_MODEL 5.0
#define SAMPLE_THRESHOLD 100
#define KNOWLEDGE_THRESHOLD 400.0
#define OCCUPATION_THRESHOLD_ODDS .5
#define DRIVE_STEP_SIZE 2.3
#define SERVO servo0
#define CM_PER_CLICK 1.6
#define WHEEL_RADIUS 25.6
#define AXLE_LENGTH 21.6
#define FORWARD 0
#define BACKWARD 1
#define RIGHT 2
#define LEFT 3
#define FORWARD_SPEED 50
#define BACKWARD_SPEED -50
#define INT_PTR_SIZE 4
#define PI 4*atan(1.0)

int SERVO_POSITIONS[] = {2, 23, 32, 3, 3};
float SONAR_ORIENTATIONS[] = {12.0, 12.0, 42.0, 2.0, 3.0};


#define L_MOTOR 3
#define R_MOTOR 2
#define G_MOTOR 1
#define LIGHT_SENSOR_L 16
#define LIGHT_SENSOR_L_IS_LEGO 0
#define LIGHT_SENSOR_R lego0
#define LIGHT_SENSOR_R_IS_LEGO 1
#define ENCODER_R lego1_counts
#define ENCODER_L lego3_counts
#define BUMP_L 12
#define BUMP_R 14


#define INITIAL_X 0.0
#define INITIAL_Y 0.0
#define INITIAL_DIRECTION 0.0

struct POS 
{
    float pos_x;
    float pos_y;
    float direction;
};

struct POS rp;

struct MAP_CELL 
{
    int col;
    int row;
};



void main()
{   
    int initLeftEncoder = getLeftEncoder();
    int initRightEncoder = getRightEncoder();
    
    rp.pos_x = INITIAL_X;
    rp.pos_y = INITIAL_Y;
    rp.direction = INITIAL_DIRECTION;
    
    testDrive();
}

void testDrive() {
    sleep( 5.0 );
    
    //turn( (float)PI/2.0 );
    //drive( 50.0 );
    
    while( 1 ) {
        int lEnc = getLeftEncoder();
        int rEnc = getRightEncoder();
        printf( "L Encoder = %d\n", getLeftEncoder() );
        sleep( 1.0 );
        printf( "R Encoder = %d\n", getRightEncoder() );
        sleep( 1.0 );
        printf( "POS_X = %f\n", rp.pos_x );
        sleep( 1.0 );
        printf( "POS_Y = %f\n", rp.pos_y );
        sleep( 1.0 );
        printf( "DIR = %f\n", rp.direction );
        sleep( 1.0 );
        //updatePOS();
        updatePOS(lEnc, rEnc);
    }
    
}

void drive( float centimeters ) {
    int initRightEncoder = getRightEncoder();
    int initLeftEncoder = getLeftEncoder();
    
    while( ( (float)( getRightEncoder() - initRightEncoder ) * CM_PER_CLICK <= centimeters ) && 
          ( (float)( getLeftEncoder() - initLeftEncoder ) * CM_PER_CLICK <= centimeters ) ) {
        
        go( FORWARD );
    }
    stop();
    
    updatePOS( initLeftEncoder, initRightEncoder );    
}

void turn( float radians ) {
    int initLeftEncoder = getLeftEncoder();
    int initRightEncoder = getRightEncoder();
    
    while( ( ( (float)( (getRightEncoder() - initRightEncoder) - (getLeftEncoder() - initLeftEncoder)) * CM_PER_CLICK ) / AXLE_LENGTH ) <= radians ) {
        if( radians > 0.0 )
          go( LEFT );
        else
          go( RIGHT );
    }
    stop();
    
    updatePOS( initLeftEncoder, initRightEncoder );
    
}

void go( int direction ) {
    if( direction == FORWARD ) {
        motor(L_MOTOR, FORWARD_SPEED);
        motor(R_MOTOR, FORWARD_SPEED);
    } 
    else if( direction == BACKWARD ) {
          motor(L_MOTOR, BACKWARD_SPEED);
          motor(R_MOTOR, BACKWARD_SPEED);  
      }
      else if( direction == LEFT ) {
            motor(L_MOTOR, -30);
            motor(R_MOTOR, 30);        
        }
        else if( direction == RIGHT ) {
              motor(L_MOTOR, 30);
              motor(R_MOTOR, -30);
          }      
}



/*

void updatePOS( int initLeftEncoder, int initRightEncoder ) {
    float vL = (float)lego3_velocity*CM_PER_CLICK*-1.0;
    float vR = (float)lego1_velocity*CM_PER_CLICK;
    float t = 4.0;
    float initDirection = rp.direction;
    
    printf("%d l %d r\n", lego3_velocity, lego1_velocity);
    sleep(1.0);
    
    if (vL == vR) {
        rp.pos_x += vL*t*cos(rp.direction);
        rp.pos_y += vR*t*sin(rp.direction);
        return;
    } else {
        float R = (float)( AXLE_LENGTH * (vL + vR) ) / ( 2.0 * (vR - vL) );
        
        rp.direction += ( vR - vL )*t / AXLE_LENGTH;
        rp.pos_x += R * ( sin( (vR - vL)*t/AXLE_LENGTH + initDirection) - sin( initDirection ) );
        rp.pos_y -= R * ( cos( (vR - vL)*t/AXLE_LENGTH + initDirection) - cos( initDirection ) );          
    }    
}
*/


void updatePOS( int initLeftEncoder, int initRightEncoder ) {
    float leftWheel = (float)(getLeftEncoder() - initLeftEncoder);
    float rightWheel = (float)(getRightEncoder() - initRightEncoder);
    float initDirection = rp.direction;
    
    if (leftWheel == rightWheel) {
        rp.pos_x += CM_PER_CLICK*leftWheel*cos(rp.direction);
        rp.pos_y += CM_PER_CLICK*leftWheel*sin(rp.direction);
        return;
    } else {
        float R = (float)( AXLE_LENGTH * ( leftWheel + rightWheel ) ) / ( 2.0 * ( rightWheel - leftWheel ) );
        
        rp.direction += ( rightWheel - leftWheel ) * CM_PER_CLICK / AXLE_LENGTH;
        rp.pos_x += R * ( sin(rp.direction) - sin( initDirection ) );
        rp.pos_y -= R * ( cos(rp.direction) - cos( initDirection ) );          
    }    
}

/*
void updatePOS( ) {
//    float initDirection = rp.direction;
    
    float Vr = (float) getRightEncoder() * CM_PER_CLICK;
    float Vl = (float) getLeftEncoder() * CM_PER_CLICK;

    float R = (float)( AXLE_LENGTH * ( Vr + Vl ) ) / ( 2.0 * ( Vr - Vl ) );
    
    
    rp.direction = ( Vr - Vl ) * CM_PER_CLICK / AXLE_LENGTH;
    rp.pos_x = R * ( sin( rp.direction ) ); // - sin( initDirection ) );
    rp.pos_y = R * ( cos( rp.direction ) ); //- cos( initDirection ) );          
}
*/
void backup( float centimeters ) {
    int initRightEncoder = getRightEncoder();
    int initLeftEncoder = getLeftEncoder();
    
    while( ( (float)( getRightEncoder() - initRightEncoder ) * CM_PER_CLICK <= centimeters ) && 
          ( (float)( getLeftEncoder() - initLeftEncoder ) * CM_PER_CLICK <= centimeters ) ) {        
        go( BACKWARD );
    }
    stop();
    
    updatePOS( initLeftEncoder, initRightEncoder );
}

void stop() {
    int initLeftEncoder = getLeftEncoder();
    int initRightEncoder = getRightEncoder();
    
    motor(L_MOTOR, 0);
    motor(R_MOTOR, 0);
    
    sleep( 1.0 );
    
    //updatePOS( initLeftEncoder, initRightEncoder );
}


int getLeftLightSensor() {
    if (LIGHT_SENSOR_L_IS_LEGO == 1) {
        return LIGHT_SENSOR_L;
    } else {
        return analog(LIGHT_SENSOR_L);
    }
}

int getRightLightSensor() {
    if (LIGHT_SENSOR_R_IS_LEGO == 1) {
        return LIGHT_SENSOR_R;
    } else {
        
        return analog(LIGHT_SENSOR_R);
    }
}

int getLeftEncoder() {
    return -ENCODER_L;
}

int getRightEncoder() {
    return ENCODER_R;
}

#if 0

void primitiveForward() {
    motor(L_MOTOR, 50);
    motor(R_MOTOR, 50);
    sleep(1.0);
    stop();
}

void primitiveBackward() {
    motor(L_MOTOR, -50);
    motor(R_MOTOR, -50);
    sleep(1.0);
    stop();
}

void openGripper() {
    motor(G_MOTOR, 20);
    sleep(0.3);
    motor(G_MOTOR, 0);
}

void closeGripper() {
    motor(G_MOTOR, -20);
    sleep(0.3);
    motor(G_MOTOR, 0);    
}


void convertPosToCell(struct POS* p, struct MAP_CELL* retval) 
{
    (*retval).col = GLOBAL_COLS * (int)((*p).pos_x / MAXIMUM_X);
    retval.row = GLOBAL_ROWS * (int)((*p).pos_y / MAXIMUM_Y);
    if ((*retval).col  < 0)
      (*retval).col = 0;
    
    if ((*retval).col >= GLOBAL_COLS)
      (*retval).col = GLOBAL_COLS - 1;
    
    if ((*retval).row < 0)
      (*retval).row = 0;
    
    if ((*retval).row >= GLOBAL_ROWS)
      (*retval).row = GLOBAL_ROWS - 1;
}

void navigator(struct POS* position, float probability[][])
{
    int i,j;
    int north, south, east, west;
    struct MAP_CELL current;
    
    /* 
   * Create and initialize the array that keeps track of whether the robot has 
   * visited a cell or not
   */
    int visited[GLOBAL_COLS][GLOBAL_ROWS];
    
    /*
   * Check the direction, and if necessary, orient the robot so that it's 
   * pointing in an axis direction
   */
    if((*position).direction > 15 && (*position).direction <= 45)
      /* turn so that it points at 0 */
      printf("Turn\n");
    else if((*position).direction > 45 && (*position).direction <= 75)
        /* turn so that it points at 90 */
        printf("Turn\n");
      else if((*position).direction > 105 && (*position).direction <= 135)
          /* turn so that it points at 90 */
          printf("Turn\n");
        else if((*position).direction > 135 && (*position).direction <= 165)
            /* turn so that it points at 180 */
            printf("Turn\n");
          else if((*position).direction > 195 && (*position).direction <= 225)
              /* turn so that it points at 180 */
              printf("Turn\n");
            else if((*position).direction > 225 && (*position).direction <= 255)
                /* turn so that it points at 270 */
                printf("Turn\n");
              else if((*position).direction > 285 && (*position).direction <= 315)
                  /* turn so that it points at 270 */
                  printf("Turn\n");
                else if((*position).direction > 315 && (*position).direction <= 345)
                    /* turn so that it points at 0 */
                    printf("Turn\n");
                  
                  /*
   * Now direction should be pointing at an axis, so get the position, 
   * convert it to a cell, then using the direction check left, ahead, and 
   * right for the lowest probability of cells that we have not visited
   */
                  
                  current = convertPosToCell(position);
    if(current.col = 0)
      {
        // don't look left
        west = 0;
    }
    if(current.col = GLOBAL_COLS - 1)
      {
        // don't look right
        east = 0;
    }
    if(current.row = 0)
      {
        // don't look down
        south = 0;
    }
    if(current.row = GLOBAL_ROWS - 1)
      {
        // don't look up
        north = 0;
    }
}


/*
 * buildMap
 * input position
 *       m (generated Map)
 */
void buildMap(struct POS* p, float** m) {
    int i, j;
    float m[GLOBAL_COLS][GLOBAL_ROWS];
    int samples[GLOBAL_COLS][GLOBAL_ROWS]; 
    
    // initialize servos
    init_expbd_servos(1);
    
    // initialize map and number of samples
    
    
    for (i = 0; i < GLOBAL_COLS; i++) {
        for (j = 0; j < GLOBAL_ROWS; j++) {
            m[i][j] = INITIAL_ODDS;
            samples[i][j] = 0;
        }
    }
    
    while (builtSufficientMap(m, samples) == 0) {
        sonarSweep(p, m, samples);
        executeNextMove(p, m);
    }
    convertOddsTableToProbabilities(m);
}

void convertOddsTableToProbabilities(float m[][]) {
    int i, j;
    for (i = 0; i < GLOBAL_COLS; i++) {
        for (j = 0; j < GLOBAL_ROWS; j++) {
            m[i][j] = oddsToProbability(m[i][j]);
        }
    }
}

void sonarSweep(struct POS* p, float m[][], int samples[][]) {
    int i;
    
    struct MAP_CELL robotCell = convertPosToCell(*p);
    updateOdds(m, samples, robotCell, 0.1);
    
    for (i = 0; i < _array_size(SERVO_POSITION); i++) {
        int dist;
        struct POS samplePos;
        struct MAP_CELL sampleCell;
        float newOdds;
        float sOrientation;
        
        servo0 = SERVO_POSITIONS[i];
        sOrientation = SONAR_ORIETATION[i] - (*p).direction;
        
        dist = sonar_sample();
        if (dist < MAX_SONAR_SAMPLE) {
            samplePos = calcSamplePos(p, (float)dist, sOrientation);
            sampleCell = convertPosToCell(&samplePos);
            newOdds = sonarModel((float)dist);
            updateOdds(m, samples, samplePos, newOdds);
        }
        updateIntermediateNodes(m, samples, *p, sOrientation, dist);
    }
}

void updateIntermediateNodes(float** m,
                             int** samples,
                             struct POS robotPos,
                             float sOrientation,
                             int dist) {
    struct POS samplePos = calcSamplePos(p, (float) dist, SOrientation);
    float newOdds = 1.0 / sonarModel((float) dist);
    float stepSize;
    struct POS curr;
    struct MAP_CELL currMapCell;
    struct MAP_CELL robotMapCell = convertCellToPos(robotPos);
    struct MAP_CELL sampleMapCell = convertCellToPos(samplePos);
    
    if (GLOBAL_COLS / MAXIMUM_X < GLOBAL_ROWS / MAXIMUM_Y) {
        stepSize = 0.1*GLOBAL_COLS / MAXIMUM_X;
    } else {
        stepSize = 0.1*GLOBAL_ROWS / MAXIMUM_Y;
    }
    curr.pos_x = robotPos.pos_x;
    curr.pos_y = robotPos.pos_y;
    currMapCell = convertPosToCell(curr);
    
    while (!(currMapCell.col == sampleMapCell.col &&
             currMapCell.row == sampleMapCell.row)) {
        struct MAP_CELL currMapCellTmp = currMapCell;
        while (currMapCellTmp.col == currMapCell.col &&
               currMapCellTmp.row == currMapCell.row) {
            float distanceMoved;
            
            curr.pos_x += stepSize*myCos(sOrientation);
            curr.pos_y += stepSize*mySin(sOrientation);
            currMapCellTmp = convertPosToCell(curr);
            distanceMoved = sqrt((curr.pos_x - robot.pos_x)*(curr.pos_x - robot.pos_x)+(curr.pos_y - robot.pos_y)*(curr.pos_y - robot.pos_y));
            if (distanceMoved > (float) dist) {
                return;
            }
        }
        currMapCell = currMapCellTmp;
        updateOdds(m, samples, currMapCell, newOdds);
    }
}

float myCos(float theta) {
    return 0.5;
}

float mySin(float theta) {
    return 0.5;
}

float sonarModel(float distance) {
    return BASE_SONAR_MODEL -
      (distance / MAX_SONAR_READING)*
      (BASE_SONAR_MODEL/2);
}

struct POS calcSamplePos(struct POS currPos, float dist, float orientation) {
    struct POS retval;
    
    retval.pos_x = currPos.pos_x + dist*myCos(orientation);
    retval.pos_y = currPos.pos_y + dist*mySin(orientation);
    return retval;
}

int builtSufficientMap(double** m, int** samples) {
    int totalSamples = 0;
    float knowledge = 0.0;
    int i, j;
    
    for (i = 0; i < GLOBAL_COLS; i++) {
        for (j = 0; j < GLOBAL_ROWS; j++) {
            totalSamples += samples[i][j];
        }
    }
    
    for (i = 0; i < GLOBAL_COLS; i++) {
        for (j = 0; j < GLOBAL_ROWS; j++) {
            float p = oddsToProbability(m[i][j]);
            knowledge += (p - 0.5)*(p - 0.5);
        }
    }
    
    if (knowledge > KNOWLEDGE_THRESHOLD || totalSamples > SAMPLE_THRESHOLD) {
        return 0;
    }
    return 1;
}

float oddsToProbability(float odds) {
    return odds / (1.0 + odds);
}

void executeNextMove(struct POS* p, double** m) {
    int i;
    struct MAP_CELL straight;
    
    straight = nextCell(*p, (*p).direction);
    if (m[straight.col][straight.row] < OCCUPATION_THRESHOLD_ODDS) {
        driveStraight(DRIVE_STEP_SIZE);
        return;
    }
    
    for (i = 1; i < 4; i ++) {
        struct MAP_CELL possibility;
        
        possibility = nextCell(*p, (*p).direction+i*90);
        if (m[possibility.col][possibility.row] < OCCUPATION_THRESHOLD_ODDS) {
            if (i == 1 || 2) {
                turnLeft(i*90);
            } else {
                turnRight(90);
            }
            driveStraight(DRIVE_STEP_SIZE);
            return;
        }
    }
    driveStraight(DRIVE_STEP_SIZE);
}


struct MAP_CELL nextCell(struct POS curr, float orientation) {
    struct MAP_CELL currMapCell, currMapCellTmp;
    float maxDist;
    convertPosToCell(&curr, &currMapCell);
    currMapCellTmp.col = currMapCell.col;
    currMapCellTmp.row = currMapCell.row;
    maxDist = sqrt((MAXIMUM_X / (float)GLOBAL_COLS)*(MAXIMUM_X / (float)GLOBAL_COLS) +
                   (MAXIMUM_Y / (float)GLOBAL_ROWS)*(MAXIMUM_Y / (float)GLOBAL_ROWS));
    
    while (currMapCellTmp.col == currMapCell.col &&
           currMapCellTmp.row == currMapCell.row) {
        float distanceMoved;
        
        curr.pos_x += stepSize*myCos(sOrientation);
        curr.pos_y += stepSize*mySin(sOrientation);
        currMapCellTmp = convertPosToCell(curr);
        distanceMoved = sqrt((curr.pos_x - robot.pos_x)*(curr.pos_x - robot.pos_x)+(curr.pos_y - robot.pos_y)*(curr.pos_y - robot.pos_y));
        if (distanceMoved > maxDist) {
            currMapCellTmp.pos_x = -1.0;
            currMapCellTmp.pos_y = -1.0;
            currMapCellTmp.direction = -1.0;
            break;
        }
    }
    return currMapCellTmp;
}


void updateOdds(float m[][],
                int samples[][],
                struct MAP_CELL p,
                float newEvidence) {
    m[p.col][p.row] *= newEvidence;
    samples[p.col][p.row]++;
}




int sonar_sample() {
    
    int start_time;
    int dist;
    
    poke(0x1023, 1); /* clear tic3 flag */
    
    start_time= peekword(0x100e); /* capture start time */
    
    bit_set(0x1000, 0x80); /* send init pulse */
    bit_clear(0x1000, 0x80);
    
    while (peek(0x1000) & 0x1) { /* wait until receive echo */
        if ((peekword(0x100e) - start_time) < 0) {
            /* if too much time has elapsed, abort */
            return -1;
        }
        defer(); /* let others run while waiting */
    }
    
    msleep(10L); /* give unit time to reset */
    
    dist= peekword(0x1014)-start_time;
    
    return dist; /* tic3 has time of echo */
}

#endif