/*
Cube Tester
Hans Andersson 2008
www.tiltedtwister.com

^^^^^^^^ REVISIONE (TestCuboV1.1) - Padovan Valter - ITALY - Feb.2009
^^^^^^^^ http://digilander.libero.it/Valter_NXT/

The program is optimized for using 6 x 1.2V rechargeable batteries
If using 1.5V batteries, the functions Tilt() and CheckBatteries() must be adjusted

Inputs:
1 Touch sensor
2 Light sensor
3 Ultrasonic sensor

Outputs:
A Center motor
B Tilter arm motor
C Light sensor motor
*/

#include "NXCDefs.h"

#define LEFTFACE      0
#define FRONTFACE     1
#define RIGHTFACE     2
#define BACKFACE      3
#define UPPERFACE     4
#define DOWNFACE      5

#define MINBATTERYLEVEL      6000      // ^^^^^^^ Riga aggiunta
#define MAXBATTERYLEVEL      8400      // ^^^^^^^ Riga aggiunta

int min[6];
int max[6];


sub Turn(int num)
{
  if (num==3)
    RotateMotor(OUT_A,100,-315);
  else
    RotateMotor(OUT_A,100,315*num);
}


sub Tilt()
{
  // This function is optimized for using 6 x 1.2V rechargeable batteries
  // If using 1.5V batteries, it has to be adjusted
  RotateMotorEx(OUT_B,50,40,false,false,false);
  RotateMotorEx(OUT_B,80,8,false,false,false);
  RotateMotorEx(OUT_B,100,7,false,false,false);
  RotateMotorEx(OUT_B,40,10,false,false,true);
  RotateMotor(OUT_B,30,-65);
}


int ReadColor(int face)
{
  int light;
  light=Sensor(IN_2);
  if(light < min[face])
    min[face]=light;
  if(light > max[face])
    max[face]=light;
  return light;
}


sub TurnNoStop()
{
    RotateMotorEx(OUT_A,100,315,false,false,false);
}

// NB aggiunta tutta la SUB
sub InitPositionLight()                            // ^^^^^^^ Riga aggiunta
{                                                  // ^^^^^^^ Riga aggiunta
  OnFwd(OUT_C,10);                                 // ^^^^^^^ Riga aggiunta
  until(SENSOR_1 == 0);                            // ^^^^^^^ Riga aggiunta
  Off(OUT_C);                                      // ^^^^^^^ Riga aggiunta
  OnRev(OUT_C,10);                                 // ^^^^^^^ Riga aggiunta
  until(SENSOR_1 == 1);                            // ^^^^^^^ Riga aggiunta
  Off(OUT_C);                                      // ^^^^^^^ Riga aggiunta
  RotateMotor(OUT_C,10,26);                        // ^^^^^^^ Riga aggiunta
}                                                  // ^^^^^^^ Riga aggiunta

sub ScanFace(int face, string title)
{
  int light;
  TextOut(20,LCD_LINE2,title,true);
//Center
  RotateMotor(OUT_C,20,110);            // ^^^^^^^ Riga modificata (era 125°)
  light = ReadColor(face);
  min[face]=light;
  max[face]=light;
  NumOut(40,LCD_LINE5,light);
//Edges
  RotateMotor(OUT_C,20,-25);            // ^^^^^^^ Riga modificata (era -17°)
  RotateMotor(OUT_A,80,35);
  light = ReadColor(face);
  NumOut(40,LCD_LINE6,light);
  TurnNoStop();
  light = ReadColor(face);
  NumOut(10,LCD_LINE5,light);
  TurnNoStop();
  light = ReadColor(face);
  NumOut(40,LCD_LINE4,light);
  Turn(1);
  light = ReadColor(face);
  NumOut(70,LCD_LINE5,light);
//Corners
  RotateMotor(OUT_C,20,-8);
  RotateMotor(OUT_A,80,160);
  light = ReadColor(face);
  NumOut(70,LCD_LINE6,light);
  TurnNoStop();
  light = ReadColor(face);
  NumOut(10,LCD_LINE6,light);
  TurnNoStop();
  light = ReadColor(face);
  NumOut(10,LCD_LINE4,light);
  Turn(1);
  light = ReadColor(face);
  NumOut(70,LCD_LINE4,light);
  RotateMotor(OUT_C,20,-60);           // ^^^^^^^ Riga modificata (era -100°)
  RotateMotor(OUT_A,80,120);
  TextOut(0,LCD_LINE8,"PRESS ORANGE BTN");
  while(ButtonPressed(BTNCENTER,false)==0);
}


sub ScanCube()
{
  SetSensorTouch(IN_1);
  SetSensorLight(IN_2);
  SetSensorMode(IN_2,SENSOR_MODE_RAW);

  //N.B. Tolto tutto il codice messo nella SUB InitPositionLight

  InitPositionLight();          // ^^^^^^^ Riga aggiunta
  ScanFace(LEFTFACE,"LEFT FACE");
  Tilt();
  Turn(1);
  InitPositionLight();          // ^^^^^^^ Riga aggiunta
  ScanFace(BACKFACE,"BACK FACE");
  Tilt();
  Turn(1);
  InitPositionLight();          // ^^^^^^^ Riga aggiunta
  ScanFace(RIGHTFACE,"RIGHT FACE");
  Turn(3);
  Tilt();
  Turn(1);
  InitPositionLight();          // ^^^^^^^ Riga aggiunta
  ScanFace(DOWNFACE,"DOWN FACE");
  Turn(1);
  Tilt();
  InitPositionLight();          // ^^^^^^^ Riga aggiunta
  ScanFace(FRONTFACE,"FRONT FACE");
  Turn(1);
  Tilt();
  InitPositionLight();          // ^^^^^^^ Riga aggiunta
  ScanFace(UPPERFACE,"UPPER FACE");

  SetSensorType(IN_2,SENSOR_TYPE_NONE);
}


sub DisplayFace(int lcdLine,string label,int face)
{
  TextOut(0,lcdLine,label);
  NumOut(40,lcdLine,min[face]);
  TextOut(65,lcdLine,"-");
  NumOut(75,lcdLine,max[face]);
}


bool CubeOK()
{
  for(int i=0;i<6;i++)
    for(int j=0;j<6;j++)
      if(i<>j)
        if(min[i]>=min[j] && min[i]<=max[j] || max[i]>=min[j] && max[i]<=max[j])
          return false;
  return true;
}


sub ValidateCube()
{
  ClearScreen();
  DisplayFace(LCD_LINE1,"Upper",UPPERFACE);
  DisplayFace(LCD_LINE2,"Left",LEFTFACE);
  DisplayFace(LCD_LINE3,"Front",FRONTFACE);
  DisplayFace(LCD_LINE4,"Right",RIGHTFACE);
  DisplayFace(LCD_LINE5,"Back",BACKFACE);
  DisplayFace(LCD_LINE6,"Down",DOWNFACE);
  if(CubeOK())
    TextOut(0,LCD_LINE8,"YES, CUBE IS OK!");
  else
    TextOut(0,LCD_LINE8,"SORRY, NOT OK!");
  while(true);
}

sub CheckBatteries()
{
  TextOut(0,LCD_LINE2,"BATTERY =  . V",true);
  NumOut(60,LCD_LINE2,BatteryLevel() / 1000,false);
  NumOut(70,LCD_LINE2,(BatteryLevel() / 100)%10,false);
  if( BatteryLevel() > MAXBATTERYLEVEL || BatteryLevel() < MINBATTERYLEVEL)// ^^^^^^^ Riga modificata
  {
    TextOut(0,LCD_LINE4,"REQUIRED = 6-8V",false);
    TextOut(0,LCD_LINE6,"CHECK BATTERIES ",false);
    TextOut(0,LCD_LINE8,"USE 6*1.2V NiMH ",false);
    while(true);
  }
  else
  {
    TextOut(20,LCD_LINE5,"GIVE ME A");
    TextOut(15,LCD_LINE7,"SOLVED CUBE !");
  }
}


sub WaitForCube()
{
  CheckBatteries();
  PlayFile("Hello.rso");
  Wait(1000);
  SetSensorLowspeed(IN_3);
  SensorUS(IN_3);
  while(true)
  {
    if(SensorUS(IN_3) < 10)
    {
      Wait(1000);
      if(SensorUS(IN_3) < 10)
        break;
    }
  }
  TextOut(20,LCD_LINE5,"THANK YOU !",true);
  PlayFile("Thank You.rso");
  Wait(2000);
}


task main()
{
  WaitForCube();
  ScanCube();
  ValidateCube();
}