Drive Configurator and Updater

/* setpointConfigurator()
 *
 * IN:  - Direc is the decision variable defining left (Direc==0), straight (Direc==1), right(Direc==2)
 *
 * ACT: setPointConfigurator() selects a setpoint from the list of setpoints in setpointPath which is 
 *      not reached yet (setpointPath[i][2]==0) and skips over reached setpoints (setpointPath[i][2]==1)
 *
 * OUT: - sets the new intermediate marker to reach the end setpoint in setpointPath[n-1][2]
 */

go_to setpointConfigurator(int Direc)
{
    go_to setpoint;
    int index =setpointPath.size()-1;
    // find next setpoint to work on

    for(int i = 0;i<setpointPath.size();i++)
    {
        if(setpointPath[i][2] == 0)
        {
            index = i;
            i = setpointPath.size();
        }
    }

    // Dealing with left setpoint
    if(Direc==0)
    {
        setpoint.x = setpointPath[index][1];
        setpoint.y = setpointPath[index][0];
        xPoint = setpointPath[index][1];
        yPoint = setpointPath[index][0];
    }
    else
    {
        setpoint.x = setpointPath[index][1];
        setpoint.y = -setpointPath[index][0];
        xPoint = setpointPath[index][1];
        yPoint = -setpointPath[index][0];
    }

    return setpoint;
}

/* setPointUpdater()
 *
 * IN:  - Direc is the decision variable defining left (==0), straight (==1), right(==2)
 *      - global go_to setpoint when setpoint is updated using odom_data
 *      - emc::LRFdata scan
 *      - emc::Odometry odom
 *
 * ACT: setPointUpdater corrects the setpoint based on its current position
 *      and traveled distance from the global marker.
 *
 * OUT: - gives the new setpoint if nothing weird happens
 *      - else it gives setpoint at zero so the robot no longer drives
 *
 */

go_to setPointUpdater(int Direc, go_to globalSetPoint, emc::OdometryData &odom, double* straight_data)
{
    go_to setpoint;
    int index;

    // left == 0
    if(Direc == 0)
    {
        double r = sqrt(pow(xPoint,2)+pow(yPoint,2));
        double theta1 = atan2(yPoint,xPoint);

        double deltaSy;
        double deltaSx;

        // Incremental change in x and y of the setpoint
        if (fmod(fabs(counterLeft-counterRight),2)==0)
        {
            deltaSy = fabs(absMarkerX-odom.x);
            deltaSx = fabs(absMarkerY-odom.y);
        }
        else
        {
            deltaSy = fabs(absMarkerX-odom.y);
            deltaSx = fabs(absMarkerY-odom.x);
        }

        double dthetaa = -absMarkerA+odom.a;

        if (deltaSx==0)
        {
            return globalSetPoint;
        }
        else
        {
            // calculate preliminaries
            double dtheta = atan2(deltaSy,deltaSx);
            double rdelta =  sqrt(pow(deltaSx,2)+pow(deltaSy,2));
            double theta = -dtheta+theta1;

            // calculate r
            double rperp = rdelta*sin(theta);
            double rL = r-rdelta*cos(theta);
            double theta4 = atan2(rperp,rL);
            double rt = rL/cos(theta4);

            // calculate theta2
            double theta2 = dtheta-dthetaa+theta+theta4;

            // update setpoint
            globalSetPoint.x = rt*cos(theta2);
            globalSetPoint.y = rt*sin(theta2);

            return globalSetPoint;
        }
    }
    // straight == 1
    else if(Direc == 1)
        {
            double delta_y = (straight_data[2] + sin(straight_data[3])) ;

            setpoint = driveStraightInitializer();
            setpoint.y = delta_y;

            return setpoint;
        }
    // right == 2
    else if(Direc == 2)
    {
        double r = sqrt(pow(xPoint,2)+pow(yPoint,2));
        double theta1 = atan2(fabs(yPoint),xPoint);

        double deltaSy;
        double deltaSx;

        // Incremental change in x and y of the setpoint
        if (fmod(fabs(counterLeft-counterRight),2)==0)
        {
            deltaSy = fabs(absMarkerX-odom.x);
            deltaSx = fabs(absMarkerY-odom.y);
        }
        else
        {
            deltaSy = fabs(absMarkerX-odom.y);
            deltaSx = fabs(absMarkerY-odom.x);
        }

        double dthetaa = -absMarkerA+odom.a;

        if (deltaSx==0)
        {
            return globalSetPoint;
        }
        else
        {
            // calculate preliminaries
            double dtheta = atan2(deltaSy,deltaSx);
            double rdelta =  sqrt(pow(deltaSx,2)+pow(deltaSy,2));
            double theta = -dtheta+theta1;

            // calculate r
            double rperp = rdelta*sin(theta);
            double rL = r-rdelta*cos(theta);
            double theta4 = atan2(rperp,rL);
            double rt = rL/cos(theta4);

            // calculate theta2
            double theta2 = -(dtheta+dthetaa+theta+theta4);

            // update setpoint
            globalSetPoint.x = rt*cos(theta2);
            globalSetPoint.y = rt*sin(theta2);

            return globalSetPoint;
        }
    }
    // something else happened
    else
    {
        setpoint.x = 0;
        setpoint.y = 0;
        return setpoint;
    }
}