/*----------------------------------------------------------------------------
**
**	Weapons.c 
**	
** Controls weapons.
**
**--------------------------------------------------------------------------*/

#include "Onboard.h"
#include "Hardware.h"
#include "Commands.h"
#include "Weapons.h"                                      
#include "Sensors.h"
          
// These are empirical timings in units of 10ms.
#define FLIPPER_EXTEND_TIME     100         // 1 second
#define FLIPPER_RETRACT_TIME    100         // 1 second
#define PNEUMATICS_TIME         50          // 0.5 seconds

typedef enum
{
    SPEAR_IDLE,
    SPEAR_FORWARD_DRIVE,
    SPEAR_BACKWARD_DRIVE
} TSpearState;    

// Enum for hammer state machine (in addition to standard FORWARDS & REVERSE)
enum
{
    PARKING=2,
    PARKED,
    PLUGGING_FORWARDS,      // These two states enable the motor rto be slowed
    PLUGGING_REVERSE        // down just as it enters the index position.
};                                     

// Enum for flipper control state machines
enum
{                       
    FLIPPER_PARKED=2,
    EXTENDING_FORWARDS,
    EXTENDING_BACKWARDS,
    EXTENDED_FORWARDS,
    EXTENDED_BACKWARDS,
    RETRACTING,
    FLIPPING
};         


// In addition to the general forwards/backwards enum
#define PARK                2         
#define DRIVE               2   // Forwards or backwards


                            
// Globals within this module
static char HammerState[2];
static char HammerRequest[2];
static char HammerMotor[2];
static char HammerDirection[2];
static char HammerSolenoid[2];
static char HammerIndex[2];
static char HammerDriveLevel = 255; // How hard hammer motors are driven (PWM / 256)

static char FlipperState = FLIPPER_PARKED;        
static int  FlipperTimer, PneumaticsTimer;    
static char LastFlipperPosition = DOWN;
static char LastFlipperExtension = FORWARDS;

static TSpearState SpearStateVar = SPEAR_IDLE;

/*----------------------------------------------------------------------------
//
//  InitialiseWeapons
//      
// Modification Record:
//  24-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void InitialiseWeapons(void)
{
    // Set up spear interrupts.
    ENABLE_SPEAR_INTS;
}

/*----------------------------------------------------------------------------
//
//  LegAssist
//      
// The leg/hammers are to assist in motion. They must drive in the correct
// direction to assist the requested direction of motion. This depends on
// which way up the robot is as well.
//  
// Modification Record:
//  24-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void LegAssist(void)
{
    if (  (Speed < 0)  ^  (WhichWayUp == RIGHT_WAY_UP)  )
        HammerRequest[FRONT] = HammerRequest[REAR] = FORWARDS;

    else
        HammerRequest[FRONT] = HammerRequest[REAR] = REVERSE;
}


/*----------------------------------------------------------------------------
//
//  FrontHammerStateMachine
//
// Control the hammer I/O lines by use of a state machine.      
//  
// Modification Record:
//  24-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void HammerStateMachine(char Hammer)
{                                  
    //
    // Park both hammers in emergency
    //
    
    if (SpecialsRequest[EMERGENCY])
    {
        if (HammerState[0] != PARKED)
            HammerState[0] = PARKING;
        if (HammerState[1] != PARKED)
            HammerState[1] = PARKING;
    }
          
    //
    // Refresh index variables.
    //
    
    HammerIndex[FRONT] = FRONT_HAMMER_INDEX_POSITION;
    HammerIndex[REAR] = REAR_HAMMER_INDEX_POSITION;
    
    //
    // Run state machine
    //
    
    switch (HammerState[Hammer])
    {                
        // ------- PARKED ---------
        case PARKED:
            if (HammerRequest[Hammer] == FORWARDS  ||  HammerRequest[Hammer] == BACKWARDS)
            {
                HammerSolenoid[Hammer] = OFF;   // Release park solenoid
                HammerDirection[Hammer] = HammerRequest[Hammer];
                HammerMotor[Hammer] = HammerDriveLevel;
                HammerState[Hammer] = HammerRequest[Hammer];
            }
            break;
        
        // ------- PARKING ---------
        case PARKING:
            if (HammerRequest[Hammer] == FORWARDS  ||  HammerRequest[Hammer] == BACKWARDS)
            {                                 
                // Command to drive has arrived while waiting for park index.
                HammerSolenoid[Hammer] = OFF;   // Release park solenoid
                HammerDirection[Hammer] = HammerRequest[Hammer];
                HammerMotor[Hammer] = HammerDriveLevel;
                HammerState[Hammer] = HammerRequest[Hammer];
            }         
            else if (FRONT_HAMMER_INDEX_POSITION)
            {
                // As soon as index position is detected, turn off the motor.
                // NOTE: It may be better to reverse the motor direction for a few
                // tens of milliseconds to slow the hammer down to prevent damage
                // the the index solenoid. This will require more states.
                HammerMotor[Hammer] = OFF;
                HammerState[Hammer] = PARKED;
            }    
            break;
            
        // ------- FORWARDS ---------
        case FORWARDS:     
            if (HammerRequest[Hammer] == BACKWARDS)
            {
                HammerDirection[Hammer] = BACKWARDS;
                HammerState[Hammer] = BACKWARDS;
            }
            else if (HammerRequest[Hammer] == PARK)
            {      
                // Activate the park solenoid which will slide along the
                // wheel unitl it clicks into the index hole.
                HammerSolenoid[Hammer] = ON;
                HammerState[Hammer] = PARKING;
            }
            break;
            
        // ------- BACKWARDS ---------
        case BACKWARDS:
            if (HammerRequest[Hammer] == FORWARDS)
            {
                HammerDirection[Hammer] = FORWARDS;
                HammerState[Hammer] = FORWARDS;
            }
            else if (HammerRequest[Hammer] == PARK)
            {      
                // Activate the park solenoid which will slide along the
                // wheel until it clicks into the index hole.
                HammerSolenoid[Hammer] = ON;
                HammerState[Hammer] = PARKING;
            }
            break;
    }                                
    
    //
    // Set appropriate I/O lines based on variable states
    //
    
    FRONT_HAMMER_PWM                = HammerMotor[FRONT];
    REAR_HAMMER_PWM                 = HammerMotor[REAR];
    FRONT_HAMMER_HOME_SOLENOID      = HammerSolenoid[FRONT];
    REAR_HAMMER_HOME_SOLENOID       = HammerSolenoid[REAR];
    FRONT_HAMMER_MOTOR_DIRECTION    = HammerDirection[FRONT];
    REAR_HAMMER_MOTOR_DIRECTION     = HammerDirection[REAR];
}

/*----------------------------------------------------------------------------
//
//  ProcessHammers
//      
// Deals with hammer requests.
//  
// Modification Record:
//  24-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void ProcessHammers(void)
{
    static char FrontHammerState = PARKED;
    static char RearHammerState = PARKED;

    //
    // If leg assist for motion mode is active, work out which direction
    // the hammers must go in to help the requested motion.
    //                                                     
    
    if (SpecialsRequest[LEG_ASSIST])
        LegAssist();
    
    //
    // No emergency, use standard state machines.
    //

    HammerStateMachine(FRONT);
    HammerStateMachine(REAR);
}


/*----------------------------------------------------------------------------
//
//  ActivateSolenoid
//      
// Set solenoid valves to move flipper. Air circuit is as follows:
//
//                   | 
//                   X--SV0     SV1
//      Piston 1     |          |
//  ----HHHHHHHH================X======|
//        Down                         |
//                                     |   Reservoir
//                                     |===HHHHHHHHH
//                                     |
//      Piston 2                       |
//  ----HHHHHHHH================X======|
//         Up        |          |
//                   X--SV2     SV3
//                   | 
//  
// Parameters:
//  Setting:        TOGGLE, PNEUMATICS_OFF, or PNEUMATICS_PURGE
//
// Modification Record:
//  26-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void ActivateSolenoid(char Setting)
{
    if (Setting == DOWN)
    {
        FLIPPER_SOLENOID_VALVE_0 = CLOSED;
        FLIPPER_SOLENOID_VALVE_1 = OPEN;
        FLIPPER_SOLENOID_VALVE_2 = OPEN;
        FLIPPER_SOLENOID_VALVE_3 = CLOSED;
    }
    else if (Setting == UP)
    {
        FLIPPER_SOLENOID_VALVE_0 = OPEN;
        FLIPPER_SOLENOID_VALVE_1 = CLOSED;
        FLIPPER_SOLENOID_VALVE_2 = CLOSED;
        FLIPPER_SOLENOID_VALVE_3 = OPEN;
    }                        
    else if (Setting == PNEUMATICS_OFF)
    {
        FLIPPER_SOLENOID_VALVE_0 = CLOSED;
        FLIPPER_SOLENOID_VALVE_1 = CLOSED;
        FLIPPER_SOLENOID_VALVE_2 = CLOSED;
        FLIPPER_SOLENOID_VALVE_3 = CLOSED;
    }                        
    else if (Setting == PNEUMATICS_PURGE)
    {
        FLIPPER_SOLENOID_VALVE_0 = OPEN;
        FLIPPER_SOLENOID_VALVE_1 = OPEN;
        FLIPPER_SOLENOID_VALVE_2 = OPEN;
        FLIPPER_SOLENOID_VALVE_3 = OPEN;
    }                        
}


/*----------------------------------------------------------------------------
//
//  TogglePneumatics
//      
// Flip the flipper!
// 
// Modification Record:
//  26-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void TogglePneumatics(void)
{                              
    if (LastFlipperPosition == UP)
        ActivateSolenoid(DOWN);
    else
        ActivateSolenoid(UP);

    LastFlipperPosition = 1 - LastFlipperPosition;
}

/*----------------------------------------------------------------------------
//
//  ComplexFlipperStateMachine
//
// State machine for Extend->flip->Retract motions.      
//  
// Modification Record:
//  26-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void ComplexFlipperStateMachine(void)
{        
    switch(FlipperState)
    {           
        //-------- FLIPPER_PARKED ----------
        case FLIPPER_PARKED:
        {                                              
            if (FlipperTimer == 0)
            {
                if (WeaponsRequest[FLIPPER_FORWARDS])
                {
                    // Extend flipper forwards...
                    FlipperTimer = FLIPPER_EXTEND_TIME;
                    FLIPPER_RETRACT_MOTOR_POS = ON;
                    FLIPPER_RETRACT_MOTOR_NEG = OFF;
                    FlipperState = EXTENDING_FORWARDS;
                }
                else    // Must be FLIPPER_BACKWARDS
                {
                    // Extend flipper backwards...
                    FlipperTimer = FLIPPER_EXTEND_TIME;
                    FLIPPER_RETRACT_MOTOR_POS = OFF;
                    FLIPPER_RETRACT_MOTOR_NEG = ON;
                    FlipperState = EXTENDING_BACKWARDS;
                }
            
                // Switch it so it starts off in the down position, ready to flip.
                if (WhichWayUp == RIGHT_WAY_UP)
                    ActivateSolenoid(DOWN);
                else
                    ActivateSolenoid(UP);
                PneumaticsTimer = PNEUMATICS_TIME;
            }
        } break;         
        
        //-------- EXTENDING_BACKWARDS ----------
        case EXTENDING_BACKWARDS: 
        {
            if (FlipperTimer == 0)
            {
                FLIPPER_RETRACT_MOTOR_NEG = OFF;
                TogglePneumatics();
                LastFlipperExtension = BACKWARDS;
                FlipperState = FLIPPING;
            }
        } break;
        
        //-------- EXTENDING_FORWARDS ----------
        case EXTENDING_FORWARDS: 
        {
            if (FlipperTimer == 0)
            {
                FLIPPER_RETRACT_MOTOR_POS = OFF;
                TogglePneumatics();
                LastFlipperExtension = FORWARDS;
                FlipperState = FLIPPING;
            }
        } break;
        
        //-------- FLIPPING ----------
        case FLIPPING:
        {       
            if (PneumaticsTimer == 0)
            {                   
                if (LastFlipperExtension == FORWARDS)
                {
                    FLIPPER_RETRACT_MOTOR_POS = OFF;
                    FLIPPER_RETRACT_MOTOR_NEG = ON;
                }                               
                else
                {
                    FLIPPER_RETRACT_MOTOR_POS = ON;
                    FLIPPER_RETRACT_MOTOR_NEG = OFF;
                }                               
                FlipperTimer = FLIPPER_RETRACT_TIME;
                FlipperState = RETRACTING;
            }
        } break;

        //-------- RETRACTING ----------
        case RETRACTING:
        {
            if (FLIPPER_INDEX_POSITION)
            {
                FLIPPER_RETRACT_MOTOR_POS = OFF;
                FLIPPER_RETRACT_MOTOR_NEG = OFF;
                FlipperState = FLIPPER_PARKED;
            }           
        } break;
    }
}


/*----------------------------------------------------------------------------
//
//  SimpleFlipperStateMachine
//      
// State machine for simple flipper commands (extend and retract)
//  
// Modification Record:
//  26-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void SimpleFlipperStateMachine(void)
{
    //
    // General state transitions from any state.
    //
                         
    if (FlipperTimer == 0)
    {
        if (WeaponsRequest[EXTEND_FLIPPER_FORWARDS])
        {          
            FlipperTimer = FLIPPER_EXTEND_TIME;
            FLIPPER_RETRACT_MOTOR_POS = ON;
            FLIPPER_RETRACT_MOTOR_NEG = OFF;
            FlipperState = EXTENDING_FORWARDS;
        }
        if (WeaponsRequest[EXTEND_FLIPPER_BACKWARDS])
        {
            FlipperTimer = FLIPPER_EXTEND_TIME;
            FLIPPER_RETRACT_MOTOR_POS = OFF;
            FLIPPER_RETRACT_MOTOR_NEG = ON;
            FlipperState = EXTENDING_BACKWARDS;
        }
        if (WeaponsRequest[RETRACT_FLIPPER])
        {
            FlipperTimer = FLIPPER_RETRACT_TIME;
            if (FlipperState == EXTENDING_FORWARDS  ||  FlipperState == EXTENDED_FORWARDS)
            {
                FLIPPER_RETRACT_MOTOR_POS = OFF;
                FLIPPER_RETRACT_MOTOR_NEG = ON;
            }
            else
            {
                FLIPPER_RETRACT_MOTOR_POS = ON;
                FLIPPER_RETRACT_MOTOR_NEG = OFF;
            }
            FlipperState = RETRACTING;
        }
    }

    //
    // Explicit state transitions
    //
        
    switch (FlipperState)
    {
        case EXTENDING_FORWARDS:
        {
            if (FlipperTimer == 0)
            {
                FLIPPER_RETRACT_MOTOR_POS = OFF;
                FLIPPER_RETRACT_MOTOR_NEG = OFF;
                FlipperState = EXTENDED_FORWARDS;
            }
        } break;
                
        case EXTENDING_BACKWARDS:
        {
            if (FlipperTimer == 0)
            {
                FLIPPER_RETRACT_MOTOR_POS = OFF;
                FLIPPER_RETRACT_MOTOR_NEG = OFF;
                FlipperState = EXTENDED_BACKWARDS;
            }
        } break;
        
        case RETRACTING:
        {
            if (FLIPPER_INDEX_POSITION)
            {
                FLIPPER_RETRACT_MOTOR_POS = OFF;
                FLIPPER_RETRACT_MOTOR_NEG = OFF;
                FlipperState = FLIPPER_PARKED;
            }           
        } break;
    }     
}                         

/*----------------------------------------------------------------------------
//
//  ProcessFlipper
//      
// Deals with flipper requests. There are simple flipper requests like extend
// retract, and flip. Then there are more complex ones such as flip forwards
// and flip backwards, which will require a state machine to control.
//  
// Modification Record:
//  26-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void ProcessFlipper(void)
{                                    
    //
    // Update state machine timers.
    //
    
    if (FlipperTimer > 0)
        FlipperTimer--;
    if (PneumaticsTimer > 0)
        PneumaticsTimer--;
                             
    //
    // Deal with pneumatics commands
    //
    
    if (WeaponsRequest[OPERATE_FLIPPER])
    { 
        PneumaticsTimer = PNEUMATICS_TIME;
        TogglePneumatics();
    }
    if (PneumaticsTimer == 0)
        ActivateSolenoid(PNEUMATICS_OFF);

    //
    // Flipper state machines
    //
    
    if (WeaponsRequest[FLIPPER_FORWARDS]  ||  WeaponsRequest[FLIPPER_BACKWARDS])
        ComplexFlipperStateMachine();
    else
        SimpleFlipperStateMachine();
}


/*----------------------------------------------------------------------------
//
//  SetSpearOutputs
//                 
// Given the state variable, set the spear outputs accordingly.
//  
// Modification Record:
//  10-Oct-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void SetSpearOutputs(void)
{
    switch (SpearStateVar)
    {
        case SPEAR_IDLE:                                 
        {
            SPEAR_DRIVE_FORWARDS = 0;
            SPEAR_DRIVE_BACKWARDS = 0;
            break;
        }
            
        case SPEAR_FORWARD_DRIVE:
        {
            SPEAR_DRIVE_FORWARDS = 1;
            SPEAR_DRIVE_BACKWARDS = 0;
            break;
        }
        
        case SPEAR_BACKWARD_DRIVE:
        {
            SPEAR_DRIVE_FORWARDS = 0;
            SPEAR_DRIVE_BACKWARDS = 1;
            break;
        }
    }
}



/*----------------------------------------------------------------------------
//
//  SpearForwardIsr
//                 
// Spear has appeared in the forward position. If in MTR mode, update the
// state machine.
//  
// Modification Record:
//  10-Oct-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
interrupt [IRQ_6] void SpearForwardIsr(void)
{
    if (WeaponsRequest[SPEAR_MULTITHROW])
    {
        SpearStateVar = SPEAR_BACKWARD_DRIVE;
        SetSpearOutputs();
    }
}


/*----------------------------------------------------------------------------
//
//  SpearBackIsr
//                 
// Spear has appeared in the back position. If in MTR mode, update the
// state machine.
//  
// Modification Record:
//  10-Oct-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
interrupt [IRQ_7] void SpearBackIsr(void)
{
    if (WeaponsRequest[SPEAR_MULTITHROW])
    {
        SpearStateVar = SPEAR_FORWARD_DRIVE;
        SetSpearOutputs();
    }
}


/*----------------------------------------------------------------------------
//
//  ProcessSpear
//      
// Deals with spear requests. There are three types of spear request: fire
// forwards, fire backwards, and multithrow, where the spear is fired back and
// forth bouncing off its endsprings. The spear functionality is controlled by
// the following state machine:
//            _____________________________________
//           /                MTR.Bi (ISR)         \
//           V                                     |
//        /------\     FR    /------\    BR     /------\
//        |      |<----------|      |---------->|      |
//        | fore |           | idle |           | back |
//        |      |---------->|      |<----------|      |
//        \------/  /MTR.Fi  \------/  /MTR.Bi  \------/
//           ^                                     |
//           \________________MTR.Fi__(ISR)________/       
//
//  Key:
//      MTR = Multithrow request
//      FR = Forward request
//      BR = Backward request
//      Fi = Forward index
//      Bi = backward index
//
// Note that the transitions from fore to back states when in multithrow
// mode are performed by the ISRs for IRQ6 and IRQ7, not by the function
// below, since a visit every 10ms may not be fast enough.
//  
// Modification Record:
//  10-Oct-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void ProcessSpear(void)
{                
    //
    // This is the state machine to decide the next state. Note that
    // multithrow index detection events are controlled by the ISRs above.
    // This function just controls the standard forwards and backwards 
    // commands.
    //
    
    switch (SpearStateVar)
    {
        case SPEAR_IDLE:
        {
            if (WeaponsRequest[SPEAR_FORWARDS]  ||  WeaponsRequest[SPEAR_MULTITHROW])
                SpearStateVar = SPEAR_FORWARD_DRIVE;
            else if (WeaponsRequest[SPEAR_BACKWARDS])
                SpearStateVar = SPEAR_BACKWARD_DRIVE;
            break;
        }
            
        case SPEAR_FORWARD_DRIVE:
        {
            if (SPEAR_FORWARD_INDEX)
                SpearStateVar = SPEAR_IDLE;
            break;
        }
        
        case SPEAR_BACKWARD_DRIVE:
        {
            if (SPEAR_BACKWARD_INDEX)
                SpearStateVar = SPEAR_IDLE;
            break;
        }
    }    
    
}

/*----------------------------------------------------------------------------
//
//  ProcessWeapons
//      
// Checks the bits in the WeaponsRequest bitfield (extracted from received
// radio data) to work out what actions are required.
//  
// Modification Record:
//  24-Jul-00   Paul Hills      First version
-----------------------------------------------------------------------------*/
void ProcessWeapons(void)
{                   
    ProcessSpear();                        
    ProcessHammers();
    ProcessFlipper();
}