#include "xc.h"
#include "config.h"
#include "extern.h"
#include "../CommonCode/types.h"
#include "libq.h"

void ManageSwitches(void)
{   
    static TOP_STATUS LastSwitchStatus;
    TOP_STATUS SwitchStatus; 

 //Read Switches related to Camera Features
    SwitchStatus.bitfields.LaserPowerRequest = LASER_PWR_SW;
    SwitchStatus.bitfields.CameraPowerRequest = CAMERA_PWR_SW;
    SwitchStatus.bitfields.ZoomExtendRequest = EXTENDER_SW;
    
    if(SwitchStatus.bitfields.LaserPowerRequest && !LastSwitchStatus.bitfields.LaserPowerRequest)
        top_status.bitfields.LaserPowerRequest = !top_status.bitfields.LaserPowerRequest;
       
    if(SwitchStatus.bitfields.ZoomExtendRequest && !LastSwitchStatus.bitfields.ZoomExtendRequest)
        top_status.bitfields.ZoomExtendRequest = !top_status.bitfields.ZoomExtendRequest;

    static int button_off_count = 0;
    if ( SerialDataIn->status.bitfields.CameraPowerState && SwitchStatus.bitfields.CameraPowerRequest )
       ++button_off_count;
    else
       button_off_count = 0;

if ( button_off_count > CAMERA_OFF_DELAY )
  top_status.bitfields.CameraPowerRequest = 0;

if ((SerialDataIn->status.bitfields.CameraPowerState == 0) && SwitchStatus.bitfields.CameraPowerRequest && !LastSwitchStatus.bitfields.CameraPowerRequest && (SerialDataIn->status.bitfields.OverTemperatureState == 0))
  top_status.bitfields.CameraPowerRequest = 1;
    


    LastSwitchStatus = SwitchStatus;  //Store for comparison next time
}