/*                                              -*- mode:C; fill-column:100 -*-
  startup.c - The SFB C-level entry point
  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
*/

#include "SFBAssert.h"
#include "SFBProcessor.h"       /* for SpeedChangeHandlerBlock */
#include "SFBTicker.h"          /* for Ticker, etc */
#include "SFBErrors.h"          /* for E_NO_BOOTLOADER */
#include "SFBPower.h"           /* for initADC, initPowerOuts */
#include "SFBReflex.h"          /* for reflex_dispatch */
#include "SFBSerial.h"          /* for Faces */
#include "SFBCodeFlow.h"        /* for codeFlow_startup_initializtion */
#include "SFBEeprom.h"          /* for eeprom_startup_initializtion */
#include "SFBBootBlock.h"       /* for bootblock_startup_initializtion */
#include "SFBHWMisc.h"          /* for init_stack_canary */
#include "timer.h"              /* for initTimer0 */
#include "irq.h"                /* for init_VIC, IENABLE */
#include "LPC23xx.h"

//#define DEBUG 0

static void reinitUARTs() {
  SFBHWSerial::reinitAll();
}

static SpeedChangeHandlerBlock * blockRoot IN_BODYRAM_BSS;
void registerSpeedChangeHandlerBlock(SpeedChangeHandlerBlock * block) {
  for (SpeedChangeHandlerBlock * b = blockRoot; b; b = b->next)
    if (block == b) return;     /* No effect if already registered */
  block->next = blockRoot;      /* Else push on front of list */
  blockRoot = block;
}

// Called after clock speed change.
extern "C"
void reinitPeripherals() {

  // First reinit required core components
  reinitADC();
  reinitTimer0();
  reinitUARTs();

  // Now reinit any registered optional components
  for (SpeedChangeHandlerBlock * b = blockRoot; b; b = b->next)
    b->handler();
}

typedef void (*nvfunc)(); 
extern "C"
void cISRWrapper(uptr isrAddr) {
  nvfunc cISRAddress = (nvfunc) isrAddr;
  Ticker.pushTimeISR(TICKER_OTHER_ISR);
  (*cISRAddress)();
  Ticker.popTimeISR();
}

void hang() { while (1) ; }

#ifdef DEBUG
void printRegister(volatile long unsigned * address) {
    NorthSerial.print("REG @ 0x");
    NorthSerial.print((int) address, 16);
    NorthSerial.print("=");
    NorthSerial.println((int) (*address), 16);
}
void printRegister(const char * msg, volatile long unsigned * address) {
  NorthSerial.print(msg);
  printRegister(address);
}
void printValue(const char * msg, u32 value) {
  NorthSerial.print(msg);
  NorthSerial.println(value,DEC);
}
void printInterruptTable(volatile long * INTTAB) {
  int i;
  NorthSerial.println("exception vectors");
  for (i = 0; i < 32; ++i) {
    NorthSerial.print((int) &INTTAB[i],16);
    NorthSerial.print(" ");
    NorthSerial.println(INTTAB[i],16);
  }
  printRegister(&VICIntEnable);
}

void printVICSlot(int slot) {
  NorthSerial.print("VICSLOT ");
  NorthSerial.print(slot);
  NorthSerial.print("=");
  printRegister((&VICVectAddr0)+slot);
}

void printCPSR() {
  u32 reg;
  asm volatile("MRS %[result],CPSR" : [result] "=r" (reg) :: "memory");
  NorthSerial.print(" CPSR_c==");
  NorthSerial.println((int) reg, 2);
}

#endif

void leds_off() {
  implDigitalWrite(BODY_RGB_RED_PIN, HIGH);
  implDigitalWrite(BODY_RGB_GREEN_PIN, HIGH);
  implDigitalWrite(BODY_RGB_BLUE_PIN, HIGH);

  implDigitalWrite(NORTH_LED_PIN, HIGH);
  implDigitalWrite(SOUTH_LED_PIN, HIGH);
  implDigitalWrite(EAST_LED_PIN, HIGH);
  implDigitalWrite(WEST_LED_PIN, HIGH);

  implDigitalWrite(NORTH_SIDE_LED_PIN, HIGH);
  implDigitalWrite(SOUTH_SIDE_LED_PIN, HIGH);
  implDigitalWrite(EAST_SIDE_LED_PIN, HIGH);
  implDigitalWrite(WEST_SIDE_LED_PIN, HIGH);
}

void leds_on() {
  implDigitalWrite(BODY_RGB_RED_PIN, LOW);
  implDigitalWrite(BODY_RGB_GREEN_PIN, LOW);
  implDigitalWrite(BODY_RGB_BLUE_PIN, LOW);

  implDigitalWrite(NORTH_LED_PIN, LOW);
  implDigitalWrite(SOUTH_LED_PIN, LOW);
  implDigitalWrite(EAST_LED_PIN, LOW);
  implDigitalWrite(WEST_LED_PIN, LOW);

  implDigitalWrite(NORTH_SIDE_LED_PIN, LOW);
  implDigitalWrite(SOUTH_SIDE_LED_PIN, LOW);
  implDigitalWrite(EAST_SIDE_LED_PIN, LOW);
  implDigitalWrite(WEST_SIDE_LED_PIN, LOW);
}


extern "C" void flashAll(int count);

void flashAll(int count) {
  volatile int i;
  const int ON_COUNT = 100000;
  const int OFF_COUNT = 800000;
  leds_off();
  for (int j = 0; j < count; ++j) {
    for (i = 0; i < OFF_COUNT/2; ++i) ;
    leds_on();
    for (i = 0; i < ON_COUNT; ++i) ;
    leds_off();
    for (i = 0; i < OFF_COUNT/2; ++i) ;
  }
}

extern void initProgramServerSketch();

static void reflex_initialization() {

  pins_startup_initialization();

  initProgramServerSketch();

  reflex_startup_initialization();

  disableInterrupts();          // If they were enabled..

  init_VIC();

  u32 ok = initTimer0();
  API_ASSERT_TRUE(ok);

  disable_timer0();             // Don't start ticking yet

  initADC();

  setADCSensingEnabled(true);   // Check voltages, by default

  setPowerSensingEnabled(false); // But not currents, by default

}

/* Take over any durable lease resources that the sketch didn't */
static void post_setup_defaults() {
  for (u32 f = NORTH; f <= WEST; ++f) {
    if (!Faces[f].isInUse()) {  /* Faces not otherwise grabbed during setup().. */
      //pinMode(pinInFace(FACE_LED_PIN,f),REFLEX_MODE);
      Faces[f].begin();       /* ..default to full packet processing! */
    }
  }
}

extern void setup() __attribute__ ((weak)); /* Defer to real sketch setup if one is loaded.. */
void setup() {  BUG_CODE("setup",E_BUG_NO_SETUP);  }   /* ..else complain about the lack */

extern void loop() __attribute__ ((weak));  /* Defer to real sketch loop if one is loaded.. */
void loop() { BUG_CODE("loop",E_BUG_NO_LOOP); } /* ..else complain about that */

static void remapInterrupts() {
  MEMMAP = MEMMAP_SRAM; 
}

#if 0
static void various_crapouts() {
  asm volatile(
               "LDR r0,=0x41000000\n" // Load bad address, then
               "BX r0\n"              // jump into hyperspace - tests diePabtException
               ".word 0x0f000000\n"   // OR: SWI opcode - tests dieSWIException
               ".word 0x06000010\n"   // OR: Undefined opcode - tests dieUndefException
               "MRS r0, CPSR\n"       // Read status register
               "BIC r1, r0, #0xdf\n"  // Clear all CPSR_c bits except T
               "ORR r1, r1, #0x10\n"  // OR: Set USR mode - tests dieDabtException
               "MSR CPSR_c, R1\n"     // Update register
               ::: "r0", "r1");       // We're clobbering r0 and r1
}
#endif


extern "C" void sketchEarlyInits();
void sketchEarlyInits() {

  init_stack_canary();

  reflex_initialization();

  eeprom_startup_initialization();

  bootblock_startup_initialization();  

  initPowerOuts();
}

extern "C" void sketchLateInits();
void sketchLateInits() {

  remapInterrupts();

  enableInterruptsFunction();

  print_startup_initialization();

  enable_timer0(); 

  Ticker.reset();

  SFBSerial::startup_initialization();

  bootblock_bootcount_initialization();  // Bumps burncount+inits randomSeed, unless overridden

}

extern "C" void runSketch();
void runSketch() {

  leds_off();
  setup();                   /* Finally!  Run user's setup routine! */
  post_setup_defaults();     /* Do various last minute one-time work */

  while (1) {                /* Then forever, */
    reflex_dispatch(millis()+5); /* take up to 5ms to run the body, and */
    loop();                  /* then do some of the user's business */
  }

}
