transform.v

module translate(CLK, reset, lock, init, busy, done,
                 Ax, Ay, Az, Dx, Dy, Dz, Bx, By, Bz);
   input CLK, reset, lock, init;
   wire  CLK, reset, lock, init;
   output busy, done;
   reg    busy, done;

   input [15:0] Ax, Ay, Az, Dx, Dy, Dz;
   wire [15:0]   Ax, Ay, Az, Dx, Dy, Dz;

   output [15:0] Bx, By, Bz;
   reg [15:0]   Bx, By, Bz;

   initial begin
      // we always have single cycle response
      busy <= 1'b0;
   end
   
   always @(posedge CLK) begin
      if (init) begin
         done <= 1'b1;

         Bx <= Ax + Dx;
         By <= Ay + Dy;
         Bz <= Az + Dz;
      end
      else begin
         done <= 1'b0;
         Bx <= 16'hzzzz;
         By <= 16'hzzzz;
         Bz <= 16'hzzzz;
      end
   end // always @ (posedge CLK)
endmodule // translate

module scale(CLK, reset, lock, init, busy, done,
             Ax, Ay, Az, Sx, Sy, Sz, Bx, By, Bz);
   input CLK, reset, lock, init;
   wire  CLK, reset, lock, init;
   output busy, done;
   reg    busy, done;

   input [15:0] Ax, Ay, Az, Sx, Sy, Sz;
   wire [15:0]   Ax, Ay, Az, Sx, Sy, Sz;

   output [15:0] Bx, By, Bz;
   reg [15:0]   Bx, By, Bz;

   initial begin
      // we always have single cycle response
      busy <= 1'b0;
   end

   wire [17:0] scaled_x, scaled_y, scaled_z;
   signed_mult multiplier_x(scaled_x, {Ax, 2'b00}, {Sx, 2'b00});
   signed_mult multiplier_y(scaled_y, {Ay, 2'b00}, {Sy, 2'b00});
   signed_mult multiplier_z(scaled_z, {Az, 2'b00}, {Sz, 2'b00});
   
   always @(posedge CLK) begin
      if (init) begin
         done <= 1'b1;

         Bx <= scaled_x[17:2];
         By <= scaled_y[17:2];
         Bz <= scaled_z[17:2];
      end
      else begin
         done <= 1'b0;
         Bx <= 16'hzzzz;
         By <= 16'hzzzz;
         Bz <= 16'hzzzz;
      end
   end // always @ (posedge CLK)
endmodule // scale

module rotate(CLK, reset, lock, init, busy, done,
             Ax, Ay, Az, theta, axis, Bx, By, Bz);
   input CLK, reset, lock, init;
   wire  CLK, reset, lock, init;
   output busy, done;
   reg    busy, done;

   input [15:0] Ax, Ay, Az, theta;
   wire [15:0]  Ax, Ay, Az, theta;
   // 2'b00 for x, 2'b01 for y, 2'b10 for z, 2'b11 for idk
   input [1:0]  axis;
   wire [1:0]   axis;

   output [15:0] Bx, By, Bz;
   reg [15:0]    Bx, By, Bz;

   initial begin
      // we always have single cycle response
      busy <= 1'b0;
   end

   wire [15:0] sin_out, cos_out;
   sineTable9x16bit sineTable(CLK, theta[8:0], sin_out);
   sineTable9x16bit cosTable(CLK, theta[8:0] + 9'd128, cos_out);

   wire [17:0] x_cos, x_sin, y_cos, y_sin, z_cos, z_sin;
   signed_mult multiplier_xcos(x_cos, {Ax, 2'b00}, {cos_out[15:0], 2'b00});
   signed_mult multiplier_xsin(x_sin, {Ax, 2'b00}, {sin_out[15:0], 2'b00});
   signed_mult multiplier_ycos(y_cos, {Ay, 2'b00}, {cos_out[15:0], 2'b00});
   signed_mult multiplier_ysin(y_sin, {Ay, 2'b00}, {sin_out[15:0], 2'b00});
   signed_mult multiplier_zcos(z_cos, {Az, 2'b00}, {cos_out[15:0], 2'b00});
   signed_mult multiplier_zsin(z_sin, {Az, 2'b00}, {sin_out[15:0], 2'b00});
   
   always @(posedge CLK) begin
      if (init) begin
         done <= 1'b1;

         if (axis == 2'b00) begin
            // rotate about x-axis
            Bx <= Ax;
            By <= y_cos[17:2] - z_cos[17:2];
            Bz <= y_sin[17:2] + z_cos[17:2];
         end
         else if (axis == 2'b01) begin
            // rotate about y-axis
            Bx <= x_cos[17:2] + z_sin[17:2];
            By <= Ay;
            Bz <= -x_sin[17:2] + z_cos[17:2];
         end
         else if (axis == 2'b10) begin
            // rotate about z-axis
            Bx <= x_cos[17:2] - y_sin[17:2];
            By <= x_sin[17:2] + y_cos[17:2];
            Bz <= Az;
         end
         else begin
            // i don't know. undefined operation
            Bx <= Ax;
            By <= Ay;
            Bz <= Az;
         end // else: !if(axis == 2'b10)
      end // if (init)
      else begin
         done <= 1'b0;
         Bx <= 16'hzzzz;
         By <= 16'hzzzz;
         Bz <= 16'hzzzz;
      end
   end // always @ (posedge CLK)
endmodule // rotate