/*
  SFBHWPinsHost.cpp - Board-specific pin definitions
  Copyright (C) 2008 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id$
*/

#include "SFBHWPinsHost.h"
#include "SFBReflex.h"        
#include "SFBAssert.h"        
#include <stdio.h>           /* For printf, etc */
#include <stdlib.h>          /* For exit */
#include <sys/time.h> /* For gettimeofday() */
#include <unistd.h> /* For usleep() */

void reenterBootloader() __attribute__ ((noreturn));

void reenterBootloader() {
  fprintf(stderr,"REENTER BOOT LOADER\n");
  exit(10);
}

void reenterBrainstem() {
  fprintf(stderr,"REENTER BRAINSTEAM\n");
  exit(11);
}

void debugRGB(int rgbBits, int wait) {
  volatile int i, j;

  if (rgbBits&(1<<2))
    REFLEX_RGB_RED_ON;
  else
    REFLEX_RGB_RED_OFF;

  if (rgbBits&(1<<1))
    REFLEX_RGB_GREEN_ON;
  else
    REFLEX_RGB_GREEN_OFF;

  if (rgbBits&(1<<0))
    REFLEX_RGB_BLUE_ON;
  else
    REFLEX_RGB_BLUE_OFF;

  for (i = 0; i < wait; ++i) {
    for (j = 0; j < 1000000; ++j) { }
  }
}

u32 millis() {
  static u32 lastTime = 0;
  struct timeval buf;
  gettimeofday(&buf,0);
  u32 now = buf.tv_sec*1000+(buf.tv_usec/1000);
  if (lastTime==0)
    lastTime = now;
  return now-lastTime;
}

u32 trueMillis() {
  return millis();
}

u32 micros() {
  static u32 lastTime = 0;
  struct timeval buf;
  gettimeofday(&buf,0);
  u32 now = buf.tv_sec*1000000+buf.tv_usec;
  if (lastTime==0)
    lastTime = now;
  return now-lastTime;
}

void delayMicroseconds(u32 usecondsToWait) {
  usleep(usecondsToWait);
}

void _dieOnBoard_(u32 blinkCode,const char * file, int lineno) {
  fprintf(stderr,"%s:%d: DIED -- BLINK_MAP(%d,%d,%d)\n",
          file,lineno,
          GET_BLINK_MAP_COLOR(blinkCode,0),
          GET_BLINK_MAP_COLOR(blinkCode,1),
          GET_BLINK_MAP_COLOR(blinkCode,2)
         );
  exit(1);
}

void _apiError_(u32 blinkCode,const char * file, int lineno) {
  _dieOnBoard_(blinkCode,file,lineno);

}

void delay(u32 msecondsToWait) {
  u32 end = millis()+msecondsToWait;
  while (IS_EARLIER(millis(),end)) {
    reflex_maybe_dispatch(end);
    delayMicroseconds(100);
  }
}

