void			fill_cone(t_env *env, int fd)
{
	char	*line;
	char	*temp;
	int		i;

	i = 0;
	while (get_next_line(fd, &line) == 1 && i < OBJ.num_cone)
	{
		if (*line != '#')
		{
			temp = line;
			temp = set_temp(++temp, 'R');
			set_vector(&CONES[i].rot, temp);
			temp = set_temp(temp, 'P');
			set_vector(&CONES[i].p, ++temp);
			temp = set_temp(++temp, 'V');
			set_vector(&CONES[i].v, ++temp);
			rotate_cone(env, i);
			temp = set_temp(++temp, 'a');
			CONES[i].alpha = ft_atoi(++temp);
			temp = set_temp(++temp, 'm');
			CONES[i].shape.material = ft_atoi(++temp);
			i++;
		}
		free(line);
	}
}

void dbg_init() {
        asm {
            ldi   r1,#60
            sc    r1,LEDS
        }
     set_vector(496,dbg_irq);     // breakpoint interrupt
        asm {
            ldi   r1,#61
            sc    r1,LEDS
        }
     set_vector(495,dbg_irq);     // single step interrupt
        asm {
            ldi   r1,#62
            sc    r1,LEDS
        }
     ssm = 0;
     bmem = (unsigned byte *)0;
     cmem = (unsigned char *)0;
     hmem = (unsigned short int *)0;
     wmem = (unsigned int *)0;
        asm {
            ldi   r1,#66
            sc    r1,LEDS
        }
     curaddr = 0x10000;
     muol = 16;
     cursz = 'b';
     curfmt = 'x';
     currep = 1;
     dbg_dbctrl = 0;
        asm {
            ldi   r1,#69
            sc    r1,LEDS
        }
}

double ddot(const int N, const double *a, const int incx, const double *b, const int incy)
{
	int i;
	
	vtype q00 = set_vector(0.); 
	vtype q01 = set_vector(0.); 
	vtype q0a, q1a;
	vtype q0b, q1b;
	//
	double c;
	//
	for (i = 0; i < N - N%4; i = i + 4)
	{
		q0a = LOAD(a + i);
		q0b = LOAD(b + i);
		q00 = vfmaq_f64(q00, q0a, q0b);	
		//q0a = vmulq_f64(q0a, q0b);
		//q00 = vaddq_f64(q0a, q00);
		//
		q0a = LOAD(a + i + 2);
                q1b = LOAD(b + i + 2);
                q01 = vfmaq_f64(q01, q0a, q0b);
                //q1a = vmulq_f64(q1a, q1b);
                //q01 = vaddq_f64(q1a, q01);
		//c += a [i]*b [i];
	}
	c = q00[0] + q00[1] + q01[0] + q01[1];
	return c;
}

//main function, execution starts here
main()
{
    int i = 0;
    vid_init();//initialize video

    printf("MTX starts in main()\n");
    init(); // initialize and create P0 as running
    set_vector(80, int80h);

    kfork("/bin/u1");     // P0 kfork() P1

    set_vector(9, kbinth);
    kbd_init();

    //timer init
    lock();
    set_vector(8,tinth);
    timer_init();

    while(1)
    {
      unlock();
      if(readyQueue)
          tswitch();
      else
          halt();
   }
}

/**
void   torus(WSGraph gd, vector ox, vector ex, int rr, int ra, int cc)

3D的なトーラスの描画．中身はccで塗りつぶされる．

@param  gd  操作対象となるグラフィックデータ構造体．
@param  ox  トーラスの中心の座標ベクトル．
@param  ex  トーラスの中心の法線ベクトル．
@param  rr  トーラスの半径(トーラスの中心から断面の円の中心まで)．
@param  ra  トーラスの断面の円の半径
@param  cc  線と塗りつぶしの濃度．
*/
void   torus(WSGraph gd, vector ox, vector ex, int rr, int ra, int cc)
{
	int	 i, nn;
	float   dt, th, xx, yy, zz, sn, cs;
	WSGraph vp;
	vector  ve, vo, vz;

	vp = make_WSGraph(2*(rr+ra)+3, 2*(rr+ra)+3, 2*ra+3);
	if (vp.gp==NULL) return;
	nn = (int)(2*PI*(rr+ra)*2);
	dt = (float)(2.0*PI/nn);

	zz = (float)((vp.zs-1)/2.);
	for (i=0; i<nn; i++) {
		th = dt*i;
   		sn = (float)sin(th);
   		cs = (float)cos(th);
		xx = (float)((vp.xs-1)/2. + rr*cs);
		yy = (float)((vp.ys-1)/2. - rr*sn);
		vo = set_vector(xx, yy, zz);
		ve = set_vector(sn, cs, 0.0);
		circle3d(vp, vo, ve, ra, cc, ON);
	}
	vz = set_vector((float)((vp.xs-1)/2.), (float)((vp.ys-1)/2.), (float)((vp.zs-1)/2.));
	ex = unit_vector(ex);

	local2world(gd, vp, ox, vz, ex, NULL, NULL);
	free(vp.gp);
	return;
}

static void	init_cylinder(t_env *env, t_tobj *tobj, t_cylinder *obj)
{
	tobj->rot = mtx_createscalemtx(1, 1, 1);
	tobj->scale = mtx_createscalemtx(1, 1, 1);
	obj->radius = 1;
	obj->h = 1;
	obj->mat = &env->base_material;
	set_vector(&obj->aabb[0], -1.0 / 0.0, -1.0 / 0.0, -1.0 / 0.0);
	set_vector(&obj->aabb[1], 1.0 / 0.0, 1.0 / 0.0, 1.0 / 0.0);
}

void Install_clock(
  rtems_isr_entry clock_isr
)
{
  volatile struct z8036_map *timer;

  Clock_driver_ticks = 0;
  Clock_isrs = rtems_configuration_get_microseconds_per_tick() / 1000;

  Old_ticker = (rtems_isr_entry) set_vector( clock_isr, CLOCK_VECTOR, 1 );
  timer = (struct z8036_map *) 0xfffb0000;
  timer->MASTER_INTR        = MICRVAL;
  timer->CT1_MODE_SPEC      = T1MSRVAL;

  *((uint16_t*)0xfffb0016) = MS_COUNT;  /* write countdown value */

  /*
   *  timer->CT1_TIME_CONST_MSB = (MS_COUNT >> 8);
   *  timer->CT1_TIME_CONST_LSB = (MS_COUNT &  0xff);
   */

  timer->MASTER_CFG         = MCCRVAL;
  timer->CT1_CMD_STATUS     = T1CSRVAL;

  /*
   * Enable interrupt via VME interrupt mask register
   */
  (*(uint8_t*)0xfffb0038) &= 0xfd;

  atexit( Clock_exit );
}

bool
c_collide(dvec3_t start, dvec3_t end, collision_t *output)
{
	float point[3];
	float vel[3];

	start.y-=60;end.y-=60;

	point[0]=start.x;point[1]=start.y;point[2]=start.z;
	vel[0]=end.x-start.x;
	vel[1]=end.y-start.y;
	vel[2]=end.z-start.z;

	c_setpool(start, end);
	trace=CollideAndSlide(point, vel);

	bool value=trace->foundCollision;

	m_free(pool);
	pool=NULL;

	output->collision=value;
	set_vector(&output->start, trace->finalPosition[0], trace->finalPosition[1], trace->finalPosition[2]);

	output->start.y+=60;

	return !value;
}

void console_initialize_interrupts( void )
{
  volatile Ring_buffer_t     *buffer;
  Console_Protocol  *protocol;
  int               i;

  for ( i=0 ; i < NUM_Z85C30_PORTS ; i++ ) {
    protocol = Ports_85C30[i].Protocol;

    /*
     * Initialize the ring buffer and set to not transmitting.
     */
    buffer = &protocol->TX_Buffer;
    Ring_buffer_Initialize( buffer );
    protocol->Is_TX_active = false;
  }

  /*
   * Connect each vector to the interupt service routine.
   */
  for (i=0; i < NUM_Z85C30_CHIPS; i++)
    set_vector( console_isr, Chips_85C30[i].vector, 1 );
  #warning "Install interrupts using proper method for PIC vectors."

  atexit( console_exit );

}

rtems_device_driver Clock_control(
  rtems_device_major_number major,
  rtems_device_minor_number minor,
  void *pargp
)
{
    uint32_t         isrlevel;
    rtems_libio_ioctl_args_t *args = pargp;

    if (args == 0)
        goto done;

    /*
     * This is hokey, but until we get a defined interface
     * to do this, it will just be this simple...
     */

    if (args->command == rtems_build_name('I', 'S', 'R', ' '))
    {
        Clock_isr(Clock_driver_vector);
    }
    else if (args->command == rtems_build_name('N', 'E', 'W', ' '))
    {
      rtems_interrupt_disable( isrlevel );
       (void) set_vector( args->buffer, Clock_driver_vector, 1 );
      rtems_interrupt_enable( isrlevel );
    }

done:
    return RTEMS_SUCCESSFUL;
}

/* benchmark_timer_initialize --
 *     Initialize timer 2 for accurate time measurement.
 *
 * PARAMETERS:
 *     none
 *
 * RETURNS:
 *     none
 */
void
benchmark_timer_initialize(void)
{
    /* Catch timer2 interrupts */
    set_vector(timerisr, BSP_INTVEC_TIMER2, 0);

    /* Initialize interrupts for timer2 */
    *MCF5206E_ICR(MBAR, MCF5206E_INTR_TIMER_2) =
        MCF5206E_ICR_AVEC |
        ((BSP_INTLVL_TIMER2 << MCF5206E_ICR_IL_S) & MCF5206E_ICR_IL) |
        ((BSP_INTPRIO_TIMER2 << MCF5206E_ICR_IP_S) & MCF5206E_ICR_IP);

    /* Enable interrupts from timer2 */
    *MCF5206E_IMR(MBAR) &= ~MCF5206E_INTR_BIT(MCF5206E_INTR_TIMER_2);

    /* Reset Timer */
    *MCF5206E_TMR(MBAR, 2) = MCF5206E_TMR_RST;
    *MCF5206E_TMR(MBAR, 2) = MCF5206E_TMR_ICLK_STOP;
    *MCF5206E_TMR(MBAR, 2) = MCF5206E_TMR_RST;
    *MCF5206E_TCN(MBAR, 2) = 0; /* Zero timer counter */
    Timer_interrupts = 0; /* Clear timer ISR counter */
    *MCF5206E_TER(MBAR, 2) = MCF5206E_TER_REF | MCF5206E_TER_CAP; /*clr pend*/
    *MCF5206E_TRR(MBAR, 2) = TRR2_VAL - 1;
    *MCF5206E_TMR(MBAR, 2) =
        (((BSP_SYSTEM_FREQUENCY / 1000000) << MCF5206E_TMR_PS_S) &
          MCF5206E_TMR_PS) |
        MCF5206E_TMR_CE_NONE | MCF5206E_TMR_ORI | MCF5206E_TMR_FRR |
        MCF5206E_TMR_RST;
    *MCF5206E_TMR(MBAR, 2) |= MCF5206E_TMR_ICLK_MSCLK;
}

static gnm_float *
compute_gradient (GnmNlsolve *nl, const gnm_float *xs)
{
	gnm_float *g;
	gnm_float y0;
	const int n = nl->vars->len;
	int i;

	set_vector (nl, xs);
	y0 = get_value (nl);

	g = g_new (gnm_float, n);
	for (i = 0; i < n; i++) {
		gnm_float x0 = xs[i];
		gnm_float dx;
		gnm_float y1;
		gnm_float eps = gnm_pow2 (-25);

		if (x0 == 0)
			dx = eps;
		else
			dx = gnm_abs (x0) * eps;

		set_value (nl, i, x0 + dx);
		y1 = get_value (nl);
		g[i] = (y1 - y0) / dx;

		set_value (nl, i, x0);
	}

	return g;
}

void bfin_interrupt_init(void) {
  int source;
  int vector;
  uint32_t r;
  int i;
  int j;

  globalMask = ~(uint32_t) 0;
  *(uint32_t volatile *) SIC_IMASK = 0;
  memset(vectors, 0, sizeof(vectors));
  /* build mask showing what SIC sources drive each CEC vector */
  source = 0;
  for (i = 0; i < SIC_IAR_COUNT; i++) {
    r = *(uint32_t volatile *) (SIC_IAR_BASE_ADDRESS + i * SIC_IAR_PITCH);
    for (j = 0; j < 8; j++) {
      vector = r & 0x0f;
      if (vector >= 0 && vector < CEC_INTERRUPT_COUNT) {
        if (vectors[vector].mask == 0)
          /* install our local handler */
          set_vector(interruptHandler, vector + CEC_INTERRUPT_BASE_VECTOR, 1);
        vectors[vector].mask |= (1 << source);
      }
      r >>= 4;
      source++;
    }
  }
}

			void move()
			{
				set_vector(trans * get_vector());
				character::move();
				if(!get_position().is_inside(paint::rect(-5, -5, 805, 605)))
					set_active(false);
			}

void bsp_spurious_initialize()
{
  uint32_t trap;
  uint32_t level;
  uint32_t mask;

  level = sparc_disable_interrupts();
  mask = LEON_REG.Interrupt_Mask;

  for ( trap=0 ; trap<256 ; trap++ ) {

    /*
     *  Skip window overflow, underflow, and flush as well as software
     *  trap 0 which we will use as a shutdown. Also avoid trap 0x70 - 0x7f
     *  which cannot happen and where some of the space is used to pass
     *  paramaters to the program.
     */

    if (( trap == 5 || trap == 6 ) ||
    	(( trap >= 0x11 ) && ( trap <= 0x1f )) ||
    	(( trap >= 0x70 ) && ( trap <= 0x83 )))
      continue;

    set_vector(
        (rtems_isr_entry) bsp_spurious_handler,
        SPARC_SYNCHRONOUS_TRAP( trap ),
        1
    );
  }

  LEON_REG.Interrupt_Mask = mask;
  sparc_enable_interrupts(level);

}

/******************************************************
  Name: Install_clock
  Input parameters: the Clock Interrupt Subroutine
  Output parameters: -
  Description: initialize the periodic interval ticker
	       called by Clock_Initialize
 *****************************************************/
static void
Install_clock (rtems_isr_entry clock_isr)
{
	uint32_t   pitr_tmp;
	uint32_t   usecs_per_tick;

	Clock_driver_ticks = 0;

	set_vector (clock_isr, CLOCK_VECTOR, 1);

	/* sets the Periodic Interrupt Control Register PICR */
	/* voir a quoi correspond exactement le Clock Vector */

	SIMPICR = ( CLOCK_IRQ_LEVEL << 8 ) | ( CLOCK_VECTOR );

	/* sets the PITR count value */
	/* this assumes a 32.765 kHz crystal */

        usecs_per_tick = rtems_configuration_get_microseconds_per_tick();
	/* find out whether prescaler should be enabled or not */
	if ( usecs_per_tick <= 31128 ) {
	   pitr_tmp = ( usecs_per_tick * 8192 ) / 1000000 ;
	} else {
	   pitr_tmp = ( usecs_per_tick / 1000000 ) * 16;
	   /* enable it */
	   pitr_tmp |= 0x100;
	}

	SIMPITR = (unsigned char) pitr_tmp;

	atexit (Clock_exit);
}

void
__gnat_install_handler_common (int t1, int t2)
{
  uint32_t         trap;
  rtems_isr_entry previous_isr;

  sigaction (SIGSEGV, &__gnat_error_vector, NULL);
  sigaction (SIGFPE, &__gnat_error_vector, NULL);
  sigaction (SIGILL, &__gnat_error_vector, NULL);

  for (trap = 0; trap < 256; trap++)
    {

      /*
         *  Skip window overflow, underflow, and flush as well as software
         *  trap 0 which we will use as a shutdown. Also avoid trap 0x70 - 0x7f
         *  which cannot happen and where some of the space is used to pass
         *  paramaters to the program.  0x80 for system traps and
	 *  0x81 - 0x83 by the remote debugging stub.
	 *  Avoid two bsp specific interrupts which normally are used
	 *  by the real-time clock and UART B.
       */

      if ((trap >= 0x11) && (trap <= 0x1f))
	{
	  if ((trap != t1) && (trap != t2))
	    rtems_interrupt_catch (__gnat_interrupt_handler, trap, &previous_isr);
	}
      else if ((trap != 5 && trap != 6) && ((trap < 0x70) || (trap > 0x83)))
	set_vector (__gnat_exception_handler, SPARC_SYNCHRONOUS_TRAP (trap), 1);
    }
}

/*
 * Called via atexit()
 * Remove the clock interrupt handler by setting handler to NULL
 */
void
Clock_exit(void)
{
  /* disable PIT and PIT interrupts */
  m821.piscr &= ~(M821_PISCR_PTE | M821_PISCR_PIE); 
  
  (void) set_vector(0, PPC_IRQ_LVL0, 1);
}

void init_uart()
{
    set_vector(SI0_ADDRESS, 0);
    SCON = 0x52;
    TMOD = 0x20;
    TCON = 0x69;
    TH1 = 0xF4;
}

static gboolean
newton_improve (GnmNlsolve *nl, gnm_float *xs, gnm_float *y, gnm_float ymax)
{
	GnmSolver *sol = nl->parent;
	const int n = nl->vars->len;
	gnm_float *g, **H, *d;
	gboolean ok;

	g = compute_gradient (nl, xs);
	H = compute_hessian (nl, xs, g);
	d = g_new (gnm_float, n);
	ok = (gnm_linear_solve (H, g, n, d) == 0);

	if (ok) {
		int i;
		gnm_float y2, *xs2 = g_new (gnm_float, n);
		gnm_float f, best_f = -1;

		ok = FALSE;
		for (f = 1; f > 1e-4; f /= 2) {
			int i;
			for (i = 0; i < n; i++)
				xs2[i] = xs[i] - f * d[i];
			set_vector (nl, xs2);
			y2 = get_value (nl);
			if (nl->debug) {
				print_vector ("xs2", xs2, n);
				g_printerr ("Obj value %.15" GNM_FORMAT_g "\n",
					    y2);
			}

			if (y2 < ymax && gnm_solver_check_constraints (sol)) {
				best_f = f;
				ymax = y2;
				break;
			}
		}

		if (best_f > 0) {
			for (i = 0; i < n; i++)
				xs[i] = xs[i] - best_f * d[i];
			*y = ymax;
			ok = TRUE;
		}

		g_free (xs2);
	} else {
		if (nl->debug)
			g_printerr ("Failed to solve Newton step.\n");
	}

	g_free (d);
	g_free (g);
	free_matrix (H, n);

	return ok;
}