/* Move an elevator from floor 'start' to 'end', updating the  *
 * indicator LEDs and pausing as appropriate.                  */
static void move_elevator(byte start, byte end) {
    signed char direction = integral_signum(end - start);

    /* Assert that the floor is valid. Disable assertion genertion *
     * using #define NDEBUG.                                       */
    assert(start >= MIN_FLOOR && end >= MIN_FLOOR &&
           start <= MAX_FLOOR && end <= MAX_FLOOR);

    --start;                           /* Offset start and end to be zero-indexed. */
    --end;

    setLED(start, true);               /* Ensure the floor is shown. */
    set7Segment(start+1, true);			 /* Ensure floor number is displayed */
    while (start != end) {
        delay();                         /* Wait ~1 second. */
        setLED(start, false);            /* Disable the last floor. */
        start += direction;              /* Move to the next floor. */
        setLED(start, true);             /* Enable the new indicator. */
        set7Segment(start+1, true);		 /* Display new floor number */
    }

    delay();                           /* Wait ~3 seconds more. */
    delay();
    delay();
}

uint8_t i2cscan(void)
{
    uint8_t i;
    uint8_t n;

    n = 0;
    
    for (i = 1; i < 127; i++)
    {
        setLED(1);
        ms_sleep(10);
        
        /* start condition */
        if (i2c_start(0x08, 1))
        {
            break;
        }
        
        printf(" %.2X", i);
        if(i % 0x20 == 0)
        printf("\r\n");
        
        /* address slave device, write */
        if (!i2c_sla_rw(i, 0, TW_MT_SLA_ACK, 0)) {
            printf("\r\nfound device at address 0x%02x\r\n", i);
            n++;
        }
        
        i2c_stop();
        setLED(0);
        ms_sleep(10);
    }

    return n;
}

void
refuse_lowvoltage(int die)
{
	extern void playBip(void);
	int no_startup_done = ((LED_GREEN == 0) && (LED_RED == 0));
	if(no_startup_done == 1) { // haven't run startup, no sound possible
		LED_GREEN = DEFAULT_LED_GREEN;
		LED_RED = DEFAULT_LED_RED;
		LED_ALL = LED_GREEN | LED_RED;
	} else {	
		playBip();
		playBip();
	}
	if(die != 0) {
		setLED(LED_ALL, FALSE);
		wait(500);
		setLED(LED_RED, TRUE);
		wait(500);
		setLED(LED_RED, FALSE);
		wait(500);
		setLED(LED_RED, TRUE);
		wait(500);
		setLED(LED_RED, FALSE);
		setOperationalMode((int)P_SLEEP);
	}
}

	//*********************************************************	
	void setSw_Initialization(char sw)
	{
		setLED(sw,1);
		#if Switch_Class == 1 && Dimmer_use == 1
			setLED(sw+1,1);
		#endif
	}

int setUSBHost(BOOL enter) {
	int trials, x;
	const int maxTrials = 20;
	

	if (enter){		
		SetSystemClockRate(48);  //48MHz for Host mode
		setLED(LED_GREEN,FALSE);
		for (trials = 0; trials < maxTrials; trials++) {
			setLED(LED_RED,TRUE);
			x = _devicemount(1); 
			setLED(LED_RED,FALSE);
			if (x == C_USBDevicesmountOK)
				break;
			wait (1000);			
		}
		if (trials < maxTrials) {
			setLED(LED_GREEN,TRUE);
			wait(1000);
			USBHost_Flag = C_USBDevicesmountOK;
		}
	} else {
			//  unmount USB devices;  
			if(_deviceunmount(1) != 0x00)
				USBHost_Flag = C_USBDiskPlugOut;
			else
				USBHost_Flag = C_USBDevicesUnmount;
			*P_USBH_Config = 0;
			*P_USBH_INTEN  = 0;	
			SetSystemClockRate(CLOCK_RATE);  //set clock to non usb host value
	}

	return 0;
}

void IconGlyph(pixel* frame, const unsigned char* gly, int16_t fx, int16_t fy, bool sel)
{
  byte cv[4] = {0, 1, 3, 15};
  byte cd[4] = {0, 1, 1, 3};

  int idx = 0;
  for (int y=0; y<16; y++)
  {
    for (int x=0; x<16; x++)
    {
//       int16_t colInd = fontBuf[idx] == 0 ? 14 : (14 - (fontBuf[idx] * 5));
// 
      if (sel && (x == 0 || y == 0 || x == 15 || y == 15))
      {
        if ((x <= 3 || x >= (15 - 3)) &&
            (y <= 3 || y >= (15 - 3)))
        setLED(frame, x + fx, y + fy, 0, 2);
        if ((x <= 1 || x >= (15 - 1)) &&
            (y <= 1 || y >= (15 - 1)))
        setLED(frame, x + fx, y + fy, 0, 10);
      }
      else
      {
//         if (fontBuf[idx] == 0)
//           setLED(frame, x + fx, y + fy, 0, 1);
//         else
        if (sel)
          setLED(frame, x + fx, y + fy, (byte)cv[gly[idx]], (byte)cv[gly[idx]]);
        else
          setLED(frame, x + fx, y + fy, (byte)cd[gly[idx]], 0);
      }
      idx ++;
    }
  }
}

//==============================================================================================
//=============================================SETUP LOOP=======================================
//==============================================================================================
void setup() {
    pinMode(RLED, OUTPUT);
    pinMode(GLED, OUTPUT);
    pinMode(BLED, OUTPUT);
    pinMode(wire1, INPUT);
    pinMode(wire2, INPUT);
    pinMode(wire3, INPUT);
    pinMode(wire4, INPUT);
    pinMode(largeKey, INPUT);
    pinMode(smallKey, INPUT);

    TCCR1A |= (1 << COM1A0);
    TCCR1B |= (1 << WGM12) | (1 << CS12);
    TIMSK1 |= (1 << TOIE1);
    sei();

    lcd.begin(16, 2);
    lcd.clear();
    lcd.print("Bomb prop V1.1");
    lcd.setCursor(0,1);
    lcd.print("J McKenna [MMS]");
    for (int i = 0 ; i <= 3 ; i++) {
        delay(500);
        setLED(0);
        delay(500);
        setLED(1);
    }
    lcd.clear();
}

void setOperationalMode(int newmode) {
	extern void buildMyStatsCSV();
	extern void saveLogFile(int);
	
  if(newmode == (int)P_WAIT) {
  	// stop();  --- should we see if we can WAIT while paused in an audio file?
  	SysIntoWaitMode();
    // when leaving wait mode, next instruction is executed, so we return here
    return;
  } else {
		// give visual feedback of shutting down (aural feedback when user causes shutdown in takeAction())
 		setLED(LED_ALL,TRUE);
  		buildMyStatsCSV();
		buildExchgOstats();
 		clearDeleteQueue();
  		write_config_bin();  // build a config.bin
		writeVersionToDisk(SYSTEM_PATH);  // make sure the version file is correct
  		confirmSNonDisk(); // make sure the serial number file is correct 
 		//cleanUpOldRevs(); // cleanup any old revs 
    	// assume calling for sleep or halt
		*P_Clock_Ctrl |= 0x200;	//bit 9 KCEN enable IOB0-IOB2 key change interrupt
		if (newmode == (int)P_HALT)
			logString((char *)"Halting",BUFFER,LOG_NORMAL);
		else // newmode == (int)P_SLEEP
			logString((char *)"Sleeping",BUFFER,LOG_NORMAL);			
		
		saveLogFile(0);	
		
	  	Snd_Stop();    // no logging
		setLED(LED_ALL,FALSE);
	
		turnAmpOff();

		disk_safe_exit(0);
// try to get the sd card in a safe state - reversw what we do on startup		
		_deviceunmount(0);
		fs_uninit();
		SD_Uninitial();		

		turnSDoff();
		turnNORoff();
	  	
	  	if (newmode == (int)P_HALT)  {
/*
	  		setRTCalarmSeconds(61);		// device should come back on in 61 seconds
*/
		  	SysIntoHaltMode();
	  	}
		else { // newmode == (int)P_SLEEP
			disk_safe_exit(0);

			_SystemOnOff();
		}
	
		while(1);	
	    // cpu reset on exiting halt/sleep mode, so nothing below here executes
     }
}

void logException(unsigned int errorCode, const char * pStrError, int takeAction) {
	// errorcode == 1 means memory error from BodyInit() and ucBSInit()
	int i; 
	char errorString[160];
	
	if(vCur_1 < V_MIN_SDWRITE_VOLTAGE) {
	}
	
	if (takeAction || LOG_WARNINGS) {
		strcpy(errorString,"\x0d\x0a" "*** ERROR! (cycle "); //cycle number
		longToDecimalString(systemCounts.powerUpNumber,(char *)(errorString+strlen(errorString)),4);
		strcat(errorString," - version " VERSION ")\x0d\x0a*** #");
		longToDecimalString((long)errorCode,(char *)(errorString+strlen(errorString)),3);
		if (takeAction) {
			strcat(errorString,"-fatal");
			stop();						
		}
		else 
			strcat(errorString,"-warning");

		if (LOG_FILE) {
			logString(errorString,ASAP);
			if (pStrError) {
				LBstrncpy(errorString,pStrError,80);
				logString(errorString,ASAP);
			}
		}
		else {
			appendStringToFile(ERROR_LOG_FILE,errorString);	
			if (pStrError) {
				LBstrncpy(errorString,pStrError,80);
				appendStringToFile(ERROR_LOG_FILE,errorString);
			}
		}
	}		
	//todo: put a parameter in fct to return instead of reset or USB
	//maybe a choice of the three RETURN, RESET, USB
	if (takeAction) {
// 		commenting out code to alert user of error -- just use lights and auto-reset to welcome msg
//		if (errorCode != 10 && errorCode != 14)  // can't access config or system boot 
//			insertSoundFile(ERROR_SOUND_FILE_IDX);
//		if (errorCode != 14) // LED_GREEN and LED_RED are not assigned without config file
			for (i=0; i < 5; i++) {
				setLED(LED_GREEN,FALSE);
				setLED(LED_RED,TRUE);
				wait(500);
				setLED(LED_RED,FALSE);
				setLED(LED_GREEN,TRUE);
				wait(500);
			}
		if (takeAction == USB_MODE) // can't load config
			setUSBDevice (TRUE);
		else if (takeAction == RESET)
			resetSystem();
		else if (takeAction ==  SHUT_DOWN)
			setOperationalMode((int)P_SLEEP);
	}
}

/* blinkLEDs(time) - blinks LED, with the specified time of blinking
 *
 * It bliks LED0 and LED1, with the specified time of blinking
 */
void WaspUtils::blinkLEDs(uint16_t time) 
{
	setLED(LED0,LED_ON);
	setLED(LED1,LED_ON);
	delay(time);
	setLED(LED0,LED_OFF);
	setLED(LED1,LED_OFF);  
	delay(time);
}

void mainLoop (void) {
	unsigned int getCurVoltageSample();
	CtnrBlock *insertBlock;
	ListItem *list;
	int inactivityCheckCounter = 0;
	
	while(1) {
		
		// check if need to load in a new package
		if (context.queuedPackageType > PKG_NONE) {
			if (context.queuedPackageNameIndex != -1)
				loadPackage(context.queuedPackageType, pkgSystem.strHeapStack + context.queuedPackageNameIndex);
			else
				loadPackage(context.queuedPackageType, NULL);
		}
		
		// check for start or end event
		// todo: do we have to check SACM_Status() to see if stopped or can that be moved into the start/end event processing?
		if (SACM_Status() && !context.isPaused && 
			compressTime(Snd_A1800_GetCurrentTime(),context.package->timePrecision) >= context.timeNextTimeframe)
				endOfTimeframe(context.idxTimeframe, FALSE);
		else if (!context.isStopped && !SACM_Status()) { // just stopped playing
			// this assume that stopped means end of audio file
			// todo: this should be checking end action for CtnrFile (doesn't exist yet)
			context.isStopped = TRUE;
			turnAmpOff();
			markEndPlay(getRTCinSeconds());
			flushLog();			
			if (GREEN_LED_WHEN_PLAYING) {
				setLED(LED_GREEN,FALSE);
				setLED(LED_RED,FALSE);				
			}
			if (context.idxActiveList == -1)		
				endOfTimeframe(context.idxTimeframe, TRUE);
			else {
				//process any end of play sound-insert
				//TODO: work this into other code; it's a bit of a one-off right now
				list = &context.package->lists[context.idxActiveList];
				insertBlock = getEndInsert(list->actionStartEnd, list->idxFirstAction);
				if (insertBlock)
					insertSound(getFileFromBlock(insertBlock),insertBlock,FALSE);
			}
			if (context.package->pkg_type == PKG_MSG) {
				insertSound(&pkgSystem.files[POST_PLAY_FILE_IDX],NULL,FALSE); 					
			}	
		}
		if (++inactivityCheckCounter > 10) {
			checkInactivity(!context.isStopped && !context.isPaused);
			inactivityCheckCounter = 0;
			if(vCur_1 < V_MIN_RUN_VOLTAGE) {
				refuse_lowvoltage(1);
				// not reached
			}
		}
		keyResponse();
	} // end of while(1) loop
}

void
MainWindow::flashRedCheckClicked(bool state)
{
	if (state)
		setLED(7, 2);
	else {
		setLED(7, (red_check->isChecked())? 1: 0); // 0=off, 1=on, 2=flash)
	}
}

// CITY_SAVED event UX - play cannon/tank shot when chem unit is fired, turn the city’s LED ring from red to blue and HOLD, play thunderstorm audio when a city is extinguished, turn off topmost green LED
void citySavedUX(uint8_t whichCity) {
	printf("City Saved UX: turn the city’s LED ring from red to blue and HOLD, play thunderstorm audio when a city is extinguished\r\n");
	// Turn off that city’s red LEDs ring
	setLED(LED_FIRST_RED << (whichCity), CITY_OFF);
	// Turn on that city’s blue LEDs ring
	setLED(LED_FIRST_BLUE<< (whichCity), CITY_ON);
	// play thunderstorm audio
	playSound(AUDIO_THUNDERSTORM);
}

/* blinkGreenLED(time, num) - blinks the green LED for 'num' times, and during 
 * 'time' milliseconds
 */
void WaspUtils::blinkGreenLED( uint16_t time, uint8_t num ) 
{
	for( int i = 0; i < num ; i++ )
	{
		setLED(LED1,LED_ON);
		delay(time);
		setLED(LED1,LED_OFF);
		delay(time);	
	}
}

void startUpdate(char *filenameUpdate) {
	int ret;
	flash  FL = {0};
	int fl_size = USB_Flash_init((flash *)0, 0);
	int flash_execution_buf[fl_size];
	char oldPath[60], newPath[60];
	struct f_info file_info;
	
	// if following file exists reprogram the flash with it
	// rename the above to Attempted during reprogramming (the file has neither succeeded or failed)
	// if Attempted exists at powerup reprogramming was interrupted so reprogram Attempted again
	//   (until it succeeds or fails)
	// upon successful reprogramming move Current to Prev, Attempted to Current
	
	ret = mkdir((LPSTR)DEFAULT_SYSTEM_PATH);
	// see if new image file exists or was attempted	

	if(filenameUpdate[0] != 0) {  // assumes only one version in update location
		strcpy(oldPath,UPDATE_FP);
		strcat(oldPath,filenameUpdate);
		strcpy(newPath,DEFAULT_SYSTEM_PATH);
		strcat(newPath,filenameUpdate);
		strcat(newPath,ATTEMPTED_EXT);
		
		ret = rename((LPSTR)oldPath, (LPSTR)newPath); 
		FL.fileHandle = open((LPSTR)newPath,O_RDONLY);
	} else {
		ret =_findfirst((LPSTR)DEFAULT_SYSTEM_PATH "*" ATTEMPTED_EXT, &file_info, D_FILE);		
		if (ret >= 0) {
			strcpy(oldPath,DEFAULT_SYSTEM_PATH);
			strcat(oldPath,file_info.f_name);
			FL.fileHandle = open((LPSTR)oldPath,O_RDONLY);		
		} else
			FL.fileHandle = -1;
	}

	if(FL.fileHandle < 0)
			return;	// no system update image in folder, return
	playBips(2);
	setLED(LED_RED,TRUE);
	
	//commenting these two lines out since we want reprogramming to happen before config file loading where log filename is given
	//strcpy(strLog, "Reprogramming with new firmware update");	
	//logString(strLog, ASAP);

	FL.flash_exe_buf = (void *) &flash_execution_buf[0];
	USB_Flash_init(&FL, 1);

	Try_SD_reprog(&FL);
	setLED(LED_RED,FALSE);

	__asm__("irq on");
	__asm__("fiq on");

}

//
// copy all files in fromdir to todir
//
static int copyfiles(char *fromdir, char *todir)
{
	int ret, r1, len_from, len_to, fret;
	char from[80], to[80];
	struct f_info fi;
	
	fret = 0;
	
	strcpy(from, fromdir);
	len_from = strlen(from);
	
	if(from[len_from-1] != '/') {
		strcat(from, "/");
		len_from++;
	}
	strcat(from, "*.*");
	
	strcpy(to, todir);
	len_to = strlen(to);
//	mkdir(to);	// just to be safe
	if(to[len_to-1] != '/') {
		strcat(to, "/");
		len_to++;
	}
			
	ret =_findfirst((LPSTR)from, &fi, D_FILE);
	while(ret >= 0) {
		if(fi.f_name[0] != '.') {
			from[len_from] = 0;
			to[len_to]= 0;
			strcat(from, fi.f_name);
			strcat(to, fi.f_name);
			if((lower(from[0]) == 'a') && (lower(to[0]) == 'a')) {
				unlink((LPSTR)to);
				r1 = rename((LPSTR)from, (LPSTR)to);
			} else {
				setLED(LED_GREEN,FALSE);
				setLED(LED_RED,TRUE);
				r1 = _copy((LPSTR)from, (LPSTR)to);
// speed up 				wait (500);
				setLED(LED_RED,FALSE);
				if (r1 != -1) {
					setLED(LED_GREEN,TRUE);
// speed up					wait(500);
				}
			}
//			logString((char *)"FROM/TO:",BUFFER);
//			logString(from,BUFFER);
//			logString(to,ASAP);
		}
		ret = _findnext(&fi);
		fret++;
	}
	return(fret);
}

void startFailUX(void) {
	printf("Started Fail UX: flash all red LEDs, play WOMP WOMP WOMP\r\n");
	// SetFlagsInactive();
	
	// startFailUX - flash all red LEDs with max frequency, play WOMP WOMP WOMP
	setLED(LED_ALL_CITIES_RED, FLASHING); 
	setLED(LED_ALL_CITIES_BLUE, CITY_OFF);
	setLED(LED_ALL_INDIV, OFF);
	// play womp womp
	playSound(AUDIO_LOSING);
}

void addChemUX(void) {
	printf("Added ChemUX: turn off light from previous chem & light up a green chem on canister, play burbling sound \r\n");
	if (thisChem == 0) {
		// if prev value in array is 0, turn off yellow
		setLED(LED_YELLOW_CHEM, OFF);
	} else {
		// if prev value in array is 1, turn off blue
		setLED(LED_BLUE_CHEM, OFF);
	}
	// play chemical burbling sound for each successful chem unit created
	playSound(AUDIO_LIQUID);
}

void nextChemUX(void) {
	printf("Next Chem UX: turn on light from next chem\r\n");
	// lastChem = thisChem;
	thisChem = rand() % 2;
	if (thisChem == 0) {
		// if value in array is 0, turn on yellow
		setLED(LED_YELLOW_CHEM, FLASHING);
	} else {
		// if value in array is 1, turn on blue
		setLED(LED_BLUE_CHEM, FLASHING);
	}
}

void resetUX(void) {
	// set all LEDs to off
	setLED(LED_ALL_INDIV,OFF);
	setLED(LED_ALL_CITIES,CITY_OFF);
	setLED(LED_START_BUTTON,FLASHING);
	
	// set all audio to off
	stopAllSounds();
	
	// set all servos to off
	SetFlagsInactive();
}