/*
  SFBHWPinsBoard.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 "SFBPins.h" /* for implDigitalWrite() */
#include "SFBReflex.h" /* for reflex_maybe_dispatch() */
#include "SFBWiring.h" /* for pinMode() */
#include "SFBAssert.h" /* for BUG_UNIMPLEMENTED */
#include "SFBSerial.h" /* for Faces */
#include "SFBBootBlock.h" /* for setBootBlockFlags, BOOTBLOCK_KILL_SKETCH, etc */
#include "SFBWatchdog.h" /* for watchdogActive() */

#include "lpc/LPC23xx.h" /* get the hardware definitions */
#include "lpc/fio.h" /* direct pin access */
#include "lpc/timer.h" /* for timer0_1msec_counter */
#include "lpc/irq.h" /* for disableInterrupts */


static void GPIOResetInit() {
  /* Reset all GPIO pins to default: primary function */
  PINSEL0 = 0x00000000;
  PINSEL1 = 0x00000000;
  PINSEL2 = 0x00000000;
  PINSEL3 = 0x00000000;
  PINSEL4 = 0x00000000;
  PINSEL5 = 0x00000000;
  PINSEL6 = 0x00000000;
  PINSEL7 = 0x00000000;
  PINSEL8 = 0x00000000;
  PINSEL9 = 0x00000000;
  PINSEL10 = 0x00000000;
    
  IODIR0 = 0x00000000;
  IODIR1 = 0x00000000;
  IOSET0 = 0x00000000;
  IOSET1 = 0x00000000;
    
  FIO0DIR = 0x00000000;
  FIO1DIR = 0x00000000;
  FIO2DIR = 0x00000000;
  FIO3DIR = 0x00000000;
  FIO4DIR = 0x00000000;
    
  FIO0SET = 0x00000000;
  FIO1SET = 0x00000000;
  FIO2SET = 0x00000000;
  FIO3SET = 0x00000000;
  FIO4SET = 0x00000000;
}

void hwpins_startup_initialization() {
  GPIOResetInit();
  SCS |= GPIOM;
}

void reenterBootloader() __attribute__ ((noreturn));

void reenterBootloader() {
  ////
  // Ensure the north uart is stopped, to help the secondary bootloader..
  Faces[NORTH].reserve();

  disableInterrupts();

  if (watchdogActive())     // If the watchdog's active..
    watchdogSuicide();      // ..this is the way to go

  void (*func)(void) = 0x0; // Otherwise we'll just
  (*func)();                // jump to 0

 noreturn: goto noreturn;   // Shut up gcc
}

void reenterBrainstem() {

  if (watchdogActive()) {    // This functionality is not available if
    reenterBootloader();     // we've got the watchdog running..
  }

  disableInterrupts();
  void (*func)(void) = (void(*)()) 0x2200;
  (*func)();
 noreturn: goto noreturn; /* NOT REACHED: Shut up gcc */
}

extern "C" void dieUndefException();
void dieUndefException() {  BUG_CODE("",E_HW_UNDEF); }

extern "C" void dieDabtException(u32 pc);
void dieDabtException(u32 pc) {  _dieOnBoard_(E_HW_DABT,(const char *) 0,pc); }

extern "C" void diePabtException();
void diePabtException() {  BUG_CODE("",E_HW_PABT); }

extern "C" void dieSWIException();
void dieSWIException() {  BUG_CODE("",E_HW_SWI); }

extern "C" void dieFIQTrap();
void dieFIQTrap() {  BUG_CODE("",E_HW_FIQ); }

void GPIOInit(u32 portNum, bool output, u32 mask) {
  if ((portNum == 0) || (portNum == 1))  {
    SCS |= GPIOM; /* set GPIOx to use Fast I/O */
  }
  if (output) {
    (*(volatile unsigned long *)(HS_PORT_DIR_BASE
                                 + portNum * HS_PORT_DIR_INDEX)) |= mask;
  } else {
    (*(volatile unsigned long *)(HS_PORT_DIR_BASE
                                 + portNum * HS_PORT_DIR_INDEX)) &= ~mask;
  }
}

extern "C" void debugRGB(int rgbBits, int wait);
void debugRGB(int rgbBits, int wait) {
  volatile int i, j;
  
  GPIOInit(2, true, LED_RED_MASK);
  GPIOInit(2, true, LED_GREEN_MASK);
  GPIOInit(2, true, LED_BLUE_MASK);

  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 < 100000; ++j) { }
  }
}

void setPinSelFunction(u32 port,u32 pinNumber,u32 function) {
  u32 bits = SFB_PINSEL_REGISTER(port,pinNumber);
  BIT_PAIR_AT_POSITION(bits,function,(pinNumber&0xf)<<1); /* Select appropriate function at position */
  SFB_PINSEL_REGISTER(port,pinNumber) = bits;
}

#define GET_COLOR_PIN_COLOR(color) GET_MAP4BY8(MAP4BY8(BODY_RGB_RED_PIN,BODY_RGB_GREEN_PIN,BODY_RGB_BLUE_PIN,0),color)
extern "C" void _apiError_(u32 blinkCode,const char * file, int lineno);
void _apiError_(u32 blinkCode,const char * file, int lineno) {
  _dieOnBoard_(blinkCode,file,lineno);
}

const u8 resetBeforeDying[] = {
  NORTH_LED_PIN,
  SOUTH_LED_PIN,
  EAST_LED_PIN,
  WEST_LED_PIN,
  BODY_RGB_RED_PIN,
  BODY_RGB_GREEN_PIN,
  BODY_RGB_BLUE_PIN,
  BODY_SWITCH0_PIN
};

#define BLINK_LOOP_MAX_MS 10000
static u8 deathWhileDying IN_BODYRAM_BSS;
void _dieOnBoard_(u32 blinkCode,const char * file, int lineno) {

  disableInterrupts();

  if (watchdogActive() && watchdogGetInterval() < BLINK_LOOP_MAX_MS) {
    watchdogSetInterval(BLINK_LOOP_MAX_MS); // Enough time to get around our blink loop
  }

  if (deathWhileDying++)        // Don't get fancy if we can't even die right..
    blinkCode = E_BUG_RECURSIVE_DEATH; 
  else {                        // Try a few last rites..

    for (u32 i = 0; i < sizeof(resetBeforeDying)/sizeof(resetBeforeDying[0]);++i) {
      pinMode(resetBeforeDying[i],REFLEX_MODE);
    }

    if (TEST_REFLEX_FLAGS(RF_KARMIC_REBIRTH)) { /* They want us to invalidate the sketch; let's try.. */
      setBootBlockFlags(getBootBlockFlags()|BOOTBLOCK_KILL_SKETCH) ;
    }
  }

  int i, color;
  u32 count;
  u32 loops = 0;

  if (TEST_REFLEX_FLAGS(RF_OKAY_NEW_GUY))   /* If they want auto restart, then do just ten loops */
    loops = 10;                             /* (else do four billion..) */

  if (TEST_REFLEX_FLAGS(RF_DIE_ALOUD)) {
    NorthFace.begin(9600,false);
    NorthFace.switchToBlocking();
  }
  while (--loops) {

    watchdogFeed();             // noop if watchdog not started..

    if (TEST_REFLEX_FLAGS(RF_DIE_ALOUD)) {
      NorthFace.println("S!");  // Shortcircuit BRN if the other end wants it
      NorthFace.println("SC");  //   ditto
      NorthFace.print("D ");
      if (file) {
        NorthFace.print(file);
        NorthFace.print(":");
        NorthFace.print(lineno);
      } else {
        NorthFace.print("@0x");
        NorthFace.print(lineno, HEX);
      }
      NorthFace.print(": BLINKCODE(");
      for (color = 0; color < 3; ++color) {    /* RGB all off during the X's */
        if (color != 0)
          NorthFace.print(",");
        NorthFace.print(GET_BLINK_MAP_COLOR(blinkCode,color));
      }
      NorthFace.println(")");
    }

    for (color = 0; color < 3; ++color) {    /* RGB all off during the X's */
      int pin = GET_COLOR_PIN_COLOR(color);
      implDigitalWrite(pin, HIGH); /* Note 'impl' -- We are dying!  Bypassing sketch control! */
    }

    for (i = 0; i < 9; ++i) { /* First show the X's */
      int v0, v1;
      if (i&1) { v0 = (i>>1)&1; v1 = !v0; }
      else { v0 = v1 = 1; }

      implDigitalWrite(NORTH_LED_PIN, v0);
      implDigitalWrite(SOUTH_LED_PIN, v0);

      implDigitalWrite(EAST_LED_PIN, v1);
      implDigitalWrite(WEST_LED_PIN, v1);
      delayMicroseconds(125*1000);
    }
    delayMicroseconds(150*1000);
    for (color = 0; color < 3; ++color) {       /* Now show the code */
      int pin = GET_COLOR_PIN_COLOR(color);
      for (count = 0; count < GET_BLINK_MAP_COLOR(blinkCode,color); ++count) {
        implDigitalWrite(pin, LOW);
        delayMicroseconds(150*1000);
        implDigitalWrite(pin, HIGH);
        delayMicroseconds(250*1000);
      }
      delayMicroseconds(400*1000);
    }
    if (implDigitalRead(BODY_SWITCH0_PIN)==LOW)   /* If button down at end of sequence, break back to bootloader */
      break;
  }
  //reenterBootloader();
  reenterBrainstem();
}

