#define COLORSENSOR IN_2
#define ROTATORPORT OUT_C
#define LOADERPORT  OUT_B

#define ROTATORPOWER 100
#define LOADERPOWER 100

mutex motorMutex;

//Rotate the Rotator to a given angle
void RotatorTo(long position) {
    //Calculate delta rotation from current rotation value
    long rotator = position - MotorRotationCount(ROTATORPORT);
    
    //Only rotate if the position differs more than a given treshold
    if (abs(rotator) >= 45) {
        RotateMotor(ROTATORPORT, ROTATORPOWER, rotator);
    }
}

//Initialisation - reset rotation and enable color sensor
void init() {
    ResetRotationCount(ROTATORPORT);
    SetSensorColorFull(COLORSENSOR);
}

//Main task - initialisation
task main() {
    init();
}

//Shutdown procedure - rotate Rotator back to starting position
void shutdown() {
    Acquire(motorMutex);

    SetSensorColorRed(COLORSENSOR);
    RotatorTo(0);
    SetSensorColorGreen(COLORSENSOR);

    Wait(500);
    StopAllTasks();
}

//Get color value from sensor
inline int GetColor() {
    int colorval;
    unsigned int raw[];
    unsigned int norm[];
    int scaled[];
    ReadSensorColorEx(COLORSENSOR, colorval, raw, norm, scaled);
    return colorval;
}

//Task that checks if a new ball is available and sorts that to the correct bin
task colorWaiter() {
    Follows(main);
    
    int color;
    //Positions of the rotator for each color
    long rotatorPosition[] = {
            0,  //NO COLOR
            0,  //BLACK
        -1650,  //GREEN
         1650,  //BLUE
         -600,  //RED
          600,  //YELLOW
            0   //WHITE
    };
    
    int tryCount = 0;
    int failCount = 0;
    
    while (true) {
        //Wait until there is a ball available
        while((color = GetColor()) == 1) {
            tryCount++;

            //Every 20 iterations, move the loader mechanism to free a stuck ball
            if (tryCount > 20) {
                Acquire(motorMutex);
                RotateMotor(LOADERPORT, LOADERPOWER, 360);
                Release(motorMutex);
            }
            
            //Repeat this 5 times
            if (tryCount == 25) {
                tryCount = 0;
                failCount++;
            }
            
            //After 5 full loops - shutdown the program
            if (failCount == 5) {
                shutdown();
            }
            
            Wait(100);
        }

        //Ball available - rotate to the correct bin
        Acquire(motorMutex);
        
        RotatorTo(rotatorPosition[color]);
        
        //Activate loader mechanism. This is done twice so there is a slight
        //pause between moving back and forth, this allows the ball to drop
        RotateMotor(LOADERPORT, LOADERPOWER, 180);
        RotateMotor(LOADERPORT, LOADERPOWER, 180);
        Release(motorMutex);
        
        //Reset wait counters
        tryCount = 0;
        failCount = 0;
    }
}

//Wait until the orange button is pressed - then exit
task shutdownWaiter() {
    Follows(main);


    until (ButtonCount(BTNCENTER, false)) {
        Wait(100);
    }

    shutdown();
}