//round values
int round(float val) {
    // rounds val to the nearest integer
    if (val < 0) {
        return val - 0.5;
    }
    else {
        return val + 0.5;
    }
}

//function for converting degrees into Radians
float Deg2Rad(float angleDeg) {

    float angleRad;

    angleRad = (angleDeg * PI) / 180; //convert degrees to radians

    return angleRad; //returns the angle in radians

}

//function for converting radians to degrees
float Rad2Deg(float angleRad) {

    float angleDeg;

    angleDeg = (180 * angleRad) / PI; //convert angle from radians to degrees

    return angleDeg;

}

//Quadratic Formula function
float Quadratic(float a, float b, float c, float plusOrMinusOne) {

    float num, denom, root;

    num = -b + plusOrMinusOne * sqrt(pow(b,2) - 4 * a * c);
    denom = 2 * a;

    if (num < 0) {
        root = num / denom;
    }

    else {
        Serial.println("Imaginary numbers are not allowed");
    }

    return root;

}

//calculate the distance a projectile would land at a specific angle
float LandingDistance(float d[], float v0, float thetaL) {

    float x0, y0, v0x, v0y, g, gInQuad, tLand, xLand, radThetaL;

    g = 9.81;
    gInQuad = g * -0.5;
    radThetaL = Deg2Rad(thetaL);

    x0 = d[1] * cos(radThetaL) - d[2] * sin(radThetaL);
    y0 = d[0] + d[1] * sin(radThetaL) + d[2] * cos(radThetaL);
    v0x = v0 * cos(radThetaL);
    v0y = v0 * sin(radThetaL);

    tLand = Quadratic(gInQuad, v0y, y0, -1);
    xLand = x0 + v0x * tLand;

    return xLand;

}

//function for finding the optimum angle for the furthest distance
float RangeAngle(float d[], float v0) {

    float range, rangeAngle, xLand,thetaL;

    //create variables that can hold new values
    rangeAngle = 0;
    thetaL = 25;
    range = 0;
    xLand = LandingDistance(d, v0, thetaL);

    while ((thetaL >= 25) && (thetaL <=85)) {

        //call LandingDistance to get the optimal distance
        xLand = LandingDistance(d, v0, thetaL);


        // make sure xLand doesn't go out of bounds
        if(xLand > range) {
            range = xLand;
            rangeAngle = thetaL;

        }

        //increment thetaL
        thetaL = thetaL + 0.05;


    }

    return rangeAngle;

}

float LaunchAngle(float d[], float v0) {

    float thetaL, xLand, xTarget, range;
    thetaL = RangeAngle(d, v0);
    xTarget = 0;
    xLand = LandingDistance(d, v0, thetaL);
    range = 1.30; // in meters


    while (xLand > xTarget){

        xLand = LandingDistance(d, v0, thetaL);
        thetaL = thetaL + 0.05;

        if (xLand <= range) {
            xTarget = xLand;
        }




        return thetaL;
    }

}

//arc tangant function
float atan(float x) {
    // this atan approx is valid ONLY for 0 <= x <= 2

    float y;
    if (x <= 1 && x >= 0 ) {
        float C[] = { -0.0606027, -0.197128, 1.04349, -0.00204033};
        y = C[0]*pow(x,3.0) + C[1]*pow(x,2.0) + C[2]*x + C[3];
    }
    else if (x <= 2 && x > 1) {
        float C[] = {0.0562944, -0.398913, 1.12442, 0.00381280};
        y = C[0]*pow(x,3.0) + C[1]*pow(x,2.0) + C[2]*x + C[3];
    }
    else {
        Serial.println("*** WARNING!!! ***");
        Serial.println("This atan function is only good for 0 <= x <= 2.");
        Serial.println("Your value of x was outside of this range,");
        Serial.println("so the function is returning a garbage value!");
    }
    return y;
}

// get the servo angle
float ThetaServo(float H[], float thetaL, float alpha, float beta, float thetaL0) {

    float theta2, theta4, thetaS, a, b, c, K1, K2, K3;
    float rTheta2, inAtanTop, inAtanBot, inAtan, dTheta4;


    //get theta2
    theta2 = thetaL - thetaL0;
    rTheta2 = Deg2Rad(theta2);

    K1 = H[0]/H[1];
    K2 = H[0]/H[3];
    K3 = (pow(H[0], 2) + pow(H[1],2) - pow(H[2],2) + pow(H[3],2)) / (2 * H[1] * H[3]);

    a = cos(rTheta2) - K1 - K2*cos(rTheta2) + K3;
    b = -2 * sin(rTheta2);
    c = K1 - (K2 + 1) * cos(rTheta2) + K3;

    inAtanTop = -b - sqrt(pow(b,2) - 4 * a * c);
    inAtanBot = 2 * a;
    inAtan = inAtanTop / inAtanBot;

    theta4 = 2 * atan(inAtan);
    dTheta4 = Rad2Deg(theta4);


    thetaS = (dTheta4 + alpha) / (1 - beta);


    return thetaS;
}


void TargetServoAngles(float d[], float v0, float H[], float alpha, float beta, float thetaL0, float xTargetVec []) {

    int i;
    float thetaS[6], thetaL[6];

    for ( i = 0; i < 6; i++) {


        Serial.print("Computing target ");
        Serial.println(i + 1);

        thetaL[i] = LaunchAngle(d,v0);
        thetaS[i] = ThetaServo(H,thetaL[i],alpha,beta,thetaL0);
        writeToServo = round(thetaS[i]);

        Serial.print("For a target of ");
        Serial.print(xTargetVec[i]);
        Serial.print(", the launch angle is ");
        Serial.print(thetaL[i]);
        Serial.print(", and the servo angle is ");
        Serial.println(writeToServo);

    }

    return;
}