#include <iostream.h>
#include "WPILib.h"
#include <taskLib.h>
#include "VisionAPI.h"

const double    PI = 3.14159265358979323;           // for conversion factor below
const double    FEET_TO_GEARTOOTH = 96.0 / PI;      // conversion factor for converting from
                                                    // amt. of feet to geartooth count
const int       RIGHT = 1;
const int       LEFT = 2;
const float     SENS = 0.5;


//======================================================================================
//
// This is a demo program showing the use of the IterativeRobot base class.
//
// The IterativeRobot class is the base of a robot application that uses similar control
// flow to that used in the FRC default code in prior years.
//
// This robot provides identically the same external behavior as the SimpleRobot demo program.
//
// This code assumes the following connections:
//   Driver Station:
//     Joystick 1 - The "right" joystick.
//                  Used for either "arcade drive" or "right" stick for tank drive
//     Joystick 2 - The "left" joystick.  Used as the "left" stick for tank drive
//
//   Robot:
//     Digital Sidecar 1:
//       PWM 1/3 - Connected to "left" drive motor(s)
//       PWM 2/4 - Connected to "right" drive motor(s)
//       PWM 5 - Connected to "arm" drive motor
//======================================================================================
class IterativeDemo : public IterativeRobot
{
    RobotDrive*     myRobot;        // robot drive system
    Jaguar*         leftJag;        // override default RobotDrive with Jaguar controllers
    Jaguar*         rightJag;
    Joystick*       rightStick;     // joystick 1 (arcade stick or right tank stick)
    Joystick*       leftStick;      // joystick 2 (tank left stick)
    DriverStation*  ds;             // driver station object
    Gyro*           gyro;           // gyro sensor
    GearTooth*      geartooth;      // geartooth sensor
    Compressor*     compressor;
    Relay*          ramprelease;    // solenoid for ramp release
    Relay*          rightramp;      // right ramp up&down
    Relay*          leftramp;       // left ramp up&down
    Ultrasonic*		ultra_sensor;	// ultrasonic sensor

    enum JUMPERS {                  // Driver Station jumpers to control program operation
        ARCADE_MODE = 1,            // Tank/Arcade (Jumper => arcade)
        DISABLE_AUTONOMOUS = 2,     // Jumper => skip autonomous code
        GYRO_RESET = 3,             // Jumper resets gyro
        AUTO_TURN_LEFT = 4          // Which way to turn at end of first autonomous move
                                    // Jumper => left (CCW), otherwise turn right (CW)
    };

    // Enumerator for keeping track of which autonomous state robot is in
    enum AUTO_STATE {
        AUTO_INIT = 1,      // Haven't started yet
        AUTO_FIRST_MOVE,    // Making initial move down the field
        AUTO_TURNING_LEFT,  // Make the 90 degree turn CCW
        AUTO_TURNING_RIGHT, // Make the 90 degree turn CW
        AUTO_SHORT_FWD,     // Forward portion of zig-zag routine
        AUTO_SHORT_BCK,		// Backward portion of zig-zag routine
        AUTO_DONE
    } auto_state;

	// Local variables to count the number of periodic loops performed
	int auto_periodic_loops;
	int disabled_periodic_loops;
	int tele_periodic_loops;
	
	bool image_prev_state;		// change-of-state variable for image-save button press
	
public:
    IterativeDemo();
    ~IterativeDemo();

    //======== Init Routines ==================================================

    // Reset the loop counter for disabled mode
    void DisabledInit()     { disabled_periodic_loops = 0; }

    // Reset the loop counter for autonomous mode
    void AutonomousInit()   { auto_periodic_loops = 0; }

    // Reset the loop counter for teleop mode
    void TeleopInit()       { tele_periodic_loops = 0; }
    //
    //=========================================================================


    //======== Periodic Routines ==============================================

    void DisabledPeriodic(void);
    void AutonomousPeriodic(void);
    void TeleopPeriodic(void);
    //
    //=========================================================================


    //======== Continuous Routines ============================================
    //
    // These routines are not used in the imitation of the SimpleDemo robot
    //
#if 0
    void DisabledContinuous(void) {}
    void AutonomousContinuous(void) {}
    void TeleopContinuous(void) {}
#endif
    //
    //=========================================================================

};


//======================================================================================
// Constructor for this "IterativeRobotDemo" Class.
// Create an instance of a RobotDrive with left and right motors plugged into PWM
// ports 0 and 1 on the first digital module.
//======================================================================================
IterativeDemo::IterativeDemo(void) :
    myRobot(0),
    leftJag(0),
    rightJag(0),
    rightStick(0),
    leftStick(0),
    gyro(0),
    geartooth(0),
    compressor(0),
    ramprelease(0),
    rightramp(0),
    leftramp(0),
    ultra_sensor(0)
{
    cout << "IterativeDemo Constructor Started\n";

    ds = DriverStation::GetInstance();
    leftJag = new Jaguar(1);
    rightJag = new Jaguar(2);
    myRobot = new RobotDrive(leftJag, rightJag, SENS);  // create robot drive base
    rightStick = new Joystick(1);           // create the joysticks
    leftStick = new Joystick(2);
    gyro = new Gyro(1, 1);
    //myRobot->SetInvertedMotor(myRobot->kFrontRightMotor, false);
    //myRobot->SetInvertedMotor(myRobot->kFrontLeftMotor, false);
    printf("Motors inverted.\n");
    geartooth = new GearTooth(1, true);
    geartooth->Start();
    compressor = new Compressor(2, 1);
    ramprelease = new Relay(4, 2, ramprelease->kBothDirections);
    rightramp = new Relay(4, 3, rightramp->kBothDirections);
    leftramp = new Relay (4, 4, leftramp->kBothDirections);
    ultra_sensor = new Ultrasonic(3,4);
    //ultra_sensor->SetAutomaticMode(true);
	image_prev_state = false;	// assume image-save button not pressed
    

    // Counters to record the number of loops completed in autonomous and teleop modes
    auto_periodic_loops = 0;
    disabled_periodic_loops = 0;
    tele_periodic_loops = 0;

    auto_state = AUTO_INIT;

    cout << "IterativeDemo Constructor Completed\n";
}

//======================================================================================
// Destructor
// Clean up all the pointers.
//======================================================================================
IterativeDemo::~IterativeDemo()
{
    delete myRobot;
    delete leftJag;
    delete rightJag;
    delete rightStick;
    delete leftStick;
    delete gyro;
    delete geartooth;
    delete compressor;
    delete ramprelease;
    delete rightramp;
    delete leftramp;
    delete ultra_sensor;
}


void IterativeDemo::DisabledPeriodic(void)
{
    // feed the user watchdog at every period when disabled
    GetWatchdog().Feed();

    // increment the number of disabled periodic loops completed
    disabled_periodic_loops++;
}

void IterativeDemo::AutonomousPeriodic(void)
{
    // feed the user watchdog at every period when in autonomous
    GetWatchdog().Feed();

    auto_periodic_loops++;
	
    if(!IsAutonomous()){
    	auto_state = AUTO_DONE;
    }

    // Skip the autonomous moves if the 'disable' jumper is in
    if ( !ds->GetDigitalIn(DISABLE_AUTONOMOUS)) {

        // Keep the robot moving as appropriate for the current state
        switch (auto_state)
        {
            case AUTO_INIT:
                gyro->Reset();
                geartooth->Reset();
                auto_state = AUTO_FIRST_MOVE;
                myRobot->Drive(0.5, 0.0);
                break;

            case AUTO_FIRST_MOVE:
                if ((double)geartooth->Get() < (25.0 * FEET_TO_GEARTOOTH)) {
                    myRobot->Drive(0.5, gyro->GetAngle() / 45.0);
                }
                else {
                    myRobot->Drive(0.0, 0.0);
                    gyro->Reset();
                    auto_state = ds->GetDigitalIn(AUTO_TURN_LEFT) ?
                                                    AUTO_TURNING_LEFT : AUTO_TURNING_RIGHT;
                }
                break;

            case AUTO_TURNING_LEFT:
                if (gyro->GetAngle() > -90.0) {
                    myRobot->SetLeftRightMotorSpeeds(0.75, -0.75);
                }
                else {
                    myRobot->Drive(0.0, 0.0);
                    gyro->Reset();
                    geartooth->Reset();
                    auto_state = AUTO_SHORT_FWD;
                }
                break;

            case AUTO_TURNING_RIGHT:
                if (gyro->GetAngle() < 90.0) {
                    myRobot->SetLeftRightMotorSpeeds(-0.75, 0.75);
                }
                else {
                    myRobot->Drive(0.0, 0.0);
                    gyro->Reset();
                    geartooth->Reset();
                    auto_state = AUTO_SHORT_FWD;
                }
                break;

            case AUTO_SHORT_FWD:
                if ((double)geartooth->Get() < (5.0 * FEET_TO_GEARTOOTH)) {
                    myRobot->Drive(0.5, gyro->GetAngle() / 45.0);
                }
                else {
                    myRobot->Drive(0.0, 0.0);
                    gyro->Reset();
                    geartooth->Reset();
                    auto_state = AUTO_SHORT_BCK;
                }
                break;

            case AUTO_SHORT_BCK:
                if ((double)geartooth->Get() < (5.0 * FEET_TO_GEARTOOTH)) {
                    myRobot->Drive(-0.5, gyro->GetAngle() / 45.0);
                }
                else {
                    myRobot->Drive(0.0, 0.0);
                    gyro->Reset();
                    geartooth->Reset();
                    auto_state = AUTO_SHORT_FWD;
                }
                break;
                
            case AUTO_DONE:
            	break;

            // No default case allows compiler to complain about missing enum items
        }
    }
} // void AutonomousPeriodic(void)

void IterativeDemo::TeleopPeriodic(void)
{
		
    // feed the user watchdog at every period when in autonomous
    GetWatchdog().Feed();

    // increment the number of teleop periodic loops completed
    tele_periodic_loops++;

    // determine if tank or arcade mode; default with no jumper is for tank drive
    if ( !ds->GetDigitalIn(ARCADE_MODE) ) {
        myRobot->TankDrive(-(leftStick->GetY()), -(rightStick->GetY()));
    } else{
        myRobot->ArcadeDrive(rightStick);
    }

    if ((tele_periodic_loops % 200) == 0) {
        // One second has passed. Output some data.
        //printf("Gear Tooth Count = %d\n", geartooth->Get());
        //printf("Gyro Angle = %f\n", gyro->GetAngle());
        printf("Ultrasonic Distance = %d inches\n", ultra_sensor->GetRangeInches());
    	//printf("Right Joystick Value: %f/n", rightStick->GetY());
    }
    

    //resets the gyro sensor if joystick button 3r is pressed
    if (rightStick->GetRawButton(3)) {
        gyro->Reset();
    }

     //start compressor if rt. trigger pressed
    if (rightStick->GetTrigger()) {
        compressor->Start();    // compressor stops above 120 psi, starts below 100
    }

     //stop compressor if lft. trigger pressed
    if (leftStick->GetTrigger()) {
        compressor->Stop();
    }

    if (rightStick->GetRawButton(2)) {
        // pressing button 2r retracts ramp-release solenoid's rod
        ramprelease->Set(ramprelease->kForward);
    }
    else {
        // releasing button pushes rod back out
        ramprelease->Set(ramprelease->kOff);
    }

    if (rightStick->GetRawButton(10)) {
        // button 10r lowers right ramp
        rightramp->Set(rightramp->kReverse);
    }
    if (rightStick->GetRawButton(11)) {
        // button 11r raises right ramp
        rightramp->Set(rightramp->kForward);
    }

    if (leftStick->GetRawButton(10)) {
        // button 10L lowers left ramp
        leftramp->Set(leftramp->kReverse);
    }
    if (leftStick->GetRawButton(11)) {
        // button 11L raises left ramp
        leftramp->Set(leftramp->kForward);
    }
    
    if (rightStick->GetRawButton(4) && !image_prev_state) {			// if (change of state)
    	Image* imageone = frcCreateImage(IMAQ_IMAGE_RGB);			// create image from camera
    	frcWriteImage(imageone, "/tmp/imageone.jpg");				// save image (*needs work*)
    	printf("Image saved.\n");									// confirm image saved
    }
	
	image_prev_state = rightStick->GetRawButton(4);		// update change-of-state variable
	
} // void TeleopPeriodic(void)



START_ROBOT_CLASS(IterativeDemo);