#include "robot36.h"
#include "sstv.h"
#include <stdint.h>

// From Dayton Paper Appendix B - ITU BT.601
// http://www.barberdsp.com/downloads/Dayton%20Paper.pdf
static void rgb_to_ycbcr(uint8_t r, uint8_t g, uint8_t b,
                          double *Y, double *RY, double *BY) {
    *Y  = 16.0  + .003906 * ((65.738  * r) + (129.057 * g) + (25.064  * b));
    *RY = 128.0 + .003906 * ((112.439 * r) + (-94.154 * g) + (-18.285 * b));
    *BY = 128.0 + .003906 * ((-37.945 * r) + (-74.494 * g) + (112.439 * b));
}

static void scan_line_y(wav_t *wav, uint8_t *line) {
    for (int x = 0; x < ROBOT36_WIDTH; x++) {
        double Y, RY, BY;
        rgb_to_ycbcr(line[x*3+0], line[x*3+1], line[x*3+2], &Y, &RY, &BY);
        sstv_tone(wav, 1500.0 + Y * 3.1372549, 88.0 / ROBOT36_WIDTH);
    }
}

static void scan_line_chroma(wav_t *wav, uint8_t *line, int is_ry) {
    for (int x = 0; x < ROBOT36_WIDTH; x++) {
        double Y, RY, BY;
        rgb_to_ycbcr(line[x*3+0], line[x*3+1], line[x*3+2], &Y, &RY, &BY);
        double c = is_ry ? RY : BY;
        sstv_tone(wav, 1500.0 + c * 3.1372549, 44.0 / ROBOT36_WIDTH);
    }
}

void robot36_encode_image(wav_t *wav, uint8_t *rgb) {
    for (int y = 0; y < ROBOT36_HEIGHT; y++) {
        uint8_t *line = &rgb[y * ROBOT36_WIDTH * 3];
        int even = (y % 2 == 0);

        sstv_tone(wav, 1200, 9.0);                // sync
        sstv_tone(wav, 1500, 3.0);                // sync porch
        scan_line_y(wav, line);
        sstv_tone(wav, even ? 1500 : 2300, 4.5);  // separator - tells decoder Cr vs Cb!
        sstv_tone(wav, 1900, 1.5);                // porch before chroma
        scan_line_chroma(wav, line, even);        // Cr on even, Cb on odd
    }
}