File content as of revision 2:d2ac643a08f6:

//INCLUDES
#include "Algorithm.h"

//R1Angle
float Algorithm::R1Angle(float th, float a)
{   
    return -tan(PI/2-2*a+th)*(REFLECTOR1_DIST+REFLECTOR1_LENGTH/2*cos(a))+
    PANEL_HEIGHT-REFLECTOR1_HEIGHT+REFLECTOR1_LENGTH/2*sin(a);
}

//R2Angle
float Algorithm::R2Angle(float th, float a)
{
    return tan(PI/2-2*a-th)*(-REFLECTOR2_DIST-REFLECTOR2_LENGTH/2*cos(a))+
    PANEL_HEIGHT-REFLECTOR2_HEIGHT+REFLECTOR2_LENGTH/2*sin(a);
}

//calcAngle
float Algorithm::calcAngle(int n, float th)
{
    float err,ap,a,an,af;
    //Initial conditions
    a = 0.1;
    an = PI/4;
    
    th *= PI/180; //Convert to rad
    
    switch(n)
    {
        case 1:
        for(int i = 0;i < LOOP_LIMIT;i++)
        {
            ap = a - Algorithm::R1Angle(th,a)*(a-an)/(Algorithm::R1Angle(th,a)-Algorithm::R1Angle(th,an));
            an = a;
            a = ap;
            err = ((ap-an) > 0)?(ap-an):(an-ap);
            
            if(err <= ERR_LIMIT)
            {
                break;
            }
        }
        af = ((ap+an)/2)*180/PI; //Convert to degrees
        af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)? REFLECTOR1_HIGH : af;
        break;
        
        case 2:
        for(int i = 0;i < LOOP_LIMIT;i++)
        {
            ap = a - Algorithm::R2Angle(th,a)*(a-an)/(Algorithm::R2Angle(th,a)-Algorithm::R2Angle(th,an));
            an = a;
            a = ap;
            err = ((ap-an) > 0)?(ap-an):(an-ap);
            
            if(err <= ERR_LIMIT)
            {
                break;
            }
        }
        af = ((ap+an)/2)*180/PI; //Convert to degrees
        af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)? REFLECTOR2_HIGH : af;
        break;
    }
    
    
    return af; 
}