#include "rotation.h"
#include "helpler_funtions.h"

void rotation_to_dcm(enum Rotation rotation, float dcm[3][3]) {

  float yaw = 0.f, roll = 0.f, pitch = 0.f;

  switch (rotation) {
  case ROTATION_NONE:
    break;

  case ROTATION_YAW_45:
    yaw = 45;
    break;
  case ROTATION_YAW_90:
    yaw = 90;
    break;
  case ROTATION_YAW_135:
    yaw = 135;
    break;
  case ROTATION_YAW_180:
    yaw = 180;
    break;
  case ROTATION_YAW_225:
    yaw = 225;
    break;

  case ROTATION_YAW_270:
    yaw = 270;
    break;

  case ROTATION_YAW_315:
    yaw = 315;
    break;

  case ROTATION_ROLL_180:
    roll = 180;
    break;

  case ROTATION_ROLL_180_YAW_45:
    roll = 180;
    yaw = 45;
    break;

  case ROTATION_ROLL_180_YAW_90:
    roll = 180;
    yaw = 90;
    break;

  case ROTATION_PITCH_180_YAW_270:
    pitch = 180;
    yaw = 270;
    break;

  case ROTATION_ROLL_180_YAW_135:
    roll = 180;
    yaw = 135;
    break;

  case ROTATION_PITCH_180:
    pitch = 180;
    break;

  case ROTATION_ROLL_180_YAW_225:
    roll = 180;
    yaw = 225;
    break;

  case ROTATION_ROLL_180_YAW_270:
    roll = 180;
    yaw = 270;
    break;

  case ROTATION_PITCH_180_YAW_90:
    pitch = 180;
    yaw = 90;
    break;

  case ROTATION_ROLL_180_YAW_315:
    roll = 180;
    yaw = 315;
    break;

  case ROTATION_ROLL_90:
    roll = 90;
    break;

  case ROTATION_ROLL_90_YAW_45:
    roll = 90;
    yaw = 45;
    break;

  case ROTATION_ROLL_90_YAW_90:
    roll = 90;
    yaw = 90;
    break;

  case ROTATION_ROLL_90_YAW_135:
    roll = 90;
    yaw = 135;
    break;

  case ROTATION_ROLL_270:
    roll = 270;
    break;

  case ROTATION_ROLL_270_YAW_45:
    roll = 270;
    yaw = 45;
    break;

  case ROTATION_ROLL_270_YAW_90:
    roll = 270;
    yaw = 90;
    break;

  case ROTATION_ROLL_270_YAW_135:
    roll = 270;
    yaw = 135;
    break;

  case ROTATION_PITCH_90:
    pitch = 90;
    break;

  case ROTATION_PITCH_270:
    pitch = 270;
    break;

  case ROTATION_ROLL_90_PITCH_90:
    roll = 90;
    pitch = 90;
    break;

  case ROTATION_ROLL_180_PITCH_90:
    roll = 180;
    pitch = 90;
    break;

  case ROTATION_ROLL_270_PITCH_90:
    roll = 270;
    pitch = 90;
    break;

  case ROTATION_ROLL_90_PITCH_180:
    roll = 90;
    pitch = 180;
    break;

  case ROTATION_ROLL_270_PITCH_180:
    roll = 270;
    pitch = 180;
    break;

  case ROTATION_ROLL_90_PITCH_270:
    roll = 90;
    pitch = 270;
    break;

  case ROTATION_ROLL_180_PITCH_270:
    roll = 180;
    pitch = 270;
    break;

  case ROTATION_ROLL_270_PITCH_270:
    roll = 270;
    pitch = 270;
    break;

  case ROTATION_ROLL_90_PITCH_180_YAW_90:
    roll = 90;
    pitch = 180;
    yaw = 90;
    break;

  case ROTATION_ROLL_90_YAW_270:
    roll = 90;
    yaw = 270;
    break;

  case ROTATION_ROLL_90_PITCH_68_YAW_293:
    roll = 90;
    pitch = 68;
    yaw = 293;
    break;

  case ROTATION_PITCH_315:
    pitch = 315;
    break;

  case ROTATION_ROLL_90_PITCH_315:
    roll = 90;
    pitch = 315;
    break;

  case ROTATION_PITCH_7:
    pitch = 7;
    break;

  case ROTATION_ROLL_45:
    roll = 45;
    break;

  case ROTATION_ROLL_315:
    roll = 315;
    break;

  default:
    break;;
  }

  roll *= DEG_TO_RAD;
  pitch *= DEG_TO_RAD;
  yaw *= DEG_TO_RAD;

  DCM_Enu_ByEuler(-yaw, pitch, roll, dcm);
}