#include <mega32.h>
#include <stdio.h>

#ifndef path
#include "raster.h"
flash unsigned char rbit[] = {0x80,0x40,0x20,0x10,0x08,0x04,0x02,0x01};
#else /* path */
#include "path.h"
#endif /* path */

signed long rcount;
signed long tcount;
unsigned char pwmcount;
unsigned char rspeed, tspeed;
unsigned char lastin, curin; // Port data.

#define R_POS   0b00100000
#define R_NEG   0b10000000
#define T_POS   0b00001000
#define T_NEG   0b00000010
#define T_EN    0b00000100
#define PEN_POS 0b01000000
#define PEN_NEG 0b00010000

#define ALL_BRAKE 0b00000000

// Sensor inputs
#define Theta0(x)    ((x) & 0b0100)
#define Theta1(x)    ((x) & 0b1000)
#define RMinLimit(x) ((x) & 0b01000000)

// Motor States
#define MOTOR_BRAKE 0
#define MOTOR_POS 1
#define MOTOR_NEG 2
#define MOTOR_FREESPIN 4
unsigned char rMotorState, tMotorState, penMotorState;

// Speeds
#define T_INITIAL_SPEED 8    
#define PEN_DOWNSPEED 3
#define PEN_UPSPEED 4
#define PEN_UPTIME 128

/* r motor full range measures:
  3d574
  3d503 */
  
/* theta motor full range is 1800 counts as spec'ed */

// Limits
#define THETA_MIN 0
#define THETA_MAX 1000 

// Steps
// make 256 steps in r, assuming 10% of the range is inaccessable.
#define RCOUNT_PER_POS 882
// make 256 steps in theta.
#define TCOUNT_PER_POS 7

// Feedback control
#define R_ERROR_SCALE 1
#ifndef path
signed long rGoal;
#else /* path */
#define T_ERROR_SCALE 1
unsigned long rGoal, tGoal;
#endif /* path */
unsigned char rPos, tPos, curPen, lastPen;

#ifdef path
// Path control
unsigned char curGoal;
#define R_THRESH 100
#define T_THRESH 100
#endif /* path */

void fatal_error(flash char* errstr)
{
#asm("cli")
  PORTA = ALL_BRAKE;
  putsf(errstr);
  while(1);
}

// External interrupt: Called on rising edge of R0
// Tracks r motor position.
interrupt [EXT_INT0] void ext_int0(void)
{
  // Posedge of r0.
  if (PIND & 0b1000)
    rcount++;
  else
    rcount--;
}

// Timer 0: track theta motor count
interrupt [TIM0_COMP] void timer0_comp(void)
{
  curin = PINB;

  // Update count on posedge of Theta0
  if (Theta0(curin) && !Theta0(lastin)) {
    // Posedge of theta0.
#ifdef theta1_works
    if (Theta1(curin))
      tcount++;
    else
      tcount--;
#else
	if (tMotorState == MOTOR_POS)
	  tcount++;
	else
	  tcount--;
#endif
  }

  // R min limit switch and calibration. (switch active low)
  if (!RMinLimit(curin)) {
    // Reset r count to 0 and brake motor.
    rMotorState = MOTOR_BRAKE;
    rcount = 0;
  }

  lastin = curin;
}

unsigned int time0;

interrupt [TIM2_COMP] void timer2_comp(void)
{
  unsigned char motorstate;
  
  // Run motor PWMs:
  pwmcount++;
  if (pwmcount == 8) pwmcount = 0;

  motorstate = 0;
  // r motor
  if (pwmcount < rspeed) {
    switch(rMotorState) {
    case MOTOR_BRAKE:
      // (already off).
      break;
    case MOTOR_POS:
      motorstate |= R_POS;
      break;
    case MOTOR_NEG:
      motorstate |= R_NEG;
      break;
    case MOTOR_FREESPIN:
      fatal_error("r motor can't freespin!");
      break;
    default:
      fatal_error("Unknown r motor state.");
    }
  }

  // theta motor
  if (tMotorState != MOTOR_FREESPIN) {
    motorstate |= T_EN;
	if(pwmcount < tspeed) {
      switch(tMotorState) {
      case MOTOR_BRAKE:
        // (already off).
        break;
      case MOTOR_POS:
        motorstate |= T_POS;
        break;
      case MOTOR_NEG:
        motorstate |= T_NEG;
        break;
      default:
        fatal_error("Unknown theta motor state.");
      }
    }
  }

  // pen motor is special -- it has 3 states:
  // pen up: brake
  // pen down: const pwm down
  // pen MOVING UP: const pwm up, then moves to brake
  // 
  // the state encoding is:
  //  state == 0 => pen up
  //  0 < state < 255 => pen moving up (decrements)
  //  state == 255 => pen down

  // motorstate |= PEN_ENABLE; not enough bits for that! <grin>

  if (penMotorState == 255 && pwmcount < PEN_DOWNSPEED) // pen down => pwm down
    motorstate |= PEN_POS;
  else if (penMotorState != 0 && pwmcount < PEN_UPSPEED) { // pen moving up => pwm up, dec motorState
    motorstate |= PEN_NEG;

    // Dec pen motor state every full PWM cycle.
    if (pwmcount == 0)
      penMotorState--;
  }

  // Set output motor state.
  PORTA = motorstate;
  
  if (time0 != 0) time0--;
}


void init(void)
{
  // set up input and output ports.
  // PORT A: motor control outputs
  DDRA = 0xff;
  // PORT B:
  //  B.2, B.3: theta motor inputs
  //  B.5, B.6: limit switch
  //  set B.5 to output low, B.6 to input pullup
  DDRB =  0b00101100;
  PORTB = 0b01000000;
  // PORT C: unused.
  // PORT D:                       
  //  D.0, D.1: USART rx,tx, for debugging
  //  D.2, D.3: external interrupt for r motor
  //  D.7: low. For GND. :)
  DDRD = 0b10000011;
  PORTD = 0b00000000;
  
  rcount = tcount = 0;

  // Init the UART
  UCSRB = 0x18; //turn on tx and rx, but not interrupts
  UBRRL = 103;	//set baud to 9600
  putsf("\r\n starting...\r\n"); //indicate start
  putsf("\r\n starting...\r\n"); //indicate start

  // Set up timer 0
  TCNT0=0;
  TCCR0=0b00001011;		//prescalar to 64 
  OCR0=62;		//63 ticks = 252 us

  // Set up INT0 rising edge external interrupt.
  GICR|=0x40;
  MCUCR=0x03;
  MCUCSR=0x00;
  GIFR=0x40;
  
  // Set up timer 2 (motor PWM)
  TCNT2=0;
  TCCR2=0b00001011; // prescalar to 64
  OCR2=200; // set pwm period

  // Enable timer interrupts
  TIMSK=0b10000010;		//turn on timer 0 and 2 cmp ISRs

#asm("sei")
}


void penUp(void)
{
  // Only pull the pen up if it was actually down.
  if (penMotorState == 255)
    penMotorState = PEN_UPTIME;
}

void penDown(void)
{
  penMotorState = 255; // pen down.
}

#ifdef path
void setGoal(void)
{
  rGoal = rPosToCount(path[curGoal].r);
  tGoal = tPosToCount(path[curGoal].t);
  if (path[curGoal].p)
    penDown();
  else
    penUp();
}
#endif /* path */


void main(void)
{
  init();

#ifdef MOVE_R_POSITIVE
  rMotorState = MOTOR_POS;
  rspeed = 8;
  while(1);
#endif

  // Pull the pen up.
  penMotorState = PEN_UPTIME;

  // First, turn r to slow negative and wait for the limit switch.
  rspeed = 6;
  rMotorState = MOTOR_NEG;
  while (RMinLimit(PINB));
  rMotorState = MOTOR_BRAKE;
  rspeed = 0;
  
#ifndef path
  // Start theta motor.
  tMotorState = MOTOR_POS;
  tspeed = T_INITIAL_SPEED;
#else /* path */
  // Now, start drawing the path.
  curGoal = 0;
  setGoal();
#endif /* path */

  while (1) {
#ifndef path
    signed long rError, rErrorAbs;
#else /* path */
    signed long rError, tError, rErrorAbs, tErrorAbs;
#endif /* path */
  
    // Avoid race conditions between us and the interrupts by
    // saving the counts temporarily here.
    unsigned long _rcount, _tcount;
    _rcount = rcount;
    _tcount = tcount;

#ifndef path
    /* main raster scan code: */

    // Check for theta wraparound.
    if (_tcount >= THETA_MAX && tMotorState == MOTOR_POS ||
	_tcount <= THETA_MIN && tMotorState == MOTOR_NEG) {
      
      // Are we done?
      if (rPos == MAX_R) {
	// Done.
	rMotorState = tMotorState = penMotorState = MOTOR_BRAKE;
	while(1);
      }

      // Advance r motor.
      rGoal = _rcount + RCOUNT_PER_POS;

      // Reverse theta motor.
      if (tMotorState == MOTOR_POS) tMotorState = MOTOR_NEG;
      else tMotorState = MOTOR_POS;
    }

    // Determine r and theta positions.
    rPos = rcount / RCOUNT_PER_POS;
    tPos = tcount / TCOUNT_PER_POS;

    // Get bit from raster.
    curPen = (raster[rPos][tPos >> 3] & rbit[tPos & 0b111]) != 0;

    if (curPen == 0 && lastPen == 1)
      penUp();
    else
      penDown();
    
    lastPen = curPen;
#endif /* ! path */

    /* feedback control is common to both. */

    // Feedback control for the r motor goal.
    rError = rGoal - rcount;

    // Really really simple first-order control response.
    rMotorState = MOTOR_BRAKE;
    if (rError > 0) {
      rErrorAbs = rError;
      rMotorState = MOTOR_POS;
    } else if (rError < 0) {
      rErrorAbs = -rError;
      rMotorState = MOTOR_NEG;
    }

    rErrorAbs >>= 7;
    if (rErrorAbs < 8) rspeed = rErrorAbs;
    else rspeed = 8;

#ifdef path
    // only path needs t feedback.

    // Feedback control for the t motor goal.
    tError = tGoal - tcount;
    // Really really simple first-order control response.
    tMotorState = MOTOR_BRAKE;
    if (tError > 0) {
      tMotorState = MOTOR_POS;
      tErrorAbs = tError;
    } else if (tError < 0) {
      tMotorState = MOTOR_NEG;
      tErrorAbs = -tError;
    }
    tspeed = tErrorAbs * T_ERROR_SCALE;


    // main path control code:

    // Check for moving to the next goal item.
    if (rErrorAbs < R_THRESH && tErrorAbs < T_THRESH) {
      // Move to next goal item.
      curGoal++;
      if (path[curGoal].p == 255) { // check for done flag
	rMotorState = tMotorState = penMotorState = MOTOR_BRAKE;
	while(1);
      }
      // Otherwise, set a course, warp 9, engage.
      setGoal();
    }
#endif /* path */

#ifdef test
    if (time0 == 0) {
      time0 = 100;
      printf("%01x, %01x, %01x\r\n", lastcount, rcount, rcount-lastcount);
    }
#endif
  }
}