/* camera.c */
#include <stdio.h>
#include <math.h>


#define _CAMERA_
#include "camera.h"

/* -------------------------------------------------------------------- */
void InitializeCamera(double _fx, double _fy, double _cx, double _cy,
                      double _k0, double _k1, double _k2, double _k3,
                      double _ax, double _ay, double _az,
                      double _tx, double _ty, double _tz,
                      t_CameraLlorca *camera) {
    double sx = sin(_ax), cx = cos(_ax), sy = sin(_ay), cy = cos(_ay), sz = sin(_az), cz = cos(_az);
    double ss = sx * sy, sc = sx * cy;

    /* intrinsic parameters */
    camera->fx = _fx;
    camera->fy = _fy;
    camera->cx = _cx;
    camera->cy = _cy;

    /* distortion parameters */
    camera->k0 = _k0;
    camera->k1 = _k1;
    camera->k2 = _k2;
    camera->k3 = _k3;

    /* extrinsic parameters wrt world */
    camera->ax = _ax;
    camera->ay = _ay;
    camera->az = _az;
    camera->tx = _tx;
    camera->ty = -_ty;
    camera->tz = _tz;

    camera->rxx = cy * cz - ss * sz;
    camera->rxy = -cx * sz;
    camera->rxz = sy * cz + sc * sz;
    camera->ryx = cy * sz + ss * cz;
    camera->ryy = cx * cz;
    camera->ryz = sy * sz - sc * cz;
    camera->rzx = -sy * cx;
    camera->rzy = sx;
    camera->rzz = cx * cy;
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void World2Cam(t_CameraLlorca *camera, double xw, double yw, double zw,
               double *xc, double *yc, double *zc) {
    xw -= camera->tx;
    yw -= camera->ty;
    zw -= camera->tz;

    *xc = camera->rxx * xw + camera->rxy * yw + camera->rxz * zw;
    *yc = camera->ryx * xw + camera->ryy * yw + camera->ryz * zw;
    *zc = camera->rzx * xw + camera->rzy * yw + camera->rzz * zw;
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Cam2Img(t_CameraLlorca *camera, double xc, double yc, double zc,
             int *xi, int *yi) {
    xc /= zc;
    yc /= zc;

    *xi = (int) floor(xc * camera->fx + camera->cx + .5);
    *yi = (int) floor(yc * camera->fy + camera->cy + .5);
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void World2Img(t_CameraLlorca *camera, double xw, double yw, double zw,
               int *xi, int *yi) {
    double xc, yc, zc;

    World2Cam(camera, xw, yw, zw, &xc, &yc, &zc);
    Cam2Img(camera, xc, yc, zc, xi, yi);
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Img2Cam(t_CameraLlorca *camera, double xi, double yi, double *xc, double *yc) {
    *yc = (yi - camera->cy) / camera->fy;
    *xc = (xi - camera->cx) / camera->fx;
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Cam2WorldX(t_CameraLlorca *camera, double xc, double yc, double xw,
                double *yw, double *zw) {
    double zc = (xw - camera->tx) / (camera->rxx * xc + camera->ryx * yc + camera->rzx);
    *yw = (camera->rxy * xc + camera->ryy * yc + camera->rzy) * zc + camera->ty;
    *zw = (camera->rxz * xc + camera->ryz * yc + camera->rzz) * zc + camera->tz;
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Cam2WorldY(t_CameraLlorca *camera, double xc, double yc, double yw,
                double *xw, double *zw) {
    double zc = (yw - camera->ty) / (camera->rxy * xc + camera->ryy * yc + camera->rzy);
    *xw = (camera->rxx * xc + camera->ryx * yc + camera->rzx) * zc + camera->tx;
    *zw = (camera->rxz * xc + camera->ryz * yc + camera->rzz) * zc + camera->tz;
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Cam2WorldZ(t_CameraLlorca *camera, double xc, double yc, double zw,
                double *xw, double *yw) {
    double zc = (zw - camera->tz) / (camera->rxz * xc + camera->ryz * yc + camera->rzz);
    *xw = (camera->rxx * xc + camera->ryx * yc + camera->rzx) * zc + camera->tx;
    *yw = (camera->rxy * xc + camera->ryy * yc + camera->rzy) * zc + camera->ty;
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Img2WorldX(t_CameraLlorca *camera, double xi, double yi, double xw,
                double *yw, double *zw) {
    double xc, yc;

    Img2Cam(camera, xi, yi, &xc, &yc);
    Cam2WorldX(camera, xc, yc, xw, yw, zw);
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Img2WorldY(t_CameraLlorca *camera, double xi, double yi, double yw,
                double *xw, double *zw) {
    double xc, yc;

    Img2Cam(camera, xi, yi, &xc, &yc);
    Cam2WorldY(camera, xc, yc, yw, xw, zw);
}
/* -------------------------------------------------------------------- */

/* -------------------------------------------------------------------- */
void Img2WorldZ(t_CameraLlorca *camera, double xi, double yi, double zw,
                double *xw, double *yw) {
    double xc, yc;

    Img2Cam(camera, xi, yi, &xc, &yc);
    Cam2WorldZ(camera, xc, yc, zw, xw, yw);
}
/* -------------------------------------------------------------------- */